├── .ipynb_checkpoints └── Untitled-checkpoint.ipynb ├── README.md ├── Untitled.ipynb ├── __init__.py ├── __pycache__ ├── __init__.cpython-36.pyc ├── arm.cpython-37.pyc ├── data.cpython-36.pyc ├── experiment_builder.cpython-36.pyc ├── experiment_builder.cpython-37.pyc ├── few_shot_learning_system.cpython-36.pyc ├── few_shot_learning_system.cpython-37.pyc ├── kuka.cpython-36.pyc ├── kuka.cpython-37.pyc ├── meta_neural_network_architectures.cpython-36.pyc ├── meta_neural_network_architectures.cpython-37.pyc ├── production_model.cpython-36.pyc ├── production_model.cpython-37.pyc ├── rbx1.cpython-36.pyc ├── rbx1.cpython-37.pyc ├── scenes.cpython-36.pyc ├── scenes.cpython-37.pyc ├── ur5.cpython-36.pyc ├── ur5.cpython-37.pyc └── utils.cpython-37.pyc ├── arm.py ├── cntrl.py ├── img.npy ├── kuka.py ├── meshes ├── objects │ ├── block.urdf │ ├── block_blue.urdf │ ├── cube.mtl │ ├── cube.obj │ ├── cube.png │ ├── cube_small.urdf │ ├── cup │ │ ├── Cup │ │ │ ├── cup_vhacd.mtl │ │ │ ├── cup_vhacd.obj │ │ │ ├── textured-0008192-diffuse_map.jpg │ │ │ ├── textured-0008192.mtl │ │ │ └── textured-0008192.obj │ │ └── cup_small.urdf │ ├── lego.obj │ ├── lego.urdf │ ├── lego_blue.urdf │ ├── lego_vhacd.obj │ ├── plate-collision01.obj │ ├── plate-collision02.obj │ ├── plate-collision03.obj │ ├── plate-collision04.obj │ ├── plate-collision05.obj │ ├── plate-collision06.obj │ ├── plate.obj │ ├── plate.urdf │ └── red.png └── ur5 │ ├── collision │ ├── base.stl │ ├── forearm.stl │ ├── shoulder.stl │ ├── upperarm.stl │ ├── wrist1.stl │ ├── wrist2.stl │ └── wrist3.stl │ └── visual │ ├── base.obj │ ├── forearm.obj │ ├── l_gripper_tip_scaled.stl │ ├── shoulder.obj │ ├── upperarm.obj │ ├── wrist1.obj │ ├── wrist2.obj │ └── wrist3.obj ├── model.py ├── octave-workspace ├── pyRobotiqGripper.py ├── rgb.jpg ├── rgb.png ├── scenes.py ├── throw.py ├── throw_ai.py ├── ur5.py └── urdf ├── backup ├── sisbot.urdf.bk ├── sisbot_noertia.urdf └── sisbot_nomass.urdf ├── real_arm.urdf └── ur5.urdf /.ipynb_checkpoints/Untitled-checkpoint.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [], 3 | "metadata": {}, 4 | "nbformat": 4, 5 | "nbformat_minor": 2 6 | } 7 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ur5pybullet 2 | UR5 sim in pybullet, with control via xyz and ori of the head, or individual motor control. Uses elements of a bunch of other sims and the kuka default example but has a very simple gripper / images rendered through GPU and array form motor actions so its a fair bit faster and thus works really well in VR. 3 | 4 | Usage 5 | 6 | xyz/ori: python arm.py --mode xyz 7 | 8 | motors: python arm.py --mode motors 9 | -------------------------------------------------------------------------------- /__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sholtodouglas/ur5pybullet/5d2a6f210e982b6cb3d78ff2be4cfd57496f1211/__init__.py -------------------------------------------------------------------------------- /__pycache__/__init__.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sholtodouglas/ur5pybullet/5d2a6f210e982b6cb3d78ff2be4cfd57496f1211/__pycache__/__init__.cpython-36.pyc -------------------------------------------------------------------------------- /__pycache__/arm.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sholtodouglas/ur5pybullet/5d2a6f210e982b6cb3d78ff2be4cfd57496f1211/__pycache__/arm.cpython-37.pyc -------------------------------------------------------------------------------- /__pycache__/data.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sholtodouglas/ur5pybullet/5d2a6f210e982b6cb3d78ff2be4cfd57496f1211/__pycache__/data.cpython-36.pyc -------------------------------------------------------------------------------- /__pycache__/experiment_builder.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sholtodouglas/ur5pybullet/5d2a6f210e982b6cb3d78ff2be4cfd57496f1211/__pycache__/experiment_builder.cpython-36.pyc -------------------------------------------------------------------------------- /__pycache__/experiment_builder.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sholtodouglas/ur5pybullet/5d2a6f210e982b6cb3d78ff2be4cfd57496f1211/__pycache__/experiment_builder.cpython-37.pyc -------------------------------------------------------------------------------- /__pycache__/few_shot_learning_system.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sholtodouglas/ur5pybullet/5d2a6f210e982b6cb3d78ff2be4cfd57496f1211/__pycache__/few_shot_learning_system.cpython-36.pyc -------------------------------------------------------------------------------- /__pycache__/few_shot_learning_system.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sholtodouglas/ur5pybullet/5d2a6f210e982b6cb3d78ff2be4cfd57496f1211/__pycache__/few_shot_learning_system.cpython-37.pyc -------------------------------------------------------------------------------- /__pycache__/kuka.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sholtodouglas/ur5pybullet/5d2a6f210e982b6cb3d78ff2be4cfd57496f1211/__pycache__/kuka.cpython-36.pyc -------------------------------------------------------------------------------- /__pycache__/kuka.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sholtodouglas/ur5pybullet/5d2a6f210e982b6cb3d78ff2be4cfd57496f1211/__pycache__/kuka.cpython-37.pyc -------------------------------------------------------------------------------- /__pycache__/meta_neural_network_architectures.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sholtodouglas/ur5pybullet/5d2a6f210e982b6cb3d78ff2be4cfd57496f1211/__pycache__/meta_neural_network_architectures.cpython-36.pyc -------------------------------------------------------------------------------- /__pycache__/meta_neural_network_architectures.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sholtodouglas/ur5pybullet/5d2a6f210e982b6cb3d78ff2be4cfd57496f1211/__pycache__/meta_neural_network_architectures.cpython-37.pyc -------------------------------------------------------------------------------- /__pycache__/production_model.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sholtodouglas/ur5pybullet/5d2a6f210e982b6cb3d78ff2be4cfd57496f1211/__pycache__/production_model.cpython-36.pyc -------------------------------------------------------------------------------- /__pycache__/production_model.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sholtodouglas/ur5pybullet/5d2a6f210e982b6cb3d78ff2be4cfd57496f1211/__pycache__/production_model.cpython-37.pyc -------------------------------------------------------------------------------- /__pycache__/rbx1.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sholtodouglas/ur5pybullet/5d2a6f210e982b6cb3d78ff2be4cfd57496f1211/__pycache__/rbx1.cpython-36.pyc -------------------------------------------------------------------------------- /__pycache__/rbx1.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sholtodouglas/ur5pybullet/5d2a6f210e982b6cb3d78ff2be4cfd57496f1211/__pycache__/rbx1.cpython-37.pyc -------------------------------------------------------------------------------- /__pycache__/scenes.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sholtodouglas/ur5pybullet/5d2a6f210e982b6cb3d78ff2be4cfd57496f1211/__pycache__/scenes.cpython-36.pyc -------------------------------------------------------------------------------- /__pycache__/scenes.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sholtodouglas/ur5pybullet/5d2a6f210e982b6cb3d78ff2be4cfd57496f1211/__pycache__/scenes.cpython-37.pyc -------------------------------------------------------------------------------- /__pycache__/ur5.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sholtodouglas/ur5pybullet/5d2a6f210e982b6cb3d78ff2be4cfd57496f1211/__pycache__/ur5.cpython-36.pyc -------------------------------------------------------------------------------- /__pycache__/ur5.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sholtodouglas/ur5pybullet/5d2a6f210e982b6cb3d78ff2be4cfd57496f1211/__pycache__/ur5.cpython-37.pyc -------------------------------------------------------------------------------- /__pycache__/utils.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sholtodouglas/ur5pybullet/5d2a6f210e982b6cb3d78ff2be4cfd57496f1211/__pycache__/utils.cpython-37.pyc -------------------------------------------------------------------------------- /arm.py: -------------------------------------------------------------------------------- 1 | # # 2 | # physicsClient = p.connect(p.GUI) #p.direct for non GUI version 3 | # p.setAdditionalSearchPath(pybullet_data.getDataPath()) #used by loadURDF 4 | # p.setGravity(0,0,-10) 5 | # planeId = p.loadURDF("plane.urdf") 6 | # cubeStartPos = [0,0,4] 7 | # cubeStartOrientation = p.getQuaternionFromEuler([0,0,0]) 8 | # boxId = p.loadURDF("r2d2.urdf",cubeStartPos, cubeStartOrientation) 9 | # p.stepSimulation() 10 | # cubePos, cubeOrn = p.getBasePositionAndOrientation(boxId) 11 | 12 | # while cubePos[2] > 2: 13 | # p.stepSimulation() 14 | # cubePos, cubeOrn = p.getBasePositionAndOrientation(boxId) 15 | # print(cubePos,cubeOrn) 16 | # p.disconnect() 17 | 18 | import os, inspect 19 | import time 20 | 21 | from tqdm import tqdm 22 | currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) 23 | print("current_dir=" + currentdir) 24 | os.sys.path.insert(0, currentdir) 25 | 26 | import click 27 | import math 28 | import gym 29 | import sys 30 | from gym import spaces 31 | from gym.utils import seeding 32 | import numpy as np 33 | import time 34 | import pybullet as p 35 | from itertools import chain 36 | 37 | import random 38 | import pybullet_data 39 | 40 | from kuka import kuka 41 | from ur5 import ur5 42 | import sys 43 | from scenes import * # where our loading stuff in functions are held 44 | 45 | 46 | viewMatrix = p.computeViewMatrixFromYawPitchRoll(cameraTargetPosition = [0,0,0], distance = 0.3, yaw = 90, pitch = -90, roll = 0, upAxisIndex = 2) 47 | projectionMatrix = p.computeProjectionMatrixFOV(fov = 120,aspect = 1,nearVal = 0.01,farVal = 10) 48 | 49 | image_renderer = p.ER_BULLET_HARDWARE_OPENGL # if the rendering throws errors, use ER_TINY_RENDERER, but its hella slow cause its cpu not gpu. 50 | 51 | 52 | 53 | def gripper_camera(obs): 54 | # Center of mass position and orientation (of link-7) 55 | pos = obs[-7:-4] 56 | ori = obs[-4:] # last 4 57 | # rotation = list(p.getEulerFromQuaternion(ori)) 58 | # rotation[2] = 0 59 | # ori = p.getQuaternionFromEuler(rotation) 60 | 61 | rot_matrix = p.getMatrixFromQuaternion(ori) 62 | rot_matrix = np.array(rot_matrix).reshape(3, 3) 63 | # Initial vectors 64 | init_camera_vector = (1, 0, 0) # z-axis 65 | init_up_vector = (0, 1, 0) # y-axis 66 | # Rotated vectors 67 | camera_vector = rot_matrix.dot(init_camera_vector) 68 | up_vector = rot_matrix.dot(init_up_vector) 69 | view_matrix_gripper = p.computeViewMatrix(pos, pos + 0.1 * camera_vector, up_vector) 70 | img = p.getCameraImage(200, 200, view_matrix_gripper, projectionMatrix,shadow=0, flags = p.ER_NO_SEGMENTATION_MASK, renderer=image_renderer) 71 | 72 | 73 | class graspingEnv(gym.Env): 74 | metadata = { 75 | 'render.modes': ['human', 'rgb_array'], 76 | 'video.frames_per_second': 50 77 | } 78 | 79 | def __init__(self, 80 | urdfRoot=pybullet_data.getDataPath(), 81 | actionRepeat=1, 82 | isEnableSelfCollision=True, 83 | renders=True, 84 | arm = 'rbx1', 85 | vr = False): 86 | print("init") 87 | 88 | self._timeStep = 1. / 240. 89 | self._urdfRoot = urdfRoot 90 | self._actionRepeat = actionRepeat 91 | self._isEnableSelfCollision = isEnableSelfCollision 92 | self._observation = [] 93 | self._envStepCounter = 0 94 | self._renders = renders 95 | self._vr = vr 96 | self.terminated = 0 97 | self._p = p 98 | 99 | 100 | if self._renders: 101 | cid = p.connect(p.SHARED_MEMORY) 102 | if (cid < 0): 103 | cid = p.connect(p.GUI) 104 | if self._vr: 105 | p.resetSimulation() 106 | #disable rendering during loading makes it much faster 107 | p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0) 108 | 109 | else: 110 | p.connect(p.DIRECT) 111 | self._seed() 112 | self._arm_str = arm 113 | self._reset() 114 | if self._vr: 115 | p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) 116 | p.setRealTimeSimulation(1) 117 | observationDim = len(self.getSceneObservation()) 118 | observation_high = np.array([np.finfo(np.float32).max] * observationDim) 119 | self.action_space = spaces.Discrete(7) 120 | self.observation_space = spaces.Box(-observation_high, observation_high) 121 | self.viewer = None 122 | 123 | 124 | 125 | def _reset(self): 126 | print("reset") 127 | self.terminated = 0 128 | p.resetSimulation() 129 | p.setPhysicsEngineParameter(numSolverIterations=150) 130 | if not self._vr: 131 | p.setTimeStep(self._timeStep) 132 | self.objects = throwing_scene() 133 | 134 | p.setGravity(0, 0, -10) 135 | if self._arm_str == 'rbx1': 136 | self._arm = rbx1(urdfRootPath=self._urdfRoot, timeStep=self._timeStep) 137 | elif self._arm_str == 'kuka': 138 | self._arm = kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep, vr = self._vr) 139 | else: 140 | self._arm = load_arm_dim_up('ur5',dim='Z') 141 | 142 | self._envStepCounter = 0 143 | p.stepSimulation() 144 | self._observation = self.getSceneObservation() 145 | p.setRealTimeSimulation(1) 146 | return np.array(self._observation) 147 | 148 | def __del__(self): 149 | p.disconnect() 150 | 151 | def _seed(self, seed=None): 152 | self.np_random, seed = seeding.np_random(seed) 153 | return [seed] 154 | 155 | def getSceneObservation(self): 156 | self._observation = self._arm.getObservation() 157 | 158 | # some block to be moved's location and oreintation. 159 | scene_obs = get_scene_observation(self.objects) 160 | 161 | # and also a block for it to be placed on. 162 | 163 | # the vector between end effector location and block to be moved. and vector between 164 | # end effector and goal block. 165 | 166 | # print(endEffectorPos, p.getEulerFromQuaternion(endEffectorOrn), blockPos, p.getEulerFromQuaternion(blockOrn)) 167 | # invEEPos,invEEOrn = p.invertTransform(endEffectorPos,endEffectorOrn) 168 | # blockPosInEE,blockOrnInEE = p.multiplyTransforms(invEEPos,invEEOrn,blockPos,blockOrn) 169 | # blockEulerInEE = p.getEulerFromQuaternion(blockOrnInEE) 170 | # self._observation.extend(list(blockPosInEE)) 171 | # self._observation.extend(list(blockEulerInEE)) 172 | 173 | 174 | # this gives a list which is 8 joint positons, 8 joint velocities, gripper xyz and orientation(quaternion), and 175 | # the position and orientation of the objects in the environment. 176 | 177 | # select which image you want! 178 | 179 | 180 | #top_down_img = p.getCameraImage(500, 500, viewMatrix,projectionMatrix, shadow=0,renderer=image_renderer) 181 | grip_img = gripper_camera(self._observation) 182 | obs = [self._observation, scene_obs] 183 | return obs, grip_img 184 | 185 | #moves motors to desired pos 186 | def step(self, action): 187 | self._arm.action(action) 188 | 189 | for i in range(self._actionRepeat): 190 | p.stepSimulation() 191 | if self._renders: 192 | time.sleep(self._timeStep) 193 | self._observation = self._arm.getObservation() #self.getSceneObservation() 194 | self._envStepCounter += 1 195 | done = self._termination() 196 | reward = self._reward() 197 | 198 | return np.array(self._observation), reward, done, {} 199 | 200 | def _termination(self): 201 | if (self.terminated or self._envStepCounter > 10000): 202 | return True 203 | 204 | def _render(self, mode='human', close=False): 205 | return 206 | 207 | def _reward(self): 208 | 209 | 210 | # use as a RL style reward if you like ################################# 211 | reward = 0 212 | # block_one = self.objects[0] 213 | # blockPos, blockOrn = p.getBasePositionAndOrientation(block_one) 214 | # closestPoints = p.getClosestPoints(block_one, self._arm.uid, 1000) 215 | 216 | # reward = -1000 217 | # numPt = len(closestPoints) 218 | 219 | # if (numPt > 0): 220 | # # print("reward:") 221 | # reward = -closestPoints[0][8] * 10 222 | 223 | # if (blockPos[2] > 0.2): 224 | # print("grasped a block!!!") 225 | # print("self._envStepCounter") 226 | # print(self._envStepCounter) 227 | # reward = reward + 1000 228 | 229 | return reward 230 | 231 | 232 | 233 | ############################################################################################################## 234 | 235 | 236 | def step_to(self, action, abs_rel = 'abs', noise = False, clip = False): 237 | motor_poses = self._arm.move_to(action, abs_rel, noise, clip) 238 | #print(motor_poses) # these are the angles of the joints. 239 | for i in range(self._actionRepeat): 240 | p.stepSimulation() 241 | if self._renders: 242 | time.sleep(self._timeStep) 243 | self._observation = self._arm.getObservation() 244 | 245 | self._envStepCounter += 1 246 | 247 | 248 | done = False #self._termination() 249 | reward = 0#self._reward() 250 | 251 | 252 | 253 | return np.array(self._observation), motor_poses, reward, done, {} 254 | 255 | 256 | def move_in_xyz(environment, arm, abs_rel): 257 | 258 | motorsIds = [] 259 | 260 | dv = 0.01 261 | abs_distance = 1.0 262 | 263 | # 264 | environment._arm.resetJointPoses() 265 | observation = environment._arm.getObservation() 266 | xyz = observation[-7:-4] 267 | ori = p.getEulerFromQuaternion(observation[-4:]) 268 | if abs_rel == 'abs': 269 | print(arm) 270 | 271 | if arm == 'ur5': 272 | xin = xyz[0] 273 | yin = xyz[1] 274 | zin = xyz[2] 275 | rin = ori[0] 276 | pitchin = ori[1] 277 | yawin = ori[2] 278 | else: 279 | xin = 0.537 280 | yin = 0.0 281 | zin = 0.5 282 | rin = math.pi/2 283 | pitchin = -math.pi/2 284 | yawin = 0 285 | 286 | motorsIds.append(environment._p.addUserDebugParameter("X", -abs_distance, abs_distance, xin)) 287 | motorsIds.append(environment._p.addUserDebugParameter("Y", -abs_distance, abs_distance, yin)) 288 | motorsIds.append(environment._p.addUserDebugParameter("Z", -abs_distance, abs_distance, zin)) 289 | motorsIds.append(environment._p.addUserDebugParameter("roll", -math.pi, math.pi, rin,)) 290 | motorsIds.append(environment._p.addUserDebugParameter("pitch", -math.pi, math.pi, pitchin)) 291 | motorsIds.append(environment._p.addUserDebugParameter("yaw", -math.pi, math.pi, yawin)) 292 | 293 | else: 294 | motorsIds.append(environment._p.addUserDebugParameter("dX", -dv, dv, 0)) 295 | motorsIds.append(environment._p.addUserDebugParameter("dY", -dv, dv, 0)) 296 | motorsIds.append(environment._p.addUserDebugParameter("dZ", -dv, dv, 0)) 297 | if arm == 'rbx1': 298 | motorsIds.append(environment._p.addUserDebugParameter("wrist_rotation", -0.1, 0.1, 0)) 299 | motorsIds.append(environment._p.addUserDebugParameter("wrist_flexsion", -0.1, 0.1, 0)) 300 | else: 301 | motorsIds.append(environment._p.addUserDebugParameter("roll", -dv, dv, 0)) 302 | motorsIds.append(environment._p.addUserDebugParameter("pitch", -dv, dv, 0)) 303 | motorsIds.append(environment._p.addUserDebugParameter("yaw", -dv, dv, 0)) 304 | motorsIds.append(environment._p.addUserDebugParameter("fingerAngle", 0, 1.5, .3)) 305 | 306 | done = False 307 | while (not done): 308 | 309 | action = [] 310 | 311 | for motorId in motorsIds: 312 | # print(environment._p.readUserDebugParameter(motorId)) 313 | action.append(environment._p.readUserDebugParameter(motorId)) 314 | 315 | 316 | update_camera(environment) 317 | 318 | #environment._p.addUserDebugLine(environment._arm.endEffectorPos, [0, 0, 0], [1, 0, 0], 5) 319 | 320 | # action is xyz positon, orietnation quaternion, gripper closedness. 321 | action = action[0:3] + list(p.getQuaternionFromEuler(action[3:6])) + [action[6]] 322 | 323 | state, motor_action, reward, done, info = environment.step_to(action, abs_rel) 324 | obs = environment.getSceneObservation() 325 | 326 | ############################################################################################################## 327 | 328 | def setup_controllable_camera(environment): 329 | environment._p.addUserDebugParameter("Camera Zoom", -15, 15, 1.674) 330 | environment._p.addUserDebugParameter("Camera Pan", -360, 360, 70) 331 | environment._p.addUserDebugParameter("Camera Tilt", -360, 360, -50.8) 332 | environment._p.addUserDebugParameter("Camera X", -10, 10,0) 333 | environment._p.addUserDebugParameter("Camera Y", -10, 10,0) 334 | environment._p.addUserDebugParameter("Camera Z", -10, 10,0) 335 | 336 | 337 | 338 | 339 | 340 | def setup_controllable_motors(environment, arm): 341 | 342 | 343 | possible_range = 3.2 # some seem to go to 3, 2.5 is a good rule of thumb to limit range. 344 | motorsIds = [] 345 | 346 | for tests in range(0, environment._arm.numJoints): # motors 347 | 348 | jointInfo = p.getJointInfo(environment._arm.uid, tests) 349 | #print(jointInfo) 350 | qIndex = jointInfo[3] 351 | 352 | if arm == 'kuka': 353 | if qIndex > -1 and jointInfo[0] != 7: 354 | 355 | motorsIds.append(environment._p.addUserDebugParameter("Motor" + str(tests), 356 | -possible_range, 357 | possible_range, 358 | 0.0)) 359 | else: 360 | motorsIds.append(environment._p.addUserDebugParameter("Motor" + str(tests), 361 | -possible_range, 362 | possible_range, 363 | 0.0)) 364 | 365 | return motorsIds 366 | 367 | def update_camera(environment): 368 | if environment._renders: 369 | #Lets reserve the first 6 user debug params for the camera 370 | p.resetDebugVisualizerCamera(environment._p.readUserDebugParameter(0), 371 | environment._p.readUserDebugParameter(1), 372 | environment._p.readUserDebugParameter(2), 373 | [environment._p.readUserDebugParameter(3), 374 | environment._p.readUserDebugParameter(4), 375 | environment._p.readUserDebugParameter(5)]) 376 | 377 | 378 | 379 | def send_commands_to_motor(environment, motorIds): 380 | 381 | done = False 382 | 383 | 384 | while (not done): 385 | action = [] 386 | 387 | for motorId in motorIds: 388 | action.append(environment._p.readUserDebugParameter(motorId)) 389 | print(action) 390 | state, reward, done, info = environment.step(action) 391 | obs = environment.getSceneObservation() 392 | update_camera(environment) 393 | 394 | 395 | environment.terminated = 1 396 | 397 | def control_individual_motors(environment, arm): 398 | motorIds = setup_controllable_motors(environment, arm) 399 | send_commands_to_motor(environment, motorIds) 400 | 401 | 402 | 403 | ################################################################################################### 404 | def make_dir(string): 405 | try: 406 | os.makedirs(string) 407 | except FileExistsError: 408 | pass # directory already exists 409 | 410 | 411 | 412 | ##################################################################################### 413 | 414 | def str_to_bool(string): 415 | if str(string).lower() == "true": 416 | string = True 417 | elif str(string).lower() == "false": 418 | string = False 419 | 420 | return string 421 | 422 | 423 | 424 | def launch(mode, arm, abs_rel, render): 425 | print(arm) 426 | 427 | environment = graspingEnv(renders=str_to_bool(render), arm = arm) 428 | 429 | if environment._renders: 430 | setup_controllable_camera(environment) 431 | 432 | print(mode) 433 | if mode == 'xyz': 434 | move_in_xyz(environment, arm, abs_rel) 435 | else: 436 | environment._arm.active = True 437 | 438 | control_individual_motors(environment, arm) 439 | 440 | 441 | 442 | @click.command() 443 | @click.option('--mode', type=str, default='xyz', help='motor: control individual motors, xyz: control xyz/rpw of gripper, demos: collect automated demos') 444 | @click.option('--abs_rel', type=str, default='abs', help='absolute or relative positioning, abs doesnt really work with rbx1 yet') 445 | @click.option('--arm', type=str, default='ur5', help='rbx1 or kuka') 446 | @click.option('--render', type=bool, default=True, help='rendering') 447 | 448 | 449 | 450 | def main(**kwargs): 451 | launch(**kwargs) 452 | 453 | if __name__ == "__main__": 454 | main() 455 | 456 | -------------------------------------------------------------------------------- /cntrl.py: -------------------------------------------------------------------------------- 1 | # URSoftware 5.1.1 2 | 3 | # Echo client program 4 | import socket 5 | import time 6 | import math 7 | import base64 8 | 9 | 10 | def init_socket(): 11 | #HOST = "169.254.52.193" # The remote host 12 | # HOST = "192.168.0.1" 13 | HOST = "192.168.0.10" 14 | PORT = 30003 # The same port as used by the server 15 | s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 16 | s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) 17 | s.connect((HOST, PORT)) 18 | print('socket',s) 19 | return s 20 | 21 | 22 | 23 | def debug(): 24 | s.send ("set_digital_out(2,True)".encode() + "\n".encode()) 25 | time.sleep(0.05) 26 | 27 | # pose = [x,y,z,rx,ry,rz] 28 | # acceleration and velocity have default values 29 | # if you set a time 't', that overrides 'a' and 'v' 30 | def movel(s, p, a=0.1,v=0.5,t=0,r=0): 31 | try: 32 | script = "movel(p[{},{},{},{},{},{}], a={}, v={}, t={}, r={})" 33 | script = script.format(p[0],p[1],p[2],p[3],p[4],p[5],a,v,t,r) 34 | s.send(bytes(script, 'utf-8')+bytes("\n", 'utf-8')) 35 | msg = s.recv(1024) 36 | except socket.error as socketerror: 37 | print("..... Some kind of error :(...") 38 | return msg 39 | 40 | # angles = [base,shoulder,elbow,wrist1,wrist2,wrist3] 41 | # angles can be given in degrees 42 | def movej(s,angles,a=0.1,v=0.5,t=0,r=0): 43 | # base = angles[0]*math.pi/180 # converting to radians 44 | # shoulder = angles[1]*math.pi/180 45 | # elbow = angles[2]*math.pi/180 46 | # w1 = angles[3]*math.pi/180 47 | # w2 = angles[4]*math.pi/180 48 | # w3 = angles[5]*math.pi/180 49 | print('movingj') 50 | base = angles[0] 51 | shoulder = angles[1] 52 | elbow = angles[2] 53 | w1 = angles[3] 54 | w2 = angles[4] 55 | w3 = angles[5] 56 | 57 | try: 58 | script = "movej([{},{},{},{},{},{}], a={}, v={}, t={}, r={})" 59 | script = script.format(base,shoulder,elbow,w1,w2,w3,a,v,t,r) 60 | s.send(bytes(script, 'utf-8')+bytes("\n", 'utf-8')) 61 | msg = s.recv(1024) 62 | except socket.error as socketerror: 63 | print("..... Some kind of error :(...") 64 | return msg 65 | 66 | # moving to stable joint angles 67 | #movej(s,[80.43,-40.59,61.4,-110.8,-89.77,80.45]) 68 | 69 | 70 | def servoj(s,angles,a=0.1,v=0.5,t=0, gain = 100, lookahead_time = 0.2): 71 | # base = angles[0]*math.pi/180 # converting to radians 72 | # shoulder = angles[1]*math.pi/180 73 | # elbow = angles[2]*math.pi/180 74 | # w1 = angles[3]*math.pi/180 75 | # w2 = angles[4]*math.pi/180 76 | # w3 = angles[5]*math.pi/180 77 | 78 | base = angles[0] 79 | shoulder = angles[1] 80 | elbow = angles[2] 81 | w1 = angles[3] 82 | w2 = angles[4] 83 | w3 = angles[5] 84 | 85 | try: 86 | 87 | script = "servoj([{},{},{},{},{},{}], a={}, v={}, t={}, lookahead_time={}, gain={})" 88 | script = script.format(base,shoulder,elbow,w1,w2,w3,a,v,t, lookahead_time, gain) 89 | s.send(bytes(script, 'utf-8')+bytes("\n", 'utf-8')) 90 | msg = s.recv(1024) 91 | except socket.error as socketerror: 92 | print("..... Some kind of error :(...") 93 | return msg 94 | 95 | 96 | 97 | def initialise(s, a=0.5,v=0.5,t=0,r=0): 98 | 99 | 100 | x = 0 # mm 101 | y = -0.6 102 | z = 0.2 103 | rx = 2.221 # radians 104 | ry = 2.221 105 | rz = 0 106 | p = [x,y,z,rx,ry,rz] 107 | try: 108 | script = "movel(p[{},{},{},{},{},{}], a={}, v={}, t={}, r={})" 109 | script = script.format(p[0],p[1],p[2],p[3],p[4],p[5],a,v,t,r) 110 | s.send(bytes(script, 'utf-8')+bytes("\n", 'utf-8')) 111 | msg = s.recv(1024) 112 | 113 | except socket.error as socketerror: 114 | print("..... Some kind of error :(...") 115 | 116 | return msg 117 | 118 | 119 | # This function doesn't work. The output doesn't make sense 120 | def decode_position(s, msg): 121 | # Decode Pose or Joints from UR 122 | time.sleep(2) 123 | current_position = [0,0,0,0,0,0] 124 | data_start = 0 125 | data_end = 0 126 | n = 0 127 | x = 0 128 | while x < len(msg): 129 | if msg[x]=="," or msg[x]=="]" or msg[x]=="e": 130 | data_end = x 131 | current_position[n] = float(msg[data_start:data_end]) 132 | if msg[x]=="e": 133 | current_position[n] = current_position[n]*math.pow(10,float(msg[x+1:x+4])) 134 | #print "e", msg[x+1:x+4] 135 | #print "e", int(msg[x+1:x+4]) 136 | if n < 5: 137 | x = x+5 138 | data_start = x 139 | else: 140 | break 141 | n=n+1 142 | if msg[x]=="[" or msg[x]==",": 143 | data_start = x+1 144 | x = x+1 145 | 146 | print(current_position) 147 | 148 | # msg = movel(s,[0,-1,0.2,2.221,2.221,0]) 149 | # #decode_position(s, msg) 150 | # print(msg.decode('ISO-8859-1')) 151 | # s = init_socket() 152 | # initialise(s, a=0.5,v=0.5,t=0,r=0) 153 | # s.close() 154 | # # print(base64.b64decode(msg)) 155 | 156 | # s.close() 157 | -------------------------------------------------------------------------------- /img.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sholtodouglas/ur5pybullet/5d2a6f210e982b6cb3d78ff2be4cfd57496f1211/img.npy -------------------------------------------------------------------------------- /kuka.py: -------------------------------------------------------------------------------- 1 | import os, inspect 2 | 3 | currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) 4 | print("current_dir=" + currentdir) 5 | os.sys.path.insert(0, currentdir) 6 | 7 | import math 8 | import gym 9 | import sys 10 | from gym import spaces 11 | from gym.utils import seeding 12 | import numpy as np 13 | import time 14 | import pybullet as p 15 | from itertools import chain 16 | 17 | import random 18 | import pybullet_data 19 | 20 | 21 | class kuka: 22 | 23 | def __init__(self, urdfRootPath=pybullet_data.getDataPath(), timeStep=0.01, vr = False): 24 | self.urdfRootPath = urdfRootPath 25 | self.timeStep = timeStep 26 | self.vr = vr 27 | 28 | self.maxForce = 200. 29 | self.fingerAForce = 6 30 | self.fingerBForce = 5.5 31 | self.fingerTipForce = 6 32 | self.useInverseKinematics = 1 33 | self.useSimulation = 1 34 | self.useNullSpace = 1 35 | self.useOrientation = 1 36 | self.endEffectorIndex = 6 37 | # lower limits for null space 38 | self.ll = [-.967, -2, -2.96, 0.19, -2.96, -2.09, -3.05] 39 | # upper limits for null space 40 | self.ul = [.967, 2, 2.96, 2.29, 2.96, 2.09, 3.05] 41 | # joint ranges for null space 42 | self.jr = [5.8, 4, 5.8, 4, 5.8, 4, 6] 43 | # restposes for null space 44 | self.rp = [0, 0, 0, 0.5 * math.pi, 0, -math.pi * 0.5 * 0.66, 0] 45 | # joint damping coefficents 46 | self.jd = [0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 47 | 0.00001, 0.00001, 0.00001] 48 | self.reset() 49 | 50 | def reset(self): 51 | 52 | # 53 | if self.vr: 54 | objects = [p.loadURDF("kuka_iiwa/model_vr_limits.urdf")] 55 | else: 56 | objects = p.loadSDF(os.path.join(self.urdfRootPath, "kuka_iiwa/kuka_with_gripper2.sdf")) 57 | self.uid = objects[0] 58 | # for i in range (p.getNumJoints(self.uid)): 59 | # print(p.getJointInfo(self.uid,i)) 60 | p.resetBasePositionAndOrientation(self.uid, [-0.100000, 0.000000, -0.130000], 61 | [0.000000, 0.000000, 0.000000, 1.000000]) 62 | self.jointPositions = [0.006418, 0.413184, -0.011401, -1.589317, 0.005379, 1.137684, -0.006539, 0.000048, 63 | -0.299912, 0.000000, -0.000043, 0.299960, 0.000000, -0.000200] 64 | self.numJoints = p.getNumJoints(self.uid) 65 | 66 | for jointIndex in range(self.numJoints): 67 | p.resetJointState(self.uid, jointIndex, self.jointPositions[jointIndex]) 68 | p.setJointMotorControl2(self.uid, jointIndex, p.POSITION_CONTROL, 69 | targetPosition=self.jointPositions[jointIndex], force= 0)#self.maxForce) 70 | 71 | # self.trayUid = p.loadURDF(os.path.join(self.urdfRootPath,"tray/tray.urdf"), 0.640000,0.075000,-0.190000,0.000000,0.000000,1.000000,0.000000) 72 | self.endEffectorPos = [0.537, 0.5, 0.5] 73 | self.endEffectorOrn = [ math.pi/2, -math.pi, 0] 74 | self.endEffectorAngle = 0 75 | 76 | self.motorNames = [] 77 | self.motorIndices = [] 78 | 79 | for i in range(self.numJoints): 80 | jointInfo = p.getJointInfo(self.uid, i) 81 | # print(jointInfo) 82 | qIndex = jointInfo[3] 83 | if qIndex > -1 and jointInfo[0] != 7: # 7 and 6 rotate on eachother, but 7 stops us from observing orientation well. 84 | # print("motorname") 85 | # print(jointInfo[1]) 86 | self.motorNames.append(str(jointInfo[1])) 87 | self.motorIndices.append(i) 88 | 89 | def getActionDimension(self): 90 | if (self.useInverseKinematics): 91 | return len(self.motorIndices) 92 | return 6 # position x,y,z and roll/pitch/yaw euler angles of end effector 93 | 94 | def getObservationDimension(self): 95 | return len(self.getObservation()) 96 | 97 | def setPosition(self, pos, quat): 98 | 99 | p.resetBasePositionAndOrientation(self.uid,pos, 100 | quat) 101 | 102 | def addGripper(self): 103 | objects = p.loadSDF("gripper/wsg50_one_motor_gripper_new_free_base.sdf") 104 | kuka_gripper = objects[0] 105 | print ("kuka gripper=") 106 | print(kuka_gripper) 107 | self._gripper = kuka_gripper 108 | 109 | p.resetBasePositionAndOrientation(kuka_gripper,[0.923103,-0.200000,1.250036],[-0.000000,0.964531,-0.000002,-0.263970]) 110 | jointPositions=[ 0.000000, -0.011130, -0.206421, 0.205143, -0.009999, 0.000000, -0.010055, 0.000000 ] 111 | for jointIndex in range (p.getNumJoints(kuka_gripper)): 112 | p.resetJointState(kuka_gripper,jointIndex,jointPositions[jointIndex]) 113 | p.setJointMotorControl2(kuka_gripper,jointIndex,p.POSITION_CONTROL,jointPositions[jointIndex],0) 114 | 115 | 116 | kuka_cid = p.createConstraint(self.uid, 6, kuka_gripper,0,p.JOINT_FIXED, [0,0,0], [0,0,0.05],[0,0,0]) 117 | 118 | pr2_cid2 = p.createConstraint(kuka_gripper,4,kuka_gripper,6,jointType=p.JOINT_GEAR,jointAxis =[1,1,1],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) 119 | p.changeConstraint(pr2_cid2,gearRatio=-1, erp=0.5, relativePositionTarget=0, maxForce=100) 120 | 121 | def getObservation(self): 122 | observation = [] 123 | state = p.getLinkState(self.uid, self.endEffectorIndex) 124 | pos = state[0] 125 | orn = state[1] 126 | euler = p.getEulerFromQuaternion(orn) 127 | 128 | observation.extend(list(pos)) 129 | observation.extend(list(orn)) 130 | 131 | joint_positions = list() 132 | joint_velocities = list() 133 | applied_torques = list() 134 | for jointIndex in range(self.numJoints): 135 | state = p.getJointState(self.uid, jointIndex) 136 | angle = state[0] 137 | dv = state[1] 138 | applied_torque = state[3] 139 | 140 | joint_positions.append(angle) 141 | joint_velocities.append(dv) 142 | applied_torques.append(applied_torque) 143 | 144 | 145 | # print(joint_positions_and_velocities) 146 | #print(np.round(np.array(applied_torques),2)[:7]) 147 | 148 | #print(len(joint_positions), len(joint_velocities), len(observation)) 149 | 150 | #print(np.array(joint_positions).shape, np.array(joint_velocities).shape, np.array(observation).shape) 151 | return joint_positions + joint_velocities + observation 152 | 153 | def action(self, motorCommands): 154 | # for the kuka, motor commands should be like this, 12 long 155 | # [ 0.006418, 0.413184, -0.011401, -1.589317, 0.005379, 1.137684, -0.006539, 0.000048, -0.299912, 0.000000, -0.000043, 0.299960] 156 | # there are 14 joints but only 12 motors? 157 | # zero indexed - 158 | # index 7 is end effector angle 159 | # index 8 is finger angle A 160 | # index 11 is finger angle B 161 | motorCommands = np.clip(motorCommands, self.ll, self.ul) 162 | 163 | #print(len(motorCommands)) 164 | for action in range(len(motorCommands)): 165 | motor = self.motorIndices[action] 166 | #print(motor) 167 | 168 | 169 | p.setJointMotorControl2(self.uid, motor, p.POSITION_CONTROL, targetPosition=motorCommands[action], 170 | force=self.maxForce) 171 | 172 | def move_to(self, position_delta, mode = 'abs', noise = False, clip = False): 173 | 174 | #mode is either absolute or relative 175 | 176 | # print ("self.numJoints") 177 | # print (self.numJoints) 178 | 179 | 180 | 181 | if (self.useInverseKinematics): 182 | 183 | if mode == 'abs': #absolute positioning 184 | 185 | self.endEffectorPos = [position_delta[0],position_delta[1],position_delta[2]] 186 | self.endEffectorOrn = [position_delta[3],position_delta[4],position_delta[5], position_delta[6]] 187 | pos = self.endEffectorPos 188 | orn = self.endEffectorOrn 189 | fingerAngle = position_delta[7] 190 | 191 | else: #mode is relative 192 | 193 | ## this where where how much we move is extracted 194 | dx = position_delta[0] 195 | dy = position_delta[1] 196 | dz = position_delta[2] 197 | # da = position_delta[3] 198 | 199 | dr,dp,dyaw = p.getEulerFromQuaternion([position_delta[3], position_delta[4], position_delta[5], position_delta[6]]) 200 | 201 | fingerAngle = position_delta[6] 202 | 203 | self.endEffectorOrn[0] = self.endEffectorOrn[0] + dr 204 | self.endEffectorOrn[1] = self.endEffectorOrn[1] + dp 205 | self.endEffectorOrn[2] = self.endEffectorOrn[2] + dyaw 206 | 207 | state = p.getLinkState(self.uid, self.endEffectorIndex) 208 | actualEndEffectorPos = state[0] 209 | actualEndEffectorOrn = state[1] 210 | 211 | self.endEffectorPos[0] = self.endEffectorPos[0] + dx 212 | self.endEffectorPos[1] = self.endEffectorPos[1] + dy 213 | if (dz > 0 or actualEndEffectorPos[2] > 0.10): 214 | self.endEffectorPos[2] = self.endEffectorPos[2] + dz 215 | if (actualEndEffectorPos[2] < 0.10): 216 | self.endEffectorPos[2] = self.endEffectorPos[2] + 0.0001 217 | 218 | # self.endEffectorAngle = self.endEffectorAngle + da 219 | pos = self.endEffectorPos 220 | # orn = p.getQuaternionFromEuler([0,-math.pi,0]) # -math.pi,yaw]) 221 | orn = p.getQuaternionFromEuler(self.endEffectorOrn) # -math.pi,yaw]) 222 | # orn = actualEndEffectorOrn 223 | 224 | if (self.useNullSpace == 1): 225 | if (self.useOrientation == 1): 226 | jointPoses = p.calculateInverseKinematics(self.uid, self.endEffectorIndex, pos, orn, 227 | self.ll, self.ul, self.jr, self.rp) 228 | else: 229 | jointPoses = p.calculateInverseKinematics(self.uid, self.endEffectorIndex, pos, 230 | lowerLimits=self.ll, upperLimits=self.ul, 231 | jointRanges=self.jr, restPoses=self.rp) 232 | else: 233 | if (self.useOrientation == 1): 234 | jointPoses = p.calculateInverseKinematics(self.uid, self.endEffectorIndex, pos, orn, 235 | jointDamping=self.jd) 236 | else: 237 | jointPoses = p.calculateInverseKinematics(self.uid, self.endEffectorIndex, pos) 238 | 239 | #print("jointPoses") 240 | #print(jointPoses) 241 | #print(len(jointPoses)) 242 | # print("self.endEffectorIndex") 243 | # print(self.endEffectorIndex) 244 | 245 | jointPoses = np.array(jointPoses) 246 | 247 | if clip == True: 248 | for i in range(self.endEffectorIndex + 1): 249 | state = p.getJointState(self.uid,i ) 250 | current_angle = state[0] 251 | difference = jointPoses[i] - current_angle 252 | clipped_diff = np.clip(difference, -0.1, 0.1) 253 | jointPoses[i] = current_angle + clipped_diff 254 | 255 | true_desired_positions = jointPoses 256 | 257 | if noise == True: 258 | 259 | noise_factor = random.random()*0.5 #noise amount 260 | #print(noise_factor) 261 | #print(jointPoses) 262 | # cause its range 0-1, centered, scaled. 263 | jointPoses = jointPoses + (np.random.rand(len(jointPoses))-0.5)*noise_factor 264 | #print(jointPoses) 265 | 266 | 267 | 268 | 269 | if (self.useSimulation): 270 | for i in range(self.endEffectorIndex + 1): 271 | #print(i) 272 | p.setJointMotorControl2(bodyIndex=self.uid, jointIndex=i, controlMode=p.POSITION_CONTROL, 273 | targetPosition=jointPoses[i], targetVelocity=0, force=self.maxForce, 274 | positionGain=0.03, velocityGain=1) 275 | else: 276 | # reset the joint state (ignoring all dynamics, not recommended to use during simulation) 277 | for i in range(self.numJoints): 278 | p.resetJointState(self.uid, i, jointPoses[i]) 279 | # fingers 280 | if not self.vr: 281 | 282 | # p.setJointMotorControl2(self.uid,7,p.POSITION_CONTROL,targetPosition=self.endEffectorAngle,force=self.maxForce) 283 | p.setJointMotorControl2(self.uid, 8, p.POSITION_CONTROL, targetPosition=-fingerAngle, 284 | force=self.fingerAForce) 285 | p.setJointMotorControl2(self.uid, 11, p.POSITION_CONTROL, targetPosition=fingerAngle, 286 | force=self.fingerBForce) 287 | 288 | p.setJointMotorControl2(self.uid, 10, p.POSITION_CONTROL, targetPosition=0, force=self.fingerTipForce) 289 | p.setJointMotorControl2(self.uid, 13, p.POSITION_CONTROL, targetPosition=0, force=self.fingerTipForce) 290 | 291 | 292 | #joint poses are the actions to be sent to the motors, i.e, what the action of the neural net will be. 293 | return true_desired_positions[:7] # Only return the true joint poses desired, not noise injected. 294 | #TODO LATER ALSO RETURN FINGER ANGLE 295 | 296 | 297 | 298 | -------------------------------------------------------------------------------- /meshes/objects/block.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 | -------------------------------------------------------------------------------- /meshes/objects/block_blue.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 | -------------------------------------------------------------------------------- /meshes/objects/cube.mtl: -------------------------------------------------------------------------------- 1 | 2 | newmtl cube 3 | Ns 10.0000 4 | Ni 1.5000 5 | d 1.0000 6 | Tr 0.0000 7 | Tf 1.0000 1.0000 1.0000 8 | illum 2 9 | Ka 0.0000 0.0000 0.0000 10 | Kd 0.5880 0.5880 0.5880 11 | Ks 0.0000 0.0000 0.0000 12 | Ke 0.0000 0.0000 0.0000 13 | map_Ka cube.tga 14 | map_Kd red.png -------------------------------------------------------------------------------- /meshes/objects/cube.obj: -------------------------------------------------------------------------------- 1 | # cube.obj 2 | # 3 | 4 | o cube 5 | mtllib cube.mtl 6 | 7 | v -0.500000 -0.500000 0.500000 8 | v 0.500000 -0.500000 0.500000 9 | v -0.500000 0.500000 0.500000 10 | v 0.500000 0.500000 0.500000 11 | v -0.500000 0.500000 -0.500000 12 | v 0.500000 0.500000 -0.500000 13 | v -0.500000 -0.500000 -0.500000 14 | v 0.500000 -0.500000 -0.500000 15 | 16 | vt 0.000000 0.000000 17 | vt 1.000000 0.000000 18 | vt 0.000000 1.000000 19 | vt 1.000000 1.000000 20 | 21 | vn 0.000000 0.000000 1.000000 22 | vn 0.000000 1.000000 0.000000 23 | vn 0.000000 0.000000 -1.000000 24 | vn 0.000000 -1.000000 0.000000 25 | vn 1.000000 0.000000 0.000000 26 | vn -1.000000 0.000000 0.000000 27 | 28 | g cube 29 | usemtl cube 30 | s 1 31 | f 1/1/1 2/2/1 3/3/1 32 | f 3/3/1 2/2/1 4/4/1 33 | s 2 34 | f 3/1/2 4/2/2 5/3/2 35 | f 5/3/2 4/2/2 6/4/2 36 | s 3 37 | f 5/4/3 6/3/3 7/2/3 38 | f 7/2/3 6/3/3 8/1/3 39 | s 4 40 | f 7/1/4 8/2/4 1/3/4 41 | f 1/3/4 8/2/4 2/4/4 42 | s 5 43 | f 2/1/5 8/2/5 4/3/5 44 | f 4/3/5 8/2/5 6/4/5 45 | s 6 46 | f 7/1/6 1/2/6 5/3/6 47 | f 5/3/6 1/2/6 3/4/6 48 | 49 | -------------------------------------------------------------------------------- /meshes/objects/cube.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sholtodouglas/ur5pybullet/5d2a6f210e982b6cb3d78ff2be4cfd57496f1211/meshes/objects/cube.png -------------------------------------------------------------------------------- /meshes/objects/cube_small.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 | -------------------------------------------------------------------------------- /meshes/objects/cup/Cup/cup_vhacd.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 14 3 | 4 | newmtl Shape.014 5 | Ns 400.000000 6 | Ka 0.000000 0.000000 0.000000 7 | Kd 0.664000 0.688000 0.616000 8 | Ks 0.250000 0.250000 0.250000 9 | Ni 1.000000 10 | d 0.500000 11 | illum 2 12 | 13 | newmtl Shape.015 14 | Ns 400.000000 15 | Ka 0.000000 0.000000 0.000000 16 | Kd 0.120000 0.744000 0.280000 17 | Ks 0.250000 0.250000 0.250000 18 | Ni 1.000000 19 | d 0.500000 20 | illum 2 21 | 22 | newmtl Shape.016 23 | Ns 400.000000 24 | Ka 0.000000 0.000000 0.000000 25 | Kd 0.688000 0.736000 0.392000 26 | Ks 0.250000 0.250000 0.250000 27 | Ni 1.000000 28 | d 0.500000 29 | illum 2 30 | 31 | newmtl Shape.017 32 | Ns 400.000000 33 | Ka 0.000000 0.000000 0.000000 34 | Kd 0.168000 0.496000 0.216000 35 | Ks 0.250000 0.250000 0.250000 36 | Ni 1.000000 37 | d 0.500000 38 | illum 2 39 | 40 | newmtl Shape.018 41 | Ns 400.000000 42 | Ka 0.000000 0.000000 0.000000 43 | Kd 0.720000 0.472000 0.504000 44 | Ks 0.250000 0.250000 0.250000 45 | Ni 1.000000 46 | d 0.500000 47 | illum 2 48 | 49 | newmtl Shape.019 50 | Ns 400.000000 51 | Ka 0.000000 0.000000 0.000000 52 | Kd 0.576000 0.288000 0.088000 53 | Ks 0.250000 0.250000 0.250000 54 | Ni 1.000000 55 | d 0.500000 56 | illum 2 57 | 58 | newmtl Shape.020 59 | Ns 400.000000 60 | Ka 0.000000 0.000000 0.000000 61 | Kd 0.544000 0.536000 0.232000 62 | Ks 0.250000 0.250000 0.250000 63 | Ni 1.000000 64 | d 0.500000 65 | illum 2 66 | 67 | newmtl Shape.021 68 | Ns 400.000000 69 | Ka 0.000000 0.000000 0.000000 70 | Kd 0.656000 0.240000 0.496000 71 | Ks 0.250000 0.250000 0.250000 72 | Ni 1.000000 73 | d 0.500000 74 | illum 2 75 | 76 | newmtl Shape.022 77 | Ns 400.000000 78 | Ka 0.000000 0.000000 0.000000 79 | Kd 0.184000 0.536000 0.280000 80 | Ks 0.250000 0.250000 0.250000 81 | Ni 1.000000 82 | d 0.500000 83 | illum 2 84 | 85 | newmtl Shape.023 86 | Ns 400.000000 87 | Ka 0.000000 0.000000 0.000000 88 | Kd 0.232000 0.016000 0.176000 89 | Ks 0.250000 0.250000 0.250000 90 | Ni 1.000000 91 | d 0.500000 92 | illum 2 93 | 94 | newmtl Shape.024 95 | Ns 400.000000 96 | Ka 0.000000 0.000000 0.000000 97 | Kd 0.464000 0.552000 0.536000 98 | Ks 0.250000 0.250000 0.250000 99 | Ni 1.000000 100 | d 0.500000 101 | illum 2 102 | 103 | newmtl Shape.025 104 | Ns 400.000000 105 | Ka 0.000000 0.000000 0.000000 106 | Kd 0.744000 0.448000 0.088000 107 | Ks 0.250000 0.250000 0.250000 108 | Ni 1.000000 109 | d 0.500000 110 | illum 2 111 | 112 | newmtl Shape.026 113 | Ns 400.000000 114 | Ka 0.000000 0.000000 0.000000 115 | Kd 0.336000 0.232000 0.584000 116 | Ks 0.250000 0.250000 0.250000 117 | Ni 1.000000 118 | d 0.500000 119 | illum 2 120 | 121 | newmtl Shape.027 122 | Ns 400.000000 123 | Ka 0.000000 0.000000 0.000000 124 | Kd 0.168000 0.152000 0.672000 125 | Ks 0.250000 0.250000 0.250000 126 | Ni 1.000000 127 | d 0.500000 128 | illum 2 129 | -------------------------------------------------------------------------------- /meshes/objects/cup/Cup/textured-0008192-diffuse_map.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sholtodouglas/ur5pybullet/5d2a6f210e982b6cb3d78ff2be4cfd57496f1211/meshes/objects/cup/Cup/textured-0008192-diffuse_map.jpg -------------------------------------------------------------------------------- /meshes/objects/cup/Cup/textured-0008192.mtl: -------------------------------------------------------------------------------- 1 | newmtl material_0 2 | # shader_type beckmann 3 | map_Kd textured-0008192-diffuse_map.jpg 4 | 5 | -------------------------------------------------------------------------------- /meshes/objects/cup/cup_small.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 | -------------------------------------------------------------------------------- /meshes/objects/lego.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 | -------------------------------------------------------------------------------- /meshes/objects/lego_blue.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 | -------------------------------------------------------------------------------- /meshes/objects/plate-collision01.obj: -------------------------------------------------------------------------------- 1 | mtllib dinnerware.mtl 2 | usemtl pan_tefal 3 | v 0.049745 0.084455 0.060000 4 | v 0.051940 0.089963 0.060000 5 | v 0.029150 0.050489 0.000000 6 | v 0.027560 0.047735 0.000000 7 | v 0.098993 0.000845 0.060600 8 | v 0.104919 0.000000 0.060600 9 | v 0.058883 0.000000 0.000000 10 | v 0.055671 0.000000 0.000000 11 | vn -0.962255 -0.069189 0.263208 12 | vn 0.888900 -0.266051 0.372926 13 | vn -0.704117 -0.151605 -0.693711 14 | vn -0.096471 -0.952673 -0.288284 15 | vn 0.353510 0.535276 0.767144 16 | vn 0.049583 0.551162 -0.832924 17 | vn -0.080990 0.081838 -0.993349 18 | vn 0.250326 0.614585 -0.748080 19 | f 1//1 2//2 4//4 20 | f 1//1 5//5 2//2 21 | f 1//1 6//6 2//2 22 | f 1//1 4//4 5//5 23 | f 1//1 5//5 6//6 24 | f 2//2 3//3 4//4 25 | f 2//2 7//7 3//3 26 | f 2//2 5//5 6//6 27 | f 2//2 6//6 7//7 28 | f 3//3 7//7 4//4 29 | f 3//3 8//8 4//4 30 | f 3//3 7//7 8//8 31 | f 4//4 8//8 5//5 32 | f 4//4 7//7 8//8 33 | f 5//5 8//8 6//6 34 | f 6//6 8//8 7//7 35 | -------------------------------------------------------------------------------- /meshes/objects/plate-collision02.obj: -------------------------------------------------------------------------------- 1 | mtllib dinnerware.mtl 2 | usemtl pan_tefal 3 | v -0.048268 0.085308 0.060000 4 | v -0.051940 0.089963 0.060000 5 | v -0.029150 0.050489 0.000000 6 | v -0.027560 0.047735 0.000000 7 | v 0.048765 0.086152 0.060600 8 | v 0.052459 0.090862 0.060600 9 | v 0.029442 0.050994 0.000000 10 | v 0.027836 0.048213 0.000000 11 | vn -0.471567 -0.127056 -0.872629 12 | vn 0.616670 0.313448 -0.722127 13 | vn 0.434277 -0.589449 -0.681141 14 | vn -0.696898 0.673938 -0.245235 15 | vn -0.964280 0.142499 -0.223288 16 | vn -0.118273 0.245678 -0.962109 17 | vn 0.333505 0.310546 -0.890133 18 | vn 0.592935 -0.751851 -0.288355 19 | f 1//1 2//2 4//4 20 | f 1//1 5//5 2//2 21 | f 1//1 6//6 2//2 22 | f 1//1 4//4 5//5 23 | f 1//1 5//5 6//6 24 | f 2//2 3//3 4//4 25 | f 2//2 7//7 3//3 26 | f 2//2 5//5 6//6 27 | f 2//2 6//6 7//7 28 | f 3//3 7//7 4//4 29 | f 3//3 8//8 4//4 30 | f 3//3 7//7 8//8 31 | f 4//4 8//8 5//5 32 | f 4//4 7//7 8//8 33 | f 5//5 8//8 6//6 34 | f 6//6 8//8 7//7 35 | -------------------------------------------------------------------------------- /meshes/objects/plate-collision03.obj: -------------------------------------------------------------------------------- 1 | mtllib dinnerware.mtl 2 | usemtl pan_tefal 3 | v -0.098012 0.000853 0.060000 4 | v -0.103880 0.000000 0.060000 5 | v -0.058300 0.000000 0.000000 6 | v -0.055120 0.000000 0.000000 7 | v -0.050228 0.085308 0.060600 8 | v -0.052459 0.090862 0.060600 9 | v -0.029441 0.050994 0.000000 10 | v -0.027836 0.048213 0.000000 11 | vn -0.570326 0.565919 -0.595368 12 | vn -0.987481 0.122113 -0.099846 13 | vn 0.256877 0.509622 0.821157 14 | vn -0.149510 -0.272737 0.950401 15 | vn 0.250981 -0.885212 0.391675 16 | vn 0.623883 0.757497 -0.192269 17 | vn 0.116527 -0.876135 -0.467769 18 | vn -0.474452 0.562082 -0.677466 19 | f 1//1 2//2 4//4 20 | f 1//1 5//5 2//2 21 | f 1//1 6//6 2//2 22 | f 1//1 4//4 5//5 23 | f 1//1 5//5 6//6 24 | f 2//2 3//3 4//4 25 | f 2//2 7//7 3//3 26 | f 2//2 5//5 6//6 27 | f 2//2 6//6 7//7 28 | f 3//3 7//7 4//4 29 | f 3//3 8//8 4//4 30 | f 3//3 7//7 8//8 31 | f 4//4 8//8 5//5 32 | f 4//4 7//7 8//8 33 | f 5//5 8//8 6//6 34 | f 6//6 8//8 7//7 35 | -------------------------------------------------------------------------------- /meshes/objects/plate-collision04.obj: -------------------------------------------------------------------------------- 1 | mtllib dinnerware.mtl 2 | usemtl pan_tefal 3 | v -0.049745 -0.084455 0.060000 4 | v -0.051940 -0.089963 0.060000 5 | v -0.029150 -0.050489 0.000000 6 | v -0.027560 -0.047735 0.000000 7 | v -0.098993 -0.000845 0.060600 8 | v -0.104919 0.000000 0.060600 9 | v -0.058883 0.000000 0.000000 10 | v -0.055671 0.000000 0.000000 11 | vn -0.454516 0.625342 -0.634321 12 | vn -0.518286 -0.578386 0.629959 13 | vn -0.173259 0.226757 -0.958417 14 | vn -0.247982 -0.510538 -0.823320 15 | vn -0.417030 0.446958 0.791400 16 | vn -0.004783 -0.717725 -0.696310 17 | vn 0.273713 0.103751 0.956199 18 | vn 0.429490 0.309534 -0.848367 19 | f 1//1 2//2 4//4 20 | f 1//1 5//5 2//2 21 | f 1//1 6//6 2//2 22 | f 1//1 4//4 5//5 23 | f 1//1 5//5 6//6 24 | f 2//2 3//3 4//4 25 | f 2//2 7//7 3//3 26 | f 2//2 5//5 6//6 27 | f 2//2 6//6 7//7 28 | f 3//3 7//7 4//4 29 | f 3//3 8//8 4//4 30 | f 3//3 7//7 8//8 31 | f 4//4 8//8 5//5 32 | f 4//4 7//7 8//8 33 | f 5//5 8//8 6//6 34 | f 6//6 8//8 7//7 35 | -------------------------------------------------------------------------------- /meshes/objects/plate-collision05.obj: -------------------------------------------------------------------------------- 1 | mtllib dinnerware.mtl 2 | usemtl pan_tefal 3 | v 0.048268 -0.085308 0.060000 4 | v 0.051940 -0.089963 0.060000 5 | v 0.029150 -0.050489 0.000000 6 | v 0.027560 -0.047735 0.000000 7 | v -0.048765 -0.086152 0.060600 8 | v -0.052459 -0.090862 0.060600 9 | v -0.029442 -0.050994 0.000000 10 | v -0.027836 -0.048213 0.000000 11 | vn 0.404852 0.441710 0.800617 12 | vn -0.882548 -0.029017 0.469326 13 | vn 0.182767 0.981935 0.048984 14 | vn -0.768458 0.392626 -0.505290 15 | vn 0.592713 0.805382 -0.007215 16 | vn 0.400028 0.869068 0.291031 17 | vn -0.642772 0.691166 0.330355 18 | vn 0.104670 0.652086 -0.750885 19 | f 1//1 2//2 4//4 20 | f 1//1 5//5 2//2 21 | f 1//1 6//6 2//2 22 | f 1//1 4//4 5//5 23 | f 1//1 5//5 6//6 24 | f 2//2 3//3 4//4 25 | f 2//2 7//7 3//3 26 | f 2//2 5//5 6//6 27 | f 2//2 6//6 7//7 28 | f 3//3 7//7 4//4 29 | f 3//3 8//8 4//4 30 | f 3//3 7//7 8//8 31 | f 4//4 8//8 5//5 32 | f 4//4 7//7 8//8 33 | f 5//5 8//8 6//6 34 | f 6//6 8//8 7//7 35 | -------------------------------------------------------------------------------- /meshes/objects/plate-collision06.obj: -------------------------------------------------------------------------------- 1 | mtllib dinnerware.mtl 2 | usemtl pan_tefal 3 | v 0.098012 -0.000853 0.060000 4 | v 0.103880 -0.000000 0.060000 5 | v 0.058300 -0.000000 0.000000 6 | v 0.055120 -0.000000 0.000000 7 | v 0.050228 -0.085308 0.060600 8 | v 0.052459 -0.090862 0.060600 9 | v 0.029442 -0.050994 0.000000 10 | v 0.027836 -0.048213 0.000000 11 | vn 0.692885 0.707963 -0.136739 12 | vn 0.408978 0.907585 0.095006 13 | vn -0.212286 -0.468709 0.857465 14 | vn 0.297056 -0.618852 0.727172 15 | vn 0.708170 0.492350 -0.506050 16 | vn 0.745762 -0.289343 -0.600100 17 | vn 0.808951 0.000863 -0.587876 18 | vn 0.926527 0.350079 -0.137811 19 | f 1//1 2//2 4//4 20 | f 1//1 5//5 2//2 21 | f 1//1 6//6 2//2 22 | f 1//1 4//4 5//5 23 | f 1//1 5//5 6//6 24 | f 2//2 3//3 4//4 25 | f 2//2 7//7 3//3 26 | f 2//2 5//5 6//6 27 | f 2//2 6//6 7//7 28 | f 3//3 7//7 4//4 29 | f 3//3 8//8 4//4 30 | f 3//3 7//7 8//8 31 | f 4//4 8//8 5//5 32 | f 4//4 7//7 8//8 33 | f 5//5 8//8 6//6 34 | f 6//6 8//8 7//7 35 | -------------------------------------------------------------------------------- /meshes/objects/plate.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 | -------------------------------------------------------------------------------- /meshes/objects/red.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sholtodouglas/ur5pybullet/5d2a6f210e982b6cb3d78ff2be4cfd57496f1211/meshes/objects/red.png -------------------------------------------------------------------------------- /meshes/ur5/collision/base.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sholtodouglas/ur5pybullet/5d2a6f210e982b6cb3d78ff2be4cfd57496f1211/meshes/ur5/collision/base.stl -------------------------------------------------------------------------------- /meshes/ur5/collision/forearm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sholtodouglas/ur5pybullet/5d2a6f210e982b6cb3d78ff2be4cfd57496f1211/meshes/ur5/collision/forearm.stl -------------------------------------------------------------------------------- /meshes/ur5/collision/shoulder.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sholtodouglas/ur5pybullet/5d2a6f210e982b6cb3d78ff2be4cfd57496f1211/meshes/ur5/collision/shoulder.stl -------------------------------------------------------------------------------- /meshes/ur5/collision/upperarm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sholtodouglas/ur5pybullet/5d2a6f210e982b6cb3d78ff2be4cfd57496f1211/meshes/ur5/collision/upperarm.stl -------------------------------------------------------------------------------- /meshes/ur5/collision/wrist1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sholtodouglas/ur5pybullet/5d2a6f210e982b6cb3d78ff2be4cfd57496f1211/meshes/ur5/collision/wrist1.stl -------------------------------------------------------------------------------- /meshes/ur5/collision/wrist2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sholtodouglas/ur5pybullet/5d2a6f210e982b6cb3d78ff2be4cfd57496f1211/meshes/ur5/collision/wrist2.stl -------------------------------------------------------------------------------- /meshes/ur5/collision/wrist3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sholtodouglas/ur5pybullet/5d2a6f210e982b6cb3d78ff2be4cfd57496f1211/meshes/ur5/collision/wrist3.stl -------------------------------------------------------------------------------- /meshes/ur5/visual/l_gripper_tip_scaled.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sholtodouglas/ur5pybullet/5d2a6f210e982b6cb3d78ff2be4cfd57496f1211/meshes/ur5/visual/l_gripper_tip_scaled.stl -------------------------------------------------------------------------------- /model.py: -------------------------------------------------------------------------------- 1 | #@title Trainer Class 2 | # Think about casting to float explicitly 3 | 4 | from __future__ import division 5 | from __future__ import print_function 6 | 7 | import numpy as np 8 | 9 | import tensorflow as tf 10 | from tensorflow.python import tf2 11 | if not tf2.enabled(): 12 | import tensorflow.compat.v2 as tf 13 | tf.enable_v2_behavior() 14 | assert tf2.enabled() 15 | 16 | # print(tf2.enabled()) 17 | 18 | import tensorflow_probability as tfp 19 | 20 | import tensorflow_datasets as tfds 21 | 22 | 23 | tfk = tf.keras 24 | tfkl = tf.keras.layers 25 | tfpl = tfp.layers 26 | tfd = tfp.distributions 27 | tfb = tfp.bijectors 28 | 29 | from tensorflow.compat.v1 import ConfigProto 30 | from tensorflow.compat.v1 import InteractiveSession 31 | 32 | 33 | from tensorflow.keras.layers import Dense, Flatten, Conv2D,Bidirectional, LSTM, Dropout 34 | from tensorflow.compat.v1.keras.layers import CuDNNLSTM 35 | from tensorflow.keras import Model 36 | from tensorflow.keras.models import Sequential 37 | import matplotlib.pyplot as plt 38 | import datetime 39 | import os 40 | from tqdm import tqdm, tqdm_notebook 41 | import random 42 | from natsort import natsorted, ns 43 | import imageio 44 | from IPython import display 45 | from PIL import Image 46 | 47 | import IPython 48 | import time 49 | import seaborn as sns 50 | import matplotlib.patheffects as PathEffects 51 | import traceback 52 | 53 | old = False 54 | 55 | 56 | 57 | #@title Trainer Class 58 | # Think about casting to float explicitly 59 | class Model_Trainer(): 60 | 61 | ### CONSTRUCTOR ### 62 | def __init__(self, dataloader, EPOCHS, BETA, ALPHA, MIN_SEQ_LEN, MAX_SEQ_LEN, 63 | MAX_EVER_SEQ_LEN, LATENT_DIM, LAYER_SIZE, ACTION_DIM, OBS_DIM , 64 | NUM_DISTRIBUTIONS, NUM_QUANTISATIONS, DISCRETIZED, RELATIVE, 65 | P_DROPOUT, BETA_ANNEAL, THRESHOLD, TEST, ARM_IN_GOAL): 66 | # Defaults 67 | # self.params = {'EPOCHS':30, 'BETA':0.0, 'ALPHA':0.0, 68 | # 'MIN_SEQ_LEN':10, 'MAX_SEQ_LEN':10, 'MAX_EVER_SEQ_LEN':0, 69 | # 'LATENT_DIM':60, 'LAYER_SIZE':1024, 70 | # 'ACTION_DIM':8, 'OBS_DIM':36, 71 | # 'NUM_DISTRIBUTIONS':3, 'NUM_QUANTISATIONS':256, 72 | # 'DISCRETIZED' : False, 'RELATIVE' : True, 73 | # 'P_DROPOUT' : 0.1, 'BETA_ANNEAL':1.0, 'THRESHOLD' : -30.0} 74 | # self.params.update(kwargs) 75 | # print(self.params) 76 | self.EPOCHS = EPOCHS 77 | self.BETA = BETA 78 | self.ALPHA = ALPHA 79 | self.MIN_SEQ_LEN = MIN_SEQ_LEN 80 | self.MAX_SEQ_LEN = MAX_SEQ_LEN 81 | self.MAX_EVER_SEQ_LEN = MAX_EVER_SEQ_LEN 82 | self.LATENT_DIM = LATENT_DIM 83 | self.LAYER_SIZE = LAYER_SIZE 84 | self.ACTION_DIM = ACTION_DIM 85 | self.ARM_IN_GOAL = ARM_IN_GOAL 86 | self.NUM_DISTRIBUTIONS = NUM_DISTRIBUTIONS 87 | self.NUM_QUANTISATIONS = NUM_QUANTISATIONS 88 | self.DISCRETIZED = DISCRETIZED 89 | self.RELATIVE = RELATIVE 90 | self.P_DROPOUT = P_DROPOUT 91 | self.BETA_ANNEAL = BETA_ANNEAL 92 | self.THRESHOLD = THRESHOLD 93 | self.BATCH_SIZE = 117 94 | self.ARM_IN_GOAL = ARM_IN_GOAL 95 | self.OBS_DIM = 14 96 | 97 | # self.dataset, self.valid_dataset, self.viz_dataset, self.viz_valid_dataset = dataloader.load_data() 98 | 99 | 100 | self.optimizer = tf.keras.optimizers.Adam(learning_rate=1e-4) 101 | 102 | self.actor = ACTOR(self.LAYER_SIZE, self.ACTION_DIM, self.OBS_DIM, self.NUM_DISTRIBUTIONS, self.NUM_QUANTISATIONS, self.DISCRETIZED, self.P_DROPOUT) 103 | self.encoder = TRAJECTORY_ENCODER_LSTM(self.LAYER_SIZE, self.LATENT_DIM, self.P_DROPOUT) 104 | 105 | 106 | if TEST: 107 | pass 108 | else: 109 | self.TRAIN_LEN = dataloader.TRAIN_LEN 110 | self.VALID_LEN = dataloader.VALID_LEN 111 | self.train_summary_writer, self.test_summary_writer = self.tensorboard_logger() 112 | self.dataloader = dataloader 113 | 114 | ############################################ Keep up to here when replacing. ############################################# 115 | 116 | 117 | def training_loop(self, i): 118 | 119 | # BETA = tf.constant(BETA) # dunno if needed 120 | 121 | IMI_val, KL_val, ent_val = np.Inf, np.Inf, np.Inf 122 | 123 | # this is shit make the OO formulation better so data is internal to dataloader class 124 | dataset = tf.data.Dataset.from_generator(self.dataloader.generator, (tf.float32, tf.float32, tf.float32, tf.int32), args = (self.dataloader.moments,'Train')) 125 | valid_dataset = tf.data.Dataset.from_generator(self.dataloader.generator, (tf.float32, tf.float32, tf.float32, tf.int32), args = (self.dataloader.moments,'Valid')) 126 | 127 | 128 | 129 | best_val_loss = 0 130 | 131 | for epoch in tqdm_notebook(range(self.EPOCHS), 'Epoch '): 132 | 133 | self.BETA *= self.BETA_ANNEAL 134 | 135 | 136 | train_set = dataset.shuffle(dataloader.TRAIN_LEN).batch(self.BATCH_SIZE) 137 | valid_set = iter(valid_dataset.shuffle(dataloader.VALID_LEN).batch(self.BATCH_SIZE)) 138 | for obs,acts, mask, lengths in train_set: 139 | IMI, KL, gripper_loss = self.train_step(obs,acts,self.BETA, mask, lengths) 140 | 141 | 142 | print("\r",f"Epoch {epoch}\t| TRAIN | IMI: {float(IMI):.2f}, KL: {float(KL):.2f}\ 143 | TEST | IMI: {float(IMI_val):.2f}, KL: {float(KL_val):.2f}, , entropy: {float(ent_val):.2f}",end="") 144 | 145 | with self.train_summary_writer.as_default(): 146 | tf.summary.scalar('imi_loss', IMI, step=self.steps) 147 | tf.summary.scalar('kl_loss', KL, step=self.steps) 148 | tf.summary.scalar('gripper_loss',gripper_loss, step=self.steps) 149 | 150 | if self.steps % 50 == 0: 151 | valid_obs, valid_acts, valid_mask, valid_lengths = valid_set.next() 152 | IMI_val, KL_val, gripper_loss_val = self.test_step(valid_obs, valid_acts, valid_mask, valid_lengths) 153 | 154 | with self.test_summary_writer.as_default(): 155 | tf.summary.scalar('imi_loss', IMI_val, step=self.steps) 156 | tf.summary.scalar('kl_loss', KL_val, step=self.steps) 157 | tf.summary.scalar('gripper_loss',gripper_loss_val, step=self.steps) 158 | 159 | 160 | 161 | self.steps+=1 162 | 163 | if IMI < best_val_loss: 164 | best_val_loss = IMI 165 | print('Saving Weights, best loss encountered') 166 | # This should be some test uid 167 | extension = 'drive/My Drive/Yeetbot_v3' 168 | self.save_weights(extension) 169 | # worksheet.update_acell('S'+str(start_row+i), str(epoch)+', '+str(float(best_val_loss))) 170 | # Write results to google sheets or similar 171 | return float(best_val_loss) 172 | 173 | 174 | # add uid 175 | def tensorboard_logger(self): 176 | current_time = datetime.datetime.now().strftime("%Y%m%d-%H%M%S")+'-Beta:'+str(self.BETA)+'-Latent:'+str(self.LATENT_DIM)+'Drp'+str(self.P_DROPOUT)+'autotest'+'_'+str(self.MAX_SEQ_LEN) 177 | train_log_dir = 'logs/gradient_tape/' + current_time + '/train' 178 | test_log_dir = 'logs/gradient_tape/' + current_time + '/test' 179 | train_summary_writer = tf.summary.create_file_writer(train_log_dir) 180 | test_summary_writer = tf.summary.create_file_writer(test_log_dir) 181 | return train_summary_writer, test_summary_writer 182 | 183 | def load_weights(self, extension): 184 | print('Loading in network weights...') 185 | # load some sample data to initialise the model 186 | # load_set = iter(self.dataset.shuffle(self.TRAIN_LEN).batch(self.BATCH_SIZE)) 187 | obs = tf.zeros((self.BATCH_SIZE,self.MAX_SEQ_LEN,self.OBS_DIM)) 188 | acts = tf.zeros((self.BATCH_SIZE,self.MAX_SEQ_LEN,self.ACTION_DIM)) 189 | mask = acts 190 | lengths = tf.cast(tf.ones(self.BATCH_SIZE), tf.int32) 191 | 192 | _, _, _= self.test_step(obs, acts, mask, lengths) 193 | 194 | 195 | # self.planner.load_weights(extension+'/planner.h5') 196 | self.encoder.load_weights(extension+'/encoder.h5') 197 | self.actor.load_weights(extension+'/actor.h5') 198 | print('Loaded.') 199 | # return actor, planner, encoder 200 | 201 | 202 | def save_weights(self, extension): 203 | try: 204 | os.mkdir(extension) 205 | except Exception as e: 206 | #print(e) 207 | pass 208 | 209 | #print(extension) 210 | self.actor.save_weights(extension+'/actor.h5') 211 | # self.planner.save_weights(extension+'/planner.h5') 212 | self.encoder.save_weights(extension+'/encoder.h5') 213 | 214 | # INFOVAE MMD loss 215 | def compute_kernel(self, x, y): 216 | x_size = tf.shape(x)[0] 217 | y_size = tf.shape(y)[0] 218 | dim = tf.shape(x)[1] 219 | tiled_x = tf.tile(tf.reshape(x, tf.stack([x_size, 1, dim])), tf.stack([1, y_size, 1])) 220 | tiled_y = tf.tile(tf.reshape(y, tf.stack([1, y_size, dim])), tf.stack([x_size, 1, 1])) 221 | return tf.exp(-tf.reduce_mean(tf.square(tiled_x - tiled_y), axis=2) / tf.cast(dim, tf.float32)) 222 | 223 | def compute_mmd(self, x, y): 224 | x_kernel = self.compute_kernel(x, x) 225 | y_kernel = self.compute_kernel(y, y) 226 | xy_kernel = self.compute_kernel(x, y) 227 | return tf.reduce_mean(x_kernel) + tf.reduce_mean(y_kernel) - 2 * tf.reduce_mean(xy_kernel) 228 | 229 | 230 | @tf.function 231 | def compute_loss(self, normal_enc, z, obs, acts, s_g,BETA, mu_enc, s_enc, mask, lengths, training=False): 232 | AVG_SEQ_LEN = obs.shape[1] 233 | CURR_BATCH_SIZE = obs.shape[0] 234 | # Automatically averaged over batch size i.e. SUM_OVER_BATCH_SIZE 235 | 236 | true_samples = tf.random.normal(tf.stack([CURR_BATCH_SIZE, self.LATENT_DIM])) 237 | # MMD 238 | # Need to convert normal_enc from tfp.Normal_distrib to tensor of sampled values 239 | #std_normal= tfd.Normal(0,2) 240 | #batch_avg_mean = tf.reduce_mean(mu_plan, axis = 0) #m_enc will batch_size, latent_dim. We want average mean across the batches so we end up with a latent dim size avg_mean_vector. Each dimension of the latent dim should be mean 0 avg across the batch, but individually can be different. 241 | #batch_avg_s = tf.reduce_mean(s_plan,axis=0) 242 | #batch_avg_normal = tfd.Normal(batch_avg_mean, batch_avg_s) 243 | #info_kl = tf.reduce_sum(tfd.kl_divergence(batch_avg_normal, std_normal)) 244 | info_kl = self.compute_mmd(true_samples, normal_enc.sample() ) 245 | KL = info_kl 246 | 247 | #KL = tf.reduce_sum(tfd.kl_divergence(normal_enc, normal_plan))/CURR_BATCH_SIZE #+ tf.reduce_sum(tfd.kl_divergence(normal_plan, normal_enc)) #KL divergence between encoder and planner distirbs 248 | #KL_reverse = tf.reduce_sum(tfd.kl_divergence(normal_plan, normal_enc))/CURR_BATCH_SIZE 249 | IMI = 0 250 | OBS_pred_loss = 0 251 | 252 | 253 | #s_g_dim= s_g.shape[-1] 254 | #s_g = tf.tile(s_g, [1,self.MAX_SEQ_LEN]) 255 | #s_g = tf.reshape(s_g, [-1, self.MAX_SEQ_LEN,s_g_dim ]) #subtract arm rel states 256 | z = tf.tile(z, [1, self.MAX_SEQ_LEN]) 257 | z = tf.reshape(z, [-1, self.MAX_SEQ_LEN, self.LATENT_DIM]) # so that both end up as BATCH, SEQ, DIM 258 | 259 | mu, scale, prob_weight, pdf, obs_pred, gripper= self.actor(obs,z,s_g, training = training) 260 | 261 | 262 | 263 | log_prob_actions = -pdf.log_prob(acts[:,:,:self.ACTION_DIM-1]) # batchsize, Maxseqlen, actions, 264 | masked_log_probs = log_prob_actions*mask[:,:,:self.ACTION_DIM-1] # should zero out all masked elements. 265 | avg_batch_wise_sum = tf.reduce_sum(masked_log_probs, axis = (1,2)) / lengths 266 | IMI = tf.reduce_mean(avg_batch_wise_sum) 267 | 268 | 269 | #sampled_acts = pdf.sample() 270 | #MSE_action_loss = tf.losses.MSE(tf.squeeze(acts[:,:,:self.ACTION_DIM-1])*tf.squeeze(mask[:,:,:self.ACTION_DIM-1]),tf.squeeze(sampled_acts)*tf.squeeze(mask[:,:,:self.ACTION_DIM-1])) 271 | #IMI = tf.reduce_mean(MSE_action_loss) 272 | 273 | gripper_loss = tf.losses.MAE(tf.squeeze(acts[:,:,self.ACTION_DIM-1])*tf.squeeze(mask[:,:,0]),tf.squeeze(gripper)*tf.squeeze(mask[:,:,0])) 274 | gripper_loss = tf.reduce_mean(gripper_loss) # lets go quartic and see if its more responsive 275 | 276 | 277 | loss = IMI + 50*gripper_loss + self.BETA*KL #+ (self.BETA/10)* info_kl#+ self.BETA*KL_reverse#ALPHA*entropy# + OBS_pred_loss*self.ALPHA 278 | 279 | #print(loss) 280 | #print(gripper_loss) 281 | return loss, IMI, KL, gripper_loss 282 | 283 | 284 | 285 | @tf.function 286 | def train_step(self, obs, acts, BETA, mask, lengths): 287 | with tf.GradientTape() as tape: 288 | 289 | # obs and acts are a trajectory, so get intial and goal 290 | s_i = obs[:,0,:] 291 | range_lens = tf.expand_dims(tf.range(tf.shape(lengths)[0]), 1) 292 | expanded_lengths = tf.expand_dims(lengths-1,1)# lengths must be subtracted by 1 to become indices. 293 | s_g = tf.gather_nd(obs, tf.concat((range_lens, expanded_lengths),1)) # get the actual last element of the sequencs. 294 | if self.ARM_IN_GOAL: 295 | s_g = s_g[:,:-6] #don't inc rel states 296 | else: 297 | s_g = s_g[:,8:-6] #don't inc rel states or arm states in goal 298 | 299 | # Encode the trajectory 300 | mu_enc, s_enc = self.encoder(obs, acts, training = True) 301 | encoder_normal = tfd.Normal(mu_enc,s_enc) 302 | z = encoder_normal.sample() 303 | 304 | #Produce a plan from the inital and goal state 305 | #mu_plan, s_plan = self.planner(s_i,s_g, training = True) 306 | #planner_normal = tfd.Normal(mu_plan,s_plan) 307 | #zp = planner_normal.sample() 308 | 309 | lengths = tf.cast(lengths, tf.float32) 310 | loss, IMI, KL, gripper_loss = self.compute_loss(encoder_normal, z, obs, acts, s_g, self.BETA, mu_enc, s_enc, mask, lengths, training = True) 311 | #find and apply gradients with total loss 312 | gradients = tape.gradient(loss,self.encoder.trainable_variables+self.actor.trainable_variables) 313 | self.optimizer.apply_gradients(zip(gradients,self.encoder.trainable_variables+self.actor.trainable_variables)) 314 | 315 | # return values for diagnostics 316 | return IMI, KL, gripper_loss 317 | 318 | 319 | @tf.function 320 | def test_step(self, obs, acts, mask, lengths): 321 | s_i = obs[:,0,:] 322 | range_lens = tf.expand_dims(tf.range(tf.shape(lengths)[0]), 1) 323 | expanded_lengths = tf.expand_dims(lengths-1,1)# lengths must be subtracted by 1 to become indices. 324 | s_g = tf.gather_nd(obs, tf.concat((range_lens, expanded_lengths),1)) # get the actual last element of the sequencs. 325 | if self.ARM_IN_GOAL: 326 | s_g = s_g[:,:-6] #don't inc rel states 327 | else: 328 | s_g = s_g[:,8:-6] #don't inc rel states or arm states in goal 329 | # Encode Trajectory 330 | mu_enc, s_enc = self.encoder(obs, acts) 331 | encoder_normal = tfd.Normal(mu_enc,s_enc) 332 | z = encoder_normal.sample() 333 | 334 | # PLan with si,sg. 335 | #mu_plan, s_plan = self.planner(s_i,s_g) 336 | #planner_normal = tfd.Normal(mu_plan,s_plan) 337 | #zp = planner_normal.sample() 338 | 339 | lengths = tf.cast(lengths, tf.float32) 340 | _, IMI, KL, gripper_loss = self.compute_loss(encoder_normal, z, obs, acts, s_g, self.BETA, mu_enc, s_enc, mask, lengths) 341 | 342 | return IMI, KL, gripper_loss 343 | 344 | 345 | 346 | class TRAJECTORY_ENCODER_LSTM(Model): 347 | def __init__(self, LAYER_SIZE, LATENT_DIM, P_DROPOUT): 348 | super(TRAJECTORY_ENCODER_LSTM, self).__init__() 349 | 350 | self.bi_lstm = Bidirectional(CuDNNLSTM(LAYER_SIZE, return_sequences=True), merge_mode=None) 351 | self.mu = Dense(LATENT_DIM) 352 | self.scale = Dense(LATENT_DIM, activation='softplus') 353 | self.dropout1 = tf.keras.layers.Dropout(P_DROPOUT) 354 | 355 | def call(self, obs, acts, training = False): 356 | x = tf.concat([obs,acts], axis = 2) # concat observations and actions together. 357 | x = self.bi_lstm(x) 358 | x = self.dropout1(x, training=training) 359 | bottom = x[0][:,-1, :] # Take the last element of the bottom row 360 | top = x[1][:,0,:] # Take the first elemetn of the top row cause Bidirectional, top row goes backward. 361 | x = tf.concat([bottom, top], axis = 1) 362 | mu = self.mu(x) 363 | s = self.scale(x) 364 | 365 | return mu, s 366 | 367 | 368 | 369 | class ACTOR(Model): 370 | def __init__(self, LAYER_SIZE, ACTION_DIM, OBS_DIM, NUM_DISTRIBUTIONS, NUM_QUANTISATIONS, DISCRETIZED, P_DROPOUT): 371 | super(ACTOR, self).__init__() 372 | self.ACTION_DIM = ACTION_DIM-1 373 | self.OBS_DIM = OBS_DIM 374 | self.NUM_DISTRIBUTIONS = NUM_DISTRIBUTIONS 375 | self.DISCRETIZED = DISCRETIZED 376 | self.NUM_QUANTISATIONS = NUM_QUANTISATIONS 377 | 378 | self.RNN1 = CuDNNLSTM(LAYER_SIZE, return_sequences=True, return_state = True) 379 | self.RNN2 = CuDNNLSTM(LAYER_SIZE, return_sequences=True, return_state = True) 380 | self.mu = Dense(self.ACTION_DIM*self.NUM_DISTRIBUTIONS) #means of our logistic distributions 381 | # softplus activations are to ensure positive values for scale and probability weighting. 382 | self.scale = Dense(self.ACTION_DIM*self.NUM_DISTRIBUTIONS,activation='softplus') # scales of our logistic distrib 383 | self.prob_weight = Dense(self.ACTION_DIM*self.NUM_DISTRIBUTIONS,activation='softplus') # weightings on each of the distribs. 384 | # self.next_obs_pred = Dense(self.OBS_DIM) 385 | self.gripper = Dense(1) 386 | self.dropout1 = tf.keras.layers.Dropout(P_DROPOUT) 387 | 388 | 389 | def call(self, s, z, s_g, training = False, past_state = None): 390 | B = z.shape[0] #dynamically get batch size 391 | s_g=tf.zeros((z.shape)) 392 | state_out = None 393 | if len(s.shape) == 3: 394 | x = tf.concat([s, z, s_g], axis = 2) # (BATCHSIZE) 395 | [x, _, _] = self.RNN1(x) 396 | [x, _, _] = self.RNN2(x) 397 | x = self.dropout1(x, training=training) 398 | 399 | else: 400 | x = tf.concat([s, z, s_g], axis = 1) # (BATCHSIZE, OBS+OBS+LATENT) 401 | x= tf.expand_dims(x, 1) # make it (BATCHSIZE, 1, OBS+OBS+LATENT) so LSTM is happy. 402 | [x, s1l1, s2l1] = self.RNN1(x, initial_state = past_state[0]) 403 | [x, s1l2, s2l2] = self.RNN2(x, initial_state = past_state[1]) 404 | state_out = [[s1l1, s2l1], [s1l2, s2l2]] 405 | x = self.dropout1(x, training=training) 406 | 407 | 408 | mu = tf.reshape(self.mu(x), [B,-1,self.ACTION_DIM, self.NUM_DISTRIBUTIONS]) 409 | #mu = tf.concat([mu[:,:,:7,:],tf.keras.activations.sigmoid(tf.expand_dims(mu[:,:,7,:], 2))], axis = 2) 410 | scale = tf.reshape(self.scale(x), [B,-1,self.ACTION_DIM, self.NUM_DISTRIBUTIONS]) 411 | prob_weight = tf.reshape(self.prob_weight(x), [B,-1,self.ACTION_DIM, self.NUM_DISTRIBUTIONS]) 412 | 413 | if self.DISCRETIZED: 414 | # multiply mean by 64 so that for a neuron between -2 and 2, it covers the full range 415 | # between -128 and 128, i.e so the weights don't have to be huge! in the mean layer, but tiny in the s and prob layers. 416 | mu = mu*self.NUM_QUANTISATIONS/4 417 | discretized_logistic_dist = tfd.QuantizedDistribution(distribution=tfd.TransformedDistribution( 418 | distribution=tfd.Logistic(loc=mu,scale=scale),bijector=tfb.AffineScalar(shift=-0.5)), 419 | low=-self.NUM_QUANTISATIONS/2, 420 | high=self.NUM_QUANTISATIONS/2) 421 | 422 | # should be batch size by action dim 423 | distributions = tfd.MixtureSameFamily(mixture_distribution=tfd.Categorical( 424 | probs=prob_weight),components_distribution=discretized_logistic_dist) 425 | else: 426 | logistic_dist = tfd.Logistic(loc=mu,scale=scale) 427 | 428 | distributions = tfd.MixtureSameFamily(mixture_distribution=tfd.Categorical( 429 | probs=prob_weight),components_distribution=logistic_dist) 430 | 431 | 432 | 433 | obs_pred = None#self.next_obs_pred(x) 434 | grip = tf.keras.activations.sigmoid(self.gripper(x)) 435 | 436 | if state_out == None: 437 | return mu,scale, prob_weight, distributions, obs_pred, grip 438 | else: 439 | return mu,scale, prob_weight, distributions, obs_pred, state_out, grip -------------------------------------------------------------------------------- /octave-workspace: -------------------------------------------------------------------------------- 1 | Octave-1-L -------------------------------------------------------------------------------- /pyRobotiqGripper.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Created on Wed Oct 17 17:33:20 2018 5 | Driver to control robotiq gripper via python 6 | @author: Benoit CASTETS 7 | Dependencies: 8 | ************* 9 | MinimalModbus: https://pypi.org/project/MinimalModbus/ 10 | """ 11 | #Libraries importation 12 | import minimalmodbus as mm 13 | import time 14 | import binascii 15 | 16 | #Communication setup 17 | mm.BAUDRATE=115200 18 | mm.BYTESIZE=8 19 | mm.PARITY="N" 20 | mm.STOPBITS=1 21 | mm.TIMEOUT=0.2 22 | 23 | 24 | __author__ = "Benoit CASTETS" 25 | __email__ = "b.castets@robotiq.com" 26 | #__license__ = "Apache License, Version 2.0" 27 | 28 | class RobotiqGripper( mm.Instrument ): 29 | """"Instrument class for Robotiq grippers (2F85, 2F140, hande,...). 30 | 31 | Communicates via Modbus RTU protocol (via RS232 or RS485), using the *MinimalModbus* Python module. 32 | Args: 33 | * portname (str): port name 34 | * slaveaddress (int): slave address in the range 1 to 247 35 | Implemented with these function codes (in decimal): 36 | 37 | ================== ==================== 38 | Description Modbus function code 39 | ================== ==================== 40 | Read registers 3 41 | Write registers 16 42 | ================== ==================== 43 | 44 | For more information for gripper communication please check gripper manual 45 | on Robotiq website. 46 | https://robotiq.com/support/2f-85-2f-140 47 | """ 48 | 49 | def __init__(self, portname, slaveaddress=9): 50 | """Create a RobotiqGripper object use to control Robotiq grippers 51 | using modbus RTU protocol USB/RS485 connection. 52 | 53 | Parameters 54 | ---------- 55 | portname: 56 | Name of the port (string) where is connected the gripper. Usually 57 | /dev/ttyUSB0 on Linux. It is necesary to allow permission to access 58 | this connection using the bash command sudo chmod 666 /dev/ttyUSB0 59 | slaveaddress: 60 | Address of the gripper (integer) usually 9. 61 | """ 62 | mm.Instrument.__init__(self, portname, slaveaddress=9) 63 | self.debug=True 64 | self.mode=mm.MODE_RTU 65 | 66 | self.processing=False 67 | 68 | self.timeOut=10 69 | 70 | self.registerDic={} 71 | self._buildRegisterDic() 72 | 73 | self.paramDic={} 74 | self.readAll() 75 | 76 | self.closemm=None 77 | self.closebit=None 78 | 79 | self.openmm=None 80 | self.openbit=None 81 | 82 | self._aCoef=None 83 | self._bCoef=None 84 | 85 | def _buildRegisterDic(self): 86 | """Build a dictionnary with comment to explain each register variable. 87 | The dictionnary is organize in 2 levels: 88 | Dictionnary key are variable names. Dictionnary value are dictionnary 89 | with comments about each statut of the variable 90 | (key=variable value, value=comment) 91 | """ 92 | ###################################################################### 93 | #input register variable 94 | self.registerDic.update({"gOBJ":{},"gSTA":{},"gGTO":{},"gACT":{}, 95 | "kFLT":{},"gFLT":{},"gPR":{},"gPO":{},"gCU":{}}) 96 | print(self.registerDic) 97 | #gOBJ 98 | gOBJdic=self.registerDic["gOBJ"] 99 | 100 | gOBJdic[0]="Fingers are in motion towards requested position. No object detected." 101 | gOBJdic[1]="Fingers have stopped due to a contact while opening before requested position. Object detected opening." 102 | gOBJdic[2]="Fingers have stopped due to a contact while closing before requested position. Object detected closing." 103 | gOBJdic[3]="Fingers are at requested position. No object detected or object has been loss / dropped." 104 | 105 | #gSTA 106 | gSTAdic=self.registerDic["gSTA"] 107 | 108 | gSTAdic[0]="Gripper is in reset ( or automatic release ) state. See Fault Status if Gripper is activated." 109 | gSTAdic[1]="Activation in progress." 110 | gSTAdic[3]="Activation is completed." 111 | 112 | #gGTO 113 | gGTOdic=self.registerDic["gGTO"] 114 | 115 | gGTOdic[0]="Stopped (or performing activation / automatic release)." 116 | gGTOdic[1]="Go to Position Request." 117 | #gGTOdic[2]="Stopped (or performing activation / automatic release)." 118 | #gGTOdic[3]="Go to Position Request." 119 | 120 | #gACT 121 | gACTdic=self.registerDic["gACT"] 122 | 123 | gACTdic[0]="Gripper reset." 124 | gACTdic[1]="Gripper activation." 125 | 126 | #kFLT 127 | kFLTdic=self.registerDic["kFLT"] 128 | i=0 129 | while i<256: 130 | kFLTdic[i]=i 131 | i+=1 132 | 133 | #See your optional Controller Manual (input registers & status). 134 | 135 | #gFLT 136 | gFLTdic=self.registerDic["gFLT"] 137 | i=0 138 | while i<256: 139 | gFLTdic[i]=i 140 | i+=1 141 | gFLTdic[0]="No fault (LED is blue)" 142 | gFLTdic[5]="Priority faults (LED is blue). Action delayed, activation (reactivation) must be completed prior to perfmoring the action." 143 | gFLTdic[7]="Priority faults (LED is blue). The activation bit must be set prior to action." 144 | gFLTdic[8]="Minor faults (LED continuous red). Maximum operating temperature exceeded, wait for cool-down." 145 | gFLTdic[9]="Minor faults (LED continuous red). No communication during at least 1 second." 146 | gFLTdic[10]="Major faults (LED blinking red/blue) - Reset is required (rising edge on activation bit rACT needed). Under minimum operating voltage." 147 | gFLTdic[11]="Major faults (LED blinking red/blue) - Reset is required (rising edge on activation bit rACT needed). Automatic release in progress." 148 | gFLTdic[12]="Major faults (LED blinking red/blue) - Reset is required (rising edge on activation bit rACT needed). Internal fault; contact support@robotiq.com." 149 | gFLTdic[13]="Major faults (LED blinking red/blue) - Reset is required (rising edge on activation bit rACT needed). Activation fault, verify that no interference or other error occurred." 150 | gFLTdic[14]="Major faults (LED blinking red/blue) - Reset is required (rising edge on activation bit rACT needed). Overcurrent triggered." 151 | gFLTdic[15]="Major faults (LED blinking red/blue) - Reset is required (rising edge on activation bit rACT needed). Automatic release completed." 152 | 153 | #gPR 154 | gPRdic=self.registerDic["gPR"] 155 | 156 | i=0 157 | while i<256: 158 | gPRdic[i]="Echo of the requested position for the Gripper: {}/255".format(i) 159 | i+=1 160 | 161 | #gPO 162 | gPOdic=self.registerDic["gPO"] 163 | i=0 164 | while i<256: 165 | gPOdic[i]="Actual position of the Gripper obtained via the encoders: {}/255".format(i) 166 | i+=1 167 | 168 | #gCU 169 | gCUdic=self.registerDic["gCU"] 170 | i=0 171 | while i<256: 172 | current=i*10 173 | gCUdic[i]="The current is read instantaneously from the motor drive, approximate current: {} mA".format(current) 174 | i+=1 175 | 176 | 177 | ###################################################################### 178 | #output register varaible 179 | self.registerDic.update({"rARD":{},"rATR":{},"rGTO":{},"rACT":{},"rPR":{}, 180 | "rFR":{},"rSP":{}}) 181 | 182 | ###################################################################### 183 | 184 | def _extractKBits(integer,position,nbrBits): 185 | """Function to extract ‘k’ bits from a given 186 | position in a number. 187 | 188 | Parameters 189 | ---------- 190 | integer: 191 | Integer to by process as a binary number 192 | position: 193 | Position of the first bit to be extracted 194 | nbrBits: 195 | Number of bits to be extracted form the first bit position. 196 | 197 | Return 198 | ------ 199 | extractedInt: 200 | Integer representation of extracted bits. 201 | """ 202 | # convert number into binary first 203 | binary=bin(integer) 204 | 205 | # remove first two characters 206 | binary = binary[2:] 207 | 208 | end = len(binary) - position 209 | start = end - nbrBits + 1 210 | 211 | # extract k bit sub-string 212 | extractedBits = binary[start : end+1] 213 | 214 | # convert extracted sub-string into decimal again 215 | extractedInt=int(extractedBits,2) 216 | 217 | return extractedInt 218 | 219 | def readAll(self): 220 | """Return a dictionnary will all variable saved in the register 221 | """ 222 | self.paramDic={} 223 | 224 | registers=self.read_registers(2000,6) 225 | 226 | ######################################### 227 | #Register 2000 228 | #gripperStatus 229 | gripperStatusReg0=bin(registers[0])[2:] 230 | gripperStatusReg0="0"*(16-len(gripperStatusReg0))+gripperStatusReg0 231 | gripperStatusReg0=gripperStatusReg0[:8] 232 | ######################################### 233 | print(gripperStatusReg0) 234 | self.paramDic["gOBJ"]=gripperStatusReg0[0:2] 235 | #Object detection 236 | self.paramDic["gSTA"]=gripperStatusReg0[2:4] 237 | #Gripper status 238 | self.paramDic["gGTO"]=gripperStatusReg0[4:6] 239 | #Action status. echo of rGTO (go to bit) 240 | self.paramDic["gACT"]=gripperStatusReg0[7] 241 | #Activation status 242 | 243 | ######################################### 244 | #Register 2002 245 | #fault status 246 | faultStatusReg2=bin(registers[2])[2:] 247 | faultStatusReg2="0"*(16-len(faultStatusReg2))+faultStatusReg2 248 | faultStatusReg2=faultStatusReg2[:8] 249 | ######################################### 250 | self.paramDic["kFLT"]=faultStatusReg2[0:4] 251 | #Universal controler 252 | self.paramDic["gFLT"]=faultStatusReg2[4:] 253 | #Fault 254 | 255 | ######################################### 256 | #Register 2003 257 | #fault status 258 | posRequestEchoReg3=bin(registers[3])[2:] 259 | posRequestEchoReg3="0"*(8-len(posRequestEchoReg3))+posRequestEchoReg3 260 | posRequestEchoReg3=posRequestEchoReg3[:8] 261 | ######################################### 262 | self.paramDic["gPR"]=posRequestEchoReg3 263 | #Echo of request position 264 | 265 | ######################################### 266 | #Register 2004 267 | #position 268 | positionReg4=bin(registers[4])[2:] 269 | positionReg4="0"*(16-len(positionReg4))+positionReg4 270 | positionReg4=positionReg4[:8] 271 | ######################################### 272 | self.paramDic["gPO"]=positionReg4 273 | #Actual position of the gripper 274 | 275 | ######################################### 276 | #Register 2005 277 | #current 278 | currentReg5=bin(registers[5])[2:] 279 | currentReg5="0"*(16-len(currentReg5))+currentReg5 280 | currentReg5=currentReg5[:8] 281 | ######################################### 282 | self.paramDic["gCU"]=currentReg5 283 | #Current 284 | 285 | #Convert of variable from str to int 286 | for key,value in self.paramDic.items(): 287 | self.paramDic[key]=int(value,2) 288 | 289 | def reset(self): 290 | """Reset the gripper (clear previous activation if any) 291 | """ 292 | #Lexique: 293 | 294 | #byte=8bit 295 | #bit=1 OR 0 296 | 297 | #Memo: 298 | 299 | #write a value with dec,hex or binary number 300 | #binary 301 | #inst.write_register(1000,int("0000000100000000",2)) 302 | #hex 303 | #inst.write_register(1000,int("0x0100", 0)) 304 | #dec 305 | #inst.write_register(1000,256) 306 | 307 | #Register 1000: Action Request 308 | #A register have a size of 2Bytes 309 | #(7-6)reserved/(5)rARD/(4)rATR/(3)rGTO/(2-1)reserved/(0)rACT/+8 unused bits 310 | 311 | #Register 1001:RESERVED 312 | #register 1002:RESERVED 313 | 314 | #Reset the gripper 315 | self.write_registers(1000,[0,0,0]) 316 | #09 10 03 E8 00 03 06 00 00 00 00 00 00 73 30 317 | 318 | def activate(self): 319 | """If not already activated. Activate the gripper 320 | """ 321 | #Turn the variable which indicate that the gripper is processing 322 | #an action to True 323 | self.processing=True 324 | #Activate the gripper 325 | self.write_registers(1000,[256,0,0]) 326 | #09 10 03 E8 00 03 06 01 00 00 00 00 00 72 E1 327 | #Waiting for activation to complete 328 | timeIni=time.time() 329 | loop=True 330 | while loop: 331 | self.readAll() 332 | gSTA=self.paramDic["gSTA"] 333 | if ((time.time()-timeIni)255: 393 | print("maximum position is 255") 394 | else: 395 | #rARD(5) rATR(4) rGTO(3) rACT(0) 396 | self.write_registers(1000,[int("00001001"+"00000000",2), 397 | position, 398 | int(self._intToHex(speed)+self._intToHex(force),16)]) 399 | timer=time.time() 400 | loop=True 401 | while loop or (time.time()-timer)>self.timeOut: 402 | self.readAll() 403 | gOBJ=self.paramDic["gOBJ"] 404 | if gOBJ==1 or gOBJ==2: 405 | objectDetected=True 406 | loop=False 407 | elif gOBJ==3: 408 | objectDetected=False 409 | loop=False 410 | elif (time.time()-timer)>self.timeOut: 411 | loop=False 412 | print("Gripper never reach its requested position and no\ 413 | object have been detected") 414 | 415 | 416 | position=self.paramDic["gPO"] 417 | 418 | #TO DO: Check if gripper is in position. If no wait. 419 | 420 | def closeGripper(self,speed=255,force=255): 421 | """Close the gripper 422 | 423 | Parameters 424 | ---------- 425 | speed: 426 | Gripper speed between 0 and 255 427 | force: 428 | Gripper force between 0 and 255 429 | """ 430 | self.goTo(255,speed,force) 431 | 432 | def openGripper(self,speed=255,force=255): 433 | """Open the gripper 434 | 435 | Parameters 436 | ---------- 437 | speed: 438 | Gripper speed between 0 and 255 439 | force: 440 | Gripper force between 0 and 255 441 | """ 442 | self.goTo(0,force,speed) 443 | 444 | def goTomm(self,positionmm,speed=255,force=255): 445 | """Go to the requested opening expressed in mm 446 | 447 | Parameters 448 | ---------- 449 | positionmm: 450 | Gripper opening in mm. 451 | speed: 452 | Gripper speed between 0 and 255 453 | force: 454 | Gripper force between 0 and 255 455 | 456 | Return 457 | ------ 458 | Return 0 if succeed, 1 if failed. 459 | """ 460 | if self.openmm is None or self.closemm is None: 461 | print("You have to calibrate the gripper before using \ 462 | the function goTomm()") 463 | return 1 464 | elif positionmm>self.openmm: 465 | print("The maximum opening is {}".format(positionmm)) 466 | return 1 467 | else: 468 | position=int(self._mmToBit(positionmm)) 469 | self.goTo(position,speed,force) 470 | return 0 471 | 472 | def getPositionCurrent(self): 473 | """Return the position of the gripper in bit. 474 | 475 | Return 476 | ------ 477 | position: 478 | Gripper position in bit 479 | current: 480 | Motor current in bit. 1bit is about 10mA. 481 | """ 482 | 483 | registers=self.read_registers(2002,1) 484 | register=self._intToHex(registers[0]) 485 | position=int(register[:2],16) 486 | current=int(register[2:],16) 487 | return position,current 488 | 489 | def _mmToBit(self,mm): 490 | """Convert a mm gripper opening in bit opening. 491 | Calibration is needed to use this function. 492 | """ 493 | bit=(mm-self._bCoef)/self._aCoef 494 | 495 | return bit 496 | 497 | 498 | def _bitTomm(self,bit): 499 | """Convert a bit gripper opening in mm opening. 500 | Calibration is needed to use this function. 501 | """ 502 | mm=self._aCoef*bit+self._bCoef 503 | 504 | return mm 505 | 506 | def getPositionmm(self): 507 | """Return the position of the gripper in mm. 508 | Calibration is need to use this function. 509 | """ 510 | position=self.getPositionCurrent()[0] 511 | 512 | positionmm=self._bitTomm(position) 513 | return positionmm 514 | 515 | def calibrate(self,closemm,openmm): 516 | """Calibrate the gripper for mm positionning 517 | """ 518 | self.closemm=closemm 519 | self.openmm=openmm 520 | 521 | self.goTo(0) 522 | #get open bit 523 | self.openbit=self.getPositionCurrent()[0] 524 | obit=self.openbit 525 | 526 | self.goTo(255) 527 | #get close bit 528 | self.closebit=self.getPositionCurrent()[0] 529 | cbit=self.closebit 530 | 531 | self._aCoef=(closemm-openmm)/(cbit-obit) 532 | self._bCoef=(openmm*cbit-obit*closemm)/(cbit-obit) 533 | 534 | def printInfo(self): 535 | """Print gripper register info in the python treminal 536 | """ 537 | self.readAll() 538 | for key,value in self.paramDic.items(): 539 | print("{} : {}".format(key,value)) 540 | print(self.registerDic[key][value]) 541 | 542 | #Test 543 | -------------------------------------------------------------------------------- /rgb.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sholtodouglas/ur5pybullet/5d2a6f210e982b6cb3d78ff2be4cfd57496f1211/rgb.jpg -------------------------------------------------------------------------------- /rgb.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sholtodouglas/ur5pybullet/5d2a6f210e982b6cb3d78ff2be4cfd57496f1211/rgb.png -------------------------------------------------------------------------------- /scenes.py: -------------------------------------------------------------------------------- 1 | import pybullet as p 2 | from kuka import kuka 3 | from ur5 import ur5 4 | import pybullet_data 5 | import math 6 | import os 7 | urdfRoot=pybullet_data.getDataPath() 8 | meshPath = os.getcwd()+"/meshes/objects/" 9 | print(meshPath) 10 | up_rot = p.getQuaternionFromEuler([-math.pi/2, math.pi,0]) # the transform from Z-Y axis up. Most meshes are Z up. 11 | 12 | def load_arm_dim_up(arm, dim = 'Z'): 13 | if arm == 'ur5': 14 | _arm = ur5() 15 | elif arm == 'kuka': 16 | _arm = kuka(urdfRootPath=urdfRoot, timeStep=1/240, vr = True) 17 | elif arm == 'rbx1': 18 | _arm = rbx1(urdfRootPath=self._urdfRoot, timeStep=self._timeStep) 19 | else: 20 | raise Exception('Not a valid arm') 21 | if dim == 'Y': 22 | arm_rot = p.getQuaternionFromEuler([-math.pi/2, (1/2)*math.pi,0]) 23 | _arm.setPosition([0,-0.1,0.5], [arm_rot[0],arm_rot[1],arm_rot[2],arm_rot[3]]) 24 | else: 25 | arm_rot =p.getQuaternionFromEuler([0,0.0,0]) 26 | _arm.setPosition([-0.5,0.0,-0.1], [arm_rot[0],arm_rot[1],arm_rot[2],arm_rot[3]]) 27 | return _arm 28 | 29 | 30 | def load_play_Z_up(): 31 | 32 | lego0 = [p.loadURDF((os.path.join(urdfRoot, "lego/lego.urdf")), 0.700000,-0.200000,0.700000,0.000000,0.000000,0.000000,1.000000)] 33 | lego1 = [p.loadURDF((os.path.join(urdfRoot, "lego/lego.urdf")), 0.700000,-0.200000,0.800000,0.000000,0.000000,0.000000,1.000000)] 34 | lego2 = [p.loadURDF((os.path.join(urdfRoot, "lego/lego.urdf")), 0.700000,-0.200000,0.900000,0.000000,0.000000,0.000000,1.000000)] 35 | lego3 = [p.loadURDF((os.path.join(urdfRoot, "lego/lego.urdf")),0.800000,-0.200000,0.600000,0.000000,0.000000,0.000000,1.000000)] 36 | lego4 = [p.loadURDF((os.path.join(urdfRoot, "lego/lego.urdf")), 0.800000,-0.200000,0.500000,0.000000,0.000000,0.000000,1.000000)] 37 | lego5 = [p.loadURDF((os.path.join(urdfRoot, "lego/lego.urdf")), 0.800000,-0.200000,0.7500000,0.000000,0.000000,0.000000,1.000000)] 38 | lego6 = [p.loadURDF((os.path.join(urdfRoot, "lego/lego.urdf")), 0.800000,-0.30000,0.8500000,0.000000,0.000000,0.000000,1.000000)] 39 | objects = [p.loadURDF((os.path.join(urdfRoot, "lego/lego.urdf")), 0.800000,-0.300000,0.800000,0.000000,0.000000,0.000000,1.000000)] 40 | objects = [p.loadURDF((os.path.join(urdfRoot, "lego/lego.urdf")), 0.800000,-0.300000,0.900000,0.000000,0.000000,0.000000,1.000000)] 41 | objects = [p.loadURDF((os.path.join(urdfRoot,"teddy_vhacd.urdf")), -0.100000,0.600000,0.850000,0.000000,0.000000,0.000000,1.000000)] 42 | objects = [p.loadURDF((os.path.join(urdfRoot,"sphere_small.urdf")), -0.100000,0.955006,1.169706,0.633232,-0.000000,-0.000000,0.773962)] 43 | objects = [p.loadURDF((os.path.join(urdfRoot,"cube_small.urdf")), 0.300000,0.600000,0.850000,0.000000,0.000000,0.000000,1.000000)] 44 | jenga1 = [p.loadURDF((os.path.join(urdfRoot,"jenga/jenga.urdf")), 1.300000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] 45 | jenga2 = [p.loadURDF((os.path.join(urdfRoot,"jenga/jenga.urdf")), 1.200000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] 46 | jenga3 = [p.loadURDF((os.path.join(urdfRoot,"jenga/jenga.urdf")), 1.100000,-0.700000,0.7510000,0.000000,0.707107,0.000000,0.707107)] 47 | jenga4 = [p.loadURDF((os.path.join(urdfRoot,"jenga/jenga.urdf")), 0.800000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] 48 | jenga5 = [p.loadURDF((os.path.join(urdfRoot,"jenga/jenga.urdf")), 0.700000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] 49 | jenga6 = [p.loadURDF((os.path.join(urdfRoot,"jenga/jenga.urdf")), 0.500000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] 50 | objects = [p.loadURDF((os.path.join(urdfRoot,"table/table.urdf")), 0.4000000,-0.200000,0.000000,0.000000,0.000000,0.707107,0.707107)] 51 | objects = [p.loadURDF((os.path.join(urdfRoot, "plane.urdf")), 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)] 52 | jenga7 = [p.loadURDF((os.path.join(urdfRoot, "jenga/jenga.urdf")), 1.300000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] 53 | jenga8 = [p.loadURDF((os.path.join(urdfRoot, "jenga/jenga.urdf")), 1.200000,-0.700000,0.850000,0.000000,0.707107,0.000000,0.707107)] 54 | jenga9 = [p.loadURDF((os.path.join(urdfRoot, "jenga/jenga.urdf")), 1.100000,-0.700000,0.950000,0.000000,0.707107,0.000000,0.707107)] 55 | jenga10 = [p.loadURDF((os.path.join(urdfRoot, "jenga/jenga.urdf")), 1.000000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] 56 | objects = [p.loadURDF((os.path.join(urdfRoot, "jenga/jenga.urdf")), 0.900000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] 57 | 58 | 59 | def load_lab_Y_up(): 60 | jenga = [p.loadURDF((os.path.join(urdfRoot,"jenga/jenga.urdf")), 0,0.4,0,0.000000,0.707107,0.000000,0.707107)] 61 | table = [p.loadURDF((os.path.join(urdfRoot,"table/table.urdf")), 0.0,-0.70000,0.000000,up_rot[0],up_rot[1],up_rot[2],up_rot[3])] 62 | teddy_rot = p.getQuaternionFromEuler([-math.pi/2, math.pi,-math.pi/4]) 63 | 64 | block_red = [p.loadURDF((os.path.join(meshPath,"block.urdf")), -0.150000,0.100000,0.10000,0,0,0,0)] 65 | plate = [p.loadURDF((os.path.join(meshPath,"plate.urdf")), 0.100000,0.200000,0.10000,0,0,0,0)] 66 | #cup = [p.loadURDF((os.path.join(meshPath,"cup/cup_small.urdf")), 0.100000,0.200000,0.10000,0,0,0,0)] 67 | block_blue = [p.loadURDF((os.path.join(meshPath,"block_blue.urdf")), -0.000000,0.400000,0.10000,0,0,0,0)] 68 | return [jenga, block_red, block_blue, plate] 69 | 70 | def load_lab_Z_up(): 71 | jenga = [p.loadURDF((os.path.join(urdfRoot,"jenga/jenga.urdf")), 0,0.0,0,0.400000,0.707107,0.000000,0.707107)] 72 | table = [p.loadURDF((os.path.join(urdfRoot,"table/table.urdf")), 0.0,0.0,-0.70000,0.000000,0.000000,0.707107,0.707107)] 73 | block_red = [p.loadURDF((os.path.join(meshPath,"block.urdf")), 0.05,0.0,0,0.400000,0.707107,0.000000,0.707107)] 74 | plate = [p.loadURDF((os.path.join(meshPath,"plate.urdf")), 0.05,0.1,0,0.800000,0.0,0.700000,0.7)] 75 | #cup = [p.loadURDF((os.path.join(meshPath,"cup/cup_small.urdf")), 0.100000,0.200000,0.10000,0,0,0,0)] 76 | block_blue = [p.loadURDF((os.path.join(meshPath,"block_blue.urdf")), -0.05,0.0,0,0.400000,0.707107,0.000000,0.707107)] 77 | return [jenga, block_red, block_blue, plate] 78 | 79 | def throwing_scene(): 80 | block_red = [p.loadURDF((os.path.join(meshPath,"cube_small.urdf")), 0.0,0.0,0.05,0.400000,0.0,0.000000,1.0)] 81 | table = [p.loadURDF((os.path.join(urdfRoot,"table/table.urdf")), 0.0,0.0,-0.6300,0.000000,0.000000,0.0,1.0)] 82 | return [block_red] 83 | 84 | 85 | def load_gripper(): 86 | objects = [p.loadURDF((os.path.join(urdfRoot,"pr2_gripper.urdf")), 0.500000,0.300006,0.700000,-0.000000,-0.000000,-0.000031,1.000000)] 87 | pr2_gripper = objects[0] 88 | print ("pr2_gripper=") 89 | print (pr2_gripper) 90 | 91 | jointPositions=[ 0.550569, 0.000000, 0.549657, 0.000000 ] 92 | for jointIndex in range (p.getNumJoints(pr2_gripper)): 93 | p.resetJointState(pr2_gripper,jointIndex,jointPositions[jointIndex]) 94 | 95 | pr2_cid = p.createConstraint(pr2_gripper,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0.2,0,0],[0.500000,0.300006,0.700000]) 96 | print ("pr2_cid") 97 | print (pr2_cid) 98 | return pr2_gripper, pr2_cid 99 | 100 | 101 | def disable_scene_render(): 102 | p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0) 103 | 104 | def enable_scene_render(): 105 | p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) 106 | 107 | 108 | def get_scene_observation(objects): 109 | observation = [] 110 | for o in objects: 111 | pos, orn = p.getBasePositionAndOrientation(o[0]) 112 | observation.extend(list(pos)) 113 | observation.extend(list(orn)) 114 | return observation 115 | 116 | 117 | 118 | -------------------------------------------------------------------------------- /ur5.py: -------------------------------------------------------------------------------- 1 | 2 | import os, inspect 3 | 4 | currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) 5 | print("current_dir=" + currentdir) 6 | os.sys.path.insert(0, currentdir) 7 | 8 | import math 9 | import gym 10 | import sys 11 | from gym import spaces 12 | from gym.utils import seeding 13 | import numpy as np 14 | import time 15 | import pybullet as p 16 | from itertools import chain 17 | from collections import deque 18 | 19 | import random 20 | import pybullet_data 21 | from collections import namedtuple 22 | from attrdict import AttrDict 23 | import functools 24 | import time 25 | import itertools 26 | from cntrl import * 27 | from pyRobotiqGripper import * 28 | real = False 29 | 30 | def setup_sisbot(p, uid): 31 | # controlJoints = ["shoulder_pan_joint","shoulder_lift_joint", 32 | # "elbow_joint", "wrist_1_joint", 33 | # "wrist_2_joint", "wrist_3_joint", 34 | # "robotiq_85_left_knuckle_joint"] 35 | controlJoints = ["shoulder_pan_joint","shoulder_lift_joint", 36 | "elbow_joint", "wrist_1_joint", 37 | "wrist_2_joint", "wrist_3_joint", 'left_gripper_motor', 'right_gripper_motor'] 38 | 39 | jointTypeList = ["REVOLUTE", "PRISMATIC", "SPHERICAL", "PLANAR", "FIXED"] 40 | numJoints = p.getNumJoints(uid) 41 | jointInfo = namedtuple("jointInfo", 42 | ["id","name","type","lowerLimit","upperLimit","maxForce","maxVelocity","controllable"]) 43 | joints = AttrDict() 44 | for i in range(numJoints): 45 | info = p.getJointInfo(uid, i) 46 | jointID = info[0] 47 | jointName = info[1].decode("utf-8") 48 | jointType = jointTypeList[info[2]] 49 | jointLowerLimit = info[8] 50 | jointUpperLimit = info[9] 51 | jointMaxForce = info[10] 52 | jointMaxVelocity = info[11] 53 | controllable = True if jointName in controlJoints else False 54 | info = jointInfo(jointID,jointName,jointType,jointLowerLimit, 55 | jointUpperLimit,jointMaxForce,jointMaxVelocity,controllable) 56 | if info.type=="REVOLUTE": # set revolute joint to static 57 | p.setJointMotorControl2(uid, info.id, p.VELOCITY_CONTROL, targetVelocity=0, force=0) 58 | joints[info.name] = info 59 | controlRobotiqC2 = False 60 | mimicParentName = False 61 | return joints, controlRobotiqC2, controlJoints, mimicParentName 62 | 63 | 64 | 65 | class ur5: 66 | 67 | def __init__(self, urdfRootPath=pybullet_data.getDataPath(), timeStep=0.01, vr = False): 68 | 69 | 70 | 71 | self.robotUrdfPath = "./urdf/real_arm.urdf" 72 | self.robotStartPos = [0.0,0.0,0.0] 73 | self.robotStartOrn = p.getQuaternionFromEuler([1.885,1.786,0.132]) 74 | 75 | self.xin = self.robotStartPos[0] 76 | self.yin = self.robotStartPos[1] 77 | 78 | self.zin = self.robotStartPos[2] 79 | self.lastJointAngle = None 80 | self.active = False 81 | if real: 82 | self.s = init_socket() 83 | 84 | if True: 85 | self.grip=RobotiqGripper("COM8") 86 | #grip.resetActivate() 87 | self.grip.reset() 88 | #grip.printInfo() 89 | self.grip.activate() 90 | #grip.printInfo() 91 | #grip.calibrate() 92 | 93 | 94 | 95 | 96 | self.reset() 97 | self.timeout = 0 98 | 99 | 100 | def reset(self): 101 | 102 | print("----------------------------------------") 103 | print("Loading robot from {}".format(self.robotUrdfPath)) 104 | self.uid = p.loadURDF(os.path.join(os.getcwd(),self.robotUrdfPath), self.robotStartPos, self.robotStartOrn, 105 | flags=p.URDF_USE_INERTIA_FROM_FILE) 106 | self.joints, self.controlRobotiqC2, self.controlJoints, self.mimicParentName = setup_sisbot(p, self.uid) 107 | self.endEffectorIndex = 7 # ee_link 108 | self.numJoints = p.getNumJoints(self.uid) 109 | self.active_joint_ids = [] 110 | for i, name in enumerate(self.controlJoints): 111 | joint = self.joints[name] 112 | self.active_joint_ids.append(joint.id) 113 | 114 | 115 | 116 | def getActionDimension(self): 117 | # if (self.useInverseKinematics): 118 | # return len(self.motorIndices) 119 | return 8 # position x,y,z and ori quat and finger angle 120 | def getObservationDimension(self): 121 | return len(self.getObservation()) 122 | 123 | def setPosition(self, pos, quat): 124 | 125 | p.resetBasePositionAndOrientation(self.uid,pos, 126 | quat) 127 | 128 | def resetJointPoses(self): 129 | # move to this ideal init point 130 | self.active = False 131 | 132 | for i in range(0,50000): 133 | self.action([0.15328961509984124, -1.8, -1.5820032364177563, -1.2879050862601897, 1.5824233979484994, 0.19581299859677043, 0.012000000476837159, -0.012000000476837159]) 134 | self.active = True 135 | 136 | if real: 137 | 138 | 139 | movej(self.s,[0.15328961509984124, -1.8, -1.5820032364177563, -1.2879050862601897, 1.5824233979484994, 0.19581299859677043, 0.012000000476837159, -0.012000000476837159],a=0.01,v=0.05,t=10.0) 140 | time.sleep(10) 141 | 142 | self.lastJointAngle = [0.15328961509984124, -1.8, -1.5820032364177563, -1.2879050862601897, 1.5824233979484994, 0.19581299859677043] 143 | 144 | 145 | 146 | 147 | def getObservation(self): 148 | observation = [] 149 | state = p.getLinkState(self.uid, self.endEffectorIndex, computeLinkVelocity = 1) 150 | #print('state',state) 151 | pos = state[0] 152 | orn = state[1] 153 | 154 | 155 | 156 | observation.extend(list(pos)) 157 | observation.extend(list(orn)) 158 | 159 | joint_states = p.getJointStates(self.uid, self.active_joint_ids) 160 | 161 | joint_positions = list() 162 | joint_velocities = list() 163 | 164 | 165 | for joint in joint_states: 166 | 167 | joint_positions.append(joint[0]) 168 | joint_velocities.append(joint[1]) 169 | 170 | 171 | 172 | return joint_positions + joint_velocities + observation 173 | 174 | 175 | def action(self, motorCommands): 176 | #print(motorCommands) 177 | 178 | poses = [] 179 | indexes = [] 180 | forces = [] 181 | 182 | 183 | # if self.lastJointAngle == None: 184 | # self.lastJointAngle = motorCommands[0:6] 185 | 186 | # rel_a = np.array(motorCommands[0:6]) - np.array(self.lastJointAngle) 187 | 188 | # clipped_a = np.clip(rel_a, -0.1, 0.1) 189 | # motorCommands[0:6] = list(clipped_a+self.lastJointAngle) 190 | # self.lastJointAngle = motorCommands[0:6] 191 | 192 | for i, name in enumerate(self.controlJoints): 193 | joint = self.joints[name] 194 | 195 | poses.append(motorCommands[i]) 196 | indexes.append(joint.id) 197 | forces.append(joint.maxForce) 198 | l = len(poses) 199 | 200 | 201 | p.setJointMotorControlArray(self.uid, indexes, p.POSITION_CONTROL, targetPositions=poses, targetVelocities =[0]*l, positionGains = [0.03]*l, forces = forces) 202 | #holy shit this is so much faster in arrayform! 203 | 204 | if real and self.active: 205 | 206 | if time.time() > self.timeout+0.05: 207 | servoj(self.s,poses[0:6],a=0,v=0,t=0.05, gain = 100, lookahead_time = 0.05) 208 | self.timeout = time.time() 209 | 210 | 211 | grip_angle = max(0, min(255,int(poses[6]*255/0.04))) # range 0 - 0.04 212 | self.grip.goTo(grip_angle) 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | def move_to(self, position_delta, mode = 'abs', noise = False, clip = False): 224 | 225 | #at the moment UR5 only absolute 226 | 227 | x = position_delta[0] 228 | y = position_delta[1] 229 | z = position_delta[2] 230 | 231 | orn = position_delta[3:7] 232 | finger_angle = position_delta[7] 233 | 234 | # define our limtis. 235 | z = max(0.14, min(0.7,z)) 236 | x = max(-0.25, min(0.3,x)) 237 | y =max(-0.4, min(0.4,y)) 238 | 239 | 240 | jointPose = list(p.calculateInverseKinematics(self.uid, self.endEffectorIndex, [x,y,z], orn)) 241 | 242 | # print(jointPose) 243 | # print(self.getObservation()[:len(self.controlJoints)]) ## get the current joint positions 244 | 245 | jointPose[7] = -finger_angle/25 246 | jointPose[6] = finger_angle/25 247 | 248 | self.action(jointPose) 249 | #print(jointPose) 250 | return jointPose 251 | 252 | -------------------------------------------------------------------------------- /urdf/backup/sisbot.urdf.bk: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 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 | 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 | 365 | 366 | 367 | 368 | 369 | 370 | 371 | 372 | 373 | 374 | 375 | 376 | 377 | 378 | 379 | 380 | 381 | 382 | 383 | 384 | 385 | 386 | 387 | 388 | 389 | 390 | 391 | 392 | 393 | 394 | 395 | 396 | 397 | 398 | 399 | 400 | 401 | 402 | 403 | 404 | 405 | 406 | 407 | 408 | 409 | 410 | 411 | 412 | 413 | 414 | 415 | 416 | 417 | 418 | 419 | 420 | 421 | 422 | 423 | 424 | 425 | 426 | 429 | 430 | 431 | 432 | 433 | 434 | 435 | 436 | 437 | 438 | 439 | 440 | 441 | 442 | 443 | 444 | 445 | 446 | -------------------------------------------------------------------------------- /urdf/backup/sisbot_noertia.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 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 | 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 | 365 | 366 | 367 | 368 | 369 | 370 | 371 | 372 | 373 | 374 | 375 | 376 | 377 | 378 | 379 | 380 | 381 | 382 | 383 | 384 | 385 | 386 | 387 | 388 | 389 | 390 | 391 | 392 | 393 | 394 | 395 | 396 | 397 | 398 | 399 | 400 | 401 | 402 | 403 | 404 | 405 | 406 | 407 | 408 | 409 | 410 | 411 | 412 | 413 | 414 | 415 | 416 | 417 | 418 | 419 | 420 | 421 | 422 | 423 | 424 | 425 | 426 | 427 | 428 | 429 | 430 | 431 | 432 | 433 | 434 | 435 | 436 | 437 | 438 | 439 | 440 | 441 | 442 | 443 | 444 | 445 | 446 | 447 | 448 | 449 | 450 | 451 | 452 | 453 | 456 | 457 | 458 | 459 | 460 | 461 | 462 | 463 | 464 | 465 | 466 | 467 | 468 | 469 | 470 | 471 | 472 | 473 | -------------------------------------------------------------------------------- /urdf/backup/sisbot_nomass.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 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 | 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 | 365 | 366 | 367 | 368 | 369 | 370 | 371 | 372 | 373 | 374 | 375 | 376 | 377 | 378 | 379 | 380 | 381 | 382 | 383 | 384 | 385 | 386 | 387 | 388 | 389 | 390 | 391 | 392 | 393 | 394 | 395 | 396 | 397 | 398 | 399 | 400 | 401 | 402 | 403 | 404 | 405 | 406 | 407 | 408 | 409 | 410 | 411 | 412 | 413 | 414 | 415 | 416 | 417 | 418 | 419 | 420 | 421 | 422 | 423 | 424 | 425 | 426 | 427 | 428 | 429 | 430 | 431 | 432 | 433 | 434 | 435 | 436 | 437 | 438 | 439 | 440 | 441 | 442 | 443 | 444 | 445 | 446 | 447 | 448 | 449 | 450 | 451 | 452 | 453 | 454 | 455 | 456 | 457 | 458 | 459 | 460 | 461 | 462 | 463 | 464 | 465 | 466 | 467 | 468 | 469 | 470 | 471 | 474 | 475 | 476 | 477 | 478 | 479 | 480 | 481 | 482 | 483 | 484 | 485 | 486 | 487 | 488 | 489 | 490 | 491 | -------------------------------------------------------------------------------- /urdf/real_arm.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 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 | 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 | 311 | 312 | 315 | 523 | 524 | 527 | 528 | 529 | 530 | 531 | 532 | 533 | 534 | 535 | 536 | 541 | 542 | 543 | -------------------------------------------------------------------------------- /urdf/ur5.urdf: -------------------------------------------------------------------------------- 1 | Skip to content 2 | 3 | Search or jump to… 4 | 5 | Pull requests 6 | Issues 7 | Marketplace 8 | Explore 9 | 10 | @sholtodouglas 11 | 1 12 | 0 0 sholtodouglas/ur5pybullet 13 | Code Issues 0 Pull requests 0 Projects 0 Wiki Security Insights Settings 14 | ur5pybullet/urdf/ur5.urdf 15 | SholtoD THROWING 16 | 10019ca 14 hours ago 17 | 543 lines (532 sloc) 18.4 KB 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 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 | 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 | 328 | 329 | 332 | 540 | 541 | 544 | 545 | 546 | 547 | 548 | 549 | 550 | 551 | 552 | 553 | 558 | 559 | 560 | 561 | 562 | --------------------------------------------------------------------------------