pinocchio  3.1.0 A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
mobilerobot.py
1 import pinocchio as pin
2 from pinocchio.robot_wrapper import RobotWrapper
3 from pinocchio.utils import eye
4
5 import numpy as np
6
7
8 class MobileRobotWrapper(RobotWrapper):
9  '''
10  The class models a mobile robot with UR5 arm, feature 3+6 DOF.
11  The configuration of the basis is represented by [X, Y, cos, sin], with the additionnal constraint
12  that (cos, sin) should have norm 1. Hence take care when sampling and integrating configurations.
13  The robot is depicted as an orange box with two black cylinders (wheels) atop of which is the
14  classical (realistic) UR5 model.
15  The model also features to OPERATIONAL frames, named "mobile" (on the front of the mobile basis, 30cm
16  above the ground) and "tool" (at the very end of the effector).
17  '''
18
19  def __init__(self, urdf, pkgs):
20  self.initFromURDF(urdf, pkgs, pin.JointModelPlanar())
21
22  M0 = pin.SE3(eye(3), np.array([.0, .0, .6]))
23  self.model.jointPlacements[2] = M0 * self.model.jointPlacements[2]
24  self.visual_model.geometryObjects[0].placement = M0 * self.visual_model.geometryObjects[0].placement
25  self.visual_data.oMg[0] = M0 * self.visual_data.oMg[0]
26
27  # Placement of the "mobile" frame wrt basis center.
28  basisMop = pin.SE3(eye(3), np.array([.3, .0, .1]))
29  self.model.addFrame(pin.Frame('mobile', 1, 1, basisMop, pin.FrameType.OP_FRAME))
30
31  # Placement of the tool frame wrt end effector frame (located at the center of the wrist)
32  effMop = pin.SE3(eye(3), np.maarraytrix([.0, .08, .095]))
33  self.model.addFrame(pin.Frame('tool', 6, 6, effMop, pin.FrameType.OP_FRAME))
34
35  # Create data again after setting frames
37
40  if loadModel and not hasattr(self, 'display'):
42
43  try:
44  # self.viewer.gui.deleteNode('world/mobilebasis', True)
45  self.viewer.gui.addBox('world/mobilebasis', .25, .25, .25, [.8, .2, .2, 1])
46  self.viewer.gui.addCylinder('world/mobilewheel1', .05, .45, [.1, .0, .0, 1])
47  self.viewer.gui.addCylinder('world/mobilewheel2', .05, .45, [.1, .0, .0, 1])
48
49  self.viewer.gui.setStaticTransform('world/mobilebasis', [.0, .0, .35, 1.0, .0, .0, .0])
50  self.viewer.gui.setStaticTransform('world/mobilewheel1', [+0.15, .0, .05, .7, .7, .0, .0])
51  self.viewer.gui.setStaticTransform('world/mobilewheel2', [-0.15, .0, .05, .7, .7, .0, .0])
52
53  self.viewer.gui.addXYZaxis('world/framebasis', [1., 0., 0., 1.], .03, .1)
54  self.viewer.gui.addXYZaxis('world/frametool', [1., 0., 0., 1.], .03, .1)
55  except:
56  print("Error when adding 3d objects in the viewer ... ignore")
57
58  def display(self, q):
59  RobotWrapper.display(self, q)