├── .gitignore ├── MANIFEST.in ├── README.md ├── pbsm ├── Robotiq2f85 │ ├── __init__.py │ └── pybullet_Robotiq2F85.py ├── __init__.py ├── models │ ├── CompositeTestObject │ │ ├── BoltsWeightsNumbering.pdf │ │ ├── GenerateObjectConfiguration.py │ │ ├── README.md │ │ ├── __init__.py │ │ ├── cad │ │ │ ├── HalfBody.SLDPRT │ │ │ └── HalfBody.STL │ │ ├── meshes │ │ │ └── outershell.stl │ │ ├── package.xml │ │ └── urdf │ │ │ └── compositeH.urdf │ ├── HammerHuge │ │ ├── Hammer.urdf │ │ ├── Mass Properties of Hammer Handle.txt │ │ ├── Mass Properties of Hammer Head.txt │ │ ├── Mass Properties of Hammer.txt │ │ └── meshes │ │ │ ├── Hammer_Handle.stl │ │ │ └── Hammer_Head.stl │ ├── HammerSmaller │ │ ├── CMakeLists.txt │ │ ├── config │ │ │ └── joint_names_HammerSmaller.yaml │ │ ├── launch │ │ │ ├── display.launch │ │ │ └── gazebo.launch │ │ ├── meshes │ │ │ ├── Hammer.ply │ │ │ ├── handle.stl │ │ │ └── head.stl │ │ ├── package.xml │ │ └── urdf │ │ │ ├── HammerSmaller.csv │ │ │ └── HammerSmaller.urdf │ ├── Robotiq_2F85 │ │ ├── CMakeLists.txt │ │ ├── README.md │ │ ├── config │ │ │ └── joint_names_Robotiq_2F85.yaml │ │ ├── launch │ │ │ ├── display.launch │ │ │ └── gazebo.launch │ │ ├── meshes │ │ │ ├── base.STL │ │ │ ├── left_coupler.STL │ │ │ ├── left_driver.STL │ │ │ ├── left_follower.STL │ │ │ ├── left_pad.STL │ │ │ ├── left_spring_link.STL │ │ │ ├── right_coupler.STL │ │ │ ├── right_driver.STL │ │ │ ├── right_follower.STL │ │ │ ├── right_pad.STL │ │ │ └── right_spring_link.STL │ │ ├── package.xml │ │ └── urdf │ │ │ ├── Mechanical_Properties.csv │ │ │ └── Robotiq_2F85.urdf.xacro │ ├── Robotiq_2F85_BuildURDF.xacro │ ├── UR5.urdf │ ├── UR5_2F85.urdf │ ├── UR5_2F85_BuildURDF.xacro │ ├── UR5_BuildURDF.xacro │ ├── cube_small │ │ ├── cube_small.obj │ │ ├── cube_small.obj.mtl │ │ └── cube_small.ply │ ├── ur5 │ │ ├── README.md │ │ ├── ur5.urdf.xacro │ │ └── ur_description │ │ │ ├── CHANGELOG.rst │ │ │ ├── CMakeLists.txt │ │ │ ├── cfg │ │ │ └── view_robot.rviz │ │ │ ├── launch │ │ │ ├── ur10_upload.launch │ │ │ ├── ur3_upload.launch │ │ │ ├── ur5_upload.launch │ │ │ ├── view_ur10.launch │ │ │ ├── view_ur3.launch │ │ │ └── view_ur5.launch │ │ │ ├── meshes │ │ │ ├── ur10 │ │ │ │ ├── collision │ │ │ │ │ ├── base.stl │ │ │ │ │ ├── forearm.stl │ │ │ │ │ ├── shoulder.stl │ │ │ │ │ ├── upperarm.stl │ │ │ │ │ ├── wrist1.stl │ │ │ │ │ ├── wrist2.stl │ │ │ │ │ └── wrist3.stl │ │ │ │ └── visual │ │ │ │ │ ├── base.dae │ │ │ │ │ ├── forearm.dae │ │ │ │ │ ├── shoulder.dae │ │ │ │ │ ├── upperarm.dae │ │ │ │ │ ├── wrist1.dae │ │ │ │ │ ├── wrist2.dae │ │ │ │ │ └── wrist3.dae │ │ │ ├── ur3 │ │ │ │ ├── collision │ │ │ │ │ ├── base.stl │ │ │ │ │ ├── forearm.stl │ │ │ │ │ ├── shoulder.stl │ │ │ │ │ ├── upperarm.stl │ │ │ │ │ ├── wrist1.stl │ │ │ │ │ ├── wrist2.stl │ │ │ │ │ └── wrist3.stl │ │ │ │ └── visual │ │ │ │ │ ├── base.dae │ │ │ │ │ ├── forearm.dae │ │ │ │ │ ├── shoulder.dae │ │ │ │ │ ├── upperarm.dae │ │ │ │ │ ├── wrist1.dae │ │ │ │ │ ├── wrist2.dae │ │ │ │ │ └── wrist3.dae │ │ │ └── ur5 │ │ │ │ ├── collision │ │ │ │ ├── base.stl │ │ │ │ ├── forearm.stl │ │ │ │ ├── shoulder.stl │ │ │ │ ├── upperarm.stl │ │ │ │ ├── wrist1.stl │ │ │ │ ├── wrist2.stl │ │ │ │ └── wrist3.stl │ │ │ │ └── visual │ │ │ │ ├── base.dae │ │ │ │ ├── forearm.dae │ │ │ │ ├── shoulder.dae │ │ │ │ ├── upperarm.dae │ │ │ │ ├── wrist1.dae │ │ │ │ ├── wrist2.dae │ │ │ │ └── wrist3.dae │ │ │ ├── package.xml │ │ │ └── urdf │ │ │ ├── common.gazebo.xacro │ │ │ ├── ur.gazebo.xacro │ │ │ ├── ur.transmission.xacro │ │ │ ├── ur10.urdf.xacro │ │ │ ├── ur10_joint_limited_robot.urdf.xacro │ │ │ ├── ur10_robot.urdf.xacro │ │ │ ├── ur3.urdf.xacro │ │ │ ├── ur3_joint_limited_robot.urdf.xacro │ │ │ ├── ur3_robot.urdf.xacro │ │ │ ├── ur5.urdf.xacro │ │ │ ├── ur5_joint_limited_robot.urdf.xacro │ │ │ └── ur5_robot.urdf.xacro │ └── ur5_compositeobject │ │ └── generate_and_simulate.py ├── ur5 │ ├── __init__.py │ └── pybullet_UR5.py └── ur5_2f85 │ ├── __init__.py │ └── pybullet_UR5_2F85.py ├── setup.py └── test_environment.py /.gitignore: -------------------------------------------------------------------------------- 1 | .venv 2 | *.egg-info 3 | __pycache__/ 4 | -------------------------------------------------------------------------------- /MANIFEST.in: -------------------------------------------------------------------------------- 1 | recursive-include pbsm/models * 2 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Simulation Models for PyBullet 2 | A repository of realistic simulation models. 3 | 4 | ## Installation 5 | To install, `cd` into the repository directory (the one with `setup.py`) and run: 6 | ```bash 7 | pip install . 8 | ``` 9 | or 10 | ```bash 11 | pip install -e . 12 | ``` 13 | The `-e` flag tells pip to install the package in-place, which lets you make changes to the code without having to reinstall every time. *Do not do this on shared workstations!* 14 | 15 | You should then be able to import the package in the Python interpreter. 16 | ```python 17 | $ python3 18 | Python 3.8.5 (default, Jul 28 2020, 12:59:40) 19 | [GCC 9.3.0] on linux 20 | Type "help", "copyright", "credits" or "license" for more information. 21 | >>> import pbsm 22 | pybullet build time: Sep 22 2020 00:56:01 23 | >>> from pbsm import UR5_2F85 24 | >>> 25 | ``` 26 | 27 | ## Quick test 28 | A example script is provided with the package and can be executed with: 29 | ```bash 30 | python3 test_environment.py 31 | ``` 32 | 33 | To build the URDF for the UR5+2F85, use: 34 | ```bash 35 | xacro models/UR5_2F85_BuildURDF.xacro > models/UR5_2F85.urdf 36 | ``` 37 | 38 | Prerequisites: 39 | - [Python > 3.6](https://www.python.org/downloads/) 40 | - [PyBullet](https://github.com/bulletphysics/bullet3) 41 | - [Numpy](https://numpy.org/install/) 42 | - [xacro](http://wiki.ros.org/xacro) (usually installed with [ROS](https://wiki.ros.org/ROS/Installation)) 43 | 44 | And the result should look like this: 45 | 46 | ![pyb-sim-models_GIF](https://user-images.githubusercontent.com/10478385/104060368-c7a74480-51c4-11eb-961f-cb820409438c.gif) 47 | 48 | # Basic Usage 49 | This package provides XACRO files used to generate URDF files. As a convenience, pre-generated URDFs are also included but you will probably want to generate your own URDF after having modified the parameters in the XACROs. 50 | 51 | The main parameters that you might want to modify are: 52 | ```xml 53 | ... 59 | ``` 60 | - Robotiq_2F85 name : Prefix of the gripper 61 | - Robotiq_2F85 enable_underactuation : Set to ```false``` if you want to force pads to stay parallel at all times. 62 | - Robotiq_2F85 soft_pads : Set to ```true``` if you want to simulate the compliance of the pads (not realistic). 63 | - Robotiq_2F85 softness : Used to modulate the amount of compliance for the pads. Used only if ```soft_pads="true"```. 64 | 65 | Finally, a Tool Center Point (TCP) is provided in the UR5+2F85 model as a convenience. This TCP is useful if you want to move the robot such that the end-effector reaches a position in a specific orientation (inverse kinematics problem). The position and orientation of the TCP is defined relative to the base of the 2F85 gripper. The distance between the origin of the base of the gripper and the middle of the pads when the gripper is closed is 130.73mm while the distance to the tip of the pads is 149.48mm. You can modify the position of the TCP depending on how you want to plan the motions of the robot. 66 | 67 | ```xml 68 | 69 | 70 | 71 | 72 | 73 | 74 | ``` 75 | # Models 76 | 77 | ## Universal Robots UR-5 78 | A model of the UR-5 is included in this package. 79 | See [here](https://github.com/utiasSTARS/pyb-sim-models/tree/main/pbsm/models/ur5) for documentation about this model. 80 | 81 | ## Robotiq 2F-85 82 | A realistic model of the under-actuated Robotiq 2F-85 gripper is distributed in this package. 83 | See [here](https://github.com/utiasSTARS/pyb-sim-models/tree/main/pbsm/models/Robotiq_2F85/README.md) for more information about this model. 84 | 85 | ## UR-5 + 2F-85 86 | The URDF tree of the whole robot can be visualized [here](https://github.com/utiasSTARS/pyb-sim-models/files/5791296/UR5_2F85.pdf). 87 | 88 | ## Dynamically Generated Test Objects Models 89 | An modular object that can easily be built from 3D printed parts and small weights is distributed in this package. This object can be useful when the inertial parameters of the object (that cannot be directly measured) must be known for real-world experiments. Models of this object can be dynamically generated by [this script](https://github.com/utiasSTARS/pyb-sim-models/tree/main/pbsm/models/CompositeTestObject/GenerateObjectConfiguration.py) exposing a class as well as a command line interface. 90 | An example of how this model can be used for simulated robotic manipulation is offered [here](https://github.com/utiasSTARS/pyb-sim-models/tree/main/pbsm/models/ur5_compositeobject/generate_and_simulate.py). 91 | See [here](https://github.com/utiasSTARS/pyb-sim-models/tree/main/pbsm/models/CompositeTestObject/README.md) for more information about this model. 92 | 93 | ## Static Test Objects Models 94 | A few other simple objects are also included in the package: 95 | - [Small cube](https://github.com/utiasSTARS/pyb-sim-models/tree/main/pbsm/models/cube_small) 96 | - [Large Hammer](https://github.com/utiasSTARS/pyb-sim-models/tree/main/pbsm/models/HammerHuge) 97 | - [Small Hammer](https://github.com/utiasSTARS/pyb-sim-models/tree/main/pbsm/models/HammerSmaller) 98 | -------------------------------------------------------------------------------- /pbsm/Robotiq2f85/__init__.py: -------------------------------------------------------------------------------- 1 | from .pybullet_Robotiq2F85 import Robotiq2F85 2 | 3 | __author__ = "Philippe Nadeau" 4 | __email__ = "philippe.nadeau@robotics.utias.utoronto.ca" 5 | -------------------------------------------------------------------------------- /pbsm/Robotiq2f85/pybullet_Robotiq2F85.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | import sys 3 | MIN_PYTHON = (3, 8) 4 | if sys.version_info < MIN_PYTHON: 5 | sys.exit("Python %s.%s or later is required.\n" % MIN_PYTHON) 6 | 7 | import os 8 | import logging 9 | import numpy as np 10 | import pybullet as p 11 | 12 | #Note about the 2F-85: 13 | #The chassis and fingers are made out of anodized aluminum 6061 T6. Mass density = 0.0027 Kg/cm^3 14 | #The finger bars are made out of stainless steel 17-4 ph. Mass density = 0.0028 Kg/cm^3 15 | #The silicon of the finger tips is 60A 16 | 17 | class Robotiq2F85: 18 | def __init__(self, simulatorConnectionID, robotUID=None, startingPosition=[0,0,0.001], startingOrientationRAD=[0,0,0]): 19 | 20 | self._cid = simulatorConnectionID 21 | 22 | #If no robot unique id (robotUID) is supplied, we assume that we want to instantiate a new 2F85 23 | if robotUID == None: 24 | startPos = startingPosition 25 | startOrientation = p.getQuaternionFromEuler(startingOrientationRAD) 26 | 27 | dirname = os.path.dirname(__file__) 28 | filename = os.path.join(dirname, '../models/Robotiq_2F85.urdf') 29 | self.gripper = [p.loadURDF(filename,startPos, startOrientation)] 30 | else: 31 | self.gripper = [robotUID] 32 | 33 | #Maximum closing of the gripper 34 | self.MAX_CLOSING = np.deg2rad(45) 35 | #Maximum force of the gripper 36 | self.MAX_FORCE = 10 37 | #Use a single actuator with gears 38 | self.USE_SINGLE_ACTUATOR = False 39 | 40 | #We need to add a constraint for each finger of the gripper to close the kinematic loop. 41 | #The constraint is specified relative to the Center of Mass of the link. The CAD can be used 42 | #to locate the joint relative to the center of mass. 43 | right_follower_index = self.getJointIndexFromName('gripper_right_follower_joint') 44 | right_springlink_index = self.getJointIndexFromName('gripper_right_spring_link_joint') 45 | c = p.createConstraint(parentBodyUniqueId=self.getUID(), parentLinkIndex=right_follower_index, 46 | childBodyUniqueId=self.getUID(), childLinkIndex=right_springlink_index, 47 | jointType=p.JOINT_POINT2POINT, jointAxis=[1, 0, 0], parentFramePosition=[6/1000,-6.7/1000,0], childFramePosition=[0,28.9/1000,0]) 48 | #Make the constraint very strong so an actuation cannot separate the links 49 | #For details about ERP: https://raw.githubusercontent.com/bulletphysics/bullet3/master/examples/Utils/b3ERPCFMHelper.hpp 50 | p.changeConstraint(c, maxForce=9999, erp=0.8) 51 | 52 | left_follower_index = self.getJointIndexFromName('gripper_left_follower_joint') 53 | left_springlink_index = self.getJointIndexFromName('gripper_left_spring_link_joint') 54 | c = p.createConstraint(parentBodyUniqueId=self.getUID(), parentLinkIndex=left_follower_index, 55 | childBodyUniqueId=self.getUID(), childLinkIndex=left_springlink_index, 56 | jointType=p.JOINT_POINT2POINT, jointAxis=[1, 0, 0], parentFramePosition=[6/1000,-6.7/1000,0], childFramePosition=[0,28.9/1000,0]) 57 | #Make the constraint very strong so an actuation cannot separate the links 58 | p.changeConstraint(c, maxForce=9999, erp=0.8) 59 | 60 | #There is a pin fixed on the driver link which limits the movement of the coupler link. If we dont use self-collision (for performances and stability purposes) 61 | #then we need to make sure that the coupler does not move through the pin. We can do that by limiting the rotation of the coupler/driver joint. This constraint 62 | #is implemented in the URDF but is respected ONLY IF its also implemented here. Try commenting out the two following lines to test it. 63 | p.changeDynamics(bodyUniqueId=self.getUID(), linkIndex=self.getJointIndexFromName('gripper_right_coupler_joint'), jointLowerLimit=0.13 , jointUpperLimit=0.79) 64 | p.changeDynamics(bodyUniqueId=self.getUID(), linkIndex=self.getJointIndexFromName('gripper_left_coupler_joint'), jointLowerLimit=0.13 , jointUpperLimit=0.79) 65 | 66 | #A JOINT_GEAR can be used to actuate the left finger when the right one is actuated but external forcces 67 | #can break the symmetry so its better to use two separate actuators instead. 68 | if self.USE_SINGLE_ACTUATOR == True: 69 | right_driver_joint_index = self.getJointIndexFromName('gripper_right_driver_joint') 70 | left_driver_joint_index = self.getJointIndexFromName('gripper_left_driver_joint') 71 | #A JOINT_GEAR is unidirectional so two constraints needs to be added to have the real gear behaviour 72 | c = p.createConstraint(parentBodyUniqueId=self.getUID(), parentLinkIndex=right_driver_joint_index, 73 | childBodyUniqueId=self.getUID(), childLinkIndex=left_driver_joint_index, jointType=p.JOINT_GEAR, 74 | jointAxis=[0,0,1], parentFramePosition=[0,0,0], childFramePosition=[0,0,0]) 75 | p.changeConstraint(c, gearRatio=-1, maxForce=self.MAX_FORCE) 76 | c = p.createConstraint(parentBodyUniqueId=self.getUID(), parentLinkIndex=left_driver_joint_index, 77 | childBodyUniqueId=self.getUID(), childLinkIndex=right_driver_joint_index, jointType=p.JOINT_GEAR, 78 | jointAxis=[0,0,1], parentFramePosition=[0,0,0], childFramePosition=[0,0,0]) 79 | p.changeConstraint(c, gearRatio=-1, maxForce=self.MAX_FORCE) 80 | 81 | # WATCH OUT!!! In PyBullet, all revolute and slider joints have active motors by default. This does not work well for an underactuated mechanism. 82 | # Therefore, we need to disable some of them by setting a maximal force to 0 and allowing a full 360 degres rotation 83 | p.setJointMotorControl2(bodyIndex=self.getUID(), jointIndex=self.getJointIndexFromName('gripper_right_follower_joint'), controlMode=p.VELOCITY_CONTROL, force=0) 84 | p.setJointMotorControl2(bodyIndex=self.getUID(), jointIndex=self.getJointIndexFromName('gripper_left_follower_joint'), controlMode=p.VELOCITY_CONTROL, force=0) 85 | p.setJointMotorControl2(bodyIndex=self.getUID(), jointIndex=self.getJointIndexFromName('gripper_left_spring_link_joint'), controlMode=p.VELOCITY_CONTROL, force=0) 86 | p.setJointMotorControl2(bodyIndex=self.getUID(), jointIndex=self.getJointIndexFromName('gripper_right_spring_link_joint'), controlMode=p.VELOCITY_CONTROL, force=0) 87 | p.setJointMotorControl2(bodyIndex=self.getUID(), jointIndex=self.getJointIndexFromName('gripper_right_coupler_joint'), controlMode=p.VELOCITY_CONTROL, force=0) 88 | p.setJointMotorControl2(bodyIndex=self.getUID(), jointIndex=self.getJointIndexFromName('gripper_left_coupler_joint'), controlMode=p.VELOCITY_CONTROL, force=0) 89 | 90 | #If the left finger is driven by the right one, the left driver must not be motorized 91 | if self.USE_SINGLE_ACTUATOR == True: 92 | p.setJointMotorControl2(bodyIndex=self.getUID(), jointIndex=self.getJointIndexFromName('gripper_left_driver_joint'), controlMode=p.VELOCITY_CONTROL, force=0) 93 | 94 | #The mass and inertia of the links are grossly inaccurate in most (all?) models available online but the impact of these values on the simulation can be very important. 95 | #The following values were obtained from the CAD supplied by Robotiq using the anodized aluminum 6061 T6 with mass density = 0.0027 Kg/cm^3 96 | #PyBullet/URDF uses Mass specified in Kg and inertia in Kg/m^2 97 | #The right inertial parameters are now implemented in the URDF 98 | 99 | #Initialize the state of the gripper to fully open 100 | p.resetJointState(bodyUniqueId=self.getUID(), jointIndex=self.getJointIndexFromName('gripper_right_driver_joint'), targetValue=0) 101 | p.resetJointState(bodyUniqueId=self.getUID(), jointIndex=self.getJointIndexFromName('gripper_left_driver_joint'), targetValue=0) 102 | 103 | #Use a single actuator with gears instead of two independant actuators 104 | def useSingleActuator(self, useSingle): 105 | self.USE_SINGLE_ACTUATOR = useSingle 106 | 107 | def setMaxForce(self, max_force): 108 | self.MAX_FORCE = max_force 109 | 110 | def getUID(self): 111 | return self.gripper[0] 112 | 113 | def initJoints(self, jointsPositionDEG): 114 | #Initialize the state of the gripper to fully open 115 | p.resetJointState(bodyUniqueId=self.getUID(), jointIndex=self.getJointIndexFromName('gripper_right_driver_joint'), targetValue=jointsPositionDEG[0]) 116 | p.resetJointState(bodyUniqueId=self.getUID(), jointIndex=self.getJointIndexFromName('gripper_left_driver_joint'), targetValue=jointsPositionDEG[1]) 117 | 118 | #Returns the joints positions of the gripper 119 | def getGripperState(self): 120 | current_joint_position = [ p.getJointState(bodyUniqueId=self.getUID(), jointIndex=self.getJointIndexFromName('gripper_left_driver_joint'))[0], 121 | p.getJointState(bodyUniqueId=self.getUID(), jointIndex=self.getJointIndexFromName('gripper_right_driver_joint'))[0] ] 122 | return current_joint_position 123 | 124 | def getCouplerState(self): 125 | current_joint_position = [ p.getJointState(bodyUniqueId=self.getUID(), jointIndex=self.getJointIndexFromName('gripper_left_coupler_joint'))[0], 126 | p.getJointState(bodyUniqueId=self.getUID(), jointIndex=self.getJointIndexFromName('gripper_right_coupler_joint'))[0] ] 127 | return current_joint_position 128 | 129 | #Returns joints position error from goal position 130 | def getStateError(self, gripper_joint_positions_goal): 131 | g = gripper_joint_positions_goal 132 | state = self.getGripperState() 133 | error = np.linalg.norm([state[0]-g[0]*(self.MAX_CLOSING/100),state[1]-g[1]*(self.MAX_CLOSING/100)]) 134 | return error 135 | 136 | def getMaximumClosing(self): 137 | #The driver joint is motorized. The value of 0.7 for maximum closing has been found by trial and error 138 | return self.MAX_CLOSING 139 | 140 | #Uses position control to actuate each driver joint of the gripper 141 | #Each of left_finger_goal, right_finger_goal is a percentage that specify how close each finger is (100 = Fully closed, 0 = Fully open) 142 | def setGoal(self, left_finger_goal, right_finger_goal): 143 | 144 | right_finger_goal = max(min(right_finger_goal,100),0) 145 | left_finger_goal = max(min(left_finger_goal,100),0) 146 | 147 | p.setJointMotorControl2(targetPosition=right_finger_goal*self.MAX_CLOSING/100, bodyIndex=self.getUID(), jointIndex=self.getJointIndexFromName('gripper_right_driver_joint'), controlMode=p.POSITION_CONTROL, maxVelocity=1, force=self.MAX_FORCE) 148 | if self.USE_SINGLE_ACTUATOR == False: 149 | p.setJointMotorControl2(targetPosition=left_finger_goal*self.MAX_CLOSING/100, bodyIndex=self.getUID(), jointIndex=self.getJointIndexFromName('gripper_left_driver_joint'), controlMode=p.POSITION_CONTROL, maxVelocity=1, force=self.MAX_FORCE) 150 | 151 | #Acts as the spring in the gripper that keeps the pads parallel 152 | p.setJointMotorControl2(targetPosition=self.MAX_CLOSING, bodyIndex=self.getUID(), jointIndex=self.getJointIndexFromName('gripper_right_spring_link_joint'), controlMode=p.POSITION_CONTROL, force=1) 153 | p.setJointMotorControl2(targetPosition=-1*self.MAX_CLOSING, bodyIndex=self.getUID(), jointIndex=self.getJointIndexFromName('gripper_left_spring_link_joint'), controlMode=p.POSITION_CONTROL, force=1) 154 | 155 | #Build a dictionnary that can then be used to efficiently retrieve 156 | #joints unique IDs from their names. 157 | def buildJointIndexFromNameDict(self): 158 | self.jointsNamesDict = {} 159 | 160 | numJoints = p.getNumJoints(self.getUID(), self._cid) 161 | 162 | for i in range(numJoints): 163 | jointInfo = p.getJointInfo(self.getUID(), i, self._cid) 164 | strJointName = jointInfo[1].decode('UTF-8') 165 | self.jointsNamesDict[strJointName] = jointInfo[0] 166 | 167 | #Retrieve the unique ID associated with a joint name. 168 | #Returns None if the joint name is not found 169 | def getJointIndexFromName(self, JointName): 170 | #Verify that the joints dictonary has been populated 171 | #if its not the case, do it. 172 | try: 173 | if len(self.jointsNamesDict) == 0: 174 | self.buildJointIndexFromNameDict() 175 | except AttributeError: 176 | self.buildJointIndexFromNameDict() 177 | 178 | jointId = self.jointsNamesDict.get(JointName) 179 | if jointId == None: 180 | logging.warning('Cannot find a joint named: '+JointName) 181 | return jointId 182 | -------------------------------------------------------------------------------- /pbsm/__init__.py: -------------------------------------------------------------------------------- 1 | from .ur5 import UR5 2 | from .Robotiq2f85 import Robotiq2F85 3 | from .ur5_2f85 import UR5_2F85 4 | from .models.CompositeTestObject import CompositeTestObject 5 | 6 | __author__ = "Philippe Nadeau" 7 | __email__ = "philippe.nadeau@robotics.utias.utoronto.ca" 8 | -------------------------------------------------------------------------------- /pbsm/models/CompositeTestObject/BoltsWeightsNumbering.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/CompositeTestObject/BoltsWeightsNumbering.pdf -------------------------------------------------------------------------------- /pbsm/models/CompositeTestObject/README.md: -------------------------------------------------------------------------------- 1 | # Composite Test Object for Simulated Robotic Manipulation Tasks 2 | This repository contains code used to generate URDF models of a variety of test objects. The objects are built from two 3d-printed structures, M6 bolts and cylindrical weights. The idea is to make it easy to produce real objects that are very similar to the simulated ones and that can be arranged in a variety of configurations depending on where the weights are placed. 3 | 4 | ![CompositeTestObjectRenderingCropped](https://user-images.githubusercontent.com/10478385/129379216-9d5654d4-48e8-4de1-a522-49244e420ea1.png) 5 | 6 | ## Usage 7 | The python script takes as arguments the locations of the bolts and weights and returns as the output the name of the generated URDF, the mass, the centre of mass and the inertia matrix of the object. 8 | ``` 9 | usage: GenerateObjectConfiguration.py [-h] [-b [POSITION ...]] [-s [POSITION ...]] [-p [POSITION ...]] [-o [FILE]] [--print] [--linkxacro] [-v] 10 | 11 | Generate the URDF of a composite object with predictable inertial parameters. 12 | 13 | optional arguments: 14 | -h, --help show this help message and exit 15 | -b [POSITION ...], --bolts [POSITION ...] 16 | List of bolt locations. 17 | -s [POSITION ...], --steelweights [POSITION ...] 18 | List of weights locations where steel weights are placed. 19 | -p [POSITION ...], --plasticweights [POSITION ...] 20 | List of weights locations where plastic weights are placed. 21 | -o [FILE], --output [FILE] 22 | Optional filename of the output URDF to generate. 23 | --print Prints the resulting inertial parameters. 24 | --linkxacro Generates a XACRO file, that can be included by a parent XACRO, containing only the link. 25 | -v, --version show program's version number and exit 26 | ``` 27 | 28 | ### Holes locations and numbering 29 | The bolt holes (Bx) and weight holes (Wy) are referred using a number system that is based on the following chart. As the input, the script accepts numbers which refers to those in the chart, but no sanity-check is done. 30 | 31 | ![BoltsWeightsNumbering](https://user-images.githubusercontent.com/10478385/120645605-99adef80-c446-11eb-883a-d9bfffcadfc3.png) 32 | 33 | 34 | ### Example 35 | ``` 36 | > python3 GenerateObjectConfiguration.py -b 0 14 -s 0 1 2 3 4 -o test.urdf --print 37 | test.urdf 38 | Inertial Parameters: 39 | Total Mass: 1.1718997410246232 40 | Center of Mass wrt. origin: [0.0000000000 0.0409352866 0.0250000000] 41 | Inertia Tensor wrt. CoM: 42 | [[0.0117931994 0.0000000000 0.0000000000] 43 | [0.0000000000 0.0025575726 0.0000000000] 44 | [0.0000000000 0.0000000000 0.0139859114]] 45 | ``` 46 | 47 | ### Interesting Weight Patterns 48 | A few patterns that can be made with the modular object might be particularly interesting. Note that the locations of the bolts are not specified because you will want to choose locations depending on your experimental setup. 49 | 50 | ``` 51 | Symbols: 52 | Steel weight (S): 0 53 | Plastic Weight (P): o 54 | No weight : . 55 | 56 | Patterns Weights Locations 57 | 58 | 0 0 0 59 | 0 S: 0 1 2 3 60 | o P: 4 5 6 8 61 | o 62 | o 63 | . o . 64 | "Hammer" 65 | 66 | 0 0 0 67 | o S: 0 1 2 7 8 9 68 | o P: 3 4 5 6 69 | o 70 | o 71 | 0 0 0 72 | "Barbell" 73 | 74 | 0 0 0 75 | 0 S: 0 1 2 3 4 5 6 76 | 0 77 | 0 78 | 0 79 | . . . 80 | "Tee" 81 | 82 | 0 0 0 83 | 0 S: 0 1 2 3 4 5 6 7 8 9 84 | 0 85 | 0 86 | 0 87 | 0 0 0 88 | "Homogeneous" 89 | 90 | 0 . 0 91 | . S: 0 2 7 9 92 | . 93 | . 94 | . 95 | 0 . 0 96 | "Corners" 97 | 98 | . 0 . 99 | 0 S: 1 3 4 5 6 8 100 | 0 101 | 0 102 | 0 103 | . 0 . 104 | "Rod" 105 | 106 | 0 0 0 107 | 0 S: 0 1 2 3 4 108 | 0 P: 5 6 7 8 9 109 | o 110 | o 111 | o o o 112 | "Half-N-Half" 113 | ``` 114 | To generate these objects, the following command lines can be used (bolts locations are not specified): 115 | ``` 116 | $ ./GenerateObjectConfiguration.py -s 0 1 2 3 -p 4 5 6 8 -o Hammer.urdf 117 | $ ./GenerateObjectConfiguration.py -s 0 1 2 7 8 9 -p 3 4 5 6 -o Barbell.urdf 118 | $ ./GenerateObjectConfiguration.py -s 0 1 2 3 4 5 6 -o Tee.urdf 119 | $ ./GenerateObjectConfiguration.py -s 0 1 2 3 4 5 6 7 8 9 -o Homogeneous.urdf 120 | $ ./GenerateObjectConfiguration.py -s 0 2 7 9 -o Corners.urdf 121 | $ ./GenerateObjectConfiguration.py -s 1 3 4 5 6 8 -o Rod.urdf 122 | $ ./GenerateObjectConfiguration.py -s 0 1 2 3 4 -p 5 6 7 8 9 -o Half-N-Half.urdf 123 | ``` 124 | 125 | 126 | ## Plastic Half-structure 127 | Inside the structure (left) and outside the structure (right). Large holes and used to host the weights and smaller holes are used to host the bolts. Both halves are alike and the space used to fit the head of the M6 bolt can also be used to fit a hex nut. 128 | 129 | Inside of Structure 130 | Outside of Structure 131 | 132 | The files needed to 3D print the two identical halves can be found [in the cad directory](https://github.com/utiasSTARS/pyb-sim-models/tree/main/pbsm/models/CompositeTestObject/cad) that should contain a [Solidworks SLDPRT file](https://github.com/utiasSTARS/pyb-sim-models/tree/main/pbsm/models/CompositeTestObject/cad/HalfBody.SLDPRT) and a [3D printable STL file](https://github.com/utiasSTARS/pyb-sim-models/tree/main/pbsm/models/CompositeTestObject/cad/HalfBody.STL). When printing the structure using PLA with a 20% infill, the weight of each half-structure should be about 320 grams. 133 | 134 | ## Weights 135 | The cylindrical weights have a diameter of 1 inch (25.4mm) and a height of 0.5 inch (12.7mm). Two kits (20 weights) were bought from [here](https://www.amazon.ca/Precision-Calibration-Digital-Balance-Jewellery/dp/B082MMGG92) and pairs of two 50 grams weights are attached together such that each cylindrical steel weight (with height 1 inch) is 100 grams (mass density of about 7.77 g/cm^3 or 7770 Kg/m^3). 136 | 137 | Similarly sized weights can easily be made out of wood, plastic or foam rods, providing a variety of weights. For instance, an ABS rod was bought from [here](https://www.mcmaster.com/8587K6/) and cut into pieces to make several 17 grams weights. 138 | 139 | ## Bolts 140 | The two halves of the structures are meant to be held together with bolts, holding the weights in between. The diameter of the bolt holes is 6.6 mm and the space for the head of the bolts has a diameter of 11.8 mm and a depth of 6 mm. The distance between two vertices of the hex nut being 11.547 mm, some clearance is left to allow the nut to turn and a small tool to hold it while a bolt is being placed. 141 | - [Bolts](https://www.mcmaster.com/91290A203/) each weighting about 11 grams 142 | - [Nuts](https://www.mcmaster.com/90592A016/) each weighting about 2 grams 143 | 144 | ## Citation 145 | If you used any part of this software in your work, please cite our paper: 146 | ``` 147 | @inproceedings{nadeau_fastInertialIdent_2022, 148 | AUTHOR = {Philippe Nadeau AND Matthew Giamou AND Jonathan Kelly}, 149 | TITLE = { {Fast Object Inertial Parameter Identification for Collaborative Robots} }, 150 | BOOKTITLE = {Proceedings of the {IEEE} International Conference on Robotics and Automation {(ICRA)}}, 151 | DATE = {2022-05-23/2022-05-27}, 152 | YEAR = {2022}, 153 | MONTH = {May}, 154 | ADDRESS = {Philadelphia, PA, USA}, 155 | DOI = {10.1109/icra46639.2022.9916213}, 156 | URL = {https://arxiv.org/abs/2203.00830}, 157 | VIDEO = {https://www.youtube.com/watch?v=BNgGSMkgfY4} 158 | } 159 | ``` 160 | -------------------------------------------------------------------------------- /pbsm/models/CompositeTestObject/__init__.py: -------------------------------------------------------------------------------- 1 | from .GenerateObjectConfiguration import CompositeTestObject 2 | 3 | __author__ = "Philippe Nadeau" 4 | __email__ = "philippe.nadeau@robotics.utias.utoronto.ca" 5 | -------------------------------------------------------------------------------- /pbsm/models/CompositeTestObject/cad/HalfBody.SLDPRT: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/CompositeTestObject/cad/HalfBody.SLDPRT -------------------------------------------------------------------------------- /pbsm/models/CompositeTestObject/cad/HalfBody.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/CompositeTestObject/cad/HalfBody.STL -------------------------------------------------------------------------------- /pbsm/models/CompositeTestObject/meshes/outershell.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/CompositeTestObject/meshes/outershell.stl -------------------------------------------------------------------------------- /pbsm/models/CompositeTestObject/package.xml: -------------------------------------------------------------------------------- 1 | 2 | Composite Test Object 3 | 1.0.0 4 | 5 |

