in sim_agent.py [0:0]
def __init__(self,
pybullet_client,
model_file,
char_info,
scale=1.0, # This affects loadURDF
ref_scale=1.0, # This will be used when reference motions are appllied to this agent
verbose=False,
kinematic_only=False,
self_collision=True,
name="agent",
actuation="spd",
):
self._name = name
self._actuation = SimAgent.Actuation.from_string(actuation)
self._pb_client = pybullet_client
self._char_info = char_info
# Load self._body_id file
char_create_flags = self._pb_client.URDF_MAINTAIN_LINK_ORDER
if self_collision:
char_create_flags = char_create_flags|\
self._pb_client.URDF_USE_SELF_COLLISION|\
self._pb_client.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
self._body_id = self._pb_client.loadURDF(model_file,
[0, 0, 0],
globalScaling=scale,
useFixedBase=False,
flags=char_create_flags)
for pair in self._char_info.collison_ignore_pairs:
self._pb_client.setCollisionFilterPair(
self._body_id,
self._body_id,
pair[0],
pair[1],
enableCollision=False)
# TODO: should ref_scale be removed?
self._ref_scale = ref_scale
self._num_joint = self._pb_client.getNumJoints(self._body_id)
self._joint_indices = range(self._num_joint)
self._link_indices = range(-1, self._num_joint)
self._joint_indices_movable = []
if kinematic_only:
self.setup_kinematics()
else:
self.setup_dynamics()
# Pre-compute informations about the agent
self._joint_type = []
self._joint_axis = []
self._joint_dofs = []
for j in self._joint_indices:
joint_info = self._pb_client.getJointInfo(self._body_id, j)
self._joint_type.append(joint_info[2])
self._joint_axis.append(np.array(joint_info[13]))
# if verbose:
# print('-----------------------')
# print(joint_info[1])
# print('joint_type', joint_info[2])
# print('joint_damping', joint_info[6])
# print('joint_friction', joint_info[7])
# print('joint_upper_limit', joint_info[8])
# print('joint_lower_limit', joint_info[9])
# print('joint_max_force', joint_info[10])
# print('joint_max_vel', joint_info[11])
for j in self._joint_indices:
if self._joint_type[j] == self._pb_client.JOINT_SPHERICAL:
self._joint_dofs.append(3)
self._joint_indices_movable.append(j)
elif self._joint_type[j] == self._pb_client.JOINT_REVOLUTE:
self._joint_dofs.append(1)
self._joint_indices_movable.append(j)
elif self._joint_type[j] == self._pb_client.JOINT_FIXED:
self._joint_dofs.append(0)
else:
raise NotImplementedError()
self._num_dofs = np.sum(self._joint_dofs)
self._joint_pose_init, self._joint_vel_init = self.get_joint_states()
self._joint_parent_link = []
self._joint_xform_from_parent_link = []
for j in self._joint_indices:
joint_info = self._pb_client.getJointInfo(self._body_id, j)
joint_local_p = np.array(joint_info[14])
joint_local_Q = np.array(joint_info[15])
link_idx = joint_info[16]
self._joint_parent_link.append(link_idx)
self._joint_xform_from_parent_link.append(
conversions.Qp2T(joint_local_Q, joint_local_p))
self._link_masses = []
self._link_total_mass = 0.0
for i in self._link_indices:
di = self._pb_client.getDynamicsInfo(self._body_id, i)
mass = di[0]
self._link_total_mass += mass
self._link_masses.append(mass)
if verbose:
print('[SimAgent] Creating an agent...', model_file)
print('num_joint <%d>, num_dofs <%d>, total_mass<%f>'\
%(self._num_joint, self._num_dofs, self._link_total_mass))