ur5x4.py
Go to the documentation of this file.
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 
18 def loadRobot(M0, name):
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]
28  robot.initDisplay(loadModel=True, viewerRootNodeName="world/" + name)
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.matrix([.3, 0, 0]).T) # 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.matrix([np.pi / 4, -np.pi / 4, -np.pi / 2, np.pi / 4, np.pi / 2, 0]).T
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.matrix([0, 0, .77]).T)
49 gepettoViewer.applyConfiguration('world/toolplate', se3ToXYZQUATtuple(Mtool))
50 gepettoViewer.refresh()
def loadRobot(M0, name)
Definition: ur5x4.py:18
Eigen::Matrix3d rotate(const std::string &axis, const double ang)
Definition: expose-rpy.cpp:19
def se3ToXYZQUATtuple(M)
Definition: deprecated.py:96
SE3Tpl< double, 0 > SE3


pinocchio
Author(s):
autogenerated on Tue Jun 1 2021 02:45:05