URDF Description package for Composite Test Object

6 |

Used to test inertial parameters identification algorithms.

7 | 8 | Philippe Nadeau 9 | 10 | BSD 11 | catkin 12 | roslaunch 13 | robot_state_publisher 14 | rviz 15 | joint_state_publisher_gui 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /pbsm/models/CompositeTestObject/urdf/compositeH.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 9 | 11 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /pbsm/models/HammerHuge/Hammer.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /pbsm/models/HammerHuge/Mass Properties of Hammer Handle.txt: -------------------------------------------------------------------------------- 1 | Mass properties of Hammer tail 2 | Configuration: GES No 3 | Coordinate system: -- default -- 4 | 5 | Density = 0 grams per cubic millimeter 6 | 7 | Mass = 130 grams 8 | 9 | Volume = 266308 cubic millimeters 10 | 11 | Surface area = 38834 square millimeters 12 | 13 | Center of mass: ( millimeters ) 14 | X = 0 15 | Y = 0 16 | Z = -184 17 | 18 | Principal axes of inertia and principal moments of inertia: ( grams * square millimeters ) 19 | Taken at the center of mass. 20 | Ix = ( 0, 0, 1) Px = 19811 21 | Iy = ( 1, 0, 0) Py = 1392773 22 | Iz = ( 0, 1, 0) Pz = 1403771 23 | 24 | Moments of inertia: ( grams * square millimeters ) 25 | Taken at the center of mass and aligned with the output coordinate system. 26 | Lxx = 1392773 Lxy = 0 Lxz = 0 27 | Lyx = 0 Lyy = 1403771 Lyz = 0 28 | Lzx = 0 Lzy = 0 Lzz = 19811 29 | 30 | Moments of inertia: ( grams * square millimeters ) 31 | Taken at the output coordinate system. 32 | Ixx = 5809967 Ixy = 0 Ixz = 0 33 | Iyx = 0 Iyy = 5820965 Iyz = 0 34 | Izx = 0 Izy = 0 Izz = 19811 -------------------------------------------------------------------------------- /pbsm/models/HammerHuge/Mass Properties of Hammer Head.txt: -------------------------------------------------------------------------------- 1 | Mass properties of Hammer Head 2 | Configuration: GES No 3 | Coordinate system: -- default -- 4 | 5 | Density = 0 grams per cubic millimeter 6 | 7 | Mass = 1832 grams 8 | 9 | Volume = 229021 cubic millimeters 10 | 11 | Surface area = 35620 square millimeters 12 | 13 | Center of mass: ( millimeters ) 14 | X = 0 15 | Y = 16 16 | Z = -5 17 | 18 | Principal axes of inertia and principal moments of inertia: ( grams * square millimeters ) 19 | Taken at the center of mass. 20 | Ix = ( 0, 1, 0) Px = 678706 21 | Iy = (-1, 0, 0) Py = 5594506 22 | Iz = ( 0, 0, 1) Pz = 5834889 23 | 24 | Moments of inertia: ( grams * square millimeters ) 25 | Taken at the center of mass and aligned with the output coordinate system. 26 | Lxx = 5594554 Lxy = -8825 Lxz = -2637 27 | Lyx = -8825 Lyy = 780911 Lyz = -718652 28 | Lzx = -2637 Lzy = -718652 Lzz = 5732637 29 | 30 | Moments of inertia: ( grams * square millimeters ) 31 | Taken at the output coordinate system. 32 | Ixx = 6099577 Ixy = -10941 Ixz = -1900 33 | Iyx = -10941 Iyy = 835613 Iyz = -875590 34 | Izx = -1900 Izy = -875590 Izz = 6182978 -------------------------------------------------------------------------------- /pbsm/models/HammerHuge/Mass Properties of Hammer.txt: -------------------------------------------------------------------------------- 1 | Mass properties of HAMMER0001 2 | Configuration: GES No 3 | Coordinate system: -- default -- 4 | 5 | Mass = 1963 grams 6 | 7 | Volume = 495329 cubic millimeters 8 | 9 | Surface area = 74454 square millimeters 10 | 11 | Center of mass: ( millimeters ) 12 | X = 0 13 | Y = 16 14 | Z = -15 15 | 16 | Principal axes of inertia and principal moments of inertia: ( grams * square millimeters ) 17 | Taken at the center of mass. 18 | Ix = ( 0, 1, 0) Px = 4247832 19 | Iy = ( 0, 0, 1) Py = 6084733 20 | Iz = ( 1, 0, 0) Pz = 9404805 21 | 22 | Moments of inertia: ( grams * square millimeters ) 23 | Taken at the center of mass and aligned with the output coordinate system. 24 | Lxx = 9404781 Lxy = -8831 Lxz = -3898 25 | Lyx = -8831 Lyy = 4580085 Lyz = -707048 26 | Lzx = -3898 Lzy = -707048 Lzz = 5752504 27 | 28 | Moments of inertia: ( grams * square millimeters ) 29 | Taken at the output coordinate system. 30 | Ixx = 10314805 Ixy = -10941 Ixz = -1899 31 | Iyx = -10941 Iyy = 5010482 Iyz = -1161393 32 | Izx = -1899 Izy = -1161393 Izz = 6232149 -------------------------------------------------------------------------------- /pbsm/models/HammerHuge/meshes/Hammer_Handle.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/HammerHuge/meshes/Hammer_Handle.stl -------------------------------------------------------------------------------- /pbsm/models/HammerHuge/meshes/Hammer_Head.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/HammerHuge/meshes/Hammer_Head.stl -------------------------------------------------------------------------------- /pbsm/models/HammerSmaller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | 3 | project(HammerSmaller) 4 | 5 | find_package(catkin REQUIRED) 6 | 7 | catkin_package() 8 | 9 | find_package(roslaunch) 10 | 11 | foreach(dir config launch meshes urdf) 12 | install(DIRECTORY ${dir}/ 13 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) 14 | endforeach(dir) 15 | -------------------------------------------------------------------------------- /pbsm/models/HammerSmaller/config/joint_names_HammerSmaller.yaml: -------------------------------------------------------------------------------- 1 | controller_joint_names: ['', ] 2 | -------------------------------------------------------------------------------- /pbsm/models/HammerSmaller/launch/display.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 7 | 11 | 15 | 20 | -------------------------------------------------------------------------------- /pbsm/models/HammerSmaller/launch/gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 9 | 15 | 20 | -------------------------------------------------------------------------------- /pbsm/models/HammerSmaller/meshes/Hammer.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/HammerSmaller/meshes/Hammer.ply -------------------------------------------------------------------------------- /pbsm/models/HammerSmaller/meshes/handle.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/HammerSmaller/meshes/handle.stl -------------------------------------------------------------------------------- /pbsm/models/HammerSmaller/meshes/head.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/HammerSmaller/meshes/head.stl -------------------------------------------------------------------------------- /pbsm/models/HammerSmaller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | HammerSmaller 3 | 1.0.0 4 | 5 |

