def set_pose(self, pose, vel=None):
'''
Velocity should be represented w.r.t. local frame
'''
# Root joint
T = pose.get_transform(self._char_info.bvh_map[self._char_info.ROOT],
local=False)
Q, p = conversions.T2Qp(T)
p *= self._ref_scale
v, w = None, None
if vel is not None:
# Here we give a root orientation to get velocities represeted in world frame.
R = conversions.Q2R(Q)
w = vel.get_angular(self._char_info.bvh_map[self._char_info.ROOT],
False, R)
v = vel.get_linear(self._char_info.bvh_map[self._char_info.ROOT],
False, R)
v *= self._ref_scale
bu.set_base_pQvw(self._pb_client, self._body_id, p, Q, v, w)
# Other joints
indices = []
state_pos = []
state_vel = []
for j in self._joint_indices:
joint_type = self._joint_type[j]
# When the target joint do not have dof, we simply ignore it
if joint_type == self._pb_client.JOINT_FIXED:
continue
# When there is no matching between the given pose and the simulated character,
# the character just tries to hold its initial pose
if self._char_info.bvh_map[j] is None:
state_pos.append(self._joint_pose_init[j])
state_vel.append(self._joint_vel_init[j])
else:
T = pose.get_transform(self._char_info.bvh_map[j], local=True)
if joint_type == self._pb_client.JOINT_SPHERICAL:
Q, p = conversions.T2Qp(T)
w = np.zeros(3) if vel is None else vel.get_angular(
self._char_info.bvh_map[j], local=True)
state_pos.append(Q)
state_vel.append(w)
elif joint_type == self._pb_client.JOINT_REVOLUTE:
joint_axis = self.get_joint_axis(j)
R, p = conversions.T2Rp(T)
w = np.zeros(3) if vel is None else vel.get_angular(
self._char_info.bvh_map[j], local=True)
state_pos.append([math.project_rotation_1D(R, joint_axis)])
state_vel.append(
[math.project_angular_vel_1D(w, joint_axis)])
else:
raise NotImplementedError()
indices.append(j)
bu.set_joint_pv(self._pb_client, self._body_id, indices, state_pos,
state_vel)
def set_pose_by_xform(self, xform):
assert len(xform) == len(self._char_info.bvh_map_inv)
''' Base '''
Q, p = conversions.T2Qp(xform[0])
p *= self._ref_scale
bu.set_base_pQvw(self._pb_client, self._body_id, p, Q, None, None)
''' Others '''
indices = []
state_pos = []
state_vel = []
idx = -1
for k, j in self._char_info.bvh_map_inv.items():
idx += 1
if idx == 0: continue
if j is None: continue
joint_type = self._joint_type[j]
if joint_type == self._pb_client.JOINT_FIXED:
continue
T = xform[idx]
if joint_type == self._pb_client.JOINT_SPHERICAL:
Q, p = conversions.T2Qp(T)
w = np.zeros(3)
state_pos.append(Q)
state_vel.append(w)
elif joint_type == self._pb_client.JOINT_REVOLUTE:
joint_axis = self.get_joint_axis(j)
R, p = conversions.T2Rp(T)
w = np.zeros(3)
state_pos.append(math.project_rotation_1D(R, joint_axis))
state_vel.append(math.project_angular_vel_1D(w, joint_axis))
else:
raise NotImplementedError()
indices.append(j)
bu.set_joint_pv(self._pb_client, self._body_id, indices, state_pos,
state_vel)
def set_transform(self, key, T, local, do_ortho_norm=True):
if local:
T1 = T
else:
T0 = self.skel.get_joint(key).xform_global
T1 = np.dot(math.invertT(T0), T)
if do_ortho_norm:
"""
This insures that the rotation part of
the given transformation is valid
"""
Q, p = conversions.T2Qp(T1)
Q = quaternion.Q_op(Q, op=["normalize"])
T1 = conversions.Qp2T(Q, p)
self.data[self.skel.get_index_joint(key)] = T1
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
def set_root_transform(self, T):
Q, p = conversions.T2Qp(T)
bu.set_base_pQvw(self._pb_client, self._body_id, p, Q, None, None)