def get_joint_pv(pb_client, body_id, indices=None):
'''
Return positions and velocities given joint indices.
Please note that the values are locally repsented w.r.t. its parent joint
'''
if indices is None:
indices = range(pb_client.getNumJoints(body_id))
num_indices = len(indices)
assert num_indices > 0
js = pb_client.getJointStatesMultiDof(body_id, indices)
ps = []
vs = []
for j in range(num_indices):
p = np.array(js[j][0])
v = np.array(js[j][1])
if len(p) == 4 and not xyzw_in:
p = quaternion.Q_op(p, op=['change_order'], xyzw_in=True)
ps.append(p)
vs.append(v)
if num_indices == 1:
return ps[0], vs[0]
else:
return ps, vs
def get_link_pQvw(pb_client, body_id, indices=None):
'''
Returns positions, orientations, linear and angular velocities given link indices.
Please use get_base_pQvw for the base link.
'''
if indices is None:
indices = range(pb_client.getNumJoints(body_id))
num_indices = len(indices)
assert num_indices > 0
ls = pb_client.getLinkStates(body_id, indices, computeLinkVelocity=True)
ps = [np.array(ls[j][0]) for j in range(num_indices)]
if not xyzw_in:
Qs = [
quaternion.Q_op(np.array(ls[j][1]), op=['change_order'], xyzw_in=True) \
for j in range(num_indices)]
else:
Qs = [np.array(ls[j][1]) for j in range(num_indices)]
vs = [np.array(ls[j][6]) for j in range(num_indices)]
ws = [np.array(ls[j][7]) for j in range(num_indices)]
if num_indices == 1:
return ps[0], Qs[0], vs[0], ws[0]
else:
return ps, Qs, vs, ws
def _state_body(self,
agent,
T_ref=None,
include_com=True,
include_p=True,
include_Q=True,
include_v=True,
include_w=True,
return_stacked=True):
if T_ref is None:
T_ref = agent.get_facing_transform(self.get_ground_height())
R_ref, p_ref = conversions.T2Rp(T_ref)
R_ref_inv = R_ref.transpose()
link_states = []
link_states.append(agent.get_root_state())
ps, Qs, vs, ws = agent.get_link_states()
for j in agent._joint_indices:
link_states.append((ps[j], Qs[j], vs[j], ws[j]))
state = []
for i, s in enumerate(link_states):
p, Q, v, w = s[0], s[1], s[2], s[3]
if include_p:
p_rel = np.dot(R_ref_inv, p - p_ref)
state.append(
p_rel) # relative position w.r.t. the reference frame
if include_Q:
Q_rel = conversions.R2Q(np.dot(R_ref_inv, conversions.Q2R(Q)))
Q_rel = quaternion.Q_op(Q_rel, op=["normalize", "halfspace"])
state.append(
Q_rel) # relative rotation w.r.t. the reference frame
if include_v:
v_rel = np.dot(R_ref_inv, v)
state.append(
v_rel) # relative linear vel w.r.t. the reference frame
if include_w:
w_rel = np.dot(R_ref_inv, w)
state.append(
w_rel) # relative angular vel w.r.t. the reference frame
if include_com:
if i == 0:
p_com = agent._link_masses[i] * p
v_com = agent._link_masses[i] * v
else:
p_com += agent._link_masses[i] * p
v_com += agent._link_masses[i] * v
if include_com:
p_com /= agent._link_total_mass
v_com /= agent._link_total_mass
state.append(np.dot(R_ref_inv, p_com - p_ref))
state.append(np.dot(R_ref_inv, v_com))
if return_stacked:
return np.hstack(state)
else:
return state
def set_base_pQvw(pb_client, body_id, p, Q, v=None, w=None):
'''
Set positions, orientations, linear and angular velocities of the base link.
'''
if not xyzw_in:
Q = quaternion.Q_op(Q, op=['change_order'], xyzw_in=False)
pb_client.resetBasePositionAndOrientation(body_id, p, Q)
if v is not None and w is not None:
pb_client.resetBaseVelocity(body_id, v, w)
def set_joint_pv(pb_client, body_id, indices, ps, vs):
'''
Set positions and velocities given joint indices.
Please note that the values are locally repsented w.r.t. its parent joint
'''
ps_processed = ps.copy()
for i in range(len(ps_processed)):
if len(ps_processed[i]) == 4 and not xyzw_in:
ps_processed[i] = \
quaternion.Q_op(ps_processed[i], op=['change_order'], xyzw_in=False)
pb_client.resetJointStatesMultiDof(body_id, indices, ps_processed, vs)
def get_base_pQvw(pb_client, body_id):
'''
Returns position, orientation, linear and angular velocities of the base link.
'''
p, Q = pb_client.getBasePositionAndOrientation(body_id)
p, Q = np.array(p), np.array(Q)
if not xyzw_in:
Q = quaternion.Q_op(Q, op=['change_order'], xyzw_in=True)
v, w = pb_client.getBaseVelocity(body_id)
v, w = np.array(v), np.array(w)
return p, Q, v, w
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
