pinocchio  2.6.21 A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
ur5x4.py
1 '''
2 Load 4 times the UR5 model, plus a plate object on top of them, to feature a simple parallel robot.
3 No optimization, this file is just an example of how to load the models.
4 '''
5
6 from os.path import join
7
8 from pinocchio import SE3
9 from pinocchio.robot_wrapper import RobotWrapper
10 from pinocchio.utils import rotate, zero, eye, se3ToXYZQUATtuple, urdf
11
12 import numpy as np
13
14 PKG = '/opt/openrobots/share'
15 URDF = join(PKG, 'ur5_description/urdf/ur5_gripper.urdf')
16
17
19  '''
20  This function load a UR5 robot n a new model, move the basis to placement <M0>
21  and add the corresponding visuals in gepetto viewer with name prefix given by string <name>.
22  It returns the robot wrapper (model,data).
23  '''
24  robot = RobotWrapper(urdf, [PKG])
25  robot.model.jointPlacements[1] = M0 * robot.model.jointPlacements[1]
26  robot.visual_model.geometryObjects[0].placement = M0 * robot.visual_model.geometryObjects[0].placement
27  robot.visual_data.oMg[0] = M0 * robot.visual_data.oMg[0]
29  return robot
30
31
32 robots = []
33 # Load 4 Ur5 robots, placed at 0.3m from origin in the 4 directions x,y,-x,-y.
34 Mt = SE3(eye(3), np.array([.3, 0, 0])) # First robot is simply translated
35 for i in range(4):
36  robots.append(loadRobot(SE3(rotate('z', np.pi / 2 * i), zero(3)) * Mt, "robot%d" % i))
37
38 # Set up the robots configuration with end effector pointed upward.
39 q0 = np.array([np.pi / 4, -np.pi / 4, -np.pi / 2, np.pi / 4, np.pi / 2, 0])
40 for i in range(4):
41  robots[i].display(q0)
42
43 # Add a new object featuring the parallel robot tool plate.
44 gepettoViewer = robots[0].viewer.gui
45 w, h, d = 0.25, 0.25, 0.005
46 color = [red, green, blue, transparency] = [1, 1, 0.78, 1.0]
47 gepettoViewer.addBox('world/toolplate', w, h, d, color)
48 Mtool = SE3(rotate('z', 1.268), np.array([0, 0, .77]))
49 gepettoViewer.applyConfiguration('world/toolplate', se3ToXYZQUATtuple(Mtool))
50 gepettoViewer.refresh()
utils
display
Definition: display.py:1