URDF Description package for HammerSmaller

6 |

This package contains configuration data, 3D models and launch files 7 | for HammerSmaller robot

8 | 9 | TODO 10 | 11 | BSD 12 | catkin 13 | roslaunch 14 | robot_state_publisher 15 | rviz 16 | joint_state_publisher_gui 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /pbsm/models/HammerSmaller/urdf/HammerSmaller.csv: -------------------------------------------------------------------------------- 1 | Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity 2 | handle,-1.2327E-08,-1.477E-09,-0.08839,0,0,0,0.044636,0.00015733,8.4377E-12,-6.4876E-11,0.00015793,-7.361E-12,2.1057E-06,0,0,0,0,0,0,package://HammerSmaller/meshes/handle.STL,1,1,1,1,0,0,0,0,0,0,package://HammerSmaller/meshes/handle.STL,,part 1 handle-1,ObjectOrigin,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,, 3 | head,-0.0059764,-9.7557E-07,-0.23599,0,0,0,0.11353,5.5211E-06,7.112E-10,1.1552E-10,4.736E-05,5.0227E-11,4.7564E-05,0,0,0,0,0,0,package://HammerSmaller/meshes/head.STL,0.77647,0.75686,0.73725,1,0,0,0,0,0,0,package://HammerSmaller/meshes/head.STL,,Part 2 head-1,ObjectOrigin,Axis_Handle_to_head,Handle_to_head,fixed,0,0,0,0,0,0,handle,0,0,1,,,,,,,,,,,, 4 | -------------------------------------------------------------------------------- /pbsm/models/HammerSmaller/urdf/HammerSmaller.urdf: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 7 | 8 | 11 | 13 | 20 | 21 | 22 | 25 | 26 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 37 | 38 | 40 | 41 | 42 | 43 | 45 | 46 | 49 | 51 | 58 | 59 | 60 | 63 | 64 | 66 | 67 | 69 | 71 | 72 | 73 | 74 | 77 | 78 | 80 | 81 | 82 | 83 | 86 | 89 | 91 | 93 | 95 | 96 | 97 | 98 | -------------------------------------------------------------------------------- /pbsm/models/Robotiq_2F85/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | 3 | project(Robotiq_2F85) 4 | 5 | find_package(catkin REQUIRED) 6 | 7 | catkin_package() 8 | 9 | find_package(roslaunch) 10 | 11 | foreach(dir config launch meshes urdf) 12 | install(DIRECTORY ${dir}/ 13 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) 14 | endforeach(dir) 15 | -------------------------------------------------------------------------------- /pbsm/models/Robotiq_2F85/README.md: -------------------------------------------------------------------------------- 1 | # Robotiq 2F-85 Model 2 | Although the gripper is heavily used in academia and industry, nearly all simulation models for this gripper that are available online are wrong or simply not working. This work tries to fill the gap by providing the community with a realistic model of the gripper. 3 | 4 | This model was produced with the following steps: 5 | 1. Downloading the STEP (volumes) files from Robotiq's website. 6 | 1. Using Solidworks to divide the gripper into parts. 7 | 1. Specify the right material properties for the parts. 8 | 1. Using Solidworks to create a constrained assembly from the parts. 9 | 1. Using [Solidworks to URDF exporter](https://github.com/ros/solidworks_urdf_exporter) to generate the URDF. 10 | 1. Using the CAD to locate the position of the additional constraints that needs to be added with PyBullet. 11 | 1. Implement the additional constraints in PyBullet. 12 | 13 | Consequently, this model must be used with PyBullet. The URDF could be used with Gazebo as long as the additional constraints are correctly implemented in the simulator. 14 | 15 | ## Mechanical properties and constraints 16 | The two-finger gripper is an adaptive end-effector as the configuration of the fingertips adapts to the shape of the object being grasped. This behaviour is made possible by the underactuation of the fingers which is made possible by the mechanical design of the gripper. As a result, a realistic simulation of the gripper is only possible if all the constraints of the gripper are respected and correctly implemented. 17 | 18 | The gripper is made of the following parts: 19 | 20 | ![2F85_Parts](https://user-images.githubusercontent.com/10478385/103466825-9a671c00-4d16-11eb-8221-638f7d2018f3.png) 21 | 22 | Each finger is made of 5 parts: driver, coupler, follower, pad and spring link. The driver is the only link that is directly actuated with a motor. 23 | 24 | ### Closed Kinematic Loop 25 | The URDF file format specifies a model as a tree. Consequently, it does not support closed kinematic loops. Clearly, each finger of the 2F-85 forms a closed kinematic loop. As a result, at least one constraint for each loop needs to be specified outside of the URDF file. The constraint we chose to specify outside the URDF is the joint between the spring link and the follower. In PyBullet, the type of joint is called JOINT_POINT2POINT and corresponds to a revolute URDF joint. The location of the joint axis must be specicfied relative to the center of mass of the link. Using the CAD, we locate the axis as such: 26 | 27 | ![Additional_Constraint](https://user-images.githubusercontent.com/10478385/103467080-398d1300-4d19-11eb-8151-f45b90a7139a.png) 28 | 29 | And the constraint can be implemented as such: 30 | ```python 31 | p.createConstraint(parentBodyUniqueId=self.getUID(), parentLinkIndex=right_follower_index, childBodyUniqueId=self.getUID(), childLinkIndex=right_springlink_index, jointType=p.JOINT_POINT2POINT, jointAxis=[1, 0, 0], parentFramePosition=[6/1000,-6.7/1000,0], childFramePosition=[0,28.9/1000,0]) 32 | ``` 33 | 34 | ### Spring Links 35 | When the fingertips are not in contact with any object, the pads are supposed to be parallel. To allow the gripper to exhibit this behavior, springs located at the joint between the base and the spring link must be simulated. Some simulators, like Gazebo, allow the user to specify a spring stiffness constant for a joint but PyBullet does not currently have this capability. Consequently, the joint is motorized with a very weak force such that the spring links are constantly trying to close toward the palm. This simulated spring can be implemented with the following code: 36 | ```python 37 | p.setJointMotorControl2(targetPosition=self.MAX_CLOSING, bodyIndex=self.getUID(), jointIndex=self.getJointIndexFromName('gripper_right_spring_link_joint'), controlMode=p.POSITION_CONTROL, force=1) 38 | ``` 39 | 40 | ### Limit Pins 41 | Another requirement to allow the fingertips to stay parallel is to enforce the constraint imposed by the pin attached to the driver link. This pin limits the movement of the coupler in one direction. Consequently, the fingertips can close toward the palm but cannot open outward. One way of enforcing this constraint would be to enable the collision checking between different parts of the same robot but that would be computationally very expensive and might produce unstable simulations. Another way is to specify a maximum angle that the driver-coupler joint can reach. Finally, a third way could disable the movement of the driver-coupler joint altogether. This method would force the fingertips to stay parallel at all times and would loose the adaptive capability of the gripper. 42 | 43 | Since, for some simulations, it might be acceptable to loose the underactuation of the gripper and keep the pads parallel at all times, we allow the user to set an XACRO parameter that would disable the underactiation. The ```enable_underactuation``` parameter can be set to ```false``` in the XACRO files to disable the driver-coupler joint and force the fingertips to stay parallel: 44 | 45 | ```xml 46 | 47 | 48 | 49 | ``` 50 | Otherwise, the constraint of the pin is modeled using a joint limit. This limit can be specified in the URDF but for a reason we still ignore, the limit needs to be also specified in PyBullet for it to work: 51 | 52 | ```python 53 | p.changeDynamics(bodyUniqueId=self.getUID(), linkIndex=self.getJointIndexFromName('gripper_right_coupler_joint'), jointLowerLimit=0.13 , jointUpperLimit=0.79) 54 | ``` 55 | 56 | ### Gears 57 | The real gripper always closes symmetrically and a blocking mechanism make it so that no holding torque is required once a position is reached. PyBullet supports the JOINT_GEAR joint type which can actuate one joint when another joint moves. This works well for some situations but problems arise when an external force is applied on one fingertip. To achieve a more realistic result, this type of constraint is not used and each finger is actuated separately with two different simulated motors. A parameter can be set by the user such that only one actuator is used and that the JOINT_GEAR constraint is added: 58 | ```python 59 | if self.USE_SINGLE_ACTUATOR == True: 60 | right_driver_joint_index = self.getJointIndexFromName('gripper_right_driver_joint') 61 | left_driver_joint_index = self.getJointIndexFromName('gripper_left_driver_joint') 62 | #A JOINT_GEAR is unidirectional so two constraints needs to be added to have the real gear behaviour 63 | c = p.createConstraint(parentBodyUniqueId=self.getUID(), parentLinkIndex=right_driver_joint_index, 64 | childBodyUniqueId=self.getUID(), childLinkIndex=left_driver_joint_index, jointType=p.JOINT_GEAR, 65 | jointAxis=[0,0,1], parentFramePosition=[0,0,0], childFramePosition=[0,0,0]) 66 | p.changeConstraint(c, gearRatio=-1, maxForce=self.MAX_FORCE) 67 | c = p.createConstraint(parentBodyUniqueId=self.getUID(), parentLinkIndex=left_driver_joint_index, 68 | childBodyUniqueId=self.getUID(), childLinkIndex=right_driver_joint_index, jointType=p.JOINT_GEAR, 69 | jointAxis=[0,0,1], parentFramePosition=[0,0,0], childFramePosition=[0,0,0]) 70 | p.changeConstraint(c, gearRatio=-1, maxForce=self.MAX_FORCE) 71 | ``` 72 | The blocking mechanism is not yet implemented but could be done by changing the maximum force exerted by the actuators when the gripper is not moving to something very large. 73 | 74 | ### Materials, Masses and Inertias 75 | Most of the parts of the gripper are made out of anodized aluminum 6061 T6 which has a mass density of 0.0027 Kg/cm^3. The spring links are made out of stainless steel 17-4 ph which has a mass density of 0.0028 Kg/cm^3. Finally, the pads are made out of silicon 60A which has a mass density of 0.0023 Kg/cm^3. Combining these details with the shapes of the parts allow us to accurately compute the masses and inertias of each component. Doing so is of the highest importance as incorrect inertias will lead to unrealistic simulations and could even lead to unstability issues. 76 | 77 | Depending on the type of pad that is used, the pad might be somewhat compliant and slightly deform when grasping an object but PyBullet does not seems to officially support soft bodies. To allow for some compliance an XACRO parameter ```soft_pads``` can be set to ```true```. Doing so creates a linear joint that allow the pad to slightly penetrate in the link that supports it. To model the stiffness of the pad, a second parameter ```softness``` can be used to set the force of the linear joint. 78 | ```xml 79 | 80 | 81 | 82 | ``` -------------------------------------------------------------------------------- /pbsm/models/Robotiq_2F85/config/joint_names_Robotiq_2F85.yaml: -------------------------------------------------------------------------------- 1 | controller_joint_names: ['', 'right_spring_link_joint', 'left_spring_link_joint', 'right_driver_joint', 'right_coupler_joint', 'right_follower_joint', 'left_driver_joint', 'left_coupler_joint', 'left_follower_joint', ] 2 | -------------------------------------------------------------------------------- /pbsm/models/Robotiq_2F85/launch/display.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 7 | 11 | 15 | 20 | 21 | -------------------------------------------------------------------------------- /pbsm/models/Robotiq_2F85/launch/gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 9 | 15 | 20 | 21 | -------------------------------------------------------------------------------- /pbsm/models/Robotiq_2F85/meshes/base.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/Robotiq_2F85/meshes/base.STL -------------------------------------------------------------------------------- /pbsm/models/Robotiq_2F85/meshes/left_coupler.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/Robotiq_2F85/meshes/left_coupler.STL -------------------------------------------------------------------------------- /pbsm/models/Robotiq_2F85/meshes/left_driver.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/Robotiq_2F85/meshes/left_driver.STL -------------------------------------------------------------------------------- /pbsm/models/Robotiq_2F85/meshes/left_follower.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/Robotiq_2F85/meshes/left_follower.STL -------------------------------------------------------------------------------- /pbsm/models/Robotiq_2F85/meshes/left_pad.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/Robotiq_2F85/meshes/left_pad.STL -------------------------------------------------------------------------------- /pbsm/models/Robotiq_2F85/meshes/left_spring_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/Robotiq_2F85/meshes/left_spring_link.STL -------------------------------------------------------------------------------- /pbsm/models/Robotiq_2F85/meshes/right_coupler.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/Robotiq_2F85/meshes/right_coupler.STL -------------------------------------------------------------------------------- /pbsm/models/Robotiq_2F85/meshes/right_driver.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/Robotiq_2F85/meshes/right_driver.STL -------------------------------------------------------------------------------- /pbsm/models/Robotiq_2F85/meshes/right_follower.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/Robotiq_2F85/meshes/right_follower.STL -------------------------------------------------------------------------------- /pbsm/models/Robotiq_2F85/meshes/right_pad.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/Robotiq_2F85/meshes/right_pad.STL -------------------------------------------------------------------------------- /pbsm/models/Robotiq_2F85/meshes/right_spring_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/Robotiq_2F85/meshes/right_spring_link.STL -------------------------------------------------------------------------------- /pbsm/models/Robotiq_2F85/package.xml: -------------------------------------------------------------------------------- 1 | 2 | Robotiq 2F85 Simulation Model 3 | 1.0.0 4 | 5 |

