def _reset_position_and_orientation(self):
"""Reset base position and orientation.
This function is wrapper of pybullet.resetBasePositionAndOrientation.
"""
p.resetBasePositionAndOrientation(self.robot_id, self.translation,
wxyz2xyzw(self.quaternion))
def __init__(self,
robot,
urdf_path=None,
use_fixed_base=False,
connect=1,
*args,
**kwargs):
_check_available()
super(PybulletRobotInterface, self).__init__(*args, **kwargs)
if urdf_path is None:
if robot.urdf_path is not None:
urdf_path = robot.urdf_path
else:
raise ValueError('urdf_path should be given.')
self.robot = robot
if connect == 2:
p.connect(connect)
elif connect == 1:
try:
p.connect(connect)
except Exception as e:
print(e)
self.robot_id = p.loadURDF(urdf_path,
self.translation,
wxyz2xyzw(self.quaternion),
useFixedBase=use_fixed_base)
self.load_bullet()
self.realtime_simulation = False
def tf_from_xytheta(xytheta):
x, y, theta = xytheta
pos = [x, y, theta]
rot = rpy_matrix(*[theta, 0, 0])
quat = wxyz2xyzw(matrix2quaternion(rot))
tf = [pos, quat.tolist()]
return tf
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_wxyz2xyzw(self):
wxyz = np.array([1, 0, 0, 0])
xyzw = wxyz2xyzw(wxyz)
testing.assert_equal(xyzw, np.array([0, 0, 0, 1]))