in sim_agent.py [0:0]
def actuate(self, pose=None, vel=None, torque=None):
if self._actuation == SimAgent.Actuation.NONE:
return
joint_indices = []
target_positions = []
target_velocities = []
kps = []
kds = []
max_forces = []
for j in self._joint_indices:
joint_type = self.get_joint_type(j)
if joint_type == self._pb_client.JOINT_FIXED:
''' Ignore fixed joints '''
continue
joint_indices.append(j)
if self._actuation==SimAgent.Actuation.TQ:
''' No need to compute target values for torque control '''
continue
if self._char_info.bvh_map[j] == None:
''' Use the initial pose if no mapping exists '''
target_pos = self._joint_pose_init[j]
target_vel = self._joint_vel_init[j]
else:
'''
Convert target pose value so that it fits to each joint type
For the hinge joint, we find the geodesic closest value given the axis
For the
'''
if pose is None:
T = constants.eye_T()
else:
T = pose.get_transform(self._char_info.bvh_map[j], local=True)
if vel is None:
w = np.zeros(3)
else:
w = vel.get_angular(self._char_info.bvh_map[j])
if joint_type == self._pb_client.JOINT_REVOLUTE:
axis = self.get_joint_axis(j)
target_pos = np.array([math.project_rotation_1D(conversions.T2R(T), axis)])
target_vel = np.array([math.project_angular_vel_1D(w, axis)])
max_force = np.array([self._char_info.max_force[j]])
elif joint_type == self._pb_client.JOINT_SPHERICAL:
Q, p = conversions.T2Qp(T)
Q = quaternion.Q_op(Q, op=["normalize", "halfspace"])
target_pos = Q
target_vel = w
max_force = np.ones(3) * self._char_info.max_force[j]
else:
raise NotImplementedError
target_positions.append(target_pos)
target_velocities.append(target_vel)
if self._actuation==SimAgent.Actuation.SPD:
kps.append(self._char_info.kp[j])
kds.append(self._char_info.kd[j])
elif self._actuation==SimAgent.Actuation.PD:
''' TODO: remove '''
kps.append(1.5*self._char_info.kp[j])
kds.append(0.01*self._char_info.kd[j])
elif self._actuation==SimAgent.Actuation.CPD or \
self._actuation==SimAgent.Actuation.CP or \
self._actuation==SimAgent.Actuation.V:
kps.append(self._char_info.cpd_ratio*self._char_info.kp[j])
kds.append(self._char_info.cpd_ratio*self._char_info.kd[j])
max_forces.append(max_force)
if self._actuation==SimAgent.Actuation.SPD:
self._pb_client.setJointMotorControlMultiDofArray(self._body_id,
joint_indices,
self._pb_client.STABLE_PD_CONTROL,
targetPositions=target_positions,
targetVelocities=target_velocities,
forces=max_forces,
positionGains=kps,
velocityGains=kds)
elif self._actuation==SimAgent.Actuation.PD:
''' Standard PD in Bullet does not support spherical joint yet '''
# self._pb_client.setJointMotorControlMultiDofArray(self._body_id,
# joint_indices,
# self._pb_client.PD_CONTROL,
# targetPositions=target_positions,
# targetVelocities=target_velocities,
# forces=max_forces,
# positionGains=kps,
# velocityGains=kds)
forces = bu.compute_PD_forces(pb_client=self._pb_client,
body_id=self._body_id,
joint_indices=joint_indices,
desired_positions=target_positions,
desired_velocities=target_velocities,
kps=kps,
kds=kds,
max_forces=max_forces)
self._pb_client.setJointMotorControlMultiDofArray(self._body_id,
joint_indices,
self._pb_client.TORQUE_CONTROL,
forces=forces)
elif self._actuation==SimAgent.Actuation.CPD:
self._pb_client.setJointMotorControlMultiDofArray(self._body_id,
joint_indices,
self._pb_client.POSITION_CONTROL,
targetPositions=target_positions,
targetVelocities=target_velocities,
forces=max_forces,
positionGains=kps,
velocityGains=kds)
elif self._actuation==SimAgent.Actuation.CP:
self._pb_client.setJointMotorControlMultiDofArray(self._body_id,
joint_indices,
self._pb_client.POSITION_CONTROL,
targetPositions=target_positions,
forces=max_forces,
positionGains=kps)
elif self._actuation==SimAgent.Actuation.V:
self._pb_client.setJointMotorControlMultiDofArray(self._body_id,
joint_indices,
self._pb_client.VELOCITY_CONTROL,
targetVelocities=target_velocities,
forces=max_forces,
velocityGains=kds)
elif self._actuation==SimAgent.Actuation.TQ:
self._pb_client.setJointMotorControlMultiDofArray(self._body_id,
joint_indices,
self._pb_client.TORQUE_CONTROL,
forces=torque)
else:
raise NotImplementedError