def pose(self):
"""Getter of Pose in pybullet phsyics simulator.
Wrapper of pybullet.getBasePositionAndOrientation.
Returns
-------
pose : skrobot.coordinates.Coordinates
pose of this robot in the phsyics simulator.
"""
pos, q_xyzw = p.getBasePositionAndOrientation(self.robot_id)
q_wxyz = xyzw2wxyz(q_xyzw)
return Coordinates(pos=pos, rot=q_wxyz)
def _callback_switch_pose(self, msg):
pos, ori = msg.pose.position, msg.pose.orientation
T_switch_to_opt = [[pos.x, pos.y, pos.z], [ori.x, ori.y, ori.z, ori.w]]
# cause skrobt's coordinte transform is bit complicated, I use my own
# NOTE mine is xyzw but skrobot's is wxyz
opt = self.robot_model.__dict__[
"narrow_stereo_optical_frame"].copy_worldcoords()
rot = wxyz2xyzw(matrix2quaternion(opt.rotation))
T_opt_to_base = [opt.translation, rot.tolist()]
T_switch_to_base = convert(T_switch_to_opt, T_opt_to_base)
# convert to skrobot coords
pos, rot = T_switch_to_base
pos[2] = 0.74 # Because we know the height of the switch hahaha
rotmat = skrobot.coordinates.math.rotation_matrix_from_quat(
xyzw2wxyz(rot))
coords = skrobot.coordinates.Coordinates(pos, rotmat)
self._switch_coords = coords
def test_xyzw2wxyz(self):
xyzw = np.array([0, 0, 0, 1])
wxyz = xyzw2wxyz(xyzw)
testing.assert_equal(wxyz, np.array([1, 0, 0, 0]))