URDF Description package for Robotiq 2F85

6 |

This package contains configuration data, 3D models and launch files 7 | for the Robotiq 2F85 gripper.

8 | 9 | Philippe Nadeau 10 | 11 | BSD 12 | catkin 13 | roslaunch 14 | robot_state_publisher 15 | rviz 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /pbsm/models/Robotiq_2F85/urdf/Mechanical_Properties.csv: -------------------------------------------------------------------------------- 1 | Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity 2 | base,6.91E-06,4.4117E-06,0.031656,0,0,0,0.60831,0.0004596,1.5559E-09,1.397E-07,0.0005181,-1.0325E-11,0.00036051,0,0,0,0,0,0,package://Robotiq_2F85/meshes/base.STL,0.89804,0.91765,0.92941,0.25,0,0,0,0,0,0,package://Robotiq_2F85/meshes/base.STL,,Base_Volume-1;Top_Volume-1,Origin_global,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,, 3 | right_spring_link,-9.7222E-12,0.028253,6.791E-10,0,0,0,0.029951,1.5103E-05,-1.0709E-16,-2.6541E-15,5.0789E-06,3.2666E-14,1.0707E-05,0,0,0,0,0,0,package://Robotiq_2F85/meshes/right_spring_link.STL,0.89804,0.91765,0.92941,1,0,0,0,0,0,0,package://Robotiq_2F85/meshes/right_spring_link.STL,,Spring_Link_Volume-1,Origin_right_spring_link_joint,Axis_right_spring_link_joint,right_spring_link_joint,revolute,0.0127,-0.061307,0,3.1416,0,0.54844,base,0,0,1,0,0,0,0,,,,,,,, 4 | left_spring_link,-9.72236180452057E-12,0.0282525952612164,6.7910144830724E-10,0,0,0,0.0299513315112356,1.5102527522536E-05,-1.07091434312545E-16,-2.65405195208214E-15,5.07891352675696E-06,3.26658387675681E-14,1.07068173445703E-05,0,0,0,0,0,0,package://Robotiq_2F85/meshes/left_spring_link.STL,0.898039215686275,0.917647058823529,0.929411764705882,1,0,0,0,0,0,0,package://Robotiq_2F85/meshes/left_spring_link.STL,,Spring_Link_Volume-2,Origin_left_spring_link_joint,Axis_left_spring_link_joint,left_spring_link_joint,revolute,-0.0127,-0.061307,0,3.1416,0,-0.71173,base,0,0,1,0,0,0,0,,,,,,,, 5 | right_driver,0.0179377478452887,0.000926031739626201,-3.67773861599777E-11,0,0,0,0.018490648931893,6.79440337262129E-07,-2.07287900219282E-07,4.9765926300611E-15,3.45929623711154E-06,7.31775756649357E-15,3.75009297936088E-06,0,0,0,0,0,0,package://Robotiq_2F85/meshes/right_driver.STL,0.898039215686275,0.917647058823529,0.929411764705882,1,0,0,0,0,0,0,package://Robotiq_2F85/meshes/right_driver.STL,,Driver_Volume-1,Origin_right_driver_joint,Axis_right_driver_joint,right_driver_joint,revolute,0.030601,-0.054792,0,3.1416,0,-0.12899,base,0,0,1,0,0,0,0,,,,,,,, 6 | right_coupler,-0.00277693278884095,0.0214010778170948,-2.65175586206742E-15,0,0,0,0.0273093985570947,8.43518772624629E-06,8.05599897942408E-07,3.98373077581302E-19,2.33117687880373E-06,-6.92056713208737E-19,6.9133328065108E-06,0,0,0,0,0,0,package://Robotiq_2F85/meshes/right_coupler.STL,0.898039215686275,0.917647058823529,0.929411764705882,1,0,0,0,0,0,0,package://Robotiq_2F85/meshes/right_coupler.STL,,Coupler_Volume-1,Origin_right_coupler_joint,Axis_right_coupler_joint,right_coupler_joint,revolute,0.031486,-0.0040855,0,0,0,-0.20562,right_driver,0,0,1,0,0,0,0,,,,,,,, 7 | right_follower,0.0118738800643235,0.013213950103123,-6.04212047771599E-16,0,0,0,0.0195548943464751,3.99125345460038E-06,-9.32228708142799E-07,-1.0278610605082E-18,1.28831055431048E-06,-9.55931308234925E-19,4.59174098263666E-06,0,0,0,0,0,0,package://Robotiq_2F85/meshes/right_follower.STL,0.898039215686275,0.917647058823529,0.929411764705882,1,0,0,0,0,0,0,package://Robotiq_2F85/meshes/right_follower.STL,,Follower_Volume-2,Origin_right_follower_joint,Axis_right_follower_joint,right_follower_joint,revolute,-0.0061148,0.047125,0,3.1416,0,-3.1395,right_coupler,0,0,1,0,0,0,0,,,,,,,, 8 | right_pad,0.000197179990555962,0.00299999999999999,-5.77175554552257E-15,0,0,0,0.0112846745192961,4.84198905594697E-07,-8.60267837055149E-23,7.18803273742121E-23,1.75287448520127E-06,-3.75504419144335E-23,1.33638362672235E-06,0,0,0,0,0,0,package://Robotiq_2F85/meshes/right_pad.STL,0.529411764705882,0.549019607843137,0.549019607843137,1,0,0,0,0,0,0,package://Robotiq_2F85/meshes/right_pad.STL,,RE_Pad-2,Origin_right_pad_joint,,right_pad_joint,fixed,0.024903,0.032264,0,3.1416,0,-1.5708,right_follower,0,0,0,,,,,,,,,,,, 9 | left_driver,0.0179377478452061,0.000926031738021603,-3.60876565319603E-11,0,0,0,0.0184906489331936,6.79440337141213E-07,-2.07287900036052E-07,4.89721675709151E-15,3.45929623720239E-06,7.2665441334179E-15,3.75009297907269E-06,0,0,0,0,0,0,package://Robotiq_2F85/meshes/left_driver.STL,0.898039215686275,0.917647058823529,0.929411764705882,1,0,0,0,0,0,0,package://Robotiq_2F85/meshes/left_driver.STL,,Driver_Volume-2,Origin_left_driver_joint,Axis_left_driver_joint,left_driver_joint,revolute,-0.030601,-0.054792,0,0,0,-3.0457,base,0,0,1,0,0,0,0,,,,,,,, 10 | left_coupler,-0.00277693278884095,0.0214010778170948,-2.67535941412636E-15,0,0,0,0.0273093985570947,8.4351877262463E-06,8.05599897942412E-07,3.98243790128093E-19,2.33117687880373E-06,-6.91609468871299E-19,6.91333280651081E-06,0,0,0,0,0,0,package://Robotiq_2F85/meshes/left_coupler.STL,0.898039215686275,0.917647058823529,0.929411764705882,1,0,0,0,0,0,0,package://Robotiq_2F85/meshes/left_coupler.STL,,Coupler_Volume-2,Origin_left_coupler_joint,Axis_left_coupler_joint,left_coupler_joint,revolute,0.031486,-0.0040855,0,-3.1416,0,3.0403,left_driver,0,0,1,0,0,0,0,,,,,,,, 11 | left_follower,0.0118738800643242,0.013213950103123,2.11704886976492E-15,0,0,0,0.0195548943464751,3.99125345460038E-06,-9.32228708142757E-07,-1.13327927247016E-18,1.28831055431046E-06,-9.53175406300521E-19,4.59174098263663E-06,0,0,0,0,0,0,package://Robotiq_2F85/meshes/left_follower.STL,0.898039215686275,0.917647058823529,0.929411764705882,1,0,0,0,0,0,0,package://Robotiq_2F85/meshes/left_follower.STL,,Follower_Volume-1,Origin_left_follower_joint,Axis_left_follower_joint,left_follower_joint,revolute,-0.0061148,0.047125,0,0,0,-0.15621,left_coupler,0,0,1,0,0,0,0,,,,,,,, 12 | left_pad,0.000197179990555976,0.00300000000000002,8.48173524774358E-15,0,0,0,0.0112846745192962,4.84198905594698E-07,1.02570395956575E-21,2.04930826678933E-22,1.75287448520127E-06,5.81091811719151E-24,1.33638362672235E-06,0,0,0,0,0,0,package://Robotiq_2F85/meshes/left_pad.STL,0.529411764705882,0.549019607843137,0.549019607843137,1,0,0,0,0,0,0,package://Robotiq_2F85/meshes/left_pad.STL,,RE_Pad-1,Origin_left_pad_joint,,left_pad_joint,fixed,0.018903,0.032265,0,0,0,-1.5708,left_follower,0,0,0,,,,,,,,,,,, 13 | -------------------------------------------------------------------------------- /pbsm/models/Robotiq_2F85_BuildURDF.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /pbsm/models/UR5.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 249 | 250 | 251 | 252 | 253 | 254 | 260 | 261 | 262 | -------------------------------------------------------------------------------- /pbsm/models/UR5_2F85_BuildURDF.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /pbsm/models/UR5_BuildURDF.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /pbsm/models/cube_small/cube_small.obj: -------------------------------------------------------------------------------- 1 | #### 2 | # 3 | # OBJ File Generated by Meshlab 4 | # 5 | #### 6 | # Object cube_small.obj 7 | # 8 | # Vertices: 8 9 | # Faces: 12 10 | # 11 | #### 12 | mtllib ./cube_small.obj.mtl 13 | 14 | vn -0.577350 -0.577350 0.577350 15 | v -0.025000 -0.025000 0.025000 16 | vn 0.577350 -0.577350 0.577350 17 | v 0.025000 -0.025000 0.025000 18 | vn -0.577350 0.577350 0.577350 19 | v -0.025000 0.025000 0.025000 20 | vn 0.577350 0.577350 0.577350 21 | v 0.025000 0.025000 0.025000 22 | vn -0.577350 0.577350 -0.577350 23 | v -0.025000 0.025000 -0.025000 24 | vn 0.577350 0.577350 -0.577350 25 | v 0.025000 0.025000 -0.025000 26 | vn -0.577350 -0.577350 -0.577350 27 | v -0.025000 -0.025000 -0.025000 28 | vn 0.577350 -0.577350 -0.577350 29 | v 0.025000 -0.025000 -0.025000 30 | # 8 vertices, 0 vertices normals 31 | 32 | 33 | usemtl material_0 34 | vt 0.000000 0.000000 35 | vt 1.000000 0.000000 36 | vt 0.000000 1.000000 37 | f 1/1/1 2/2/2 3/3/3 38 | vt 1.000000 1.000000 39 | f 3/3/3 2/2/2 4/4/4 40 | f 3/1/3 4/2/4 5/3/5 41 | f 5/3/5 4/2/4 6/4/6 42 | f 5/4/5 6/3/6 7/2/7 43 | f 7/2/7 6/3/6 8/1/8 44 | f 7/1/7 8/2/8 1/3/1 45 | f 1/3/1 8/2/8 2/4/2 46 | f 2/1/2 8/2/8 4/3/4 47 | f 4/3/4 8/2/8 6/4/6 48 | f 7/1/7 1/2/1 5/3/5 49 | f 5/3/5 1/2/1 3/4/3 50 | # 12 faces, 4 coords texture 51 | 52 | # End of File 53 | -------------------------------------------------------------------------------- /pbsm/models/cube_small/cube_small.obj.mtl: -------------------------------------------------------------------------------- 1 | # 2 | # Wavefront material file 3 | # Converted by Meshlab Group 4 | # 5 | 6 | newmtl material_0 7 | Ka 0.200000 0.200000 0.200000 8 | Kd 0.752941 0.752941 0.752941 9 | Ks 1.000000 1.000000 1.000000 10 | Tr 1.000000 11 | illum 2 12 | Ns 0.000000 13 | 14 | -------------------------------------------------------------------------------- /pbsm/models/cube_small/cube_small.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/cube_small/cube_small.ply -------------------------------------------------------------------------------- /pbsm/models/ur5/README.md: -------------------------------------------------------------------------------- 1 | # Universal Robot UR-5 Model 2 | This model is a very slightly modified version of the model produced by the manufacturer (see [here](https://github.com/ros-industrial/universal_robot)). The only modifications made to the XACRO file provided by the package are to disable certain features specifically made for ROS or Gazebo. 3 | 4 | -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package ur_description 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.2.7 (2019-11-23) 6 | ------------------ 7 | 8 | 1.2.6 (2019-11-19) 9 | ------------------ 10 | * Add optional safety_controller tags to all joints in xacro macros (`#437 `_) 11 | * Migrated all package.xml files to format=2 (`#439 `_) 12 | * Corrected dimensions and positions of inertias (`#426 `_) 13 | * Add description view launch files for all descriptions to easily check them (`#435 `_) 14 | * Contributors: Felix Mauch, JeremyZoss, Miguel Prada, Qiang Qiu, gavanderhoorn 15 | 16 | 1.2.5 (2019-04-05) 17 | ------------------ 18 | * Add transmission_hw_interface to UR xacro and expose everywhere (`#392 `_) 19 | * Update maintainer listing: add Miguel (`#410 `_) 20 | * Updated xacro namespace. 21 | * Update maintainer and author information. 22 | * Updated mesh ambience so the model isn't so dark in Gazebo 23 | * Fix overlapping variable names between robot definition files (`#356 `_) 24 | * Improve meshes shading (`#233 `_) 25 | * Added run_depend for xacro 26 | * Using the 'doc' attribute on 'arg' elements. 27 | * Enable self collision in gazebo 28 | * Contributors: Dave Niewinski, Felix von Drigalski, Harsh Deshpande, Joe, Marcel Schnirring, Miguel Prada, MonteroJJ, ipa-fxm 29 | 30 | 1.2.1 (2018-01-06) 31 | ------------------ 32 | * Merge pull request `#329 `_ from tecnalia-medical-robotics/joint_limits 33 | Homogenize xacro macro arguments. 34 | * Merge pull request `#332 `_ from davetcoleman/kinetic_hw_iface_warning 35 | Remove UR3 ROS Control Hardware Interface warning 36 | * Remove UR3 ROS Control Hardware Interface warning 37 | * Extend changes to '_robot.urdf.xacro' variants as well. 38 | * Homogenize xacro macro arguments. 39 | Joint limits for the limited version could be set using arguments for the UR10 40 | but not for the UR3 and UR5. Same lower and upper limit arguments are added to 41 | the UR3 and UR5 xacro macros. 42 | * Fix elbow joint limits (`#268 `_) 43 | * Remove warning 'redefining global property: pi' (Jade+) (`#315 `_) 44 | * Contributors: Beatriz Leon, Dave Coleman, Felix Messmer, Miguel Prada 45 | 46 | 1.2.0 (2017-08-04) 47 | ------------------ 48 | 49 | 1.1.9 (2017-01-02) 50 | ------------------ 51 | * reintroduce 'pi', unbrake dependent xacros. 52 | * use '--inorder' to trigger use of jade+ xacro on Indigo. 53 | * Contributors: gavanderhoorn 54 | 55 | 1.1.8 (2016-12-30) 56 | ------------------ 57 | * all: update maintainers. 58 | * Contributors: gavanderhoorn 59 | 60 | 1.1.7 (2016-12-29) 61 | ------------------ 62 | * Fix xacro warnings in Jade (`#251 `_) 63 | * added default values to xacro macro 64 | * tested joint limits modification 65 | * Contributors: Dave Coleman, G.A. vd. Hoorn, philip 14.04 66 | 67 | 1.1.6 (2016-04-01) 68 | ------------------ 69 | * unify mesh names 70 | * add color to avoid default color 'red' for collision meshes 71 | * use correct DH parameter + colored meshes 72 | * introducing urdf for ur3 - first draft 73 | * unify common xacro files 74 | * remove obsolete urdf files 75 | * description: add '_joint' suffix to newly introduced joint tags. 76 | This is more in-line with naming of existing joint tags. 77 | * description: add ROS-I base and tool0 frames. Fix `#49 `_ and `#95 `_. 78 | Note that 'base' is essentially 'base_link' but rotated by 180 79 | degrees over the Z-axis. This is necessary as the visual and 80 | collision geometries appear to also have their origins rotated 81 | 180 degrees wrt the real robot. 82 | 'tool0' is similar to 'ee_link', but with its orientation such 83 | that it coincides with an all-zeros TCP setting on the UR 84 | controller. Users are expected to attach their own TCP frames 85 | to this frame, instead of updating it (see also [1]). 86 | [1] http://wiki.ros.org/Industrial/Tutorials/WorkingWithRosIndustrialRobotSupportPackages#Standardised_links\_.2BAC8_frames 87 | * description: minor whitespace cleanup of UR5 & 10 xacros. 88 | * regenerate urdf files 89 | * use PositionJointInterface as hardwareInterface in transmissions - affects simulation only 90 | * Contributors: gavanderhoorn, ipa-fxm 91 | 92 | 1.0.2 (2014-03-31) 93 | ------------------ 94 | 95 | 1.0.1 (2014-03-31) 96 | ------------------ 97 | * changes due to file renaming 98 | * generate urdfs from latest xacros 99 | * file renaming 100 | * adapt launch files in order to be able to use normal/limited xacro 101 | * fixed typo in limits 102 | * add joint_limited urdf.xacros for both robots 103 | * (re-)add ee_link for both robots 104 | * updates for latest gazebo under hydro 105 | * remove ee_link - as in ur10 106 | * use same xacro params as ur10 107 | * use new transmission interfaces 108 | * update xml namespaces for hydro 109 | * remove obsolete urdf file 110 | * remove obsolete urdf file 111 | * Contributors: ipa-fxm 112 | 113 | * Update ur10.urdf.xacro 114 | Corrected UR10's urdf to faithfully represent joint effort thresholds, velocity limits, and dynamics parameters. 115 | * Update ur5.urdf.xacro 116 | Corrected effort thresholds and friction values for UR5 urdf. 117 | * added corrected mesh file 118 | * Added definitions for adding tergets in install folder. Issue `#10 `_. 119 | * Corrected warning on xacro-files in hydro. 120 | * Added definitions for adding tergets in install folder. Issue `#10 `_. 121 | * Updated to catkin. ur_driver's files were added to nested Python directory for including in other packages. 122 | * fixed name of ur5 transmissions 123 | * patched gazebo.urdf.xacro to be compatible with gazebo 1.5 124 | * fixed copy&paste error (?) 125 | * prefix versions of gazebo and transmission macros 126 | * Added joint limited urdf and associated moveit package. The joint limited package is friendlier to the default KLD IK solution 127 | * Added ur5 moveit library. The Kinematics used by the ur5 move it library is unreliable and should be replaced with the ur_kinematics 128 | * Updated urdf files use collision/visual models. 129 | * Reorganized meshes to include both collision and visual messhes (like other ROS-I robots). Modified urdf xacro to include new models. Removed extra robot pedestal link from urdf (urdfs should only include the robot itself). 130 | * minor changes on ur5 xacro files 131 | * Removed extra stl files and fixed indentions 132 | * Renamed packages and new groovy version 133 | * Added ur10 and renamed packages 134 | * Contributors: Denis Štogl, IPR-SR2, Kelsey, Mathias Lüdtke, Shaun Edwards, ipa-nhg, jrgnicho, kphawkins, robot 135 | -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ur_description) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED) 8 | 9 | ## System dependencies are found with CMake's conventions 10 | # find_package(Boost REQUIRED COMPONENTS system) 11 | 12 | 13 | ## Uncomment this if the package has a setup.py. This macro ensures 14 | ## modules and global scripts declared therein get installed 15 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 16 | # catkin_python_setup() 17 | 18 | ####################################### 19 | ## Declare ROS messages and services ## 20 | ####################################### 21 | 22 | ## Generate messages in the 'msg' folder 23 | # add_message_files( 24 | # FILES 25 | # Message1.msg 26 | # Message2.msg 27 | # ) 28 | 29 | ## Generate services in the 'srv' folder 30 | # add_service_files( 31 | # FILES 32 | # Service1.srv 33 | # Service2.srv 34 | # ) 35 | 36 | ## Generate added messages and services with any dependencies listed here 37 | # generate_messages( 38 | # DEPENDENCIES 39 | # std_msgs # Or other packages containing msgs 40 | # ) 41 | 42 | ################################### 43 | ## catkin specific configuration ## 44 | ################################### 45 | ## The catkin_package macro generates cmake config files for your package 46 | ## Declare things to be passed to dependent projects 47 | ## LIBRARIES: libraries you create in this project that dependent projects also need 48 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 49 | ## DEPENDS: system dependencies of this project that dependent projects also need 50 | catkin_package( 51 | # INCLUDE_DIRS include 52 | # LIBRARIES ur_description 53 | # CATKIN_DEPENDS urdf 54 | # DEPENDS system_lib 55 | ) 56 | 57 | ########### 58 | ## Build ## 59 | ########### 60 | 61 | ## Specify additional locations of header files 62 | ## Your package locations should be listed before other locations 63 | # include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) 64 | 65 | ## Declare a cpp library 66 | # add_library(ur_description 67 | # src/${PROJECT_NAME}/ur_description.cpp 68 | # ) 69 | 70 | ## Declare a cpp executable 71 | # add_executable(ur_description_node src/ur_description_node.cpp) 72 | 73 | ## Add cmake target dependencies of the executable/library 74 | ## as an example, message headers may need to be generated before nodes 75 | # add_dependencies(ur_description_node ur_description_generate_messages_cpp) 76 | 77 | ## Specify libraries to link a library or executable target against 78 | # target_link_libraries(ur_description_node 79 | # ${catkin_LIBRARIES} 80 | # ) 81 | 82 | ############# 83 | ## Install ## 84 | ############# 85 | 86 | install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 87 | install(DIRECTORY urdf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 88 | install(DIRECTORY meshes DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 89 | 90 | # all install targets should use catkin DESTINATION variables 91 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 92 | 93 | ## Mark executable scripts (Python etc.) for installation 94 | ## in contrast to setup.py, you can choose the destination 95 | # install(PROGRAMS 96 | # scripts/my_python_script 97 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 98 | # ) 99 | 100 | ## Mark executables and/or libraries for installation 101 | # install(TARGETS ur_description ur_description_node 102 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 103 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 104 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 105 | # ) 106 | 107 | ## Mark cpp header files for installation 108 | # install(DIRECTORY include/${PROJECT_NAME}/ 109 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 110 | # FILES_MATCHING PATTERN "*.h" 111 | # PATTERN ".svn" EXCLUDE 112 | # ) 113 | 114 | ## Mark other files for installation (e.g. launch and bag files, etc.) 115 | # install(FILES 116 | # # myfile1 117 | # # myfile2 118 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 119 | # ) 120 | 121 | ############# 122 | ## Testing ## 123 | ############# 124 | 125 | ## Add gtest based cpp test target and link libraries 126 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_ur_description.cpp) 127 | # if(TARGET ${PROJECT_NAME}-test) 128 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 129 | # endif() 130 | 131 | ## Add folders to be run by python nosetests 132 | # catkin_add_nosetests(test) 133 | -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/cfg/view_robot.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /RobotModel1 10 | - /TF1 11 | Splitter Ratio: 0.5 12 | Tree Height: 787 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.588679 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: "" 31 | Visualization Manager: 32 | Class: "" 33 | Displays: 34 | - Alpha: 0.5 35 | Cell Size: 1 36 | Class: rviz/Grid 37 | Color: 160; 160; 164 38 | Enabled: true 39 | Line Style: 40 | Line Width: 0.03 41 | Value: Lines 42 | Name: Grid 43 | Normal Cell Count: 0 44 | Offset: 45 | X: 0 46 | Y: 0 47 | Z: 0 48 | Plane: XY 49 | Plane Cell Count: 10 50 | Reference Frame: 51 | Value: true 52 | - Alpha: 1 53 | Class: rviz/RobotModel 54 | Collision Enabled: false 55 | Enabled: true 56 | Links: 57 | base_link: 58 | Alpha: 1 59 | Show Axes: false 60 | Show Trail: false 61 | Value: true 62 | link_1: 63 | Alpha: 1 64 | Show Axes: false 65 | Show Trail: false 66 | Value: true 67 | link_2: 68 | Alpha: 1 69 | Show Axes: false 70 | Show Trail: false 71 | Value: true 72 | link_3: 73 | Alpha: 1 74 | Show Axes: false 75 | Show Trail: false 76 | Value: true 77 | link_4: 78 | Alpha: 1 79 | Show Axes: false 80 | Show Trail: false 81 | Value: true 82 | link_5: 83 | Alpha: 1 84 | Show Axes: false 85 | Show Trail: false 86 | Value: true 87 | link_6: 88 | Alpha: 1 89 | Show Axes: false 90 | Show Trail: false 91 | Value: true 92 | Name: RobotModel 93 | Robot Description: robot_description 94 | TF Prefix: "" 95 | Update Interval: 0 96 | Value: true 97 | Visual Enabled: true 98 | - Class: rviz/TF 99 | Enabled: true 100 | Frame Timeout: 15 101 | Frames: 102 | /base_link: 103 | Value: true 104 | /link_1: 105 | Value: true 106 | /link_2: 107 | Value: true 108 | /link_3: 109 | Value: true 110 | /link_4: 111 | Value: true 112 | /link_5: 113 | Value: true 114 | /link_6: 115 | Value: true 116 | /tool0: 117 | Value: true 118 | All Enabled: true 119 | Marker Scale: 1 120 | Name: TF 121 | Show Arrows: true 122 | Show Axes: true 123 | Show Names: true 124 | Tree: 125 | /base_link: 126 | /link_1: 127 | /link_2: 128 | /link_3: 129 | /link_4: 130 | /link_5: 131 | /link_6: 132 | /tool0: 133 | {} 134 | Update Interval: 0 135 | Value: true 136 | Enabled: true 137 | Global Options: 138 | Background Color: 48; 48; 48 139 | Fixed Frame: /base_link 140 | Name: root 141 | Tools: 142 | - Class: rviz/Interact 143 | Hide Inactive Objects: true 144 | - Class: rviz/MoveCamera 145 | - Class: rviz/Select 146 | - Class: rviz/Measure 147 | - Class: rviz/SetInitialPose 148 | Topic: /initialpose 149 | - Class: rviz/SetGoal 150 | Topic: /move_base_simple/goal 151 | Value: true 152 | Views: 153 | Current: 154 | Class: rviz/Orbit 155 | Distance: 10 156 | Focal Point: 157 | X: 0 158 | Y: 0 159 | Z: 0 160 | Name: Current View 161 | Near Clip Distance: 0.01 162 | Pitch: 0.115398 163 | Target Frame: 164 | Value: Orbit (rviz) 165 | Yaw: 0.930399 166 | Saved: ~ 167 | Window Geometry: 168 | Displays: 169 | collapsed: false 170 | Height: 1000 171 | Hide Left Dock: false 172 | Hide Right Dock: false 173 | QMainWindow State: 000000ff00000000fd00000004000000000000013c000003a2fc0200000005fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000003a2000000dd00ffffff000000010000010f000003a2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000003a2000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f700fffffffb0000000800540069006d00650100000000000004500000000000000000000002a9000003a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 174 | Selection: 175 | collapsed: false 176 | Time: 177 | collapsed: false 178 | Tool Properties: 179 | collapsed: false 180 | Views: 181 | collapsed: false 182 | Width: 1280 183 | X: 60 184 | Y: 60 185 | -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/launch/ur10_upload.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/launch/ur3_upload.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/launch/ur5_upload.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/launch/view_ur10.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/launch/view_ur3.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/launch/view_ur5.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/meshes/ur10/collision/base.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/ur5/ur_description/meshes/ur10/collision/base.stl -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/meshes/ur10/collision/forearm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/ur5/ur_description/meshes/ur10/collision/forearm.stl -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/meshes/ur10/collision/shoulder.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/ur5/ur_description/meshes/ur10/collision/shoulder.stl -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/meshes/ur10/collision/upperarm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/ur5/ur_description/meshes/ur10/collision/upperarm.stl -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/meshes/ur10/collision/wrist1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/ur5/ur_description/meshes/ur10/collision/wrist1.stl -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/meshes/ur10/collision/wrist2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/ur5/ur_description/meshes/ur10/collision/wrist2.stl -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/meshes/ur10/collision/wrist3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/ur5/ur_description/meshes/ur10/collision/wrist3.stl -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/meshes/ur3/collision/base.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/ur5/ur_description/meshes/ur3/collision/base.stl -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/meshes/ur3/collision/forearm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/ur5/ur_description/meshes/ur3/collision/forearm.stl -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/meshes/ur3/collision/shoulder.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/ur5/ur_description/meshes/ur3/collision/shoulder.stl -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/meshes/ur3/collision/upperarm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/ur5/ur_description/meshes/ur3/collision/upperarm.stl -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/meshes/ur3/collision/wrist1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/ur5/ur_description/meshes/ur3/collision/wrist1.stl -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/meshes/ur3/collision/wrist2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/ur5/ur_description/meshes/ur3/collision/wrist2.stl -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/meshes/ur3/collision/wrist3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/ur5/ur_description/meshes/ur3/collision/wrist3.stl -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/meshes/ur5/collision/base.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/ur5/ur_description/meshes/ur5/collision/base.stl -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/meshes/ur5/collision/forearm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/ur5/ur_description/meshes/ur5/collision/forearm.stl -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/meshes/ur5/collision/shoulder.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/ur5/ur_description/meshes/ur5/collision/shoulder.stl -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/meshes/ur5/collision/upperarm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/ur5/ur_description/meshes/ur5/collision/upperarm.stl -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/meshes/ur5/collision/wrist1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/ur5/ur_description/meshes/ur5/collision/wrist1.stl -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/meshes/ur5/collision/wrist2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/ur5/ur_description/meshes/ur5/collision/wrist2.stl -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/meshes/ur5/collision/wrist3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utiasSTARS/pyb-sim-models/cb2a9e5be12ef6dda231eaf1e976b86a2a3541fc/pbsm/models/ur5/ur_description/meshes/ur5/collision/wrist3.stl -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ur_description 4 | 1.2.7 5 | 6 | URDF description for Universal UR5/10 robot arms 7 | 8 | 9 | Wim Meeussen 10 | Kelsey Hawkins 11 | Mathias Ludtke 12 | Felix Messmer 13 | G.A. vd. Hoorn 14 | Miguel Prada Sarasola 15 | Nadia Hammoudeh Garcia 16 | 17 | BSD 18 | 19 | http://ros.org/wiki/ur_description 20 | 21 | catkin 22 | joint_state_publisher 23 | robot_state_publisher 24 | rviz 25 | urdf 26 | xacro 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/urdf/common.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/urdf/ur.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | true 8 | 9 | 10 | true 11 | 12 | 13 | true 14 | 15 | 16 | true 17 | 18 | 19 | true 20 | 21 | 22 | true 23 | 24 | 25 | true 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/urdf/ur.transmission.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | transmission_interface/SimpleTransmission 8 | 9 | ${hw_interface} 10 | 11 | 12 | 1 13 | 14 | 15 | 16 | 17 | transmission_interface/SimpleTransmission 18 | 19 | ${hw_interface} 20 | 21 | 22 | 1 23 | 24 | 25 | 26 | 27 | transmission_interface/SimpleTransmission 28 | 29 | ${hw_interface} 30 | 31 | 32 | 1 33 | 34 | 35 | 36 | 37 | transmission_interface/SimpleTransmission 38 | 39 | ${hw_interface} 40 | 41 | 42 | 1 43 | 44 | 45 | 46 | 47 | transmission_interface/SimpleTransmission 48 | 49 | ${hw_interface} 50 | 51 | 52 | 1 53 | 54 | 55 | 56 | 57 | transmission_interface/SimpleTransmission 58 | 59 | ${hw_interface} 60 | 61 | 62 | 1 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/urdf/ur10.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 19 | 20 | 21 | 22 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | 296 | 297 | 298 | 299 | 300 | 301 | 302 | 303 | 304 | 305 | 306 | 307 | 308 | 309 | 310 | 311 | 312 | 313 | 314 | 315 | 316 | 317 | 318 | 319 | 320 | 321 | 322 | 323 | 324 | 325 | 326 | 327 | 328 | 329 | 330 | 331 | 332 | 333 | 334 | 335 | 336 | 337 | 338 | 339 | 340 | 341 | 342 | 343 | 344 | 345 | 346 | 347 | 348 | 349 | 353 | 354 | 355 | 356 | 357 | 358 | 359 | 360 | 361 | 362 | 363 | 364 | 365 | 366 | 367 | 368 | -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/urdf/ur10_joint_limited_robot.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/urdf/ur10_robot.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/urdf/ur3.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 18 | 19 | 20 | 21 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | 296 | 297 | 298 | 299 | 300 | 301 | 302 | 303 | 304 | 305 | 306 | 307 | 308 | 309 | 310 | 311 | 312 | 313 | 314 | 315 | 316 | 317 | 318 | 319 | 320 | 321 | 322 | 323 | 324 | 325 | 326 | 327 | 328 | 329 | 330 | 331 | 332 | 333 | 334 | 335 | 336 | 337 | 338 | 339 | 340 | 341 | 342 | 343 | 344 | 345 | 346 | 347 | 351 | 352 | 353 | 354 | 355 | 356 | 357 | 358 | 359 | 360 | 361 | 362 | 363 | 364 | 365 | 366 | -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/urdf/ur3_joint_limited_robot.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/urdf/ur3_robot.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/urdf/ur5.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 14 | 15 | 16 | 17 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | 296 | 297 | 298 | 299 | 300 | 301 | 302 | 303 | 304 | 305 | 306 | 307 | 308 | 309 | 310 | 311 | 312 | 313 | 314 | 315 | 316 | 317 | 318 | 319 | 320 | 321 | 322 | 323 | 324 | 325 | 326 | 327 | 328 | 329 | 330 | 331 | 332 | 333 | 334 | 335 | 336 | 337 | 338 | 339 | 340 | 341 | 342 | 343 | 344 | 345 | 346 | 347 | 348 | 349 | 350 | 351 | 352 | 353 | 354 | 355 | 356 | 357 | 358 | 359 | 360 | 361 | 362 | 363 | 364 | 368 | 369 | 370 | 371 | 372 | 373 | 374 | 375 | 376 | 377 | 378 | 379 | 380 | 381 | 382 | 383 | -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/urdf/ur5_joint_limited_robot.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /pbsm/models/ur5/ur_description/urdf/ur5_robot.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /pbsm/models/ur5_compositeobject/generate_and_simulate.py: -------------------------------------------------------------------------------- 1 | import pbsm 2 | from pbsm import UR5,CompositeTestObject 3 | import numpy as np 4 | import os 5 | import time 6 | import pybullet as p 7 | import pybullet_data 8 | 9 | 10 | #Dictionnary of interesting configurations 11 | # {Name: [Steel_weights_positions, Plastic_weights_positions]} 12 | configurations = { 13 | "Hammer": [[0,1,2], [3,4,5,6,8]], 14 | "Barbell": [[0,1,2,7,8,9], [3,4,5,6]], 15 | "Tee": [[0,1,2,3,4,5,6], []], 16 | "Homogeneous": [[0,1,2,3,4,5,6,7,8,9], []], 17 | "Corners": [[0,2,7,9], []], 18 | "Rod": [[1,3,4,5,6,8], []], 19 | "Half-N-Half": [[0,1,2,3,4], [5,6,7,8,9]] 20 | } 21 | 22 | CHOSEN_CONFIG = "Hammer" 23 | steel_weights = configurations[CHOSEN_CONFIG][0] 24 | plastic_weights = configurations[CHOSEN_CONFIG][1] 25 | 26 | #The orientation and position of the object relative to the end-effector of the robot depends 27 | #on which bolts are used to attach the object onto the flange of the robot. 28 | #There are ten possibilities, each centering a weight hole with the end-effector. 29 | mm_to_m = 1e-3 30 | weight_pos = mm_to_m*np.array([[65,125,25],[0,125,25],[-65,125,25],[0,75,25],[0,25,25],[0,-25,25],[0,-75,25],[0,-125,25],[-65,-125,25],[65,-125,25]]) 31 | #The contact point is on the surface of the object (z=0), not at its center (z=25). 32 | weight_pos[:,2] = np.zeros((10,)) 33 | 34 | from math import radians 35 | anchors = { 36 | "1-2": [[1, 2], weight_pos[0], [0,radians(90),0]], 37 | "0-5": [[0, 5], weight_pos[1], [0,radians(90),0]], 38 | "3-4": [[3, 4], weight_pos[2], [0,radians(90),0]], 39 | "5-6": [[5, 6], weight_pos[3], [0,radians(90),0]], 40 | "6-7": [[6, 7], weight_pos[4], [0,radians(90),0]], 41 | "7-8": [[7, 8], weight_pos[5], [0,radians(90),0]], 42 | "8-9": [[8, 9], weight_pos[6], [0,radians(90),0]], 43 | "10-11": [[10,11], weight_pos[7], [0,radians(90),0]], 44 | "9-14": [[9, 14], weight_pos[8], [0,radians(90),0]], 45 | "12-13": [[12,13], weight_pos[9], [0,radians(90),0]] 46 | } 47 | 48 | CHOSEN_ANCHOR = "7-8" 49 | anchoring_bolts = anchors[CHOSEN_ANCHOR][0] 50 | object_pos_wrt_ee = anchors[CHOSEN_ANCHOR][1] 51 | object_rpy_wrt_ee = anchors[CHOSEN_ANCHOR][2] 52 | 53 | #More bolts can be added to hold the object together, if needed. 54 | extra_bolts = [0,14] 55 | bolts_attached = anchoring_bolts + extra_bolts 56 | 57 | #Generates the XACRO output file 58 | PBSM_PATH = os.path.dirname(pbsm.__file__) 59 | output_xacro_path = PBSM_PATH+'/models/CompositeTestObject/object.xacro' 60 | CompositeTestObject(bolts_attached, steel_weights, plastic_weights, output_xacro_path, print_params=False, linkxacro=True) 61 | 62 | #Read resulting XACRO file 63 | obj_xacro = open(output_xacro_path, 'r') 64 | content = obj_xacro.read() 65 | #Modify paths such that the main XACRO can reach the meshes 66 | content = content.replace('filename="','filename="./CompositeTestObject/') 67 | 68 | #Write main XACRO file 69 | main_xacro_content = \ 70 | ''' 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | {} 104 | 105 | 106 | '''.format(object_rpy_wrt_ee[0], object_rpy_wrt_ee[1], object_rpy_wrt_ee[2], object_pos_wrt_ee[0], object_pos_wrt_ee[1], object_pos_wrt_ee[2], content) 107 | 108 | #Write main XACRO file in current folder 109 | main_xacro_basename = PBSM_PATH + "/models/UR5_compositeobject" 110 | with open(main_xacro_basename+'.xacro', 'w') as fmain: 111 | fmain.write(main_xacro_content) 112 | 113 | #Generate URDF from XACRO 114 | command = 'xacro {} > {}'.format(main_xacro_basename+'.xacro', main_xacro_basename+'.urdf') 115 | os.system(command) 116 | 117 | #Load URDF and Simulate 118 | cid = p.connect(p.GUI)#or p.DIRECT for non-graphical version 119 | 120 | print('Current Simulation Parameters:') 121 | print(p.getPhysicsEngineParameters(cid)) 122 | 123 | #The fixedTimeStep and numSolverIterations are the most important parameters to trade-off quality versus performance 124 | fixedTimeStep = 1. / 240 #Default is 1./240 125 | numSolverIterations = 500 #Default is 50 126 | 127 | #Its best to keep default values for simulation parameters. 128 | p.setPhysicsEngineParameter(numSolverIterations=numSolverIterations) 129 | p.setTimeStep(fixedTimeStep) 130 | p.setRealTimeSimulation(0) 131 | 132 | #Only one additional search path can be supplied to PyBullet 133 | p.setAdditionalSearchPath(pybullet_data.getDataPath()) 134 | 135 | #Disable rendering during loading makes it much faster 136 | p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0) 137 | 138 | #Instanciate the UR5+object 139 | robot = UR5(cid, alternateModelFilename = main_xacro_basename+'.urdf') 140 | 141 | #You might want to play with these values but use reasonable values. 142 | robot.setMaxVelocity(0.25) 143 | robot.setMaxForce(150) 144 | 145 | #Initialize joints position 146 | initial_joints_positions_degrees = [0, -90, 60, -90, -90, 0] 147 | robot.initJoints(initial_joints_positions_degrees) 148 | 149 | #Add a force-torque sensor on the ee_link_gripper_base_joint 150 | gripper_base_index = robot.getJointIndexFromName('ee_fixed_joint') 151 | p.enableJointForceTorqueSensor(robot.getUID(), gripper_base_index) 152 | 153 | #Position the vizualizer camera 154 | p.resetDebugVisualizerCamera(cameraDistance=0.5,cameraYaw=60,cameraPitch=0,cameraTargetPosition=[0.45,0,0.4]) 155 | 156 | #Re-enable rendering 157 | p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) 158 | 159 | #Set the downward gravity acceleration 160 | p.setGravity(0,0,-9.81) 161 | 162 | #Add visual reference frame on the Force-Torque Sensor 163 | p.addUserDebugLine(lineFromXYZ=[0,0,0], lineToXYZ=[1,0,0], lineColorRGB=[1,0,0], lineWidth=0.1, lifeTime=0, parentObjectUniqueId=robot.getUID(), parentLinkIndex=robot.getJointIndexFromName('ee_fixed_joint')) 164 | p.addUserDebugLine(lineFromXYZ=[0,0,0], lineToXYZ=[0,1,0], lineColorRGB=[0,1,0], lineWidth=0.1, lifeTime=0, parentObjectUniqueId=robot.getUID(), parentLinkIndex=robot.getJointIndexFromName('ee_fixed_joint')) 165 | p.addUserDebugLine(lineFromXYZ=[0,0,0], lineToXYZ=[0,0,1], lineColorRGB=[0,0,1], lineWidth=0.1, lifeTime=0, parentObjectUniqueId=robot.getUID(), parentLinkIndex=robot.getJointIndexFromName('ee_fixed_joint')) 166 | 167 | #Add visual reference frame on the End-effector. The positions are relative to the center of mass of the specified link 168 | p.addUserDebugLine(lineFromXYZ=[0,0,0], lineToXYZ=[1,0,0], lineColorRGB=[1,0,0], lineWidth=0.1, lifeTime=0, parentObjectUniqueId=robot.getUID(), parentLinkIndex=robot.getJointIndexFromName('tool_center_point')) 169 | p.addUserDebugLine(lineFromXYZ=[0,0,0], lineToXYZ=[0,1,0], lineColorRGB=[0,1,0], lineWidth=0.1, lifeTime=0, parentObjectUniqueId=robot.getUID(), parentLinkIndex=robot.getJointIndexFromName('tool_center_point')) 170 | p.addUserDebugLine(lineFromXYZ=[0,0,0], lineToXYZ=[0,0,1], lineColorRGB=[0,0,1], lineWidth=0.1, lifeTime=0, parentObjectUniqueId=robot.getUID(), parentLinkIndex=robot.getJointIndexFromName('tool_center_point')) 171 | 172 | #Add the plane only after having initialized the position of the robot arm since moving the robot arm to its initial position can cause a collision 173 | #between the plane and the robot arm. 174 | planeId = p.loadURDF("plane.urdf") 175 | 176 | #The hammer is added in the URDF 177 | hammerId = 0 178 | 179 | #Add visual reference frame on the Object. The positions are relative to the center of mass of the specified link 180 | p.addUserDebugLine(lineFromXYZ=[0,0,0], lineToXYZ=[1,0,0], lineColorRGB=[1,0,0], lineWidth=0.1, lifeTime=0, parentObjectUniqueId=hammerId, parentLinkIndex=-1) 181 | p.addUserDebugLine(lineFromXYZ=[0,0,0], lineToXYZ=[0,1,0], lineColorRGB=[0,1,0], lineWidth=0.1, lifeTime=0, parentObjectUniqueId=hammerId, parentLinkIndex=-1) 182 | p.addUserDebugLine(lineFromXYZ=[0,0,0], lineToXYZ=[0,0,1], lineColorRGB=[0,0,1], lineWidth=0.1, lifeTime=0, parentObjectUniqueId=hammerId, parentLinkIndex=-1) 183 | 184 | 185 | for i in range(0,8000): 186 | p.stepSimulation() 187 | A = np.deg2rad(45) 188 | f0 = 0.7/10 * fixedTimeStep 189 | f1 = 1/10 * fixedTimeStep 190 | f2 = 1.3/10 * fixedTimeStep 191 | f3 = 1.6/10 * fixedTimeStep 192 | joint_goal = [ np.deg2rad(initial_joints_positions_degrees[0]), 193 | np.deg2rad(initial_joints_positions_degrees[1]), 194 | np.deg2rad(initial_joints_positions_degrees[2]), 195 | np.deg2rad(initial_joints_positions_degrees[3])+A*np.sin(2*np.pi*f1*i), 196 | np.deg2rad(initial_joints_positions_degrees[4])+A*np.sin(2*np.pi*f2*i), 197 | np.deg2rad(initial_joints_positions_degrees[5])+A*np.sin(2*np.pi*f3*i)] 198 | robot.setGoal(joint_goal) 199 | time.sleep(fixedTimeStep) 200 | 201 | #Disconnect from the physics server 202 | p.disconnect() -------------------------------------------------------------------------------- /pbsm/ur5/__init__.py: -------------------------------------------------------------------------------- 1 | from .pybullet_UR5 import UR5 2 | 3 | __author__ = "Philippe Nadeau" 4 | __email__ = "philippe.nadeau@robotics.utias.utoronto.ca" 5 | -------------------------------------------------------------------------------- /pbsm/ur5/pybullet_UR5.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | import sys 3 | MIN_PYTHON = (3, 8) 4 | if sys.version_info < MIN_PYTHON: 5 | sys.exit("Python %s.%s or later is required.\n" % MIN_PYTHON) 6 | 7 | import os 8 | import logging 9 | import numpy as np 10 | import pybullet as p 11 | from pathlib import Path 12 | 13 | class UR5: 14 | def __init__(self, simulatorConnectionID, robotUID=None, startingPosition=[0,0,0.001], startingOrientationRAD=[0,0,0], alternateModelFilename=None): 15 | self._cid = simulatorConnectionID 16 | 17 | #If no robot unique id (robotUID) is supplied, we assume that we want to instantiate a new UR5 18 | if robotUID == None: 19 | startPos = startingPosition 20 | startOrientation = p.getQuaternionFromEuler(startingOrientationRAD) 21 | #A slightly modified URDF can be used 22 | if alternateModelFilename != None and Path(alternateModelFilename).exists(): 23 | filename = alternateModelFilename 24 | else: 25 | dirname = os.path.dirname(__file__) 26 | filename = os.path.join(dirname, '../models/UR5.urdf') 27 | self.arm = [p.loadURDF(filename,startPos, startOrientation)] 28 | else: 29 | self.arm = [robotUID] 30 | 31 | self.MAX_VELOCITY = 0.25 32 | self.MAX_FORCE = 150 33 | 34 | def setMaxVelocity(self, max_vel): 35 | #A sanity check should probably be done here 36 | self.MAX_VELOCITY = max_vel 37 | 38 | def setMaxForce(self, max_force): 39 | #A sanity check should probably be done here 40 | #Unreasonable forces can break everything. 41 | self.MAX_FORCE = max_force 42 | 43 | def getUID(self): 44 | return self.arm[0] 45 | 46 | def initJoints(self, jointsPositionDEG): 47 | p.resetJointState(targetValue=np.deg2rad(jointsPositionDEG[0]), bodyUniqueId=self.getUID(), jointIndex=self.getJointIndexFromName('shoulder_pan_joint')) 48 | p.resetJointState(targetValue=np.deg2rad(jointsPositionDEG[1]), bodyUniqueId=self.getUID(), jointIndex=self.getJointIndexFromName('shoulder_lift_joint')) 49 | p.resetJointState(targetValue=np.deg2rad(jointsPositionDEG[2]), bodyUniqueId=self.getUID(), jointIndex=self.getJointIndexFromName('elbow_joint')) 50 | p.resetJointState(targetValue=np.deg2rad(jointsPositionDEG[3]), bodyUniqueId=self.getUID(), jointIndex=self.getJointIndexFromName('wrist_1_joint')) 51 | p.resetJointState(targetValue=np.deg2rad(jointsPositionDEG[4]), bodyUniqueId=self.getUID(), jointIndex=self.getJointIndexFromName('wrist_2_joint')) 52 | p.resetJointState(targetValue=np.deg2rad(jointsPositionDEG[5]), bodyUniqueId=self.getUID(), jointIndex=self.getJointIndexFromName('wrist_3_joint')) 53 | 54 | #Returns the joints positions of the arm 55 | def getArmState(self): 56 | current_joint_position = [ p.getJointState(bodyUniqueId=self.getUID(), jointIndex=self.getJointIndexFromName('shoulder_pan_joint'))[0], 57 | p.getJointState(bodyUniqueId=self.getUID(), jointIndex=self.getJointIndexFromName('shoulder_lift_joint'))[0], 58 | p.getJointState(bodyUniqueId=self.getUID(), jointIndex=self.getJointIndexFromName('elbow_joint'))[0], 59 | p.getJointState(bodyUniqueId=self.getUID(), jointIndex=self.getJointIndexFromName('wrist_1_joint'))[0], 60 | p.getJointState(bodyUniqueId=self.getUID(), jointIndex=self.getJointIndexFromName('wrist_2_joint'))[0], 61 | p.getJointState(bodyUniqueId=self.getUID(), jointIndex=self.getJointIndexFromName('wrist_3_joint'))[0] ] 62 | return current_joint_position 63 | 64 | #Returns joints position error from goal position 65 | def getStateError(self, arm_joint_positions_goal): 66 | state = self.getArmState() 67 | g = arm_joint_positions_goal 68 | error = np.linalg.norm([state[0]-g[0], state[1]-g[1], state[2]-g[2], state[3]-g[3], state[4]-g[4], state[5]-g[5]]) 69 | return error 70 | 71 | #Uses position control to actuate each joint of the robot arm toward the goal 72 | #The joints_goal should be a list of 6 radians 73 | def setGoal(self, joints_goal): 74 | p.setJointMotorControl2(targetPosition=joints_goal[0], bodyIndex=self.getUID(), jointIndex=self.getJointIndexFromName('shoulder_pan_joint'), controlMode=p.POSITION_CONTROL, maxVelocity=self.MAX_VELOCITY, force=self.MAX_FORCE) 75 | p.setJointMotorControl2(targetPosition=joints_goal[1], bodyIndex=self.getUID(), jointIndex=self.getJointIndexFromName('shoulder_lift_joint'), controlMode=p.POSITION_CONTROL, maxVelocity=self.MAX_VELOCITY, force=self.MAX_FORCE) 76 | p.setJointMotorControl2(targetPosition=joints_goal[2], bodyIndex=self.getUID(), jointIndex=self.getJointIndexFromName('elbow_joint'), controlMode=p.POSITION_CONTROL, maxVelocity=self.MAX_VELOCITY, force=self.MAX_FORCE) 77 | p.setJointMotorControl2(targetPosition=joints_goal[3], bodyIndex=self.getUID(), jointIndex=self.getJointIndexFromName('wrist_1_joint'), controlMode=p.POSITION_CONTROL, maxVelocity=self.MAX_VELOCITY, force=self.MAX_FORCE) 78 | p.setJointMotorControl2(targetPosition=joints_goal[4], bodyIndex=self.getUID(), jointIndex=self.getJointIndexFromName('wrist_2_joint'), controlMode=p.POSITION_CONTROL, maxVelocity=self.MAX_VELOCITY, force=self.MAX_FORCE) 79 | p.setJointMotorControl2(targetPosition=joints_goal[5], bodyIndex=self.getUID(), jointIndex=self.getJointIndexFromName('wrist_3_joint'), controlMode=p.POSITION_CONTROL, maxVelocity=self.MAX_VELOCITY, force=self.MAX_FORCE) 80 | 81 | 82 | #Build a dictionnary that can then be used to efficiently retrieve 83 | #joints unique IDs from their names. 84 | def buildJointIndexFromNameDict(self): 85 | self.jointsNamesDict = {} 86 | 87 | numJoints = p.getNumJoints(self.getUID(), self._cid) 88 | 89 | for i in range(numJoints): 90 | jointInfo = p.getJointInfo(self.getUID(), i, self._cid) 91 | strJointName = jointInfo[1].decode('UTF-8') 92 | self.jointsNamesDict[strJointName] = jointInfo[0] 93 | 94 | #Retrieve the unique ID associated with a joint name. 95 | #Returns None if the joint name is not found 96 | def getJointIndexFromName(self, JointName): 97 | #Verify that the joints dictonary has been populated 98 | #if its not the case, do it. 99 | try: 100 | if len(self.jointsNamesDict) == 0: 101 | self.buildJointIndexFromNameDict() 102 | except AttributeError: 103 | self.buildJointIndexFromNameDict() 104 | 105 | jointId = self.jointsNamesDict.get(JointName) 106 | if jointId == None: 107 | logging.warning('Cannot find a joint named: '+JointName) 108 | return jointId -------------------------------------------------------------------------------- /pbsm/ur5_2f85/__init__.py: -------------------------------------------------------------------------------- 1 | from .pybullet_UR5_2F85 import UR5_2F85 2 | 3 | __author__ = "Philippe Nadeau" 4 | __email__ = "philippe.nadeau@robotics.utias.utoronto.ca" 5 | -------------------------------------------------------------------------------- /pbsm/ur5_2f85/pybullet_UR5_2F85.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | import sys 3 | MIN_PYTHON = (3, 8) 4 | if sys.version_info < MIN_PYTHON: 5 | sys.exit("Python %s.%s or later is required.\n" % MIN_PYTHON) 6 | 7 | import os 8 | from pathlib import Path 9 | import numpy as np 10 | import pybullet as p 11 | from pbsm import UR5 12 | from pbsm import Robotiq2F85 13 | 14 | class UR5_2F85: 15 | def __init__(self, simulatorConnectionID, startingPosition=[0,0,0.001], startingOrientationRAD=[0,0,0], alternateModelFilename=None): 16 | self._cid = simulatorConnectionID 17 | startPos = startingPosition 18 | startOrientation = p.getQuaternionFromEuler(startingOrientationRAD) 19 | 20 | #An slightly modified URDF can be used 21 | if alternateModelFilename != None and Path(alternateModelFilename).exists(): 22 | filename = alternateModelFilename 23 | else: 24 | dirname = os.path.dirname(__file__) 25 | filename = os.path.join(dirname, '../models/UR5_2F85.urdf') 26 | 27 | self.robot = [p.loadURDF(filename,startPos, startOrientation)] 28 | 29 | self.ur5 = UR5(self._cid, robotUID=self.getUID()) 30 | self.Rob2f85 = Robotiq2F85(self._cid, robotUID=self.getUID()) 31 | 32 | def setGripperMaxForce(self, max_force): 33 | self.Rob2f85.setMaxForce(max_force) 34 | 35 | def setArmMaxVelocity(self, max_vel): 36 | self.ur5.setMaxVelocity(max_vel) 37 | 38 | def setArmMaxForce(self, max_force): 39 | self.ur5.setMaxForce(max_force) 40 | 41 | def getUID(self): 42 | return self.robot[0] 43 | 44 | def initJoints(self, jointsPositionDEG): 45 | ur5_joints = jointsPositionDEG[0:6] 46 | r2f85 = jointsPositionDEG[6:8] 47 | self.ur5.initJoints(ur5_joints) 48 | self.Rob2f85.initJoints(r2f85) 49 | 50 | #Computes the error between the goal state and the current state 51 | #The state is [position_end_effector, orientation_end_effector, left_gripper_closing, right_gripper_closing] 52 | def stateError(self, arm_joint_positions_goal, gripper_joint_positions_goal): 53 | arm_error = self.ur5.getStateError(arm_joint_positions_goal) 54 | gripper_error = self.Rob2f85.getStateError(gripper_joint_positions_goal) 55 | state_error = np.linalg.norm([arm_error, gripper_error]) 56 | return state_error 57 | 58 | #Given the current joints positions and a relative joints motion provided in relative_joints 59 | #computes the goal in terms of the absolute positions of the joints 60 | def relativeJointsToAbsolute(self, relative_joints): 61 | arm_rel_joints = relative_joints[0] 62 | gripper_rel_joints = relative_joints[1] 63 | 64 | arm_current_joints_pos = self.ur5.getArmState() 65 | gripper_current_joints_pos = self.Rob2f85.getGripperState() 66 | 67 | absolute_joints = [arm_current_joints_pos[0]+arm_rel_joints[0], 68 | arm_current_joints_pos[1]+arm_rel_joints[1], 69 | arm_current_joints_pos[2]+arm_rel_joints[2], 70 | arm_current_joints_pos[3]+arm_rel_joints[3], 71 | arm_current_joints_pos[4]+arm_rel_joints[4], 72 | arm_current_joints_pos[5]+arm_rel_joints[5], 73 | gripper_current_joints_pos[0]*100/self.Rob2f85.MAX_CLOSING+gripper_rel_joints[0], 74 | gripper_current_joints_pos[1]*100/self.Rob2f85.MAX_CLOSING+gripper_rel_joints[1]] 75 | return absolute_joints 76 | 77 | #Given current goal and a list of next goals to iteratively reach, verify if the current goal has been reached 78 | #Call the appropriate functions to move toward the next goal 79 | def setCurrentGoal(self): 80 | #If the error between current state and goal is below this threshold, consider that the goal is reached 81 | ERROR_THRESHOLD = 0.0075 82 | 83 | #The first time setCurrentGoal() is called, self.arm_joint_positions_goal and self.gripper_joint_positions_goal wont exists 84 | #and we need to initialize them (see below except AttributeError) 85 | try: 86 | #If the error is below the treshold AND we have not yet reached the end of the sequence 87 | state_error = self.stateError(self.arm_joint_positions_goal, self.gripper_joint_positions_goal) 88 | if state_error < ERROR_THRESHOLD and (self.robotGoalsSequenceIndex + 1) < len(self.robotGoalsSequence): 89 | #Increment the sequence index 90 | self.robotGoalsSequenceIndex = self.robotGoalsSequenceIndex + 1 91 | #Compute the joints positions goals from the cartesian goal 92 | current_goal = self.robotGoalsSequence[self.robotGoalsSequenceIndex] 93 | #If the goal is expressed as 4 terms, then we need to compute the inverse kinematics 94 | #If the goal is expressed as 2 terms, then it means that the goal is expressing relative joint positions 95 | #If the goal is expressed as 8 terms, then it means that the goal is expressing absolute joint positions 96 | if len(current_goal) == 4: 97 | self.arm_joint_positions_goal = p.calculateInverseKinematics(bodyUniqueId=self.getUID(), endEffectorLinkIndex=self.ur5.getJointIndexFromName('tool_center_point'), 98 | targetPosition=current_goal[0], targetOrientation=current_goal[1], 99 | residualThreshold=0.001, maxNumIterations=100) 100 | self.gripper_joint_positions_goal = current_goal[2:4] 101 | if len(current_goal) == 2: 102 | abs_goal = self.relativeJointsToAbsolute(current_goal) 103 | self.arm_joint_positions_goal = abs_goal[0:6] 104 | self.gripper_joint_positions_goal = abs_goal[6:8] 105 | if len(current_goal) == 8: 106 | self.arm_joint_positions_goal = current_goal[0:6] 107 | self.gripper_joint_positions_goal = current_goal[6:8] 108 | 109 | #This is run as often as possible to keep the position control accurate 110 | self.setArmJointsGoal(self.arm_joint_positions_goal) 111 | self.setGripperGoal(self.gripper_joint_positions_goal[0], self.gripper_joint_positions_goal[1]) 112 | except AttributeError: 113 | #Compute the joints positions goals from the cartesian goal 114 | current_goal = self.robotGoalsSequence[self.robotGoalsSequenceIndex] 115 | self.arm_joint_positions_goal = p.calculateInverseKinematics(bodyUniqueId=self.getUID(), endEffectorLinkIndex=self.ur5.getJointIndexFromName('tool_center_point'), 116 | targetPosition=current_goal[0], targetOrientation=current_goal[1], 117 | residualThreshold=0.001, maxNumIterations=100) 118 | self.gripper_joint_positions_goal = [current_goal[2], current_goal[3]] 119 | self.setArmJointsGoal(self.arm_joint_positions_goal) 120 | self.setGripperGoal(self.gripper_joint_positions_goal[0], self.gripper_joint_positions_goal[1]) 121 | 122 | next_goal = self.robotGoalsSequence[self.robotGoalsSequenceIndex] 123 | 124 | return next_goal 125 | 126 | #Add a goal, which is defined in term of the robot state, to the list the robot will go through sequencially 127 | #Robot state is: [position_end_effector, orientation_end_effector, left_gripper_closing, right_gripper_closing] 128 | def addGoal(self, goal): 129 | #Verify that the list of goals exists, if it does not, then initialize it. 130 | try: 131 | self.robotGoalsSequence.append(goal) 132 | except AttributeError: 133 | self.initGoalSequence() 134 | self.robotGoalsSequence.append(goal) 135 | 136 | def initGoalSequence(self): 137 | self.robotGoalsSequence = [] 138 | self.robotGoalsSequenceIndex = 0 139 | 140 | #Uses position control to actuate each joint of the robot arm toward the goal 141 | #The joints_goal should be a list of 6 radians 142 | def setArmJointsGoal(self, joints_goal): 143 | self.ur5.setGoal(joints_goal) 144 | 145 | #Uses position control to actuate each driver joint of the gripper 146 | #Each of left_finger_goal, right_finger_goal is a percentage that specify how close each finger is (100 = Fully closed, 0 = Fully open) 147 | def setGripperGoal(self, left_finger_goal, right_finger_goal): 148 | self.Rob2f85.setGoal(left_finger_goal, right_finger_goal) 149 | 150 | #Retrieve the unique ID associated with a joint name. 151 | #Returns None if the joint name is not found 152 | def getJointIndexFromName(self, JointName): 153 | return self.ur5.getJointIndexFromName(JointName) 154 | 155 | def printJointsInfo(self): 156 | numJoints = p.getNumJoints(self.getUID(), self._cid) 157 | for i in range(numJoints): 158 | jointInfo = p.getJointInfo(self.getUID(), i, self._cid) 159 | print('Index: \t\t'+str(jointInfo[0])) 160 | print('Name: \t\t'+str(jointInfo[1])) 161 | print('Type: \t\t' + str(jointInfo[2])) 162 | print('qIndex: \t' + str(jointInfo[3])) 163 | print('uIndex: \t' + str(jointInfo[4])) 164 | print('Flags: \t\t' + str(jointInfo[5])) 165 | print('Damping: \t' + str(jointInfo[6])) 166 | print('Friction: \t' + str(jointInfo[7])) 167 | print('LowerLimit: \t' + str(jointInfo[8])) 168 | print('UpperLimit: \t' + str(jointInfo[9])) 169 | print('MaxForce: \t' + str(jointInfo[10])) 170 | print('MaxVelocity: \t' + str(jointInfo[11])) 171 | print('LinkName: \t' + str(jointInfo[12])) 172 | print('Axis: \t\t' + str(jointInfo[13])) 173 | print('FramePos: \t' + str(jointInfo[14])) 174 | print('FrameOrient: \t' + str(jointInfo[15])) 175 | print('ParentIndex: \t' + str(jointInfo[16])) 176 | print('----') 177 | 178 | def printLinksInfo(self): 179 | numJoints = p.getNumJoints(self.getUID(), self._cid) 180 | for i in range(numJoints): 181 | linkInfo = p.getDynamicsInfo(bodyUniqueId=self.getUID(), linkIndex=i) 182 | jointInfo = p.getJointInfo(self.getUID(), i) 183 | print('Name: \t' + str(jointInfo[12])) 184 | print('Mass: \t\t'+str(linkInfo[0])) 185 | print('Lateral friction coeff.: \t\t'+str(linkInfo[1])) 186 | print('Local inertia diagonal: \t\t' + str(linkInfo[2])) 187 | print('Position of inertial frame: \t' + str(linkInfo[3])) 188 | print('Orient. of inertial frame: \t' + str(linkInfo[4])) 189 | print('Coeff. of restitution: \t\t' + str(linkInfo[5])) 190 | print('Rolling friction coeff.: \t' + str(linkInfo[6])) 191 | print('Spinning friction coeff.: \t' + str(linkInfo[7])) 192 | print('Contact damping: \t' + str(linkInfo[8])) 193 | print('Contact stiffness: \t' + str(linkInfo[9])) 194 | print('Body type: \t' + str(linkInfo[10])) 195 | print('Collision margin: \t' + str(linkInfo[11])) 196 | print('----') -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | 3 | setup( 4 | name='pyb-sim-models', 5 | version='0.1.2', 6 | description='Simulation Models for PyBullet', 7 | author='Philippe Nadeau', 8 | author_email='philippe.nadeau@robotics.utias.utoronto.ca', 9 | license='MIT', 10 | packages=['pbsm', 'pbsm.ur5_2f85', 'pbsm.ur5', 'pbsm.Robotiq2f85'], 11 | install_requires=['pybullet>=3.2', 'numpy>=1.24', 'scipy>=1.9'], 12 | include_package_data=True 13 | ) 14 | -------------------------------------------------------------------------------- /test_environment.py: -------------------------------------------------------------------------------- 1 | import os 2 | import time 3 | import math 4 | import numpy as np 5 | import pybullet as p 6 | import pybullet_data 7 | import pbsm 8 | from pbsm import UR5_2F85, UR5, Robotiq2F85 9 | 10 | cid = p.connect(p.GUI)#or p.DIRECT for non-graphical version 11 | 12 | print('Current Simulation Parameters:') 13 | print(p.getPhysicsEngineParameters(cid)) 14 | 15 | #The fixedTimeStep and numSolverIterations are the most important parameters to trade-off quality versus performance 16 | fixedTimeStep = 1. / 240 #Default is 1./240 17 | numSolverIterations = 500 #Default is 50 18 | 19 | #Its best to keep default values for simulation parameters. 20 | p.setPhysicsEngineParameter(numSolverIterations=numSolverIterations) 21 | #"Warning: in many cases it is best to leave the timeStep to default, which is 240Hz. Several 22 | #parameters are tuned with this value in mind. For example the number of solver iterations and 23 | #the error reduction parameters (erp) for contact, friction and non-contact joints are related to the 24 | #time step. If you change the time step, you may need to re-tune those values accordingly, 25 | #especially the erp values." -- The Manual 26 | p.setTimeStep(fixedTimeStep) 27 | p.setRealTimeSimulation(0) 28 | 29 | #Only one additional search path can be supplied to PyBullet 30 | p.setAdditionalSearchPath(pybullet_data.getDataPath()) 31 | print("PyBullet data path: ") 32 | print(pybullet_data.getDataPath()) 33 | 34 | #Disable rendering during loading makes it much faster 35 | p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0) 36 | 37 | #Instanciate the UR5 only 38 | #arm = UR5(cid) 39 | #or instanciate the 2F85 only 40 | #gripper = Robotiq2F85(cid, startingOrientationRAD=[np.deg2rad(-90),0,0]) 41 | #or instanciate a UR5 equipped with a 2F85 42 | robot = UR5_2F85(cid) 43 | 44 | #You might want to play with these values 45 | #but use reasonable values. 46 | robot.setGripperMaxForce(10) 47 | robot.setArmMaxVelocity(0.25) 48 | robot.setArmMaxForce(150) 49 | 50 | #Initialize joints position 51 | initial_joints_positions_degrees = [0, -90, 45, -45, -90, 0, 0, 0] 52 | robot.initJoints(initial_joints_positions_degrees) 53 | 54 | #Re-enable rendering 55 | p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) 56 | 57 | #Set the downward gravity acceleration 58 | p.setGravity(0,0,-9.81) 59 | 60 | #Add the plane only after having initialized the position of the robot arm since moving the robot arm to its initial position can cause a collision 61 | #between the plane and the robot arm. 62 | planeId = p.loadURDF("plane.urdf") 63 | 64 | #A wild hammer enters the scene... 65 | hammer_model_path = os.path.dirname(pbsm.__file__) + "/models/HammerSmaller/urdf/HammerSmaller.urdf" 66 | hammerId = p.loadURDF(hammer_model_path, [0.35,0,0.03], p.getQuaternionFromEuler([0,math.radians(90),math.radians(180)])) 67 | p.changeDynamics(hammerId,-1,lateralFriction=1, spinningFriction=1) 68 | 69 | #Perform a sequence 70 | robot.addGoal([(0.45,0,0.1),p.getQuaternionFromEuler([0,np.deg2rad(-180),0]),0,0]) #Above the hammer 71 | robot.addGoal([(0.45,0,0.02),p.getQuaternionFromEuler([0,np.deg2rad(-180),0]),0,0]) #Pre-grasp 72 | robot.addGoal([(0.45,0,0.02),p.getQuaternionFromEuler([0,np.deg2rad(-180),0]),83,83]) #Grasp 73 | robot.addGoal([(0.45,0,0.2),p.getQuaternionFromEuler([0,np.deg2rad(-180),0]),83,83]) #Lift up 74 | robot.addGoal([(0,0,0,0,np.deg2rad(-30),0),(0,0)]) # Move wrist (Relative joint movement) 75 | robot.addGoal([(0,0,0,0,np.deg2rad(+60),0),(0,0)]) # Move wrist (Relative joint movement) 76 | robot.addGoal([(0,0,0,0,np.deg2rad(-30),0),(0,0)]) # Move wrist (Relative joint movement) 77 | robot.addGoal([(0,0,0,np.deg2rad(-30),0,0),(0,0)]) # Move wrist (Relative joint movement) 78 | robot.addGoal([(0,0,0,np.deg2rad(+60),0,0),(0,0)]) # Move wrist (Relative joint movement) 79 | robot.addGoal([(0,0,0,np.deg2rad(-30),0,0),(0,0)]) # Move wrist (Relative joint movement) 80 | 81 | 82 | #Run the simulator 83 | for i in range(8000): 84 | p.stepSimulation() 85 | robot.setCurrentGoal() 86 | time.sleep(fixedTimeStep) 87 | 88 | #Disconnect from the physics server 89 | p.disconnect() 90 | --------------------------------------------------------------------------------