2 import pinocchio
as pin
3 from pinocchio.robot_wrapper
import RobotWrapper
4 from pinocchio.utils
import eye
9 The class models a mobile robot with UR5 arm, feature 3+6 DOF.
10 The configuration of the basis is represented by [X, Y, cos, sin], with the additionnal constraint
11 that (cos, sin) should have norm 1. Hence take care when sampling and integrating configurations.
12 The robot is depicted as an orange box with two black cylinders (wheels) atop of which is the
13 classical (realistic) UR5 model.
14 The model also features to OPERATIONAL frames, named "mobile" (on the front of the mobile basis, 30cm
15 above the ground) and "tool" (at the very end of the effector).
18 def __init__(self, urdf, pkgs):
19 self.initFromURDF(urdf, pkgs, pin.JointModelPlanar())
21 M0 = pin.SE3(eye(3), np.array([0.0, 0.0, 0.6]))
22 self.model.jointPlacements[2] = M0 * self.model.jointPlacements[2]
23 self.visual_model.geometryObjects[0].placement = (
24 M0 * self.visual_model.geometryObjects[0].placement
26 self.visual_data.oMg[0] = M0 * self.visual_data.oMg[0]
29 basisMop = pin.SE3(eye(3), np.array([0.3, 0.0, 0.1]))
30 self.model.addFrame(pin.Frame(
"mobile", 1, 1, basisMop, pin.FrameType.OP_FRAME))
33 effMop = pin.SE3(eye(3), np.maarraytrix([0.0, 0.08, 0.095]))
34 self.model.addFrame(pin.Frame(
"tool", 6, 6, effMop, pin.FrameType.OP_FRAME))
37 self.
datadata = self.model.createData()
39 def initDisplay(self, loadModel):
40 RobotWrapper.initDisplay(self, loadModel=loadModel)
41 if loadModel
and not hasattr(self,
"display"):
42 RobotWrapper.initDisplay(self, loadModel=
False)
46 self.viewer.gui.addBox(
47 "world/mobilebasis", 0.25, 0.25, 0.25, [0.8, 0.2, 0.2, 1]
49 self.viewer.gui.addCylinder(
50 "world/mobilewheel1", 0.05, 0.45, [0.1, 0.0, 0.0, 1]
52 self.viewer.gui.addCylinder(
53 "world/mobilewheel2", 0.05, 0.45, [0.1, 0.0, 0.0, 1]
56 self.viewer.gui.setStaticTransform(
57 "world/mobilebasis", [0.0, 0.0, 0.35, 1.0, 0.0, 0.0, 0.0]
59 self.viewer.gui.setStaticTransform(
60 "world/mobilewheel1", [+0.15, 0.0, 0.05, 0.7, 0.7, 0.0, 0.0]
62 self.viewer.gui.setStaticTransform(
63 "world/mobilewheel2", [-0.15, 0.0, 0.05, 0.7, 0.7, 0.0, 0.0]
66 self.viewer.gui.addXYZaxis(
67 "world/framebasis", [1.0, 0.0, 0.0, 1.0], 0.03, 0.1
69 self.viewer.gui.addXYZaxis(
70 "world/frametool", [1.0, 0.0, 0.0, 1.0], 0.03, 0.1
73 print(
"Error when adding 3d objects in the viewer ... ignore")
76 RobotWrapper.display(self, q)
77 M1 = self.
datadata.oMi[1]
78 self.viewer.gui.applyConfiguration(
79 "world/mobilebasis", pin.SE3ToXYZQUATtuple(M1)
81 self.viewer.gui.applyConfiguration(
82 "world/mobilewheel1", pin.SE3ToXYZQUATtuple(M1)
84 self.viewer.gui.applyConfiguration(
85 "world/mobilewheel2", pin.SE3ToXYZQUATtuple(M1)
87 self.viewer.gui.refresh()
89 pin.updateFramePlacements(self.model, self.
datadata)
90 self.viewer.gui.applyConfiguration(
91 "world/framebasis", pin.SE3ToXYZQUATtuple(self.
datadata.oMf[-2])
93 self.viewer.gui.applyConfiguration(
94 "world/frametool", pin.SE3ToXYZQUATtuple(self.
datadata.oMf[-1])
97 self.viewer.gui.refresh()