def get_link_states(self):
pose, vel = self.get_pose()
self.skeleton.set_pose(pose)
self.skeleton.set_vel(vel)
states = self.get_link_states_by_ids(self.skeleton.get_link_ids())
pos_array = []
rot_array = []
for s in states:
pos_array.append(s[0:3])
rot_array.append(s[3:7])
# belly
#pos_root = np.array(pos_array[0])
#pos_chest = np.array(pos_array[1])
#pos_belly = 0.5*(pos_root + pos_chest)
#states.append([pos_belly[0], pos_belly[1], pos_belly[2], 0, 0, 0, 1])
root_pos = np.array(self.skeleton.get_joint_global_pos(0))
root_rot = Quaternion.fromXYZW(rot_array[0])
root_pos += root_rot.rotate(np.array([0,0.205,0]))
states.append(root_pos.tolist() + [0, 0, 0, 1])
# joints
states.append(self.skeleton.get_joint_global_pos(3) + [0, 0, 0, 1])
states.append(self.skeleton.get_joint_global_pos(9) + [0, 0, 0, 1])
states.append(self.skeleton.get_joint_global_pos(6) + [0, 0, 0, 1])
states.append(self.skeleton.get_joint_global_pos(12) + [0, 0, 0, 1])
# states.append(self.skeleton.get_joint_global_pos(4) + [0, 0, 0, 1])
# states.append(self.skeleton.get_joint_global_pos(10) + [0, 0, 0, 1])
# states.append(self.skeleton.get_joint_global_pos(7) + [0, 0, 0, 1])
# states.append(self.skeleton.get_joint_global_pos(13) + [0, 0, 0, 1])
# neck
neck_pos = np.array(self.skeleton.get_joint_global_pos(2))
head_rot = Quaternion.fromXYZW(rot_array[2])
neck_pos += head_rot.rotate(np.array([0,0.02,0]))
states.append(neck_pos.tolist() + [0, 0, 0, 1])
# clavicle
chest_rot = Quaternion.fromXYZW(rot_array[1])
j_chest_pos = np.array(self.skeleton.get_joint_global_pos(1))
r_clavicle_pos = chest_rot.rotate(np.array([-0.011, 0.24, 0.095])) + j_chest_pos
r_clavicle_rot = chest_rot.mul(Quaternion.fromEulerZYX([-1.64, -0.21, 0.0338])).xyzw()
states.append(r_clavicle_pos.tolist() + r_clavicle_rot.tolist())
l_clavicle_pos = chest_rot.rotate(np.array([-0.011, 0.24, -0.095])) + j_chest_pos
l_clavicle_rot = chest_rot.mul(Quaternion.fromEulerZYX([1.64, 0.21, 0.0338])).xyzw()
states.append(l_clavicle_pos.tolist() + l_clavicle_rot.tolist())
return states
def get_link_states(self):
pos_stick1, rot_stick1 = self.sim_client.getBasePositionAndOrientation(
self.stick1)
pos_stick2, rot_stick2 = self.sim_client.getBasePositionAndOrientation(
self.stick2)
pos_bar, rot_bar = self.sim_client.getBasePositionAndOrientation(
self.object_id)
rot_bar = Quaternion.fromXYZW(rot_bar).mul(
Quaternion.fromXYZW([np.sin(np.pi / 4), 0, 0,
np.cos(np.pi / 4)]))
return [
pos_stick1 + rot_stick1,
pos_stick2 + rot_stick2,
np.array(pos_bar).tolist() + rot_bar.xyzw().tolist(),
]
def get_link_states(self):
angle = self.__wallAngle / 180 * np.pi
offset = np.array([np.cos(angle), 0, np.sin(angle)])
l_bar_pos = np.array(
self.__basePos) - offset * (3.0 - self.__vis_offset)
l_bar_pos[1] = 0
r_bar_pos = np.array(
self.__basePos) + offset * (3.0 + self.__vis_offset)
r_bar_pos[1] = 0
t_bar_quat = Quaternion.fromXYZW(self.__baseOri).mul(
Quaternion.fromAngleAxis(np.pi / 2, np.array([0, 0, 1])))
t_bar_pos = np.array(self.__basePos) + offset * self.__vis_offset
t_bar_pos[1] = self.height - 0.02
return [
l_bar_pos.tolist() + self.__baseOri,
r_bar_pos.tolist() + self.__baseOri,
t_bar_pos.tolist() + t_bar_quat.xyzw().tolist()
]