first commit
This commit is contained in:
王子祥
commit
1e1dcaa6a3
|
|
@ -0,0 +1,135 @@
|
|||
# Byte-compiled / optimized / DLL files
|
||||
__pycache__/
|
||||
*.py[cod]
|
||||
*$py.class
|
||||
|
||||
# C extensions
|
||||
*.so
|
||||
|
||||
# Distribution / packaging
|
||||
.Python
|
||||
build/
|
||||
develop-eggs/
|
||||
dist/
|
||||
downloads/
|
||||
eggs/
|
||||
.eggs/
|
||||
lib/
|
||||
lib64/
|
||||
parts/
|
||||
sdist/
|
||||
var/
|
||||
wheels/
|
||||
pip-wheel-metadata/
|
||||
share/python-wheels/
|
||||
*.egg-info/
|
||||
.installed.cfg
|
||||
*.egg
|
||||
MANIFEST
|
||||
|
||||
# PyInstaller
|
||||
# Usually these files are written by a python script from a template
|
||||
# before PyInstaller builds the exe, so as to inject date/other infos into it.
|
||||
*.manifest
|
||||
*.spec
|
||||
|
||||
# Installer logs
|
||||
pip-log.txt
|
||||
pip-delete-this-directory.txt
|
||||
|
||||
# Unit test / coverage reports
|
||||
htmlcov/
|
||||
.tox/
|
||||
.nox/
|
||||
.coverage
|
||||
.coverage.*
|
||||
.cache
|
||||
nosetests.xml
|
||||
coverage.xml
|
||||
*.cover
|
||||
*.py,cover
|
||||
.hypothesis/
|
||||
.pytest_cache/
|
||||
|
||||
# Translations
|
||||
*.mo
|
||||
*.pot
|
||||
|
||||
# Django stuff:
|
||||
*.log
|
||||
local_settings.py
|
||||
db.sqlite3
|
||||
db.sqlite3-journal
|
||||
|
||||
# Flask stuff:
|
||||
instance/
|
||||
.webassets-cache
|
||||
|
||||
# Scrapy stuff:
|
||||
.scrapy
|
||||
|
||||
# Sphinx documentation
|
||||
docs/_build/
|
||||
|
||||
# PyBuilder
|
||||
target/
|
||||
|
||||
# Jupyter Notebook
|
||||
.ipynb_checkpoints
|
||||
|
||||
# IPython
|
||||
profile_default/
|
||||
ipython_config.py
|
||||
|
||||
# pyenv
|
||||
.python-version
|
||||
|
||||
# pipenv
|
||||
# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
|
||||
# However, in case of collaboration, if having platform-specific dependencies or dependencies
|
||||
# having no cross-platform support, pipenv may install dependencies that don't work, or not
|
||||
# install all needed dependencies.
|
||||
#Pipfile.lock
|
||||
|
||||
# PEP 582; used by e.g. github.com/David-OConnor/pyflow
|
||||
__pypackages__/
|
||||
|
||||
# Celery stuff
|
||||
celerybeat-schedule
|
||||
celerybeat.pid
|
||||
|
||||
# SageMath parsed files
|
||||
*.sage.py
|
||||
|
||||
# Environments
|
||||
.env
|
||||
.venv
|
||||
env/
|
||||
venv/
|
||||
ENV/
|
||||
env.bak/
|
||||
venv.bak/
|
||||
|
||||
# Spyder project settings
|
||||
.spyderproject
|
||||
.spyproject
|
||||
|
||||
# Rope project settings
|
||||
.ropeproject
|
||||
|
||||
# mkdocs documentation
|
||||
/site
|
||||
|
||||
# mypy
|
||||
.mypy_cache/
|
||||
.dmypy.json
|
||||
dmypy.json
|
||||
|
||||
# Pyre type checker
|
||||
.pyre/
|
||||
|
||||
# VS code
|
||||
.vscode
|
||||
|
||||
/network_output
|
||||
/results
|
||||
|
|
@ -0,0 +1,48 @@
|
|||
## Learning to Grasp Objects in Highly Cluttered Environments using Deep Convolutional Neural Networks
|
||||
|
||||
|
||||
<p align="center">
|
||||
<img src="images/isolated.gif" width="250" title="">
|
||||
<img src="images/packed.gif" width="250" title="">
|
||||
<img src="images/pile.gif" width="250" title="">
|
||||
|
||||
|
||||
</p>
|
||||
<p align="left">
|
||||
Our experimental setup consists of a UR5e robotic arm, an RGB-D camera, a set of objects placed on the desk in three scenarios: (left) isolated scenario, (center) packed scenario, and (right) pile scenario. The green line denotes the camera’s line of sight.
|
||||
</p>
|
||||
|
||||
###### Jeroen Oude Vrielink, [Hamidreza Kasaei](https://hkasaei.github.io/) | [video](https://youtu.be/fXpZMnZUZoA) | [thesis](https://fse.studenttheses.ub.rug.nl/25369/)
|
||||
|
||||
|
||||
##
|
||||
|
||||
### This repository
|
||||
This repository implements the grasp inference method of Kumra et al. (2020) in a robotic simulation developed in PyBullet. Kumra et al. (2020) propose a generative residual convolutional neural network which predicts one or multiple antipodal grasps using both RGB and depth images of the object scene. Three different grasping scenarios have been implemented. These include objects in isolation, objects packed together, and objects in a pile (Kasaei et al., 2021).
|
||||
|
||||
All code in the directory 'network' is an adaptation of Kumra's open source code that was taken from the following repository: https://github.com/skumra/robotic-grasping
|
||||
The simulation code is an adaptation from the following repository: https://github.com/ElectronicElephant/pybullet_ur5_robotiq
|
||||
Object models were taken from the following repository: https://github.com/eleramp/pybullet-object-models
|
||||
|
||||
## Requirements
|
||||
|
||||
Ensure you are running Python>=3.6.5 and import the required libraries by running:
|
||||
```
|
||||
pip install -r requirements.txt
|
||||
```
|
||||
|
||||
It will install a set of packages, including: numpy, opencv-python, matplotlib, scikit-image, imageio, torch, torchvision, torchsummary, tensorboardX, pyrealsense2, Pillow, pandas, matplotlib, attrdict, pybullet
|
||||
|
||||
## Demo
|
||||
Running the script 'demo.py' gives a demonstration of the simulation. The demo can be run with three different grasping scenarios. Run 'demo.py --help' to see a full list of options.
|
||||
|
||||
Example:
|
||||
```bash
|
||||
python demo.py --scenario=isolated --runs=1 --show-network-output=False
|
||||
```
|
||||
|
||||
|
||||
## References
|
||||
Sulabh Kumra, Shirin Joshi, and Ferat Sahin. Antipodal robotic grasping using generative residual convolutional neural network. In 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS), pages 9626–9633, 2020. doi: 10.1109/IROS45743.2020.9340777.
|
||||
|
||||
Hamidreza Kasaei and Mohammadreza Kasaei. MV-grasp: Real-time multi-view 3D object grasping in highly cluttered environments. arXiv preprint arXiv:2103.10997, 2021
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
rm -r results/2021*
|
||||
rm network_output/*.png
|
||||
|
|
@ -0,0 +1,196 @@
|
|||
from grasp_generator import GraspGenerator
|
||||
from environment.utilities import Camera
|
||||
from environment.env import Environment
|
||||
from utils import YcbObjects
|
||||
import pybullet as p
|
||||
import argparse
|
||||
import os
|
||||
import sys
|
||||
sys.path.append('network')
|
||||
|
||||
|
||||
def parse_args():
|
||||
parser = argparse.ArgumentParser(description='Grasping demo')
|
||||
|
||||
parser.add_argument('--scenario', type=str, default='isolated',
|
||||
help='Grasping scenario (isolated/pack/pile)')
|
||||
parser.add_argument('--runs', type=int, default=1,
|
||||
help='Number of runs the scenario is executed')
|
||||
parser.add_argument('--show-network-output', dest='output', type=bool, default=False,
|
||||
help='Show network output (True/False)')
|
||||
|
||||
args = parser.parse_args()
|
||||
return args
|
||||
|
||||
|
||||
def isolated_obj_scenario(n, vis, output, debug):
|
||||
|
||||
objects = YcbObjects('objects/ycb_objects',
|
||||
mod_orn=['ChipsCan', 'MustardBottle',
|
||||
'TomatoSoupCan'],
|
||||
mod_stiffness=['Strawberry'])
|
||||
center_x, center_y = 0.05, -0.52
|
||||
network_path = 'network/trained-models/cornell-randsplit-rgbd-grconvnet3-drop1-ch32/epoch_19_iou_0.98'
|
||||
camera = Camera((center_x, center_y, 1.9), (center_x,
|
||||
center_y, 0.785), 0.2, 2.0, (224, 224), 40)
|
||||
env = Environment(camera, vis=vis, debug=debug)
|
||||
generator = GraspGenerator(network_path, camera, 5)
|
||||
|
||||
objects.shuffle_objects()
|
||||
for _ in range(n):
|
||||
for obj_name in objects.obj_names:
|
||||
print(obj_name)
|
||||
|
||||
path, mod_orn, mod_stiffness = objects.get_obj_info(obj_name)
|
||||
env.load_isolated_obj(path, mod_orn, mod_stiffness)
|
||||
env.move_away_arm()
|
||||
|
||||
rgb, depth, _ = camera.get_cam_img()
|
||||
grasps, save_name = generator.predict_grasp(
|
||||
rgb, depth, n_grasps=3, show_output=output)
|
||||
for i, grasp in enumerate(grasps):
|
||||
x, y, z, roll, opening_len, obj_height = grasp
|
||||
if vis:
|
||||
debug_id = p.addUserDebugLine(
|
||||
[x, y, z], [x, y, 1.2], [0, 0, 1], lineWidth=3)
|
||||
|
||||
succes_grasp, succes_target = env.grasp(
|
||||
(x, y, z), roll, opening_len, obj_height)
|
||||
if vis:
|
||||
p.removeUserDebugItem(debug_id)
|
||||
if succes_target:
|
||||
if save_name is not None:
|
||||
os.rename(save_name + '.png', save_name +
|
||||
f'_SUCCESS_grasp{i}.png')
|
||||
break
|
||||
env.reset_all_obj()
|
||||
env.remove_all_obj()
|
||||
|
||||
|
||||
def pile_scenario(n, vis, output, debug):
|
||||
|
||||
objects = YcbObjects('objects/ycb_objects',
|
||||
mod_orn=['ChipsCan', 'MustardBottle',
|
||||
'TomatoSoupCan'],
|
||||
mod_stiffness=['Strawberry'],
|
||||
exclude=['CrackerBox', 'Hammer'])
|
||||
center_x, center_y = 0.05, -0.52
|
||||
network_path = 'network/trained-models/cornell-randsplit-rgbd-grconvnet3-drop1-ch32/epoch_19_iou_0.98'
|
||||
camera = Camera((center_x, center_y, 1.9), (center_x,
|
||||
center_y, 0.785), 0.2, 2.0, (224, 224), 40)
|
||||
env = Environment(camera, vis=vis, debug=debug, finger_length=0.06)
|
||||
generator = GraspGenerator(network_path, camera, 5)
|
||||
|
||||
for i in range(n):
|
||||
print(f'Trial {i}')
|
||||
straight_fails = 0
|
||||
objects.shuffle_objects()
|
||||
|
||||
env.move_away_arm()
|
||||
info = objects.get_n_first_obj_info(5)
|
||||
env.create_pile(info)
|
||||
|
||||
straight_fails = 0
|
||||
while len(env.obj_ids) != 0 and straight_fails < 3:
|
||||
env.move_away_arm()
|
||||
env.reset_all_obj()
|
||||
rgb, depth, _ = camera.get_cam_img()
|
||||
grasps, save_name = generator.predict_grasp(
|
||||
rgb, depth, n_grasps=3, show_output=output)
|
||||
|
||||
for i, grasp in enumerate(grasps):
|
||||
x, y, z, roll, opening_len, obj_height = grasp
|
||||
|
||||
if vis:
|
||||
debugID = p.addUserDebugLine(
|
||||
[x, y, z], [x, y, 1.2], [0, 0, 1], lineWidth=3)
|
||||
|
||||
succes_grasp, succes_target = env.grasp(
|
||||
(x, y, z), roll, opening_len, obj_height)
|
||||
if vis:
|
||||
p.removeUserDebugItem(debugID)
|
||||
if succes_target:
|
||||
straight_fails = 0
|
||||
if save_name is not None:
|
||||
os.rename(save_name + '.png', save_name +
|
||||
f'_SUCCESS_grasp{i}.png')
|
||||
break
|
||||
else:
|
||||
straight_fails += 1
|
||||
|
||||
if straight_fails == 3 or len(env.obj_ids) == 0:
|
||||
break
|
||||
|
||||
env.reset_all_obj()
|
||||
env.remove_all_obj()
|
||||
|
||||
|
||||
def pack_scenario(n, vis, output, debug):
|
||||
vis = vis
|
||||
output = output
|
||||
debug = debug
|
||||
|
||||
objects = YcbObjects('objects/ycb_objects',
|
||||
mod_orn=['ChipsCan', 'MustardBottle',
|
||||
'TomatoSoupCan'],
|
||||
mod_stiffness=['Strawberry'])
|
||||
center_x, center_y = 0.05, -0.52
|
||||
network_path = 'network/trained-models/cornell-randsplit-rgbd-grconvnet3-drop1-ch32/epoch_19_iou_0.98'
|
||||
camera = Camera((center_x, center_y, 1.9), (center_x,
|
||||
center_y, 0.785), 0.2, 2.0, (224, 224), 40)
|
||||
env = Environment(camera, vis=vis, debug=debug, finger_length=0.06)
|
||||
generator = GraspGenerator(network_path, camera, 5)
|
||||
|
||||
for i in range(n):
|
||||
print(f'Trial {i}')
|
||||
straight_fails = 0
|
||||
objects.shuffle_objects()
|
||||
info = objects.get_n_first_obj_info(5)
|
||||
env.create_packed(info)
|
||||
|
||||
straight_fails = 0
|
||||
while len(env.obj_ids) != 0 and straight_fails < 3:
|
||||
env.move_away_arm()
|
||||
env.reset_all_obj()
|
||||
rgb, depth, _ = camera.get_cam_img()
|
||||
grasps, save_name = generator.predict_grasp(
|
||||
rgb, depth, n_grasps=3, show_output=output)
|
||||
|
||||
for i, grasp in enumerate(grasps):
|
||||
x, y, z, roll, opening_len, obj_height = grasp
|
||||
|
||||
if vis:
|
||||
debugID = p.addUserDebugLine(
|
||||
[x, y, z], [x, y, 1.2], [0, 0, 1], lineWidth=3)
|
||||
|
||||
succes_grasp, succes_target = env.grasp(
|
||||
(x, y, z), roll, opening_len, obj_height)
|
||||
if vis:
|
||||
p.removeUserDebugItem(debugID)
|
||||
if succes_target:
|
||||
straight_fails = 0
|
||||
if save_name is not None:
|
||||
os.rename(save_name + '.png', save_name +
|
||||
f'_SUCCESS_grasp{i}.png')
|
||||
break
|
||||
else:
|
||||
straight_fails += 1
|
||||
|
||||
if straight_fails == 3 or len(env.obj_ids) == 0:
|
||||
break
|
||||
|
||||
env.reset_all_obj()
|
||||
env.remove_all_obj()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
args = parse_args()
|
||||
output = args.output
|
||||
runs = args.runs
|
||||
|
||||
if args.scenario == 'isolated':
|
||||
isolated_obj_scenario(runs, vis=True, output=output, debug=False)
|
||||
elif args.scenario == 'pack':
|
||||
pack_scenario(runs, vis=True, output=output, debug=False)
|
||||
elif args.scenario == 'pile':
|
||||
pile_scenario(runs, vis=True, output=output, debug=False)
|
||||
|
|
@ -0,0 +1,609 @@
|
|||
from environment.utilities import setup_sisbot, Camera
|
||||
import math
|
||||
import time
|
||||
import numpy as np
|
||||
import pybullet as p
|
||||
import pybullet_data
|
||||
import random
|
||||
|
||||
|
||||
class FailToReachTargetError(RuntimeError):
|
||||
pass
|
||||
|
||||
|
||||
class Environment:
|
||||
OBJECT_INIT_HEIGHT = 1.05
|
||||
GRIPPER_MOVING_HEIGHT = 1.25
|
||||
GRIPPER_GRASPED_LIFT_HEIGHT = 1.4
|
||||
TARGET_ZONE_POS = [0.7, 0.0, 0.685]
|
||||
SIMULATION_STEP_DELAY = 1 / 240.
|
||||
FINGER_LENGTH = 0.06
|
||||
Z_TABLE_TOP = 0.785
|
||||
GRIP_REDUCTION = 0.60
|
||||
|
||||
def __init__(self, camera: Camera, vis=False, debug=False, gripper_type='140', finger_length=0.06) -> None:
|
||||
self.vis = vis
|
||||
self.debug = debug
|
||||
self.camera = camera
|
||||
|
||||
self.obj_init_pos = (camera.x, camera.y)
|
||||
self.obj_ids = []
|
||||
self.obj_positions = []
|
||||
self.obj_orientations = []
|
||||
|
||||
if gripper_type not in ('85', '140'):
|
||||
raise NotImplementedError(
|
||||
'Gripper %s not implemented.' % gripper_type)
|
||||
self.gripper_type = gripper_type
|
||||
self.finger_length = finger_length
|
||||
|
||||
# define environment
|
||||
self.physicsClient = p.connect(p.GUI if self.vis else p.DIRECT)
|
||||
p.setAdditionalSearchPath(pybullet_data.getDataPath())
|
||||
p.setGravity(0, 0, -10)
|
||||
self.planeID = p.loadURDF('plane.urdf')
|
||||
self.tableID = p.loadURDF('environment/urdf/objects/table.urdf',
|
||||
[0.0, -0.65, 0.76],
|
||||
p.getQuaternionFromEuler([0, 0, 0]),
|
||||
useFixedBase=True)
|
||||
self.target_table_id = p.loadURDF('environment/urdf/objects/target_table.urdf',
|
||||
[0.7, 0.0, 0.66],
|
||||
p.getQuaternionFromEuler([0, 0, 0]),
|
||||
useFixedBase=True)
|
||||
self.target_id = p.loadURDF('environment/urdf/objects/traybox.urdf',
|
||||
self.TARGET_ZONE_POS,
|
||||
p.getQuaternionFromEuler([0, 0, 0]),
|
||||
useFixedBase=True,
|
||||
globalScaling=0.7)
|
||||
self.UR5Stand_id = p.loadURDF('environment/urdf/objects/ur5_stand.urdf',
|
||||
[-0.7, -0.36, 0.0],
|
||||
p.getQuaternionFromEuler([0, 0, 0]),
|
||||
useFixedBase=True)
|
||||
self.robot_id = p.loadURDF('environment/urdf/ur5_robotiq_%s.urdf' % gripper_type,
|
||||
[0, 0, 0.0], # StartPosition
|
||||
p.getQuaternionFromEuler(
|
||||
[0, 0, 0]), # StartOrientation
|
||||
useFixedBase=True,
|
||||
flags=p.URDF_USE_INERTIA_FROM_FILE)
|
||||
self.joints, self.controlGripper, self.controlJoints, self.mimicParentName =\
|
||||
setup_sisbot(p, self.robot_id, gripper_type)
|
||||
self.eef_id = 7 # ee_link
|
||||
|
||||
# Add force sensors
|
||||
p.enableJointForceTorqueSensor(
|
||||
self.robot_id, self.joints['left_inner_finger_pad_joint'].id)
|
||||
p.enableJointForceTorqueSensor(
|
||||
self.robot_id, self.joints['right_inner_finger_pad_joint'].id)
|
||||
|
||||
# Change the friction of the gripper
|
||||
p.changeDynamics(
|
||||
self.robot_id, self.joints['left_inner_finger_pad_joint'].id, lateralFriction=1)
|
||||
p.changeDynamics(
|
||||
self.robot_id, self.joints['right_inner_finger_pad_joint'].id, lateralFriction=1)
|
||||
|
||||
# custom sliders to tune parameters (name of the parameter,range,initial value)
|
||||
# Task space (Cartesian space)
|
||||
if debug:
|
||||
self.xin = p.addUserDebugParameter('x', -0.4, 0.4, 0.11)
|
||||
self.yin = p.addUserDebugParameter('y', -0.8, 0, -0.49)
|
||||
self.zin = p.addUserDebugParameter('z', 0.9, 1.3, 1.1)
|
||||
self.rollId = p.addUserDebugParameter(
|
||||
'roll', -3.14, 3.14, 0) # -1.57 yaw
|
||||
self.pitchId = p.addUserDebugParameter(
|
||||
'pitch', -3.14, 3.14, np.pi/2)
|
||||
self.yawId = p.addUserDebugParameter(
|
||||
'yaw', -np.pi/2, np.pi/2, 0) # -3.14 pitch
|
||||
self.gripper_opening_length_control = p.addUserDebugParameter(
|
||||
'gripper_opening_length', 0, 0.1, 0.085)
|
||||
|
||||
# Add debug lines for end effector and camera
|
||||
if vis:
|
||||
self.eef_debug_lineID = None
|
||||
p.addUserDebugLine([camera.x, camera.y, 0], [
|
||||
camera.x, camera.y, camera.z], [0, 1, 0], lineWidth=3)
|
||||
dist = 1.0
|
||||
yaw = 30
|
||||
pitch = -50
|
||||
target = [0.2, -0.40, 0.785]
|
||||
p.resetDebugVisualizerCamera(dist, yaw, pitch, target)
|
||||
|
||||
# Setup some Limit
|
||||
self.gripper_open_limit = (0.0, 0.1)
|
||||
self.ee_position_limit = ((-0.8, 0.8),
|
||||
(-0.8, 0.8),
|
||||
(0.785, 1.4))
|
||||
self.reset_robot()
|
||||
|
||||
def step_simulation(self):
|
||||
"""
|
||||
Hook p.stepSimulation()
|
||||
"""
|
||||
p.stepSimulation()
|
||||
if self.vis:
|
||||
if self.debug:
|
||||
if self.eef_debug_lineID is not None:
|
||||
p.removeUserDebugItem(self.eef_debug_lineID)
|
||||
eef_xyz = p.getLinkState(self.robot_id, self.eef_id)[0:1]
|
||||
end = np.array(eef_xyz[0])
|
||||
end[2] -= 0.5
|
||||
self.eef_debug_lineID = p.addUserDebugLine(
|
||||
np.array(eef_xyz[0]), end, [1, 0, 0])
|
||||
time.sleep(self.SIMULATION_STEP_DELAY)
|
||||
|
||||
@staticmethod
|
||||
def is_still(handle):
|
||||
still_eps = 1e-3
|
||||
lin_vel, ang_vel = p.getBaseVelocity(handle)
|
||||
# print(np.abs(lin_vel).sum() + np.abs(ang_vel).sum())
|
||||
return np.abs(lin_vel).sum() + np.abs(ang_vel).sum() < still_eps
|
||||
|
||||
def wait_until_still(self, objID, max_wait_epochs=1000):
|
||||
for _ in range(max_wait_epochs):
|
||||
self.step_simulation()
|
||||
if self.is_still(objID):
|
||||
return
|
||||
if self.debug:
|
||||
print('Warning: Not still after MAX_WAIT_EPOCHS = %d.' %
|
||||
max_wait_epochs)
|
||||
|
||||
def wait_until_all_still(self, max_wait_epochs=1000):
|
||||
for _ in range(max_wait_epochs):
|
||||
self.step_simulation()
|
||||
if np.all(list(self.is_still(obj_id) for obj_id in self.obj_ids)):
|
||||
return
|
||||
if self.debug:
|
||||
print('Warning: Not still after MAX_WAIT_EPOCHS = %d.' %
|
||||
max_wait_epochs)
|
||||
|
||||
def read_debug_parameter(self):
|
||||
# read the value of task parameter
|
||||
x = p.readUserDebugParameter(self.xin)
|
||||
y = p.readUserDebugParameter(self.yin)
|
||||
z = p.readUserDebugParameter(self.zin)
|
||||
roll = p.readUserDebugParameter(self.rollId)
|
||||
pitch = p.readUserDebugParameter(self.pitchId)
|
||||
yaw = p.readUserDebugParameter(self.yawId)
|
||||
gripper_opening_length = p.readUserDebugParameter(
|
||||
self.gripper_opening_length_control)
|
||||
|
||||
return x, y, z, roll, pitch, yaw, gripper_opening_length
|
||||
|
||||
def reset_robot(self):
|
||||
user_parameters = (-1.5690622952052096, -1.5446774605904932, 1.343946009733127, -1.3708613585093699,
|
||||
-1.5707970583733368, 0.0009377758247187636, 0.085)
|
||||
for _ in range(60):
|
||||
for i, name in enumerate(self.controlJoints):
|
||||
if i == 6:
|
||||
self.controlGripper(
|
||||
controlMode=p.POSITION_CONTROL, targetPosition=user_parameters[i])
|
||||
break
|
||||
joint = self.joints[name]
|
||||
# control robot joints
|
||||
p.setJointMotorControl2(self.robot_id, joint.id, p.POSITION_CONTROL,
|
||||
targetPosition=user_parameters[i], force=joint.maxForce,
|
||||
maxVelocity=joint.maxVelocity)
|
||||
self.step_simulation()
|
||||
|
||||
def move_away_arm(self):
|
||||
joint = self.joints['shoulder_pan_joint']
|
||||
for _ in range(200):
|
||||
p.setJointMotorControl2(self.robot_id, joint.id, p.POSITION_CONTROL,
|
||||
targetPosition=0., force=joint.maxForce,
|
||||
maxVelocity=joint.maxVelocity)
|
||||
self.step_simulation()
|
||||
|
||||
def check_grasped(self):
|
||||
left_index = self.joints['left_inner_finger_pad_joint'].id
|
||||
right_index = self.joints['right_inner_finger_pad_joint'].id
|
||||
|
||||
contact_left = p.getContactPoints(
|
||||
bodyA=self.robot_id, linkIndexA=left_index)
|
||||
contact_right = p.getContactPoints(
|
||||
bodyA=self.robot_id, linkIndexA=right_index)
|
||||
contact_ids = set(item[2] for item in contact_left +
|
||||
contact_right if item[2] in [self.obj_id])
|
||||
if len(contact_ids) == 1:
|
||||
return True
|
||||
return False
|
||||
|
||||
def check_grasped_id(self):
|
||||
left_index = self.joints['left_inner_finger_pad_joint'].id
|
||||
right_index = self.joints['right_inner_finger_pad_joint'].id
|
||||
|
||||
contact_left = p.getContactPoints(
|
||||
bodyA=self.robot_id, linkIndexA=left_index)
|
||||
contact_right = p.getContactPoints(
|
||||
bodyA=self.robot_id, linkIndexA=right_index)
|
||||
contact_ids = set(item[2] for item in contact_left +
|
||||
contact_right if item[2] in self.obj_ids)
|
||||
if len(contact_ids) > 1:
|
||||
if self.debug:
|
||||
print('Warning: Multiple items in hand!')
|
||||
return list(item_id for item_id in contact_ids if item_id in self.obj_ids)
|
||||
|
||||
def check_contact(self, id_a, id_b):
|
||||
contact_a = p.getContactPoints(bodyA=id_a)
|
||||
contact_ids = set(item[2] for item in contact_a if item[2] in [id_b])
|
||||
if len(contact_ids) == 1:
|
||||
return True
|
||||
return False
|
||||
|
||||
def check_target_reached(self, obj_id):
|
||||
aabb = p.getAABB(self.target_id, -1)
|
||||
x_min, x_max = aabb[0][0], aabb[1][0]
|
||||
y_min, y_max = aabb[0][1], aabb[1][1]
|
||||
pos = p.getBasePositionAndOrientation(obj_id)
|
||||
x, y = pos[0][0], pos[0][1]
|
||||
if x > x_min and x < x_max and y > y_min and y < y_max:
|
||||
return True
|
||||
return False
|
||||
|
||||
def gripper_contact(self, bool_operator='and', force=100):
|
||||
left_index = self.joints['left_inner_finger_pad_joint'].id
|
||||
right_index = self.joints['right_inner_finger_pad_joint'].id
|
||||
|
||||
contact_left = p.getContactPoints(
|
||||
bodyA=self.robot_id, linkIndexA=left_index)
|
||||
contact_right = p.getContactPoints(
|
||||
bodyA=self.robot_id, linkIndexA=right_index)
|
||||
|
||||
if bool_operator == 'and' and not (contact_right and contact_left):
|
||||
return False
|
||||
|
||||
# Check the force
|
||||
left_force = p.getJointState(self.robot_id, left_index)[
|
||||
2][:3] # 6DOF, Torque is ignored
|
||||
right_force = p.getJointState(self.robot_id, right_index)[2][:3]
|
||||
left_norm, right_norm = np.linalg.norm(
|
||||
left_force), np.linalg.norm(right_force)
|
||||
# print(left_norm, right_norm)
|
||||
if bool_operator == 'and':
|
||||
return left_norm > force and right_norm > force
|
||||
else:
|
||||
return left_norm > force or right_norm > force
|
||||
|
||||
def move_gripper(self, gripper_opening_length: float, step: int = 120):
|
||||
gripper_opening_length = np.clip(
|
||||
gripper_opening_length, *self.gripper_open_limit)
|
||||
gripper_opening_angle = 0.715 - \
|
||||
math.asin((gripper_opening_length - 0.010) /
|
||||
0.1143) # angle calculation
|
||||
for _ in range(step):
|
||||
self.controlGripper(controlMode=p.POSITION_CONTROL,
|
||||
targetPosition=gripper_opening_angle)
|
||||
self.step_simulation()
|
||||
|
||||
def auto_close_gripper(self, step: int = 120, check_contact: bool = False) -> bool:
|
||||
# Get initial gripper open position
|
||||
initial_position = p.getJointState(
|
||||
self.robot_id, self.joints[self.mimicParentName].id)[0]
|
||||
initial_position = math.sin(0.715 - initial_position) * 0.1143 + 0.010
|
||||
for step_idx in range(1, step):
|
||||
current_target_open_length = initial_position - step_idx / step * initial_position
|
||||
|
||||
self.move_gripper(current_target_open_length, 1)
|
||||
if current_target_open_length < 1e-5:
|
||||
return False
|
||||
|
||||
# time.sleep(1 / 120)
|
||||
if check_contact and self.gripper_contact():
|
||||
return True
|
||||
return False
|
||||
|
||||
def calc_z_offset(self, gripper_opening_length: float):
|
||||
gripper_opening_length = np.clip(
|
||||
gripper_opening_length, *self.gripper_open_limit)
|
||||
gripper_opening_angle = 0.715 - \
|
||||
math.asin((gripper_opening_length - 0.010) / 0.1143)
|
||||
if self.gripper_type == '140':
|
||||
gripper_length = 10.3613 * \
|
||||
np.sin(1.64534-0.24074 * (gripper_opening_angle / np.pi)) - 10.1219
|
||||
else:
|
||||
gripper_length = 1.231 - 1.1
|
||||
return gripper_length
|
||||
|
||||
def remove_obj(self, obj_id):
|
||||
# Get index of obj in id list, then remove object from simulation
|
||||
idx = self.obj_ids.index(obj_id)
|
||||
self.obj_orientations.pop(idx)
|
||||
self.obj_positions.pop(idx)
|
||||
self.obj_ids.pop(idx)
|
||||
p.removeBody(obj_id)
|
||||
|
||||
def remove_all_obj(self):
|
||||
self.obj_positions.clear()
|
||||
self.obj_orientations.clear()
|
||||
for obj_id in self.obj_ids:
|
||||
p.removeBody(obj_id)
|
||||
self.obj_ids.clear()
|
||||
|
||||
def reset_all_obj(self):
|
||||
for i, obj_id in enumerate(self.obj_ids):
|
||||
p.resetBasePositionAndOrientation(
|
||||
obj_id, self.obj_positions[i], self.obj_orientations[i])
|
||||
self.wait_until_all_still()
|
||||
|
||||
def update_obj_states(self):
|
||||
for i, obj_id in enumerate(self.obj_ids):
|
||||
pos, orn = p.getBasePositionAndOrientation(obj_id)
|
||||
self.obj_positions[i] = pos
|
||||
self.obj_orientations[i] = orn
|
||||
|
||||
def load_obj(self, path, pos, yaw, mod_orn=False, mod_stiffness=False):
|
||||
orn = p.getQuaternionFromEuler([0, 0, yaw])
|
||||
obj_id = p.loadURDF(path, pos, orn)
|
||||
# adjust position according to height
|
||||
aabb = p.getAABB(obj_id, -1)
|
||||
if mod_orn:
|
||||
minm, maxm = aabb[0][1], aabb[1][1]
|
||||
orn = p.getQuaternionFromEuler([0, np.pi*0.5, yaw])
|
||||
else:
|
||||
minm, maxm = aabb[0][2], aabb[1][2]
|
||||
|
||||
pos[2] += (maxm - minm) / 2
|
||||
p.resetBasePositionAndOrientation(obj_id, pos, orn)
|
||||
# change dynamics
|
||||
if mod_stiffness:
|
||||
p.changeDynamics(obj_id,
|
||||
-1, lateralFriction=1,
|
||||
rollingFriction=0.001,
|
||||
spinningFriction=0.002,
|
||||
restitution=0.01,
|
||||
contactStiffness=100000,
|
||||
contactDamping=0.0)
|
||||
else:
|
||||
p.changeDynamics(obj_id,
|
||||
-1, lateralFriction=1,
|
||||
rollingFriction=0.002,
|
||||
spinningFriction=0.001,
|
||||
restitution=0.01)
|
||||
self.obj_ids.append(obj_id)
|
||||
self.obj_positions.append(pos)
|
||||
self.obj_orientations.append(orn)
|
||||
return obj_id, pos, orn
|
||||
|
||||
def load_isolated_obj(self, path, mod_orn=False, mod_stiffness=False):
|
||||
r_x = random.uniform(
|
||||
self.obj_init_pos[0] - 0.1, self.obj_init_pos[0] + 0.1)
|
||||
r_y = random.uniform(
|
||||
self.obj_init_pos[1] - 0.1, self.obj_init_pos[1] + 0.1)
|
||||
yaw = random.uniform(0, np.pi)
|
||||
|
||||
pos = [r_x, r_y, self.Z_TABLE_TOP]
|
||||
obj_id, _, _ = self.load_obj(path, pos, yaw, mod_orn, mod_stiffness)
|
||||
for _ in range(10):
|
||||
self.step_simulation()
|
||||
self.wait_until_still(obj_id)
|
||||
self.update_obj_states()
|
||||
|
||||
def create_temp_box(self, width, num):
|
||||
box_width = width
|
||||
box_height = 0.2
|
||||
box_z = self.Z_TABLE_TOP + (box_height/2)
|
||||
id1 = p.loadURDF(f'environment/urdf/objects/slab{num}.urdf',
|
||||
[self.obj_init_pos[0] - box_width /
|
||||
2, self.obj_init_pos[1], box_z],
|
||||
p.getQuaternionFromEuler([0, 0, 0]),
|
||||
useFixedBase=True)
|
||||
id2 = p.loadURDF(f'environment/urdf/objects/slab{num}.urdf',
|
||||
[self.obj_init_pos[0] + box_width /
|
||||
2, self.obj_init_pos[1], box_z],
|
||||
p.getQuaternionFromEuler([0, 0, 0]),
|
||||
useFixedBase=True)
|
||||
id3 = p.loadURDF(f'environment/urdf/objects/slab{num}.urdf',
|
||||
[self.obj_init_pos[0], self.obj_init_pos[1] +
|
||||
box_width/2, box_z],
|
||||
p.getQuaternionFromEuler([0, 0, np.pi*0.5]),
|
||||
useFixedBase=True)
|
||||
id4 = p.loadURDF(f'environment/urdf/objects/slab{num}.urdf',
|
||||
[self.obj_init_pos[0], self.obj_init_pos[1] -
|
||||
box_width/2, box_z],
|
||||
p.getQuaternionFromEuler([0, 0, np.pi*0.5]),
|
||||
useFixedBase=True)
|
||||
return [id1, id2, id3, id4]
|
||||
|
||||
def create_pile(self, obj_info):
|
||||
box_ids = self.create_temp_box(0.30, 1)
|
||||
for path, mod_orn, mod_stiffness in obj_info:
|
||||
margin = 0.025
|
||||
r_x = random.uniform(
|
||||
self.obj_init_pos[0] - margin, self.obj_init_pos[0] + margin)
|
||||
r_y = random.uniform(
|
||||
self.obj_init_pos[1] - margin, self.obj_init_pos[1] + margin)
|
||||
yaw = random.uniform(0, np.pi)
|
||||
pos = [r_x, r_y, 1.0]
|
||||
|
||||
obj_id, _, _ = self.load_obj(
|
||||
path, pos, yaw, mod_orn, mod_stiffness)
|
||||
for _ in range(10):
|
||||
self.step_simulation()
|
||||
self.wait_until_still(obj_id, 150)
|
||||
|
||||
self.wait_until_all_still()
|
||||
for handle in box_ids:
|
||||
p.removeBody(handle)
|
||||
box_ids = self.create_temp_box(0.45, 2)
|
||||
self.wait_until_all_still()
|
||||
for handle in box_ids:
|
||||
p.removeBody(handle)
|
||||
self.wait_until_all_still()
|
||||
self.update_obj_states()
|
||||
|
||||
def move_obj_along_axis(self, obj_id, axis, operator, step, stop):
|
||||
collison = False
|
||||
while not collison:
|
||||
pos, orn = p.getBasePositionAndOrientation(obj_id)
|
||||
new_pos = list(pos)
|
||||
if operator == '+':
|
||||
new_pos[axis] += step
|
||||
if new_pos[axis] > stop:
|
||||
break
|
||||
else:
|
||||
new_pos[axis] -= step
|
||||
if new_pos[axis] < stop:
|
||||
break
|
||||
# Move object towards center
|
||||
p.resetBasePositionAndOrientation(obj_id, new_pos, orn)
|
||||
p.stepSimulation()
|
||||
contact_a = p.getContactPoints(obj_id)
|
||||
# If object collides with any other object, stop
|
||||
contact_ids = set(item[2]
|
||||
for item in contact_a if item[2] in self.obj_ids)
|
||||
if len(contact_ids) != 0:
|
||||
collison = True
|
||||
# Move one step back
|
||||
pos, orn = p.getBasePositionAndOrientation(obj_id)
|
||||
new_pos = list(pos)
|
||||
if operator == '+':
|
||||
new_pos[axis] -= step
|
||||
else:
|
||||
new_pos[axis] += step
|
||||
p.resetBasePositionAndOrientation(obj_id, new_pos, orn)
|
||||
|
||||
def create_packed(self, obj_info):
|
||||
init_x, init_y, init_z = self.obj_init_pos[0], self.obj_init_pos[1], self.Z_TABLE_TOP
|
||||
yaw = random.uniform(0, np.pi)
|
||||
path, mod_orn, mod_stiffness = obj_info[0]
|
||||
center_obj, _, _ = self.load_obj(
|
||||
path, [init_x, init_y, init_z], yaw, mod_orn, mod_stiffness)
|
||||
|
||||
margin = 0.3
|
||||
yaw = random.uniform(0, np.pi)
|
||||
path, mod_orn, mod_stiffness = obj_info[1]
|
||||
left_obj_id, _, _ = self.load_obj(
|
||||
path, [init_x-margin, init_y, init_z], yaw, mod_orn, mod_stiffness)
|
||||
yaw = random.uniform(0, np.pi)
|
||||
path, mod_orn, mod_stiffness = obj_info[2]
|
||||
top_obj_id, _, _ = self.load_obj(
|
||||
path, [init_x, init_y+margin, init_z], yaw, mod_orn, mod_stiffness)
|
||||
yaw = random.uniform(0, np.pi)
|
||||
path, mod_orn, mod_stiffness = obj_info[3]
|
||||
right_obj_id, _, _ = self.load_obj(
|
||||
path, [init_x+margin, init_y, init_z], yaw, mod_orn, mod_stiffness)
|
||||
yaw = random.uniform(0, np.pi)
|
||||
path, mod_orn, mod_stiffness = obj_info[4]
|
||||
down_obj_id, _, _ = self.load_obj(
|
||||
path, [init_x, init_y-margin, init_z], yaw, mod_orn, mod_stiffness)
|
||||
|
||||
self.wait_until_all_still()
|
||||
step = 0.01
|
||||
self.move_obj_along_axis(left_obj_id, 0, '+', step, init_x)
|
||||
self.move_obj_along_axis(top_obj_id, 1, '-', step, init_y)
|
||||
self.move_obj_along_axis(right_obj_id, 0, '-', step, init_x)
|
||||
self.move_obj_along_axis(down_obj_id, 1, '+', step, init_y)
|
||||
self.update_obj_states()
|
||||
|
||||
def move_ee(self, action, max_step=300, check_collision_config=None, custom_velocity=None,
|
||||
try_close_gripper=False, verbose=False):
|
||||
x, y, z, orn = action
|
||||
x = np.clip(x, *self.ee_position_limit[0])
|
||||
y = np.clip(y, *self.ee_position_limit[1])
|
||||
z = np.clip(z, *self.ee_position_limit[2])
|
||||
# set damping for robot arm and gripper
|
||||
jd = [0.01, 0.01, 0.01, 0.01, 0.01, 0.01,
|
||||
0.01, 0.01, 0.01, 0.01, 0.01, 0.01]
|
||||
jd = jd * 0
|
||||
still_open_flag_ = True # Hot fix
|
||||
for _ in range(max_step):
|
||||
# apply IK
|
||||
joint_poses = p.calculateInverseKinematics(self.robot_id, self.eef_id, [x, y, z], orn,
|
||||
maxNumIterations=100, jointDamping=jd
|
||||
)
|
||||
# Filter out the gripper
|
||||
for i, name in enumerate(self.controlJoints[:-1]):
|
||||
joint = self.joints[name]
|
||||
pose = joint_poses[i]
|
||||
# control robot end-effector
|
||||
p.setJointMotorControl2(self.robot_id, joint.id, p.POSITION_CONTROL,
|
||||
targetPosition=pose, force=joint.maxForce,
|
||||
maxVelocity=joint.maxVelocity if custom_velocity is None else custom_velocity * (i+1))
|
||||
|
||||
self.step_simulation()
|
||||
if try_close_gripper and still_open_flag_ and not self.gripper_contact():
|
||||
still_open_flag_ = self.close_gripper(check_contact=True)
|
||||
# Check if contact with objects
|
||||
if check_collision_config and self.gripper_contact(**check_collision_config):
|
||||
if self.debug:
|
||||
print('Collision detected!', self.check_grasped_id())
|
||||
return False, p.getLinkState(self.robot_id, self.eef_id)[0:2]
|
||||
# Check xyz and rpy error
|
||||
real_xyz, real_xyzw = p.getLinkState(
|
||||
self.robot_id, self.eef_id)[0:2]
|
||||
roll, pitch, yaw = p.getEulerFromQuaternion(orn)
|
||||
real_roll, real_pitch, real_yaw = p.getEulerFromQuaternion(
|
||||
real_xyzw)
|
||||
if np.linalg.norm(np.array((x, y, z)) - real_xyz) < 0.001 \
|
||||
and np.abs((roll - real_roll, pitch - real_pitch, yaw - real_yaw)).sum() < 0.001:
|
||||
if verbose:
|
||||
print('Reach target with', _, 'steps')
|
||||
return True, (real_xyz, real_xyzw)
|
||||
|
||||
# raise FailToReachTargetError
|
||||
if self.debug:
|
||||
print('Failed to reach the target')
|
||||
return False, p.getLinkState(self.robot_id, self.eef_id)[0:2]
|
||||
|
||||
def grasp(self, pos: tuple, roll: float, gripper_opening_length: float, obj_height: float, debug: bool = False):
|
||||
"""
|
||||
Method to perform grasp
|
||||
pos [x y z]: The axis in real-world coordinate
|
||||
roll: float, for grasp, it should be in [-pi/2, pi/2)
|
||||
"""
|
||||
succes_grasp, succes_target = False, False
|
||||
grasped_obj_id = None
|
||||
|
||||
x, y, z = pos
|
||||
# Substracht gripper finger length from z
|
||||
z -= self.finger_length
|
||||
z = np.clip(z, *self.ee_position_limit[2])
|
||||
|
||||
# Move above target
|
||||
self.reset_robot()
|
||||
self.move_gripper(0.1)
|
||||
orn = p.getQuaternionFromEuler([roll, np.pi/2, 0.0])
|
||||
self.move_ee([x, y, self.GRIPPER_MOVING_HEIGHT, orn])
|
||||
|
||||
# Reduce grip to get a tighter grip
|
||||
gripper_opening_length *= self.GRIP_REDUCTION
|
||||
|
||||
# Grasp and lift object
|
||||
z_offset = self.calc_z_offset(gripper_opening_length)
|
||||
self.move_ee([x, y, z + z_offset, orn])
|
||||
# self.move_gripper(gripper_opening_length)
|
||||
self.auto_close_gripper(check_contact=True)
|
||||
for _ in range(15):
|
||||
self.step_simulation()
|
||||
self.move_ee([x, y, self.GRIPPER_MOVING_HEIGHT, orn])
|
||||
|
||||
# If the object has been grasped and lifted off the table
|
||||
grasped_id = self.check_grasped_id()
|
||||
if len(grasped_id) == 1:
|
||||
succes_grasp = True
|
||||
grasped_obj_id = grasped_id[0]
|
||||
else:
|
||||
return succes_target, succes_grasp
|
||||
|
||||
# Move object to target zone
|
||||
y_drop = self.TARGET_ZONE_POS[2] + z_offset + obj_height + 0.15
|
||||
y_orn = p.getQuaternionFromEuler([-np.pi*0.25, np.pi/2, 0.0])
|
||||
|
||||
self.move_away_arm()
|
||||
self.move_ee([self.TARGET_ZONE_POS[0],
|
||||
self.TARGET_ZONE_POS[1], 1.25, y_orn])
|
||||
self.move_ee([self.TARGET_ZONE_POS[0],
|
||||
self.TARGET_ZONE_POS[1], y_drop, y_orn])
|
||||
self.move_gripper(0.085)
|
||||
self.move_ee([self.TARGET_ZONE_POS[0], self.TARGET_ZONE_POS[1],
|
||||
self.GRIPPER_MOVING_HEIGHT, y_orn])
|
||||
|
||||
# Wait then check if object is in target zone
|
||||
for _ in range(50):
|
||||
self.step_simulation()
|
||||
if self.check_target_reached(grasped_obj_id):
|
||||
succes_target = True
|
||||
self.remove_obj(grasped_obj_id)
|
||||
|
||||
return succes_grasp, succes_target
|
||||
|
||||
def close(self):
|
||||
p.disconnect(self.physicsClient)
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
File diff suppressed because one or more lines are too long
File diff suppressed because it is too large
Load Diff
File diff suppressed because one or more lines are too long
File diff suppressed because it is too large
Load Diff
File diff suppressed because one or more lines are too long
File diff suppressed because it is too large
Load Diff
File diff suppressed because one or more lines are too long
File diff suppressed because it is too large
Load Diff
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
|
|
@ -0,0 +1,104 @@
|
|||
<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
|
||||
<asset>
|
||||
<created>2016-07-17T22:25:43.361178</created>
|
||||
<modified>2016-07-17T22:25:43.361188</modified>
|
||||
<up_axis>Z_UP</up_axis>
|
||||
</asset>
|
||||
<library_effects>
|
||||
<effect name="effect0" id="effect0">
|
||||
<profile_COMMON>
|
||||
<technique sid="common">
|
||||
<phong>
|
||||
<emission>
|
||||
<color>0.0 0.0 0.0 1.0</color>
|
||||
</emission>
|
||||
<ambient>
|
||||
<color>0.0 0.0 0.0 1.0</color>
|
||||
</ambient>
|
||||
<diffuse>
|
||||
<color>0.7 0.7 0.7 1.0</color>
|
||||
</diffuse>
|
||||
<specular>
|
||||
<color>1 1 1 1.0</color>
|
||||
</specular>
|
||||
<shininess>
|
||||
<float>0.0</float>
|
||||
</shininess>
|
||||
<reflective>
|
||||
<color>0.0 0.0 0.0 1.0</color>
|
||||
</reflective>
|
||||
<reflectivity>
|
||||
<float>0.0</float>
|
||||
</reflectivity>
|
||||
<transparent>
|
||||
<color>0.0 0.0 0.0 1.0</color>
|
||||
</transparent>
|
||||
<transparency>
|
||||
<float>1.0</float>
|
||||
</transparency>
|
||||
</phong>
|
||||
</technique>
|
||||
<extra>
|
||||
<technique profile="GOOGLEEARTH">
|
||||
<double_sided>0</double_sided>
|
||||
</technique>
|
||||
</extra>
|
||||
</profile_COMMON>
|
||||
</effect>
|
||||
</library_effects>
|
||||
<library_geometries>
|
||||
<geometry id="geometry0" name="Part__Feature001">
|
||||
<mesh>
|
||||
<source id="cubenormals-array0">
|
||||
<float_array count="36" id="cubenormals-array0-array">4.38531e-14 -1 -4.451336e-05 4.38531e-14 -1 -4.451336e-05 -4.38531e-14 1 4.451336e-05 -4.38531e-14 1 4.451336e-05 -1 -4.385301e-14 -2.011189e-15 -1 -4.385301e-14 -2.011189e-15 -2.009237e-15 -4.451336e-05 1 -2.009237e-15 -4.451336e-05 1 1 4.385301e-14 2.011189e-15 1 4.385301e-14 2.011189e-15 2.009237e-15 4.451336e-05 -1 2.009237e-15 4.451336e-05 -1</float_array>
|
||||
<technique_common>
|
||||
<accessor count="12" source="#cubenormals-array0-array" stride="3">
|
||||
<param type="float" name="X"/>
|
||||
<param type="float" name="Y"/>
|
||||
<param type="float" name="Z"/>
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<source id="cubeverts-array0">
|
||||
<float_array count="24" id="cubeverts-array0-array">-10 -23.90175 13.51442 10 -23.9033 48.51442 -10 -23.9033 48.51442 10 -23.90175 13.51442 -10 -18.90175 13.51464 -10 -18.9033 48.51464 10 -18.90175 13.51464 10 -18.9033 48.51464</float_array>
|
||||
<technique_common>
|
||||
<accessor count="8" source="#cubeverts-array0-array" stride="3">
|
||||
<param type="float" name="X"/>
|
||||
<param type="float" name="Y"/>
|
||||
<param type="float" name="Z"/>
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<vertices id="cubeverts-array0-vertices">
|
||||
<input source="#cubeverts-array0" semantic="POSITION"/>
|
||||
</vertices>
|
||||
<triangles count="12" material="materialref">
|
||||
<input source="#cubenormals-array0" semantic="NORMAL" offset="1"/>
|
||||
<input source="#cubeverts-array0-vertices" semantic="VERTEX" offset="0"/>
|
||||
<p>0 0 1 0 2 0 3 1 1 1 0 1 4 2 5 2 6 2 5 3 7 3 6 3 2 4 5 4 4 4 2 5 4 5 0 5 5 6 2 6 1 6 5 7 1 7 7 7 7 8 1 8 6 8 1 9 3 9 6 9 0 10 4 10 3 10 4 11 6 11 3 11</p>
|
||||
</triangles>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</library_geometries>
|
||||
<library_materials>
|
||||
<material name="mymaterial" id="material0">
|
||||
<instance_effect url="#effect0"/>
|
||||
</material>
|
||||
</library_materials>
|
||||
<library_visual_scenes>
|
||||
<visual_scene id="myscene">
|
||||
<node name="node0" id="node0">
|
||||
<instance_geometry url="#geometry0">
|
||||
<bind_material>
|
||||
<technique_common>
|
||||
<instance_material symbol="materialref" target="#material0"/>
|
||||
</technique_common>
|
||||
</bind_material>
|
||||
</instance_geometry>
|
||||
</node>
|
||||
</visual_scene>
|
||||
</library_visual_scenes>
|
||||
<scene>
|
||||
<instance_visual_scene url="#myscene"/>
|
||||
</scene>
|
||||
</COLLADA>
|
||||
Binary file not shown.
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,23 @@
|
|||
Copyright (c) 2013, ROS-Industrial
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without modification,
|
||||
are permitted provided that the following conditions are met:
|
||||
|
||||
Redistributions of source code must retain the above copyright notice, this
|
||||
list of conditions and the following disclaimer.
|
||||
|
||||
Redistributions in binary form must reproduce the above copyright notice, this
|
||||
list of conditions and the following disclaimer in the documentation and/or
|
||||
other materials provided with the distribution.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
|
||||
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
|
||||
ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
|
|
@ -0,0 +1,30 @@
|
|||
<robot name="block">
|
||||
<link name="block">
|
||||
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" />
|
||||
<mass value="0.1" />
|
||||
<inertia ixx="0.00003375" ixy="0.0" ixz="0.0" iyy="0.00003375" iyz="0.0" izz="0.00003375" />
|
||||
</inertial>
|
||||
|
||||
<visual>
|
||||
<origin xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.085 0.085 0.045" />
|
||||
</geometry>
|
||||
<material name="red">
|
||||
<color rgba="0.4 0.4 1.0 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.085 0.085 0.045" />
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
</link>
|
||||
|
||||
|
||||
</robot>
|
||||
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
|
|
@ -0,0 +1,27 @@
|
|||
<robot name="slab">
|
||||
<link name="slab">
|
||||
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" />
|
||||
<mass value="0.1" />
|
||||
<inertia ixx="0.00003375" ixy="0.0" ixz="0.0" iyy="0.00003375" iyz="0.0" izz="0.00003375" />
|
||||
</inertial>
|
||||
|
||||
<visual>
|
||||
<origin xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.02 0.35 0.2" />
|
||||
</geometry>
|
||||
<material name="white">
|
||||
<color rgba="1.0 1.0 1.0 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.02 0.35 0.2" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
</robot>
|
||||
|
|
@ -0,0 +1,27 @@
|
|||
<robot name="slab2">
|
||||
<link name="slab2">
|
||||
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" />
|
||||
<mass value="0.1" />
|
||||
<inertia ixx="0.00003375" ixy="0.0" ixz="0.0" iyy="0.00003375" iyz="0.0" izz="0.00003375" />
|
||||
</inertial>
|
||||
|
||||
<visual>
|
||||
<origin xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.02 0.50 0.2" />
|
||||
</geometry>
|
||||
<material name="white">
|
||||
<color rgba="1.0 1.0 1.0 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.02 0.50 0.2" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
</robot>
|
||||
|
|
@ -0,0 +1,37 @@
|
|||
<robot name="table">
|
||||
<link name="table">
|
||||
|
||||
<!-- rosrun gazebo_ros spawn_model -file $(rospack find ur5_single_arm_tufts)/urdf/objects/table.urdf -urdf -x 0.5 -y -0 -z 0.75 -model table -->
|
||||
|
||||
<inertial>
|
||||
<origin xyz="0.23 0 0.755" rpy="0 0 0" />
|
||||
<mass value="1"/>
|
||||
<inertia
|
||||
ixx="0.07" ixy="0.0" ixz="0.0"
|
||||
iyy="0.40" iyz="0.0"
|
||||
izz="0.33"/>
|
||||
</inertial>
|
||||
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="1.0 0.65 0.05"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="1.0 0.65 0.05"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
</link>
|
||||
|
||||
<gazebo reference="table">
|
||||
<material>Gazebo/Wood</material>
|
||||
</gazebo>
|
||||
|
||||
<gazebo>
|
||||
<static>true</static>
|
||||
</gazebo>
|
||||
|
||||
</robot>
|
||||
|
|
@ -0,0 +1,37 @@
|
|||
<robot name="table">
|
||||
<link name="table">
|
||||
|
||||
<!-- rosrun gazebo_ros spawn_model -file $(rospack find ur5_single_arm_tufts)/urdf/objects/table.urdf -urdf -x 0.5 -y -0 -z 0.75 -model table -->
|
||||
|
||||
<inertial>
|
||||
<origin xyz="0.23 0 0.755" rpy="0 0 0" />
|
||||
<mass value="1"/>
|
||||
<inertia
|
||||
ixx="0.07" ixy="0.0" ixz="0.0"
|
||||
iyy="0.40" iyz="0.0"
|
||||
izz="0.33"/>
|
||||
</inertial>
|
||||
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.5 0.7 0.05"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.5 0.7 0.05"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
</link>
|
||||
|
||||
<gazebo reference="table">
|
||||
<material>Gazebo/Wood</material>
|
||||
</gazebo>
|
||||
|
||||
<gazebo>
|
||||
<static>true</static>
|
||||
</gazebo>
|
||||
|
||||
</robot>
|
||||
|
|
@ -0,0 +1,27 @@
|
|||
<robot name="zone">
|
||||
<link name="zone">
|
||||
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" />
|
||||
<mass value="0.1" />
|
||||
<inertia ixx="0.00003375" ixy="0.0" ixz="0.0" iyy="0.00003375" iyz="0.0" izz="0.00003375" />
|
||||
</inertial>
|
||||
|
||||
<visual>
|
||||
<origin xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.2 0.2 0.0005" />
|
||||
</geometry>
|
||||
<material name="red">
|
||||
<color rgba="1 0 0 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.2 0.2 0.0005"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
</link>
|
||||
</robot>
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 361 KiB |
|
|
@ -0,0 +1,13 @@
|
|||
# Blender MTL File: 'None'
|
||||
# Material Count: 1
|
||||
|
||||
newmtl None
|
||||
Ns 0.000000
|
||||
Ka 0.000000 0.000000 0.000000
|
||||
Kd 0.800000 0.800000 0.800000
|
||||
Ks 0.800000 0.800000 0.800000
|
||||
Ke 0.000000 0.000000 0.000000
|
||||
Ni 1.000000
|
||||
d 1.000000
|
||||
illum 2
|
||||
map_Kd tray.jpg
|
||||
|
|
@ -0,0 +1,213 @@
|
|||
# Blender v2.78 (sub 0) OBJ File: ''
|
||||
# www.blender.org
|
||||
mtllib tray_textured.mtl
|
||||
o edge_4_Cube.001
|
||||
v -0.573309 0.580000 0.261247
|
||||
v -0.426691 0.419400 -0.002214
|
||||
v -0.590083 0.580000 0.250354
|
||||
v -0.573309 -0.580000 0.261247
|
||||
v -0.409917 0.419400 0.008679
|
||||
v -0.590083 -0.580000 0.250354
|
||||
v -0.409917 -0.419400 0.009162
|
||||
v -0.426691 -0.419400 -0.001731
|
||||
vt 0.9046 0.2397
|
||||
vt 0.7929 0.2434
|
||||
vt 0.9174 0.2393
|
||||
vt 0.9537 0.7559
|
||||
vt 0.7801 0.2438
|
||||
vt 0.9664 0.7554
|
||||
vt 0.8291 0.7599
|
||||
vt 0.8419 0.7595
|
||||
vn 0.2565 0.8821 -0.3950
|
||||
vn 0.8392 0.0002 0.5438
|
||||
vn 0.8395 0.0000 0.5433
|
||||
vn 0.8396 -0.0000 0.5432
|
||||
vn 0.2568 -0.8819 -0.3954
|
||||
vn -0.8396 -0.0002 -0.5433
|
||||
vn -0.8392 -0.0000 -0.5438
|
||||
vn -0.8391 0.0000 -0.5439
|
||||
vn 0.5446 -0.0005 -0.8387
|
||||
vn -0.5446 -0.0000 0.8387
|
||||
vn 0.8391 0.0003 0.5440
|
||||
vn -0.8397 -0.0003 -0.5430
|
||||
usemtl None
|
||||
s 1
|
||||
f 1/1/1 2/2/1 3/3/1
|
||||
f 4/4/2 5/5/3 1/1/4
|
||||
f 6/6/5 7/7/5 4/4/5
|
||||
f 3/3/6 8/8/7 6/6/8
|
||||
f 5/5/9 8/8/9 2/2/9
|
||||
f 4/4/10 3/3/10 6/6/10
|
||||
f 1/1/1 5/5/1 2/2/1
|
||||
f 4/4/2 7/7/11 5/5/3
|
||||
f 6/6/5 8/8/5 7/7/5
|
||||
f 3/3/6 2/2/12 8/8/7
|
||||
f 5/5/9 7/7/9 8/8/9
|
||||
f 4/4/10 1/1/10 3/3/10
|
||||
o edge_1_Cube.003
|
||||
v 0.580000 0.590083 0.250354
|
||||
v -0.419960 0.426691 -0.001860
|
||||
v -0.580000 0.590083 0.250354
|
||||
v 0.580000 0.573309 0.261247
|
||||
v 0.420014 0.426691 -0.001059
|
||||
v -0.580000 0.573309 0.261247
|
||||
v 0.420014 0.409917 0.009834
|
||||
v -0.419960 0.409917 0.009033
|
||||
vt 0.8346 0.9187
|
||||
vt 0.2203 0.8574
|
||||
vt 0.1480 0.9187
|
||||
vt 0.8346 0.9129
|
||||
vt 0.7623 0.8574
|
||||
vt 0.1480 0.9129
|
||||
vt 0.7623 0.8511
|
||||
vt 0.2203 0.8511
|
||||
vn 0.0004 0.8386 -0.5448
|
||||
vn 0.0001 0.8391 -0.5439
|
||||
vn 0.0000 0.8393 -0.5437
|
||||
vn 0.8823 -0.2564 -0.3948
|
||||
vn -0.0004 -0.8392 0.5439
|
||||
vn -0.0001 -0.8386 0.5447
|
||||
vn 0.0000 -0.8385 0.5449
|
||||
vn -0.8826 -0.2560 -0.3942
|
||||
vn 0.0008 -0.5446 -0.8387
|
||||
vn 0.0000 0.5446 0.8387
|
||||
vn 0.0005 0.8383 -0.5452
|
||||
vn -0.0005 -0.8394 0.5435
|
||||
usemtl None
|
||||
s 1
|
||||
f 9/9/13 10/10/14 11/11/15
|
||||
f 12/12/16 13/13/16 9/9/16
|
||||
f 14/14/17 15/15/18 12/12/19
|
||||
f 11/11/20 16/16/20 14/14/20
|
||||
f 13/13/21 16/16/21 10/10/21
|
||||
f 12/12/22 11/11/22 14/14/22
|
||||
f 9/9/13 13/13/23 10/10/14
|
||||
f 12/12/16 15/15/16 13/13/16
|
||||
f 14/14/17 16/16/24 15/15/18
|
||||
f 11/11/20 10/10/20 16/16/20
|
||||
f 13/13/21 15/15/21 16/16/21
|
||||
f 12/12/22 9/9/22 11/11/22
|
||||
o edge_2_Cube
|
||||
v 0.590083 0.580000 0.250354
|
||||
v 0.409917 0.420060 0.009390
|
||||
v 0.573309 0.580000 0.261247
|
||||
v 0.590083 -0.580000 0.250354
|
||||
v 0.426691 0.420060 -0.001503
|
||||
v 0.573309 -0.580000 0.261247
|
||||
v 0.426691 -0.419158 -0.002053
|
||||
v 0.409917 -0.419158 0.008840
|
||||
vt 0.9410 0.8520
|
||||
vt 0.7523 0.8566
|
||||
vt 0.9234 0.8524
|
||||
vt 0.8896 0.1426
|
||||
vt 0.7698 0.8562
|
||||
vt 0.8721 0.1430
|
||||
vt 0.7185 0.1468
|
||||
vt 0.7009 0.1472
|
||||
vn -0.2561 0.8826 -0.3943
|
||||
vn 0.8394 0.0003 -0.5435
|
||||
vn 0.8390 0.0001 -0.5441
|
||||
vn 0.8389 0.0000 -0.5443
|
||||
vn -0.2569 -0.8818 -0.3956
|
||||
vn -0.8390 -0.0003 0.5441
|
||||
vn -0.8394 -0.0001 0.5436
|
||||
vn -0.8395 -0.0000 0.5434
|
||||
vn -0.5446 0.0005 -0.8387
|
||||
vn 0.5446 -0.0000 0.8387
|
||||
vn 0.8396 0.0004 -0.5433
|
||||
vn -0.8388 -0.0004 0.5444
|
||||
usemtl None
|
||||
s 1
|
||||
f 17/17/25 18/18/25 19/19/25
|
||||
f 20/20/26 21/21/27 17/17/28
|
||||
f 22/22/29 23/23/29 20/20/29
|
||||
f 19/19/30 24/24/31 22/22/32
|
||||
f 21/21/33 24/24/33 18/18/33
|
||||
f 20/20/34 19/19/34 22/22/34
|
||||
f 17/17/25 21/21/25 18/18/25
|
||||
f 20/20/26 23/23/35 21/21/27
|
||||
f 22/22/29 24/24/29 23/23/29
|
||||
f 19/19/30 18/18/36 24/24/31
|
||||
f 21/21/33 23/23/33 24/24/33
|
||||
f 20/20/34 17/17/34 19/19/34
|
||||
o base_Cube.004
|
||||
v 0.420000 0.420000 0.010000
|
||||
v -0.420000 0.420000 -0.010000
|
||||
v -0.420000 0.420000 0.010000
|
||||
v 0.420000 -0.420000 0.010000
|
||||
v 0.420000 0.420000 -0.010000
|
||||
v -0.420000 -0.420000 0.010000
|
||||
v 0.420000 -0.420000 -0.010000
|
||||
v -0.420000 -0.420000 -0.010000
|
||||
vt 0.7524 0.8072
|
||||
vt -0.3038 0.8371
|
||||
vt -0.3038 0.8371
|
||||
vt 0.7012 0.1905
|
||||
vt 0.7524 0.8072
|
||||
vt -0.3550 0.2204
|
||||
vt 0.7012 0.1905
|
||||
vt -0.3550 0.2204
|
||||
vn 0.0000 1.0000 0.0000
|
||||
vn 1.0000 -0.0000 0.0000
|
||||
vn 0.0000 -1.0000 0.0000
|
||||
vn -1.0000 -0.0000 0.0000
|
||||
vn 0.0000 0.0000 -1.0000
|
||||
vn 0.0000 -0.0000 1.0000
|
||||
usemtl None
|
||||
s 1
|
||||
f 25/25/37 26/26/37 27/27/37
|
||||
f 28/28/38 29/29/38 25/25/38
|
||||
f 30/30/39 31/31/39 28/28/39
|
||||
f 27/27/40 32/32/40 30/30/40
|
||||
f 29/29/41 32/32/41 26/26/41
|
||||
f 28/28/42 27/27/42 30/30/42
|
||||
f 25/25/37 29/29/37 26/26/37
|
||||
f 28/28/38 31/31/38 29/29/38
|
||||
f 30/30/39 32/32/39 31/31/39
|
||||
f 27/27/40 26/26/40 32/32/40
|
||||
f 29/29/41 31/31/41 32/32/41
|
||||
f 28/28/42 25/25/42 27/27/42
|
||||
o edge_3_Cube.002
|
||||
v 0.580000 -0.573309 0.261247
|
||||
v -0.419400 -0.409917 0.008678
|
||||
v -0.580000 -0.573309 0.261247
|
||||
v 0.580000 -0.590083 0.250354
|
||||
v 0.419883 -0.409917 0.009162
|
||||
v -0.580000 -0.590083 0.250354
|
||||
v 0.419883 -0.426691 -0.001731
|
||||
v -0.419400 -0.426691 -0.002215
|
||||
vt 0.8690 0.1040
|
||||
vt 0.1365 0.1739
|
||||
vt 0.0188 0.1040
|
||||
vt 0.8690 0.0968
|
||||
vt 0.7517 0.1739
|
||||
vt 0.0188 0.0968
|
||||
vt 0.7517 0.1668
|
||||
vt 0.1365 0.1668
|
||||
vn -0.0002 0.8392 0.5438
|
||||
vn -0.0000 0.8395 0.5433
|
||||
vn 0.0000 0.8396 0.5432
|
||||
vn 0.8825 0.2562 -0.3945
|
||||
vn 0.0002 -0.8396 -0.5433
|
||||
vn 0.0000 -0.8392 -0.5438
|
||||
vn 0.0000 -0.8391 -0.5439
|
||||
vn -0.8821 0.2565 -0.3950
|
||||
vn -0.8822 0.2565 -0.3950
|
||||
vn 0.0005 0.5446 -0.8387
|
||||
vn 0.0000 -0.5446 0.8387
|
||||
vn -0.0003 0.8391 0.5440
|
||||
vn 0.0003 -0.8397 -0.5430
|
||||
usemtl None
|
||||
s 1
|
||||
f 33/33/43 34/34/44 35/35/45
|
||||
f 36/36/46 37/37/46 33/33/46
|
||||
f 38/38/47 39/39/48 36/36/49
|
||||
f 35/35/50 40/40/51 38/38/51
|
||||
f 37/37/52 40/40/52 34/34/52
|
||||
f 36/36/53 35/35/53 38/38/53
|
||||
f 33/33/43 37/37/54 34/34/44
|
||||
f 36/36/46 39/39/46 37/37/46
|
||||
f 38/38/47 40/40/55 39/39/48
|
||||
f 35/35/50 34/34/51 40/40/51
|
||||
f 37/37/52 39/39/52 40/40/52
|
||||
f 36/36/53 33/33/53 35/35/53
|
||||
|
|
@ -0,0 +1,49 @@
|
|||
<robot name="tabletop">
|
||||
<link name="base_link">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value="0"/>
|
||||
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="tray_textured.obj" scale="0.5 0.5 0.5"/>
|
||||
</geometry>
|
||||
<material name="tray_material">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.005"/>
|
||||
<geometry>
|
||||
<box size=".6 .6 .02"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<collision>
|
||||
<origin rpy="0 0.575469961 0" xyz="0.25 0 0.059"/>
|
||||
<geometry>
|
||||
<box size=".02 .6 .15"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<collision>
|
||||
<origin rpy="0 -0.575469961 0" xyz="-0.25 0 0.059"/>
|
||||
<geometry>
|
||||
<box size=".02 .6 .15"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<collision>
|
||||
<origin rpy="0.575469961 0 0" xyz="0 -0.25 0.059"/>
|
||||
<geometry>
|
||||
<box size=".6 .02 .15"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<collision>
|
||||
<origin rpy="-0.575469961 0 0" xyz="0 0.25 0.059"/>
|
||||
<geometry>
|
||||
<box size=".6 .02 .15"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
</link>
|
||||
</robot>
|
||||
|
|
@ -0,0 +1,30 @@
|
|||
<robot name="ur5_stand">
|
||||
<link name="ur5_stand">
|
||||
|
||||
<inertial>
|
||||
<mass value="1.0" />
|
||||
<origin xyz="0 0 0" />
|
||||
<inertia ixx="0.0003375" ixy="0.0" ixz="0.0" iyy="0.0003375" iyz="0.0" izz="0.0003375" />
|
||||
</inertial>
|
||||
<collision name="collision">
|
||||
<geometry>
|
||||
<mesh filename="package://objects/mesh/UR_BANCHETTO1.dae" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name="visual">
|
||||
<geometry>
|
||||
<mesh filename="package://objects/mesh/UR_BANCHETTO1.dae" />
|
||||
</geometry>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<gazebo reference="surgical_tool">
|
||||
<material> Gazebo/WoodPallet</material>
|
||||
<mu1>1000</mu1>
|
||||
<mu2>1000</mu2>
|
||||
</gazebo>
|
||||
|
||||
</robot>
|
||||
|
|
@ -0,0 +1,434 @@
|
|||
<?xml version="1.0" ?>
|
||||
<!-- =================================================================================== -->
|
||||
<!-- | This document was autogenerated by xacro from ros2_mara_ws/src/robotiq_modular_gripper/robotiq_140_gripper_description/urdf/robotiq_140_world.urdf.xacro | -->
|
||||
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||
<!-- =================================================================================== -->
|
||||
<robot name="robotiq_gripper">
|
||||
<!-- control -->
|
||||
<link name="base_link">
|
||||
<inertial>
|
||||
<mass value="0" />
|
||||
<inertia ixx = "0" ixy = "0" ixz = "0"
|
||||
iyx = "0" iyy = "0" iyz = "0"
|
||||
izx = "0" izy = "0" izz = "0" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="center_x" type="prismatic">
|
||||
<parent link="base_link"/>
|
||||
<child link="y_control"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit effort="1000.0" lower="-0.5" upper="0.5" velocity="0.2"/>
|
||||
</joint>
|
||||
|
||||
<link name="y_control">
|
||||
<inertial>
|
||||
<mass value="0.1" />
|
||||
<inertia ixx = "0" ixy = "0" ixz = "0"
|
||||
iyx = "0" iyy = "0" iyz = "0"
|
||||
izx = "0" izy = "0" izz = "0" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="center_y" type="prismatic">
|
||||
<parent link="y_control"/>
|
||||
<child link="z_control"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit effort="1000.0" lower="-0.5" upper="0.5" velocity="0.2"/>
|
||||
</joint>
|
||||
|
||||
<link name="z_control">
|
||||
<inertial>
|
||||
<mass value="0.1" />
|
||||
<inertia ixx = "0" ixy = "0" ixz = "0"
|
||||
iyx = "0" iyy = "0" iyz = "0"
|
||||
izx = "0" izy = "0" izz = "0" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="center_z" type="prismatic">
|
||||
<parent link="z_control"/>
|
||||
<child link="yaw_control"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="1000.0" lower="-0.25" upper="1" velocity="0.2"/>
|
||||
</joint>
|
||||
|
||||
<link name="yaw_control">
|
||||
<inertial>
|
||||
<mass value="0.1" />
|
||||
<inertia ixx = "0" ixy = "0" ixz = "0"
|
||||
iyx = "0" iyy = "0" iyz = "0"
|
||||
izx = "0" izy = "0" izz = "0" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="gripper_yaw" type="revolute">
|
||||
<parent link="yaw_control"/>
|
||||
<child link="pitch_control"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit lower="-31.4" upper="31.4" velocity="3.14" effort="10000"/>
|
||||
|
||||
</joint>
|
||||
|
||||
<link name="pitch_control">
|
||||
<inertial>
|
||||
<mass value="0.1" />
|
||||
<inertia ixx = "0" ixy = "0" ixz = "0"
|
||||
iyx = "0" iyy = "0" iyz = "0"
|
||||
izx = "0" izy = "0" izz = "0" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="gripper_pitch" type="revolute">
|
||||
<parent link="pitch_control"/>
|
||||
<child link="roll_control"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit lower="-31.4" upper="31.4" velocity="3.14" effort="10000"/>
|
||||
|
||||
</joint>
|
||||
|
||||
<link name="roll_control">
|
||||
<inertial>
|
||||
<mass value="0.1" />
|
||||
<inertia ixx = "0" ixy = "0" ixz = "0"
|
||||
iyx = "0" iyy = "0" iyz = "0"
|
||||
izx = "0" izy = "0" izz = "0" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="gripper_roll" type="revolute">
|
||||
<parent link="roll_control"/>
|
||||
<child link="dummy_center_indicator_link"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit lower="-31.4" upper="31.4" velocity="3.14" effort="100000"/>
|
||||
</joint>
|
||||
|
||||
<link name="dummy_center_indicator_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.020 0.085 0.002"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<inertial>
|
||||
<mass value="0.1" />
|
||||
<inertia ixx = "0" ixy = "0" ixz = "0"
|
||||
iyx = "0" iyy = "0" iyz = "0"
|
||||
izx = "0" izy = "0" izz = "0" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="dummy_center_fixed_joint" type="fixed">
|
||||
<parent link="dummy_center_indicator_link"/>
|
||||
<child link="robotiq_140_robotiq_arg2f_base_link"/>
|
||||
<origin xyz="-0.137 0 0" />
|
||||
</joint>
|
||||
<!-- robot -->
|
||||
<!-- robotiq_85_base_link -->
|
||||
<link name="robotiq_140_robotiq_arg2f_base_link">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="-0.00013 0.00011 0.03284"/>
|
||||
<mass value="0.52954"/>
|
||||
<inertia ixx="573495E-9" ixy="-554E-9" ixz="2628E-9" iyy="681780E-9" iyz="607E-9" izz="483281E-9"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://robotiq_140_gripper_description/meshes/GRIPPER_base_axis.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="Gazebo/Black">
|
||||
<color rgba="0.1 0.1 0.1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://robotiq_140_gripper_description/meshes/GRIPPER_base_axis.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<gazebo reference="robotiq_140_robotiq_arg2f_base_link">
|
||||
<material>Gazebo/Black</material>
|
||||
<selfCollide>True</selfCollide>
|
||||
</gazebo>
|
||||
<!-- robotiq_85_left_knuckle_link -->
|
||||
<link name="robotiq_140_left_outer_knuckle">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.01616 0 -0.00133"/>
|
||||
<mass value="0.0131"/>
|
||||
<inertia ixx="325.5E-9" ixy="0" ixz="197.94E-9" iyy="2306.36E-9" iyz="0" izz="2176E-9"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://robotiq_140_gripper_description/meshes/robotiq_140_left_outer_knuckle.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://robotiq_140_gripper_description/meshes/robotiq_140_left_outer_knuckle.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<gazebo reference="robotiq_140_left_outer_knuckle">
|
||||
<material>Gazebo/Grey</material>
|
||||
<selfCollide>True</selfCollide>
|
||||
</gazebo>
|
||||
<!-- robotiq_85_right_knuckle_link -->
|
||||
<link name="robotiq_140_right_outer_knuckle">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="-0.01616 0 -0.00133"/>
|
||||
<mass value="0.0131"/>
|
||||
<inertia ixx="325.5E-9" ixy="0" ixz="-197.94E-9" iyy="2306.36E-9" iyz="0" izz="2176E-9"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://robotiq_140_gripper_description/meshes/robotiq_140_right_outer_knuckle.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://robotiq_140_gripper_description/meshes/robotiq_140_right_outer_knuckle.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<gazebo reference="robotiq_140_right_outer_knuckle">
|
||||
<material>Gazebo/Grey</material>
|
||||
<selfCollide>True</selfCollide>
|
||||
</gazebo>
|
||||
<!-- robotiq_85_left_inner_knuckle_link -->
|
||||
<link name="robotiq_140_left_inner_knuckle">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.03332 0 0.0379"/>
|
||||
<mass value="0.04476"/>
|
||||
<inertia ixx="28054E-9" ixy="0" ixz="-19174E-9" iyy="39758E-9" iyz="0" izz="23453E-9"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://robotiq_140_gripper_description/meshes/robotiq_140_left_inner_knuckle.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="Gazebo/Black">
|
||||
<color rgba="0.1 0.1 0.1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://robotiq_140_gripper_description/meshes/robotiq_140_left_inner_knuckle.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<gazebo reference="robotiq_140_left_inner_knuckle">
|
||||
<material>Gazebo/Black</material>
|
||||
<selfCollide>False</selfCollide>
|
||||
</gazebo>
|
||||
<!-- robotiq_85_right_inner_knuckle_link -->
|
||||
<link name="robotiq_140_right_inner_knuckle">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="-0.03332 0 0.0379"/>
|
||||
<mass value="0.04476"/>
|
||||
<inertia ixx="28054E-9" ixy="0" ixz="19174E-9" iyy="39758E-9" iyz="0" izz="23453E-9"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://robotiq_140_gripper_description/meshes/robotiq_140_right_inner_knuckle.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="Gazebo/Black">
|
||||
<color rgba="0.1 0.1 0.1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://robotiq_140_gripper_description/meshes/robotiq_140_right_inner_knuckle.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<gazebo reference="robotiq_140_right_inner_knuckle">
|
||||
<material>Gazebo/Black</material>
|
||||
<selfCollide>False</selfCollide>
|
||||
</gazebo>
|
||||
<!-- robotiq_85_left_finger_link -->
|
||||
<link name="robotiq_140_left_inner_finger">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="-0.01563 0 0.02522"/>
|
||||
<mass value="0.045"/>
|
||||
<inertia ixx="26663E-9" ixy="0" ixz="5579E-9" iyy="28009E-9" iyz="0" izz="4368E-9"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://robotiq_140_gripper_description/meshes/robotiq_140_left_inner_finger.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="Gazebo/Black">
|
||||
<color rgba="0.1 0.1 0.1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://robotiq_140_gripper_description/meshes/robotiq_140_left_inner_finger.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<gazebo reference="robotiq_140_left_inner_finger">
|
||||
<material>Gazebo/Black</material>
|
||||
<selfCollide>False</selfCollide>
|
||||
</gazebo>
|
||||
<!-- robotiq_85_right_finger_link -->
|
||||
<link name="robotiq_140_right_inner_finger">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.01563 0 0.02522"/>
|
||||
<mass value="0.045"/>
|
||||
<inertia ixx="26663E-9" ixy="0" ixz="-5579E-9" iyy="28009E-9" iyz="0" izz="4368E-9"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://robotiq_140_gripper_description/meshes/robotiq_140_right_inner_finger.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="Gazebo/Black">
|
||||
<color rgba="0.1 0.1 0.1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://robotiq_140_gripper_description/meshes/robotiq_140_right_inner_finger.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<gazebo reference="robotiq_140_right_inner_finger">
|
||||
<material>Gazebo/Black</material>
|
||||
<selfCollide>False</selfCollide>
|
||||
</gazebo>
|
||||
<!-- robot -->
|
||||
<link name="robotiq_140_left_outer_finger">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.01482 0 0.03451"/>
|
||||
<mass value="0.07637"/>
|
||||
<inertia ixx="45513E-9" ixy="0" ixz="-20691E-9" iyy="52570E-9" iyz="0" izz="17804E-9"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://robotiq_140_gripper_description/meshes/robotiq_140_left_outer_finger.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="Gazebo/Black">
|
||||
<color rgba="0.1 0.1 0.1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://robotiq_140_gripper_description/meshes/robotiq_140_left_outer_finger.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<gazebo reference="robotiq_140_left_outer_finger">
|
||||
<material>Gazebo/Black</material>
|
||||
<selfCollide>True</selfCollide>
|
||||
</gazebo>
|
||||
<!-- robot -->
|
||||
<link name="robotiq_140_right_outer_finger">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="-0.01482 0 0.03451"/>
|
||||
<mass value="0.07637"/>
|
||||
<inertia ixx="45513E-9" ixy="0" ixz="20691E-9" iyy="52570E-9" iyz="0" izz="17804E-9"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://robotiq_140_gripper_description/meshes/robotiq_140_right_outer_finger.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="Gazebo/Black">
|
||||
<color rgba="0.1 0.1 0.1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://robotiq_140_gripper_description/meshes/robotiq_140_right_outer_finger.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<gazebo reference="robotiq_140_right_outer_finger">
|
||||
<material>Gazebo/Black</material>
|
||||
<selfCollide>True</selfCollide>
|
||||
</gazebo>
|
||||
<!-- robot -->
|
||||
<joint name="robotiq_arg2f_base_to_robotiq_140_left_outer_knuckle" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0.0306 0 0.05466"/>
|
||||
<parent link="robotiq_140_robotiq_arg2f_base_link"/>
|
||||
<child link="robotiq_140_left_outer_knuckle"/>
|
||||
<axis xyz="0 -1 0"/>
|
||||
<limit effort="18.65" lower="0" upper="0.78574722925" velocity="2.0"/>
|
||||
<mimic joint="robotiq_140_joint_finger" multiplier="1" offset="0"/>
|
||||
</joint>
|
||||
<!-- robot -->
|
||||
<joint name="robotiq_140_joint_finger" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="-0.0306 0 0.05466"/>
|
||||
<parent link="robotiq_140_robotiq_arg2f_base_link"/>
|
||||
<child link="robotiq_140_right_outer_knuckle"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit effort="18.65" lower="0" upper="0.78574722925" velocity="3.54"/>
|
||||
</joint>
|
||||
<!-- robot -->
|
||||
<joint name="robotiq_arg2f_base_to_robotiq_140_left_inner_knuckle" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0.0127 0 0.06118"/>
|
||||
<parent link="robotiq_140_robotiq_arg2f_base_link"/>
|
||||
<child link="robotiq_140_left_inner_knuckle"/>
|
||||
<axis xyz="0 -1 0"/>
|
||||
<limit effort="18.65" lower="0" upper="0.78574722925" velocity="3.54"/>
|
||||
<mimic joint="robotiq_140_joint_finger" multiplier="1" offset="0"/>
|
||||
</joint>
|
||||
<!-- robot -->
|
||||
<joint name="robotiq_arg2f_base_to_robotiq_140_right_inner_knuckle" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="-0.0127 0 0.06118"/>
|
||||
<parent link="robotiq_140_robotiq_arg2f_base_link"/>
|
||||
<child link="robotiq_140_right_inner_knuckle"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit effort="18.65" lower="0" upper="0.78574722925" velocity="3.54"/>
|
||||
<mimic joint="robotiq_140_joint_finger" multiplier="1" offset="0"/>
|
||||
</joint>
|
||||
<!-- robot -->
|
||||
<joint name="robotiq_140_left_outer_knuckle_to_finger" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0.03142 0 -0.00453"/>
|
||||
<parent link="robotiq_140_left_outer_knuckle"/>
|
||||
<child link="robotiq_140_left_outer_finger"/>
|
||||
</joint>
|
||||
<!-- robot -->
|
||||
<joint name="robotiq_140_right_outer_knuckle_to_finger" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="-0.03142 0 -0.00453"/>
|
||||
<parent link="robotiq_140_right_outer_knuckle"/>
|
||||
<child link="robotiq_140_right_outer_finger"/>
|
||||
</joint>
|
||||
<!-- robot -->
|
||||
<joint name="robotiq_140_left_outer_finger_to_inner" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0.03543 0 0.0789"/>
|
||||
<parent link="robotiq_140_left_outer_finger"/>
|
||||
<child link="robotiq_140_left_inner_finger"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit effort="18.65" lower="0" upper="0.78574722925" velocity="3.54"/>
|
||||
<mimic joint="robotiq_140_joint_finger" multiplier="1" offset="0"/>
|
||||
</joint>
|
||||
<!-- robot -->
|
||||
<joint name="robotiq_140_right_outer_finger_to_inner" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="-0.03543 0 0.0789"/>
|
||||
<parent link="robotiq_140_right_outer_finger"/>
|
||||
<child link="robotiq_140_right_inner_finger"/>
|
||||
<axis xyz="0 -1 0"/>
|
||||
<limit effort="18.65" lower="0" upper="0.78574722925" velocity="3.54"/>
|
||||
<mimic joint="robotiq_140_joint_finger" multiplier="1" offset="0"/>
|
||||
</joint>
|
||||
</robot>
|
||||
|
|
@ -0,0 +1,328 @@
|
|||
<?xml version="1.0" ?>
|
||||
<!-- =================================================================================== -->
|
||||
<!-- | This document was autogenerated by xacro from robotiq_arg2f_85_model.xacro | -->
|
||||
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||
<!-- =================================================================================== -->
|
||||
<robot name="robotiq_arg2f_85_model">
|
||||
<link name="robotiq_arg2f_base_link">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="8.625E-08 -4.6583E-06 0.03145"/>
|
||||
<mass value="0.22652"/>
|
||||
<inertia ixx="0.00020005" ixy="-4.2442E-10" ixz="-2.9069E-10" iyy="0.00017832" iyz="-3.4402E-08" izz="0.00013478"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://robotiq_2f_85_gripper_visualization/meshes/visual/robotiq_arg2f_85_base_link.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.1 0.1 0.1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://robotiq_2f_85_gripper_visualization/meshes/collision/robotiq_arg2f_base_link.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="left_outer_knuckle">
|
||||
<!--<inertial>
|
||||
<origin xyz="-0.000200000000003065 0.0199435877845359 0.0292245259211331" rpy="0 0 0" />
|
||||
<mass value="0.00853198276973456" />
|
||||
<inertia ixx="2.89328108496468E-06" ixy="-1.57935047237397E-19" ixz="-1.93980378593255E-19" iyy="1.86719750325683E-06" iyz="-1.21858577871576E-06" izz="1.21905238907251E-06" />
|
||||
</inertial> -->
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://robotiq_2f_85_gripper_visualization/meshes/visual/robotiq_arg2f_85_outer_knuckle.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://robotiq_2f_85_gripper_visualization/meshes/collision/robotiq_arg2f_85_outer_knuckle.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="left_outer_finger">
|
||||
<!--<inertial>
|
||||
<origin xyz="0.00030115855001899 0.0373907951953854 -0.0208027427000385" rpy="0 0 0" />
|
||||
<mass value="0.022614240507152" />
|
||||
<inertia ixx="1.52518312458174E-05" ixy="9.76583423954399E-10" ixz="-5.43838577022588E-10" iyy="6.17694243867776E-06" iyz="6.78636130740228E-06" izz="1.16494917907219E-05" />
|
||||
</inertial> -->
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://robotiq_2f_85_gripper_visualization/meshes/visual/robotiq_arg2f_85_outer_finger.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.1 0.1 0.1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://robotiq_2f_85_gripper_visualization/meshes/collision/robotiq_arg2f_85_outer_finger.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="left_inner_finger">
|
||||
<!--<inertial>
|
||||
<origin xyz="0.000299999999999317 0.0160078233491243 -0.0136945669206257" rpy="0 0 0" />
|
||||
<mass value="0.0104003125914103" />
|
||||
<inertia ixx="2.71909453810972E-06" ixy="1.35402465472579E-21" ixz="-7.1817349065269E-22" iyy="7.69100314106116E-07" iyz="6.74715432769696E-07" izz="2.30315190420171E-06" />
|
||||
</inertial> -->
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://robotiq_2f_85_gripper_visualization/meshes/visual/robotiq_arg2f_85_inner_finger.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.1 0.1 0.1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://robotiq_2f_85_gripper_visualization/meshes/collision/robotiq_arg2f_85_inner_finger.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="left_inner_finger_pad">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.022 0.00635 0.0375"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.9 0.9 0.9 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.022 0.00635 0.0375"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.9 0.0 0.0 1"/>
|
||||
</material>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="left_inner_knuckle">
|
||||
<!--<inertial>
|
||||
<origin xyz="0.000123011831763771 0.0507850843201817 0.00103968640075166" rpy="0 0 0" />
|
||||
<mass value="0.0271177346495152" />
|
||||
<inertia ixx="2.61910379223783E-05" ixy="-2.43616858946494E-07" ixz="-6.37789906117123E-09" iyy="2.8270243746167E-06" iyz="-5.37200748039765E-07" izz="2.83695868220296E-05" />
|
||||
</inertial> -->
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://robotiq_2f_85_gripper_visualization/meshes/visual/robotiq_arg2f_85_inner_knuckle.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.1 0.1 0.1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://robotiq_2f_85_gripper_visualization/meshes/collision/robotiq_arg2f_85_inner_knuckle.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="right_outer_knuckle">
|
||||
<!--<inertial>
|
||||
<origin xyz="-0.000200000000003065 0.0199435877845359 0.0292245259211331" rpy="0 0 0" />
|
||||
<mass value="0.00853198276973456" />
|
||||
<inertia ixx="2.89328108496468E-06" ixy="-1.57935047237397E-19" ixz="-1.93980378593255E-19" iyy="1.86719750325683E-06" iyz="-1.21858577871576E-06" izz="1.21905238907251E-06" />
|
||||
</inertial> -->
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://robotiq_2f_85_gripper_visualization/meshes/visual/robotiq_arg2f_85_outer_knuckle.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://robotiq_2f_85_gripper_visualization/meshes/collision/robotiq_arg2f_85_outer_knuckle.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="right_outer_finger">
|
||||
<!--<inertial>
|
||||
<origin xyz="0.00030115855001899 0.0373907951953854 -0.0208027427000385" rpy="0 0 0" />
|
||||
<mass value="0.022614240507152" />
|
||||
<inertia ixx="1.52518312458174E-05" ixy="9.76583423954399E-10" ixz="-5.43838577022588E-10" iyy="6.17694243867776E-06" iyz="6.78636130740228E-06" izz="1.16494917907219E-05" />
|
||||
</inertial> -->
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://robotiq_2f_85_gripper_visualization/meshes/visual/robotiq_arg2f_85_outer_finger.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.1 0.1 0.1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://robotiq_2f_85_gripper_visualization/meshes/collision/robotiq_arg2f_85_outer_finger.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="right_inner_finger">
|
||||
<!--<inertial>
|
||||
<origin xyz="0.000299999999999317 0.0160078233491243 -0.0136945669206257" rpy="0 0 0" />
|
||||
<mass value="0.0104003125914103" />
|
||||
<inertia ixx="2.71909453810972E-06" ixy="1.35402465472579E-21" ixz="-7.1817349065269E-22" iyy="7.69100314106116E-07" iyz="6.74715432769696E-07" izz="2.30315190420171E-06" />
|
||||
</inertial> -->
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://robotiq_2f_85_gripper_visualization/meshes/visual/robotiq_arg2f_85_inner_finger.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.1 0.1 0.1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://robotiq_2f_85_gripper_visualization/meshes/collision/robotiq_arg2f_85_inner_finger.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="right_inner_finger_pad">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.022 0.00635 0.0375"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.9 0.9 0.9 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.022 0.00635 0.0375"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.9 0.0 0.0 1"/>
|
||||
</material>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="right_inner_knuckle">
|
||||
<!--<inertial>
|
||||
<origin xyz="0.000123011831763771 0.0507850843201817 0.00103968640075166" rpy="0 0 0" />
|
||||
<mass value="0.0271177346495152" />
|
||||
<inertia ixx="2.61910379223783E-05" ixy="-2.43616858946494E-07" ixz="-6.37789906117123E-09" iyy="2.8270243746167E-06" iyz="-5.37200748039765E-07" izz="2.83695868220296E-05" />
|
||||
</inertial> -->
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://robotiq_2f_85_gripper_visualization/meshes/visual/robotiq_arg2f_85_inner_knuckle.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.1 0.1 0.1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://robotiq_2f_85_gripper_visualization/meshes/collision/robotiq_arg2f_85_inner_knuckle.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="finger_joint" type="revolute">
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 -0.0306011 0.054904"/>
|
||||
<parent link="robotiq_arg2f_base_link"/>
|
||||
<child link="left_outer_knuckle"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit effort="1000" lower="0" upper="0.8" velocity="2.0"/>
|
||||
</joint>
|
||||
<joint name="left_outer_finger_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0.0315 -0.0041"/>
|
||||
<parent link="left_outer_knuckle"/>
|
||||
<child link="left_outer_finger"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
</joint>
|
||||
<joint name="left_inner_knuckle_joint" type="revolute">
|
||||
<!-- <origin xyz="0 ${reflect * -0.0127} 0.06142" rpy="${pi / 2 + .725} 0 ${(reflect - 1) * pi / 2}" /> -->
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 -0.0127 0.06142"/>
|
||||
<parent link="robotiq_arg2f_base_link"/>
|
||||
<child link="left_inner_knuckle"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit effort="1000" lower="0" upper="0.8757" velocity="2.0"/>
|
||||
<mimic joint="finger_joint" multiplier="1" offset="0"/>
|
||||
</joint>
|
||||
<joint name="left_inner_finger_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0 0.0061 0.0471"/>
|
||||
<parent link="left_outer_finger"/>
|
||||
<child link="left_inner_finger"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit effort="1000" lower="-0.8757" upper="0" velocity="2.0"/>
|
||||
<mimic joint="finger_joint" multiplier="-1" offset="0"/>
|
||||
</joint>
|
||||
<joint name="left_inner_finger_pad_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 -0.0220203446692936 0.03242"/>
|
||||
<parent link="left_inner_finger"/>
|
||||
<child link="left_inner_finger_pad"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
</joint>
|
||||
<joint name="right_outer_knuckle_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0 0.0306011 0.054904"/>
|
||||
<parent link="robotiq_arg2f_base_link"/>
|
||||
<child link="right_outer_knuckle"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit effort="1000" lower="0" upper="0.81" velocity="2.0"/>
|
||||
<mimic joint="finger_joint" multiplier="1" offset="0"/>
|
||||
</joint>
|
||||
<joint name="right_outer_finger_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0.0315 -0.0041"/>
|
||||
<parent link="right_outer_knuckle"/>
|
||||
<child link="right_outer_finger"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
</joint>
|
||||
<joint name="right_inner_knuckle_joint" type="revolute">
|
||||
<!-- <origin xyz="0 ${reflect * -0.0127} 0.06142" rpy="${pi / 2 + .725} 0 ${(reflect - 1) * pi / 2}" /> -->
|
||||
<origin rpy="0 0 0.0" xyz="0 0.0127 0.06142"/>
|
||||
<parent link="robotiq_arg2f_base_link"/>
|
||||
<child link="right_inner_knuckle"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit effort="1000" lower="0" upper="0.8757" velocity="2.0"/>
|
||||
<mimic joint="finger_joint" multiplier="1" offset="0"/>
|
||||
</joint>
|
||||
<joint name="right_inner_finger_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0 0.0061 0.0471"/>
|
||||
<parent link="right_outer_finger"/>
|
||||
<child link="right_inner_finger"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit effort="1000" lower="-0.8757" upper="0" velocity="2.0"/>
|
||||
<mimic joint="finger_joint" multiplier="-1" offset="0"/>
|
||||
</joint>
|
||||
<joint name="right_inner_finger_pad_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 -0.0220203446692936 0.03242"/>
|
||||
<parent link="right_inner_finger"/>
|
||||
<child link="right_inner_finger_pad"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
</joint>
|
||||
<transmission name="finger_joint_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="finger_joint">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="finger_joint_motor">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
</robot>
|
||||
|
||||
|
|
@ -0,0 +1,213 @@
|
|||
# Blender v2.78 (sub 0) OBJ File: ''
|
||||
# www.blender.org
|
||||
mtllib tray_textured.mtl
|
||||
o edge_4_Cube.001
|
||||
v -0.573309 0.580000 0.261247
|
||||
v -0.426691 0.419400 -0.002214
|
||||
v -0.590083 0.580000 0.250354
|
||||
v -0.573309 -0.580000 0.261247
|
||||
v -0.409917 0.419400 0.008679
|
||||
v -0.590083 -0.580000 0.250354
|
||||
v -0.409917 -0.419400 0.009162
|
||||
v -0.426691 -0.419400 -0.001731
|
||||
vt 0.9046 0.2397
|
||||
vt 0.7929 0.2434
|
||||
vt 0.9174 0.2393
|
||||
vt 0.9537 0.7559
|
||||
vt 0.7801 0.2438
|
||||
vt 0.9664 0.7554
|
||||
vt 0.8291 0.7599
|
||||
vt 0.8419 0.7595
|
||||
vn 0.2565 0.8821 -0.3950
|
||||
vn 0.8392 0.0002 0.5438
|
||||
vn 0.8395 0.0000 0.5433
|
||||
vn 0.8396 -0.0000 0.5432
|
||||
vn 0.2568 -0.8819 -0.3954
|
||||
vn -0.8396 -0.0002 -0.5433
|
||||
vn -0.8392 -0.0000 -0.5438
|
||||
vn -0.8391 0.0000 -0.5439
|
||||
vn 0.5446 -0.0005 -0.8387
|
||||
vn -0.5446 -0.0000 0.8387
|
||||
vn 0.8391 0.0003 0.5440
|
||||
vn -0.8397 -0.0003 -0.5430
|
||||
usemtl None
|
||||
s 1
|
||||
f 1/1/1 2/2/1 3/3/1
|
||||
f 4/4/2 5/5/3 1/1/4
|
||||
f 6/6/5 7/7/5 4/4/5
|
||||
f 3/3/6 8/8/7 6/6/8
|
||||
f 5/5/9 8/8/9 2/2/9
|
||||
f 4/4/10 3/3/10 6/6/10
|
||||
f 1/1/1 5/5/1 2/2/1
|
||||
f 4/4/2 7/7/11 5/5/3
|
||||
f 6/6/5 8/8/5 7/7/5
|
||||
f 3/3/6 2/2/12 8/8/7
|
||||
f 5/5/9 7/7/9 8/8/9
|
||||
f 4/4/10 1/1/10 3/3/10
|
||||
o edge_1_Cube.003
|
||||
v 0.580000 0.590083 0.250354
|
||||
v -0.419960 0.426691 -0.001860
|
||||
v -0.580000 0.590083 0.250354
|
||||
v 0.580000 0.573309 0.261247
|
||||
v 0.420014 0.426691 -0.001059
|
||||
v -0.580000 0.573309 0.261247
|
||||
v 0.420014 0.409917 0.009834
|
||||
v -0.419960 0.409917 0.009033
|
||||
vt 0.8346 0.9187
|
||||
vt 0.2203 0.8574
|
||||
vt 0.1480 0.9187
|
||||
vt 0.8346 0.9129
|
||||
vt 0.7623 0.8574
|
||||
vt 0.1480 0.9129
|
||||
vt 0.7623 0.8511
|
||||
vt 0.2203 0.8511
|
||||
vn 0.0004 0.8386 -0.5448
|
||||
vn 0.0001 0.8391 -0.5439
|
||||
vn 0.0000 0.8393 -0.5437
|
||||
vn 0.8823 -0.2564 -0.3948
|
||||
vn -0.0004 -0.8392 0.5439
|
||||
vn -0.0001 -0.8386 0.5447
|
||||
vn 0.0000 -0.8385 0.5449
|
||||
vn -0.8826 -0.2560 -0.3942
|
||||
vn 0.0008 -0.5446 -0.8387
|
||||
vn 0.0000 0.5446 0.8387
|
||||
vn 0.0005 0.8383 -0.5452
|
||||
vn -0.0005 -0.8394 0.5435
|
||||
usemtl None
|
||||
s 1
|
||||
f 9/9/13 10/10/14 11/11/15
|
||||
f 12/12/16 13/13/16 9/9/16
|
||||
f 14/14/17 15/15/18 12/12/19
|
||||
f 11/11/20 16/16/20 14/14/20
|
||||
f 13/13/21 16/16/21 10/10/21
|
||||
f 12/12/22 11/11/22 14/14/22
|
||||
f 9/9/13 13/13/23 10/10/14
|
||||
f 12/12/16 15/15/16 13/13/16
|
||||
f 14/14/17 16/16/24 15/15/18
|
||||
f 11/11/20 10/10/20 16/16/20
|
||||
f 13/13/21 15/15/21 16/16/21
|
||||
f 12/12/22 9/9/22 11/11/22
|
||||
o edge_2_Cube
|
||||
v 0.590083 0.580000 0.250354
|
||||
v 0.409917 0.420060 0.009390
|
||||
v 0.573309 0.580000 0.261247
|
||||
v 0.590083 -0.580000 0.250354
|
||||
v 0.426691 0.420060 -0.001503
|
||||
v 0.573309 -0.580000 0.261247
|
||||
v 0.426691 -0.419158 -0.002053
|
||||
v 0.409917 -0.419158 0.008840
|
||||
vt 0.9410 0.8520
|
||||
vt 0.7523 0.8566
|
||||
vt 0.9234 0.8524
|
||||
vt 0.8896 0.1426
|
||||
vt 0.7698 0.8562
|
||||
vt 0.8721 0.1430
|
||||
vt 0.7185 0.1468
|
||||
vt 0.7009 0.1472
|
||||
vn -0.2561 0.8826 -0.3943
|
||||
vn 0.8394 0.0003 -0.5435
|
||||
vn 0.8390 0.0001 -0.5441
|
||||
vn 0.8389 0.0000 -0.5443
|
||||
vn -0.2569 -0.8818 -0.3956
|
||||
vn -0.8390 -0.0003 0.5441
|
||||
vn -0.8394 -0.0001 0.5436
|
||||
vn -0.8395 -0.0000 0.5434
|
||||
vn -0.5446 0.0005 -0.8387
|
||||
vn 0.5446 -0.0000 0.8387
|
||||
vn 0.8396 0.0004 -0.5433
|
||||
vn -0.8388 -0.0004 0.5444
|
||||
usemtl None
|
||||
s 1
|
||||
f 17/17/25 18/18/25 19/19/25
|
||||
f 20/20/26 21/21/27 17/17/28
|
||||
f 22/22/29 23/23/29 20/20/29
|
||||
f 19/19/30 24/24/31 22/22/32
|
||||
f 21/21/33 24/24/33 18/18/33
|
||||
f 20/20/34 19/19/34 22/22/34
|
||||
f 17/17/25 21/21/25 18/18/25
|
||||
f 20/20/26 23/23/35 21/21/27
|
||||
f 22/22/29 24/24/29 23/23/29
|
||||
f 19/19/30 18/18/36 24/24/31
|
||||
f 21/21/33 23/23/33 24/24/33
|
||||
f 20/20/34 17/17/34 19/19/34
|
||||
o base_Cube.004
|
||||
v 0.420000 0.420000 0.010000
|
||||
v -0.420000 0.420000 -0.010000
|
||||
v -0.420000 0.420000 0.010000
|
||||
v 0.420000 -0.420000 0.010000
|
||||
v 0.420000 0.420000 -0.010000
|
||||
v -0.420000 -0.420000 0.010000
|
||||
v 0.420000 -0.420000 -0.010000
|
||||
v -0.420000 -0.420000 -0.010000
|
||||
vt 0.7524 0.8072
|
||||
vt -0.3038 0.8371
|
||||
vt -0.3038 0.8371
|
||||
vt 0.7012 0.1905
|
||||
vt 0.7524 0.8072
|
||||
vt -0.3550 0.2204
|
||||
vt 0.7012 0.1905
|
||||
vt -0.3550 0.2204
|
||||
vn 0.0000 1.0000 0.0000
|
||||
vn 1.0000 -0.0000 0.0000
|
||||
vn 0.0000 -1.0000 0.0000
|
||||
vn -1.0000 -0.0000 0.0000
|
||||
vn 0.0000 0.0000 -1.0000
|
||||
vn 0.0000 -0.0000 1.0000
|
||||
usemtl None
|
||||
s 1
|
||||
f 25/25/37 26/26/37 27/27/37
|
||||
f 28/28/38 29/29/38 25/25/38
|
||||
f 30/30/39 31/31/39 28/28/39
|
||||
f 27/27/40 32/32/40 30/30/40
|
||||
f 29/29/41 32/32/41 26/26/41
|
||||
f 28/28/42 27/27/42 30/30/42
|
||||
f 25/25/37 29/29/37 26/26/37
|
||||
f 28/28/38 31/31/38 29/29/38
|
||||
f 30/30/39 32/32/39 31/31/39
|
||||
f 27/27/40 26/26/40 32/32/40
|
||||
f 29/29/41 31/31/41 32/32/41
|
||||
f 28/28/42 25/25/42 27/27/42
|
||||
o edge_3_Cube.002
|
||||
v 0.580000 -0.573309 0.261247
|
||||
v -0.419400 -0.409917 0.008678
|
||||
v -0.580000 -0.573309 0.261247
|
||||
v 0.580000 -0.590083 0.250354
|
||||
v 0.419883 -0.409917 0.009162
|
||||
v -0.580000 -0.590083 0.250354
|
||||
v 0.419883 -0.426691 -0.001731
|
||||
v -0.419400 -0.426691 -0.002215
|
||||
vt 0.8690 0.1040
|
||||
vt 0.1365 0.1739
|
||||
vt 0.0188 0.1040
|
||||
vt 0.8690 0.0968
|
||||
vt 0.7517 0.1739
|
||||
vt 0.0188 0.0968
|
||||
vt 0.7517 0.1668
|
||||
vt 0.1365 0.1668
|
||||
vn -0.0002 0.8392 0.5438
|
||||
vn -0.0000 0.8395 0.5433
|
||||
vn 0.0000 0.8396 0.5432
|
||||
vn 0.8825 0.2562 -0.3945
|
||||
vn 0.0002 -0.8396 -0.5433
|
||||
vn 0.0000 -0.8392 -0.5438
|
||||
vn 0.0000 -0.8391 -0.5439
|
||||
vn -0.8821 0.2565 -0.3950
|
||||
vn -0.8822 0.2565 -0.3950
|
||||
vn 0.0005 0.5446 -0.8387
|
||||
vn 0.0000 -0.5446 0.8387
|
||||
vn -0.0003 0.8391 0.5440
|
||||
vn 0.0003 -0.8397 -0.5430
|
||||
usemtl None
|
||||
s 1
|
||||
f 33/33/43 34/34/44 35/35/45
|
||||
f 36/36/46 37/37/46 33/33/46
|
||||
f 38/38/47 39/39/48 36/36/49
|
||||
f 35/35/50 40/40/51 38/38/51
|
||||
f 37/37/52 40/40/52 34/34/52
|
||||
f 36/36/53 35/35/53 38/38/53
|
||||
f 33/33/43 37/37/54 34/34/44
|
||||
f 36/36/46 39/39/46 37/37/46
|
||||
f 38/38/47 40/40/55 39/39/48
|
||||
f 35/35/50 34/34/51 40/40/51
|
||||
f 37/37/52 39/39/52 40/40/52
|
||||
f 36/36/53 33/33/53 35/35/53
|
||||
|
|
@ -0,0 +1,49 @@
|
|||
<robot name="tabletop">
|
||||
<link name="base_link">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value="0"/>
|
||||
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="tray_textured.obj" scale="0.5 0.5 0.5"/>
|
||||
</geometry>
|
||||
<material name="tray_material">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.005"/>
|
||||
<geometry>
|
||||
<box size=".6 .6 .02"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<collision>
|
||||
<origin rpy="0 0.575469961 0" xyz="0.25 0 0.059"/>
|
||||
<geometry>
|
||||
<box size=".02 .6 .15"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<collision>
|
||||
<origin rpy="0 -0.575469961 0" xyz="-0.25 0 0.059"/>
|
||||
<geometry>
|
||||
<box size=".02 .6 .15"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<collision>
|
||||
<origin rpy="0.575469961 0 0" xyz="0 -0.25 0.059"/>
|
||||
<geometry>
|
||||
<box size=".6 .02 .15"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<collision>
|
||||
<origin rpy="-0.575469961 0 0" xyz="0 0.25 0.059"/>
|
||||
<geometry>
|
||||
<box size=".6 .02 .15"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
</link>
|
||||
</robot>
|
||||
|
|
@ -0,0 +1,209 @@
|
|||
<?xml version="1.0" ?>
|
||||
<!--FOR GRIPPER, THERE IS NO LINK INERTIA DEFINED HERE-->
|
||||
<robot name="universal_robot">
|
||||
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/visual/base.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/collision/base.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="4.0"/>
|
||||
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
|
||||
<inertia ixx="0.00443333156" ixy="0.0" ixz="0.0" iyy="0.00443333156" iyz="0.0" izz="0.0072"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="shoulder_pan_joint" type="revolute">
|
||||
<parent link="base_link"/>
|
||||
<child link="shoulder_link"/>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.089159"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="150.0" lower="-6.28318530718" upper="6.28318530718" velocity="3.15"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<link name="shoulder_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/visual/shoulder.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/collision/shoulder.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="3.7"/>
|
||||
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
|
||||
<inertia ixx="0.010267495893" ixy="0.0" ixz="0.0" iyy="0.010267495893" iyz="0.0" izz="0.00666"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="shoulder_lift_joint" type="revolute">
|
||||
<parent link="shoulder_link"/>
|
||||
<child link="upper_arm_link"/>
|
||||
<origin rpy="0.0 1.57079632679 0.0" xyz="0.0 0.13585 0.0"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit effort="150.0" lower="-6.28318530718" upper="6.28318530718" velocity="3.15"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<link name="upper_arm_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/visual/upperarm.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/collision/upperarm.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="8.393"/>
|
||||
<origin rpy="0 0 0" xyz="0.0 0.0 0.28"/>
|
||||
<inertia ixx="0.22689067591" ixy="0.0" ixz="0.0" iyy="0.22689067591" iyz="0.0" izz="0.0151074"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="elbow_joint" type="revolute">
|
||||
<parent link="upper_arm_link"/>
|
||||
<child link="forearm_link"/>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 -0.1197 0.425"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit effort="150.0" lower="-3.14159265359" upper="3.14159265359" velocity="3.15"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<link name="forearm_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/visual/forearm.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/collision/forearm.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="2.275"/>
|
||||
<origin rpy="0 0 0" xyz="0.0 0.0 0.25"/>
|
||||
<inertia ixx="0.049443313556" ixy="0.0" ixz="0.0" iyy="0.049443313556" iyz="0.0" izz="0.004095"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="wrist_1_joint" type="revolute">
|
||||
<parent link="forearm_link"/>
|
||||
<child link="wrist_1_link"/>
|
||||
<origin rpy="0.0 1.57079632679 0.0" xyz="0.0 0.0 0.39225"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit effort="28.0" lower="-6.28318530718" upper="6.28318530718" velocity="3.2"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<link name="wrist_1_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/visual/wrist1.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/collision/wrist1.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1.219"/>
|
||||
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
|
||||
<inertia ixx="0.111172755531" ixy="0.0" ixz="0.0" iyy="0.111172755531" iyz="0.0" izz="0.21942"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="wrist_2_joint" type="revolute">
|
||||
<parent link="wrist_1_link"/>
|
||||
<child link="wrist_2_link"/>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.093 0.0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="28.0" lower="-6.28318530718" upper="6.28318530718" velocity="3.2"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<link name="wrist_2_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/visual/wrist2.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/collision/wrist2.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1.219"/>
|
||||
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
|
||||
<inertia ixx="0.111172755531" ixy="0.0" ixz="0.0" iyy="0.111172755531" iyz="0.0" izz="0.21942"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="wrist_3_joint" type="revolute">
|
||||
<parent link="wrist_2_link"/>
|
||||
<child link="wrist_3_link"/>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.09465"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit effort="28.0" lower="-6.28318530718" upper="6.28318530718" velocity="3.2"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<link name="wrist_3_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/visual/wrist3.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/collision/wrist3.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.1879"/>
|
||||
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
|
||||
<inertia ixx="0.0171364731454" ixy="0.0" ixz="0.0" iyy="0.0171364731454" iyz="0.0" izz="0.033822"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="ee_fixed_joint" type="fixed">
|
||||
<parent link="wrist_3_link"/>
|
||||
<child link="ee_link"/>
|
||||
<origin rpy="0.0 0.0 1.57079632679" xyz="0.0 0.0823 0.0"/>
|
||||
</joint>
|
||||
<link name="ee_link">
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.01 0.01 0.01"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="-0.01 0 0"/>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
</robot>
|
||||
|
||||
|
|
@ -0,0 +1,568 @@
|
|||
<?xml version="1.0" ?>
|
||||
<!-- =================================================================================== -->
|
||||
<!-- | This document was autogenerated by xacro from sisbot.xacro | -->
|
||||
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||
<!-- =================================================================================== -->
|
||||
<robot name="ur5_robotiq_140" xmlns:xacro="http://ros.org/wiki/xacro">
|
||||
<!--################################################
|
||||
######## arm #######
|
||||
####################################################-->
|
||||
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/visual/base.obj"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/collision/base.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="4.0"/>
|
||||
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
|
||||
<inertia ixx="0.00443333156" ixy="0.0" ixz="0.0" iyy="0.00443333156" iyz="0.0" izz="0.0072"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="shoulder_pan_joint" type="revolute">
|
||||
<parent link="base_link"/>
|
||||
<child link="shoulder_link"/>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.089159"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="150.0" lower="-3.14159265359" upper="3.14159265359" velocity="3.15"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<link name="shoulder_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/visual/shoulder.obj"/>
|
||||
</geometry>
|
||||
<material name="DarkGray">
|
||||
<color rgba="0.5 0.5 0.5 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/collision/shoulder.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="3.7"/>
|
||||
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
|
||||
<inertia ixx="0.010267495893" ixy="0.0" ixz="0.0" iyy="0.010267495893" iyz="0.0" izz="0.00666"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="shoulder_lift_joint" type="revolute">
|
||||
<parent link="shoulder_link"/>
|
||||
<child link="upper_arm_link"/>
|
||||
<origin rpy="0.0 1.57079632679 0.0" xyz="0.0 0.13585 0.0"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit effort="150.0" lower="-3.14159265359" upper="3.14159265359" velocity="3.15"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<link name="upper_arm_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/visual/upperarm.obj"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/collision/upperarm.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="8.393"/>
|
||||
<origin rpy="0 0 0" xyz="0.0 0.0 0.28"/>
|
||||
<inertia ixx="0.22689067591" ixy="0.0" ixz="0.0" iyy="0.22689067591" iyz="0.0" izz="0.0151074"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="elbow_joint" type="revolute">
|
||||
<parent link="upper_arm_link"/>
|
||||
<child link="forearm_link"/>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 -0.1197 0.425"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit effort="150.0" lower="-3.14159265359" upper="3.14159265359" velocity="3.15"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<link name="forearm_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/visual/forearm.obj"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/collision/forearm.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="2.275"/>
|
||||
<origin rpy="0 0 0" xyz="0.0 0.0 0.25"/>
|
||||
<inertia ixx="0.049443313556" ixy="0.0" ixz="0.0" iyy="0.049443313556" iyz="0.0" izz="0.004095"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="wrist_1_joint" type="revolute">
|
||||
<parent link="forearm_link"/>
|
||||
<child link="wrist_1_link"/>
|
||||
<origin rpy="0.0 1.57079632679 0.0" xyz="0.0 0.0 0.39225"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit effort="28.0" lower="-3.14159265359" upper="3.14159265359" velocity="3.2"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<link name="wrist_1_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/visual/wrist1.obj"/>
|
||||
</geometry>
|
||||
<material name="DarkGray">
|
||||
<color rgba="0.5 0.5 0.5 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/collision/wrist1.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1.219"/>
|
||||
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
|
||||
<inertia ixx="0.111172755531" ixy="0.0" ixz="0.0" iyy="0.111172755531" iyz="0.0" izz="0.21942"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="wrist_2_joint" type="revolute">
|
||||
<parent link="wrist_1_link"/>
|
||||
<child link="wrist_2_link"/>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.093 0.0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="28.0" lower="-3.14159265359" upper="3.14159265359" velocity="3.2"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<link name="wrist_2_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/visual/wrist2.obj"/>
|
||||
</geometry>
|
||||
<material name="DarkGray">
|
||||
<color rgba="0.5 0.5 0.5 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/collision/wrist2.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1.219"/>
|
||||
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
|
||||
<inertia ixx="0.111172755531" ixy="0.0" ixz="0.0" iyy="0.111172755531" iyz="0.0" izz="0.21942"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="wrist_3_joint" type="revolute">
|
||||
<parent link="wrist_2_link"/>
|
||||
<child link="wrist_3_link"/>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.09465"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit effort="28.0" lower="-3.14159265359" upper="3.14159265359" velocity="3.2"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<link name="wrist_3_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/visual/wrist3.obj"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/collision/wrist3.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.1879"/>
|
||||
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
|
||||
<inertia ixx="0.0171364731454" ixy="0.0" ixz="0.0" iyy="0.0171364731454" iyz="0.0" izz="0.033822"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="ee_fixed_joint" type="fixed">
|
||||
<parent link="wrist_3_link"/>
|
||||
<child link="ee_link"/>
|
||||
<origin rpy="0.0 0.0 1.57079632679" xyz="0.0 0.0823 0.0"/>
|
||||
</joint>
|
||||
<link name="ee_link">
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.01 0.01 0.01"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="-0.01 0 0"/>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<!--################################################
|
||||
######## gripper #####
|
||||
####################################################-->
|
||||
|
||||
<link name="robotiq_arg2f_base_link">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="8.625E-08 -4.6583E-06 0.03145"/>
|
||||
<mass value="0.22652"/>
|
||||
<inertia ixx="0.00020005" ixy="-4.2442E-10" ixz="-2.9069E-10" iyy="0.00017832" iyz="-3.4402E-08" izz="0.00013478"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/robotiq_140/visual/robotiq_arg2f_base_link.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.1 0.1 0.1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/robotiq_140/collision/robotiq_arg2f_base_link.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="left_outer_knuckle">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="-0.000200000000003065 0.0199435877845359 0.0292245259211331"/>
|
||||
<mass value="0.00853198276973456"/>
|
||||
<inertia ixx="2.89328108496468E-06" ixy="-1.57935047237397E-19" ixz="-1.93980378593255E-19" iyy="1.86719750325683E-06" iyz="-1.21858577871576E-06" izz="1.21905238907251E-06"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/robotiq_140/visual/robotiq_arg2f_140_outer_knuckle.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 0.1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/robotiq_140/collision/robotiq_arg2f_140_outer_knuckle.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="left_outer_finger">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.00030115855001899 0.0373907951953854 -0.0208027427000385"/>
|
||||
<mass value="0.022614240507152"/>
|
||||
<inertia ixx="1.52518312458174E-05" ixy="9.76583423954399E-10" ixz="-5.43838577022588E-10" iyy="6.17694243867776E-06" iyz="6.78636130740228E-06" izz="1.16494917907219E-05"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/robotiq_140/visual/robotiq_arg2f_140_outer_finger.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.1 0.1 0.1 0.2"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/robotiq_140/collision/robotiq_arg2f_140_outer_finger.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="left_inner_finger">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.000299999999999317 0.0160078233491243 -0.0136945669206257"/>
|
||||
<mass value="0.0104003125914103"/>
|
||||
<inertia ixx="2.71909453810972E-06" ixy="1.35402465472579E-21" ixz="-7.1817349065269E-22" iyy="7.69100314106116E-07" iyz="6.74715432769696E-07" izz="2.30315190420171E-06"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/robotiq_140/visual/robotiq_arg2f_140_inner_finger.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.1 0.1 0.1 0.3"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/robotiq_140/collision/robotiq_arg2f_140_inner_finger.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="left_inner_finger_pad">
|
||||
<inertial>
|
||||
<mass value="1."/>
|
||||
<inertia ixx="1E-10" ixy="1E-10" ixz="1E-10" iyy="1E-10" iyz="1E-10" izz="1E-10"/>
|
||||
</inertial>
|
||||
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.027 0.065 0.0075"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0 0 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.03 0.07 0.0075"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.9 0.0 0.0 1"/>
|
||||
</material>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="left_inner_knuckle">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.000123011831763771 0.0507850843201817 0.00103968640075166"/>
|
||||
<mass value="0.0271177346495152"/>
|
||||
<inertia ixx="2.61910379223783E-05" ixy="-2.43616858946494E-07" ixz="-6.37789906117123E-09" iyy="2.8270243746167E-06" iyz="-5.37200748039765E-07" izz="2.83695868220296E-05"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/robotiq_140/visual/robotiq_arg2f_140_inner_knuckle.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.1 0.1 0.1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/robotiq_140/collision/robotiq_arg2f_140_inner_knuckle.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="right_outer_knuckle">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="-0.000200000000003065 0.0199435877845359 0.0292245259211331"/>
|
||||
<mass value="0.00853198276973456"/>
|
||||
<inertia ixx="2.89328108496468E-06" ixy="-1.57935047237397E-19" ixz="-1.93980378593255E-19" iyy="1.86719750325683E-06" iyz="-1.21858577871576E-06" izz="1.21905238907251E-06"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/robotiq_140/visual/robotiq_arg2f_140_outer_knuckle.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/robotiq_140/collision/robotiq_arg2f_140_outer_knuckle.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="right_outer_finger">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.00030115855001899 0.0373907951953854 -0.0208027427000385"/>
|
||||
<mass value="0.022614240507152"/>
|
||||
<inertia ixx="1.52518312458174E-05" ixy="9.76583423954399E-10" ixz="-5.43838577022588E-10" iyy="6.17694243867776E-06" iyz="6.78636130740228E-06" izz="1.16494917907219E-05"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/robotiq_140/visual/robotiq_arg2f_140_outer_finger.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.1 0.1 0.1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/robotiq_140/collision/robotiq_arg2f_140_outer_finger.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="right_inner_finger">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.000299999999999317 0.0160078233491243 -0.0136945669206257"/>
|
||||
<mass value="0.0104003125914103"/>
|
||||
<inertia ixx="2.71909453810972E-06" ixy="1.35402465472579E-21" ixz="-7.1817349065269E-22" iyy="7.69100314106116E-07" iyz="6.74715432769696E-07" izz="2.30315190420171E-06"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/robotiq_140/visual/robotiq_arg2f_140_inner_finger.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.1 0.1 0.1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/robotiq_140/collision/robotiq_arg2f_140_inner_finger.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="right_inner_finger_pad">
|
||||
<inertial>
|
||||
<mass value="1."/>
|
||||
<inertia ixx="1E-10" ixy="1E-10" ixz="1E-10" iyy="1E-10" iyz="1E-10" izz="1E-10"/>
|
||||
</inertial>
|
||||
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.027 0.065 0.0075"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 0 0 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.03 0.07 0.0075"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.9 0.0 0.0 1"/>
|
||||
</material>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="right_inner_knuckle">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.000123011831763771 0.0507850843201817 0.00103968640075166"/>
|
||||
<mass value="0.0271177346495152"/>
|
||||
<inertia ixx="2.61910379223783E-05" ixy="-2.43616858946494E-07" ixz="-6.37789906117123E-09" iyy="2.8270243746167E-06" iyz="-5.37200748039765E-07" izz="2.83695868220296E-05"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/robotiq_140/visual/robotiq_arg2f_140_inner_knuckle.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.1 0.1 0.1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/robotiq_140/collision/robotiq_arg2f_140_inner_knuckle.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="finger_joint" type="revolute">
|
||||
<origin rpy="2.2957963267948966 0 0" xyz="0 -0.030601 0.054905"/>
|
||||
<parent link="robotiq_arg2f_base_link"/>
|
||||
<child link="left_outer_knuckle"/>
|
||||
<axis xyz="-1 0 0"/>
|
||||
<limit effort="1000" lower="0" upper="0.7" velocity="2.0"/>
|
||||
</joint>
|
||||
<joint name="left_outer_finger_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0.01821998610742 0.0260018192872234"/>
|
||||
<parent link="left_outer_knuckle"/>
|
||||
<child link="left_outer_finger"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
</joint>
|
||||
<joint name="left_inner_knuckle_joint" type="revolute">
|
||||
<origin rpy="2.2957963267948966 0 0.0" xyz="0 -0.0127 0.06142"/>
|
||||
<parent link="robotiq_arg2f_base_link"/>
|
||||
<child link="left_inner_knuckle"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit effort="1000" upper="0" lower="-0.8757" velocity="2.0"/>
|
||||
<mimic joint="finger_joint" multiplier="-1" offset="0"/>
|
||||
</joint>
|
||||
<joint name="left_inner_finger_joint" type="revolute">
|
||||
<origin rpy="-0.725 0 0" xyz="0 0.0817554015893473 -0.0282203446692936"/>
|
||||
<parent link="left_outer_finger"/>
|
||||
<child link="left_inner_finger"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit effort="1000" lower="0" upper="0.8757" velocity="2.0"/>
|
||||
<mimic joint="finger_joint" multiplier="1" offset="0"/>
|
||||
</joint>
|
||||
<joint name="left_inner_finger_pad_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0.0457554015893473 -0.0272203446692936"/>
|
||||
<parent link="left_inner_finger"/>
|
||||
<child link="left_inner_finger_pad"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
</joint>
|
||||
<joint name="right_outer_knuckle_joint" type="revolute">
|
||||
<origin rpy="2.2957963267948966 0 3.141592653589793" xyz="0 0.030601 0.054905"/>
|
||||
<parent link="robotiq_arg2f_base_link"/>
|
||||
<child link="right_outer_knuckle"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit effort="1000" upper="0" lower="-0.725" velocity="2.0"/>
|
||||
<mimic joint="finger_joint" multiplier="-1" offset="0"/>
|
||||
</joint>
|
||||
<joint name="right_outer_finger_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0.01821998610742 0.0260018192872234"/>
|
||||
<parent link="right_outer_knuckle"/>
|
||||
<child link="right_outer_finger"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
</joint>
|
||||
<joint name="right_inner_knuckle_joint" type="revolute">
|
||||
<origin rpy="2.2957963267948966 0 -3.141592653589793" xyz="0 0.0127 0.06142"/>
|
||||
<parent link="robotiq_arg2f_base_link"/>
|
||||
<child link="right_inner_knuckle"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit effort="1000" upper="0" lower="-0.8757" velocity="2.0"/>
|
||||
<mimic joint="finger_joint" multiplier="-1" offset="0"/>
|
||||
</joint>
|
||||
<joint name="right_inner_finger_joint" type="revolute">
|
||||
<origin rpy="-0.725 0 0" xyz="0 0.0817554015893473 -0.0282203446692936"/>
|
||||
<parent link="right_outer_finger"/>
|
||||
<child link="right_inner_finger"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit effort="1000" lower="0" upper="0.8757" velocity="2.0"/>
|
||||
<mimic joint="finger_joint" multiplier="1" offset="0"/>
|
||||
</joint>
|
||||
<joint name="right_inner_finger_pad_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0.0457554015893473 -0.0272203446692936"/>
|
||||
<parent link="right_inner_finger"/>
|
||||
<child link="right_inner_finger_pad"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
</joint>
|
||||
<!-- <transmission name="finger_joint_trans">-->
|
||||
<!-- <type>transmission_interface/SimpleTransmission</type>-->
|
||||
<!-- <joint name="finger_joint">-->
|
||||
<!-- <hardwareInterface>PositionJointInterface</hardwareInterface>-->
|
||||
<!-- </joint>-->
|
||||
<!-- <actuator name="finger_joint_motor">-->
|
||||
<!-- <mechanicalReduction>1</mechanicalReduction>-->
|
||||
<!-- </actuator>-->
|
||||
<!-- </transmission>-->
|
||||
|
||||
<!--################################################
|
||||
######## connect model #####
|
||||
####################################################-->
|
||||
|
||||
<link name="world"/>
|
||||
<!-- connect gripper to arm -->
|
||||
<joint name="arm_gripper_joint" type="fixed">
|
||||
<parent link="ee_link"/>
|
||||
<child link="robotiq_arg2f_base_link"/>
|
||||
<origin rpy="-1.57 1.57 -1.57" xyz="0.0 0.0 0.0"/>
|
||||
</joint>
|
||||
|
||||
<!-- connect arm to world -->
|
||||
<joint name="world_arm_joint" type="fixed">
|
||||
<parent link="world"/>
|
||||
<child link="base_link"/>
|
||||
<origin rpy="0.0 0.0 0" xyz="0 0 0.78"/>
|
||||
</joint>
|
||||
|
||||
|
||||
</robot>
|
||||
|
||||
|
|
@ -0,0 +1,565 @@
|
|||
<?xml version="1.0" ?>
|
||||
<!-- =================================================================================== -->
|
||||
<!-- | This document was autogenerated by xacro from sisbot.xacro | -->
|
||||
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||
<!-- =================================================================================== -->
|
||||
<robot name="ur5_robotiq_85" xmlns:xacro="http://ros.org/wiki/xacro">
|
||||
<!--################################################
|
||||
######## arm #######
|
||||
####################################################-->
|
||||
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/visual/base.obj"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/collision/base.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="4.0"/>
|
||||
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
|
||||
<inertia ixx="0.00443333156" ixy="0.0" ixz="0.0" iyy="0.00443333156" iyz="0.0" izz="0.0072"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="shoulder_pan_joint" type="revolute">
|
||||
<parent link="base_link"/>
|
||||
<child link="shoulder_link"/>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.089159"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="150.0" lower="-3.14159265359" upper="3.14159265359" velocity="3.15"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<link name="shoulder_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/visual/shoulder.obj"/>
|
||||
</geometry>
|
||||
<material name="UR-Blue">
|
||||
<color rgba="0.376 0.576 0.674 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/collision/shoulder.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="3.7"/>
|
||||
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
|
||||
<inertia ixx="0.010267495893" ixy="0.0" ixz="0.0" iyy="0.010267495893" iyz="0.0" izz="0.00666"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="shoulder_lift_joint" type="revolute">
|
||||
<parent link="shoulder_link"/>
|
||||
<child link="upper_arm_link"/>
|
||||
<origin rpy="0.0 1.57079632679 0.0" xyz="0.0 0.13585 0.0"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit effort="150.0" lower="-3.14159265359" upper="3.14159265359" velocity="3.15"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<link name="upper_arm_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/visual/upperarm.obj"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/collision/upperarm.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="8.393"/>
|
||||
<origin rpy="0 0 0" xyz="0.0 0.0 0.28"/>
|
||||
<inertia ixx="0.22689067591" ixy="0.0" ixz="0.0" iyy="0.22689067591" iyz="0.0" izz="0.0151074"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="elbow_joint" type="revolute">
|
||||
<parent link="upper_arm_link"/>
|
||||
<child link="forearm_link"/>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 -0.1197 0.425"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit effort="150.0" lower="-3.14159265359" upper="3.14159265359" velocity="3.15"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<link name="forearm_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/visual/forearm.obj"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/collision/forearm.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="2.275"/>
|
||||
<origin rpy="0 0 0" xyz="0.0 0.0 0.25"/>
|
||||
<inertia ixx="0.049443313556" ixy="0.0" ixz="0.0" iyy="0.049443313556" iyz="0.0" izz="0.004095"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="wrist_1_joint" type="revolute">
|
||||
<parent link="forearm_link"/>
|
||||
<child link="wrist_1_link"/>
|
||||
<origin rpy="0.0 1.57079632679 0.0" xyz="0.0 0.0 0.39225"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit effort="28.0" lower="-3.14159265359" upper="3.14159265359" velocity="3.2"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<link name="wrist_1_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/visual/wrist1.obj"/>
|
||||
</geometry>
|
||||
<material name="UR-Blue">
|
||||
<color rgba="0.376 0.576 0.674 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/collision/wrist1.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1.219"/>
|
||||
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
|
||||
<inertia ixx="0.111172755531" ixy="0.0" ixz="0.0" iyy="0.111172755531" iyz="0.0" izz="0.21942"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="wrist_2_joint" type="revolute">
|
||||
<parent link="wrist_1_link"/>
|
||||
<child link="wrist_2_link"/>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.093 0.0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="28.0" lower="-3.14159265359" upper="3.14159265359" velocity="3.2"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<link name="wrist_2_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/visual/wrist2.obj"/>
|
||||
</geometry>
|
||||
<material name="DarkGray">
|
||||
<color rgba="0.5 0.5 0.5 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/collision/wrist2.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1.219"/>
|
||||
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
|
||||
<inertia ixx="0.111172755531" ixy="0.0" ixz="0.0" iyy="0.111172755531" iyz="0.0" izz="0.21942"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="wrist_3_joint" type="revolute">
|
||||
<parent link="wrist_2_link"/>
|
||||
<child link="wrist_3_link"/>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.09465"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit effort="28.0" lower="-3.14159265359" upper="3.14159265359" velocity="3.2"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<link name="wrist_3_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/visual/wrist3.obj"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/ur5/collision/wrist3.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.1879"/>
|
||||
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
|
||||
<inertia ixx="0.0171364731454" ixy="0.0" ixz="0.0" iyy="0.0171364731454" iyz="0.0" izz="0.033822"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="ee_fixed_joint" type="fixed">
|
||||
<parent link="wrist_3_link"/>
|
||||
<child link="ee_link"/>
|
||||
<origin rpy="0.0 0.0 1.57079632679" xyz="0.0 0.0823 0.0"/>
|
||||
</joint>
|
||||
<link name="ee_link">
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.01 0.01 0.01"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="-0.01 0 0"/>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<!--################################################
|
||||
######## gripper #####
|
||||
####################################################-->
|
||||
|
||||
<link name="robotiq_arg2f_base_link">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="8.625E-08 -4.6583E-06 0.03145"/>
|
||||
<mass value="0.22652"/>
|
||||
<inertia ixx="0.00020005" ixy="-4.2442E-10" ixz="-2.9069E-10" iyy="0.00017832" iyz="-3.4402E-08" izz="0.00013478"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/robotiq_85/visual/robotiq_arg2f_85_base_link.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/robotiq_85/collision/robotiq_arg2f_base_link.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="finger_joint" type="revolute">
|
||||
<origin rpy="0 0 3.14159265359" xyz="0 -0.0306011 0.054904"/>
|
||||
<parent link="robotiq_arg2f_base_link"/>
|
||||
<child link="left_outer_knuckle"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit effort="1000" lower="0" upper="0.8" velocity="2.0"/>
|
||||
</joint>
|
||||
<link name="left_outer_knuckle">
|
||||
<inertial>
|
||||
<origin xyz="-0.000200000000003065 0.0199435877845359 0.0292245259211331" rpy="0 0 0" />
|
||||
<mass value="0.00853198276973456" />
|
||||
<inertia ixx="2.89328108496468E-06" ixy="-1.57935047237397E-19" ixz="-1.93980378593255E-19" iyy="1.86719750325683E-06" iyz="-1.21858577871576E-06" izz="1.21905238907251E-06" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/robotiq_85/visual/robotiq_arg2f_85_outer_knuckle.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 0 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/robotiq_85/collision/robotiq_arg2f_85_outer_knuckle.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="left_outer_finger_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0.0315 -0.0041"/>
|
||||
<parent link="left_outer_knuckle"/>
|
||||
<child link="left_outer_finger"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
</joint>
|
||||
<link name="left_outer_finger">
|
||||
<inertial>
|
||||
<origin xyz="0.00030115855001899 0.0373907951953854 -0.0208027427000385" rpy="0 0 0" />
|
||||
<mass value="0.022614240507152" />
|
||||
<inertia ixx="1.52518312458174E-05" ixy="9.76583423954399E-10" ixz="-5.43838577022588E-10" iyy="6.17694243867776E-06" iyz="6.78636130740228E-06" izz="1.16494917907219E-05" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/robotiq_85/visual/robotiq_arg2f_85_outer_finger.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0 1 1 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/robotiq_85/collision/robotiq_arg2f_85_outer_finger.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="left_inner_finger_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0 0.0061 0.0471"/>
|
||||
<parent link="left_outer_finger"/>
|
||||
<child link="left_inner_finger"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit effort="1000" lower="-0.8757" upper="0" velocity="2.0"/>
|
||||
<mimic joint="finger_joint" multiplier="-1" offset="0"/>
|
||||
</joint>
|
||||
<link name="left_inner_finger">
|
||||
<inertial>
|
||||
<origin xyz="0.000299999999999317 0.0160078233491243 -0.0136945669206257" rpy="0 0 0" />
|
||||
<mass value="0.0104003125914103" />
|
||||
<inertia ixx="2.71909453810972E-06" ixy="1.35402465472579E-21" ixz="-7.1817349065269E-22" iyy="7.69100314106116E-07" iyz="6.74715432769696E-07" izz="2.30315190420171E-06" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/robotiq_85/visual/robotiq_arg2f_85_inner_finger.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="1 0 1 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/robotiq_85/collision/robotiq_arg2f_85_inner_finger.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="left_inner_finger_pad_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 -0.0220203446692936 0.03242"/>
|
||||
<parent link="left_inner_finger"/>
|
||||
<child link="left_inner_finger_pad"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
</joint>
|
||||
<link name="left_inner_finger_pad">
|
||||
<inertial>
|
||||
<mass value="1."/>
|
||||
<inertia ixx="1E-10" ixy="1E-10" ixz="1E-10" iyy="1E-10" iyz="1E-10" izz="1E-10"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.022 0.00635 0.0375"/>
|
||||
</geometry>
|
||||
<material name="UR-Blue">
|
||||
<color rgba="0.376 0.576 0.674 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.022 0.00635 0.0375"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.9 0.0 0.0 1"/>
|
||||
</material>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="left_inner_knuckle_joint" type="revolute">
|
||||
<!-- <origin xyz="0 ${reflect * -0.0127} 0.06142" rpy="${pi / 2 + .725} 0 ${(reflect - 1) * pi / 2}" /> -->
|
||||
<origin rpy="0 0 3.14159265359" xyz="0 -0.0127 0.06142"/>
|
||||
<parent link="robotiq_arg2f_base_link"/>
|
||||
<child link="left_inner_knuckle"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit effort="1000" lower="0" upper="0.8757" velocity="2.0"/>
|
||||
<mimic joint="finger_joint" multiplier="1" offset="0"/>
|
||||
</joint>
|
||||
<link name="left_inner_knuckle">
|
||||
<inertial>
|
||||
<origin xyz="0.000123011831763771 0.0507850843201817 0.00103968640075166" rpy="0 0 0" />
|
||||
<mass value="0.0271177346495152" />
|
||||
<inertia ixx="2.61910379223783E-05" ixy="-2.43616858946494E-07" ixz="-6.37789906117123E-09" iyy="2.8270243746167E-06" iyz="-5.37200748039765E-07" izz="2.83695868220296E-05" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/robotiq_85/visual/robotiq_arg2f_85_inner_knuckle.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0 0 1 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/robotiq_85/collision/robotiq_arg2f_85_inner_knuckle.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="right_outer_knuckle_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0 0.0306011 0.054904"/>
|
||||
<parent link="robotiq_arg2f_base_link"/>
|
||||
<child link="right_outer_knuckle"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit effort="1000" lower="0" upper="0.81" velocity="2.0"/>
|
||||
<mimic joint="finger_joint" multiplier="1" offset="0"/>
|
||||
</joint>
|
||||
<link name="right_outer_knuckle">
|
||||
<inertial>
|
||||
<origin xyz="-0.000200000000003065 0.0199435877845359 0.0292245259211331" rpy="0 0 0" />
|
||||
<mass value="0.00853198276973456" />
|
||||
<inertia ixx="2.89328108496468E-06" ixy="-1.57935047237397E-19" ixz="-1.93980378593255E-19" iyy="1.86719750325683E-06" iyz="-1.21858577871576E-06" izz="1.21905238907251E-06" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/robotiq_85/visual/robotiq_arg2f_85_outer_knuckle.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/robotiq_85/collision/robotiq_arg2f_85_outer_knuckle.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="right_outer_finger_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0.0315 -0.0041"/>
|
||||
<parent link="right_outer_knuckle"/>
|
||||
<child link="right_outer_finger"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
</joint>
|
||||
<link name="right_outer_finger">
|
||||
<inertial>
|
||||
<origin xyz="0.00030115855001899 0.0373907951953854 -0.0208027427000385" rpy="0 0 0" />
|
||||
<mass value="0.022614240507152" />
|
||||
<inertia ixx="1.52518312458174E-05" ixy="9.76583423954399E-10" ixz="-5.43838577022588E-10" iyy="6.17694243867776E-06" iyz="6.78636130740228E-06" izz="1.16494917907219E-05" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/robotiq_85/visual/robotiq_arg2f_85_outer_finger.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/robotiq_85/collision/robotiq_arg2f_85_outer_finger.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="right_inner_finger_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0 0.0061 0.0471"/>
|
||||
<parent link="right_outer_finger"/>
|
||||
<child link="right_inner_finger"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit effort="1000" lower="-0.8757" upper="0" velocity="2.0"/>
|
||||
<mimic joint="finger_joint" multiplier="-1" offset="0"/>
|
||||
</joint>
|
||||
<link name="right_inner_finger">
|
||||
<inertial>
|
||||
<origin xyz="0.000299999999999317 0.0160078233491243 -0.0136945669206257" rpy="0 0 0" />
|
||||
<mass value="0.0104003125914103" />
|
||||
<inertia ixx="2.71909453810972E-06" ixy="1.35402465472579E-21" ixz="-7.1817349065269E-22" iyy="7.69100314106116E-07" iyz="6.74715432769696E-07" izz="2.30315190420171E-06" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/robotiq_85/visual/robotiq_arg2f_85_inner_finger.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.1 0.1 0.1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/robotiq_85/collision/robotiq_arg2f_85_inner_finger.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="right_inner_finger_pad_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 -0.0220203446692936 0.03242"/>
|
||||
<parent link="right_inner_finger"/>
|
||||
<child link="right_inner_finger_pad"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
</joint>
|
||||
<link name="right_inner_finger_pad">
|
||||
<inertial>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="1E-10" ixy="1E-10" ixz="1E-10" iyy="1E-10" iyz="1E-10" izz="1E-10"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.022 0.00635 0.0375"/>
|
||||
</geometry>
|
||||
<material name="UR-Blue">
|
||||
<color rgba="0.376 0.576 0.674 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.022 0.00635 0.0375"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="right_inner_knuckle_joint" type="revolute">
|
||||
<!-- <origin xyz="0 ${reflect * -0.0127} 0.06142" rpy="${pi / 2 + .725} 0 ${(reflect - 1) * pi / 2}" /> -->
|
||||
<origin rpy="0 0 0.0" xyz="0 0.0127 0.06142"/>
|
||||
<parent link="robotiq_arg2f_base_link"/>
|
||||
<child link="right_inner_knuckle"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit effort="1000" lower="0" upper="0.8757" velocity="2.0"/>
|
||||
<mimic joint="finger_joint" multiplier="1" offset="0"/>
|
||||
</joint>
|
||||
<link name="right_inner_knuckle">
|
||||
<inertial>
|
||||
<origin xyz="0.000123011831763771 0.0507850843201817 0.00103968640075166" rpy="0 0 0" />
|
||||
<mass value="0.0271177346495152" />
|
||||
<inertia ixx="2.61910379223783E-05" ixy="-2.43616858946494E-07" ixz="-6.37789906117123E-09" iyy="2.8270243746167E-06" iyz="-5.37200748039765E-07" izz="2.83695868220296E-05" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/robotiq_85/visual/robotiq_arg2f_85_inner_knuckle.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../meshes/robotiq_85/collision/robotiq_arg2f_85_inner_knuckle.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<!--################################################
|
||||
######## connect model #####
|
||||
####################################################-->
|
||||
|
||||
<link name="world"/>
|
||||
<!-- connect gripper to arm -->
|
||||
<joint name="robotiq_85_base_joint" type="fixed">
|
||||
<parent link="ee_link"/>
|
||||
<child link="robotiq_arg2f_base_link"/>
|
||||
<origin rpy="0 1.57079632679 0" xyz="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<!-- <joint name="arm_gripper_joint" type="fixed">-->
|
||||
<!-- <parent link="ee_link"/>-->
|
||||
<!-- <child link="robotiq_140_robotiq_arg2f_base_link"/>-->
|
||||
<!-- <origin rpy="-1.57 1.57 -1.57" xyz="0.0 0.0 0.0"/>-->
|
||||
<!-- </joint>-->
|
||||
|
||||
<!-- connect arm to world -->
|
||||
<joint name="world_arm_joint" type="fixed">
|
||||
<parent link="world"/>
|
||||
<child link="base_link"/>
|
||||
<origin rpy="0.0 0.0 0" xyz="0 0 0.78"/>
|
||||
</joint>
|
||||
|
||||
|
||||
</robot>
|
||||
|
||||
|
|
@ -0,0 +1,213 @@
|
|||
import pybullet as p
|
||||
from collections import namedtuple
|
||||
from attrdict import AttrDict
|
||||
import functools
|
||||
import os
|
||||
from datetime import datetime
|
||||
|
||||
|
||||
def setup_sisbot(p, robotID, gripper_type):
|
||||
controlJoints = ["shoulder_pan_joint", "shoulder_lift_joint",
|
||||
"elbow_joint", "wrist_1_joint",
|
||||
"wrist_2_joint", "wrist_3_joint",
|
||||
"finger_joint"]
|
||||
jointTypeList = ["REVOLUTE", "PRISMATIC", "SPHERICAL", "PLANAR", "FIXED"]
|
||||
numJoints = p.getNumJoints(robotID)
|
||||
jointInfo = namedtuple("jointInfo",
|
||||
["id", "name", "type", "lowerLimit", "upperLimit", "maxForce", "maxVelocity",
|
||||
"controllable"])
|
||||
joints = AttrDict()
|
||||
for i in range(numJoints):
|
||||
info = p.getJointInfo(robotID, i)
|
||||
jointID = info[0]
|
||||
jointName = info[1].decode("utf-8")
|
||||
jointType = jointTypeList[info[2]]
|
||||
jointLowerLimit = info[8]
|
||||
jointUpperLimit = info[9]
|
||||
jointMaxForce = info[10]
|
||||
jointMaxVelocity = info[11]
|
||||
controllable = True if jointName in controlJoints else False
|
||||
info = jointInfo(jointID, jointName, jointType, jointLowerLimit,
|
||||
jointUpperLimit, jointMaxForce, jointMaxVelocity, controllable)
|
||||
if info.type == "REVOLUTE": # set revolute joint to static
|
||||
p.setJointMotorControl2(
|
||||
robotID, info.id, p.VELOCITY_CONTROL, targetVelocity=0, force=0)
|
||||
joints[info.name] = info
|
||||
|
||||
# explicitly deal with mimic joints
|
||||
def controlGripper(robotID, parent, children, mul, **kwargs):
|
||||
controlMode = kwargs.pop("controlMode")
|
||||
if controlMode == p.POSITION_CONTROL:
|
||||
pose = kwargs.pop("targetPosition")
|
||||
# move parent joint
|
||||
p.setJointMotorControl2(robotID, parent.id, controlMode, targetPosition=pose,
|
||||
force=parent.maxForce, maxVelocity=parent.maxVelocity)
|
||||
# move child joints
|
||||
for name in children:
|
||||
child = children[name]
|
||||
childPose = pose * mul[child.name]
|
||||
p.setJointMotorControl2(robotID, child.id, controlMode, targetPosition=childPose,
|
||||
force=child.maxForce, maxVelocity=child.maxVelocity)
|
||||
else:
|
||||
raise NotImplementedError(
|
||||
"controlGripper does not support \"{}\" control mode".format(controlMode))
|
||||
# check if there
|
||||
if len(kwargs) is not 0:
|
||||
raise KeyError("No keys {} in controlGripper".format(
|
||||
", ".join(kwargs.keys())))
|
||||
|
||||
assert gripper_type in ['85', '140']
|
||||
mimicParentName = "finger_joint"
|
||||
if gripper_type == '85':
|
||||
mimicChildren = {"right_outer_knuckle_joint": 1,
|
||||
"left_inner_knuckle_joint": 1,
|
||||
"right_inner_knuckle_joint": 1,
|
||||
"left_inner_finger_joint": -1,
|
||||
"right_inner_finger_joint": -1}
|
||||
else:
|
||||
mimicChildren = {
|
||||
"right_outer_knuckle_joint": -1,
|
||||
"left_inner_knuckle_joint": -1,
|
||||
"right_inner_knuckle_joint": -1,
|
||||
"left_inner_finger_joint": 1,
|
||||
"right_inner_finger_joint": 1}
|
||||
parent = joints[mimicParentName]
|
||||
children = AttrDict((j, joints[j])
|
||||
for j in joints if j in mimicChildren.keys())
|
||||
controlRobotiqC2 = functools.partial(
|
||||
controlGripper, robotID, parent, children, mimicChildren)
|
||||
|
||||
return joints, controlRobotiqC2, controlJoints, mimicParentName
|
||||
|
||||
|
||||
def setup_sisbot_force(p, robotID, gripper_type):
|
||||
controlJoints = ["shoulder_pan_joint", "shoulder_lift_joint",
|
||||
"elbow_joint", "wrist_1_joint",
|
||||
"wrist_2_joint", "wrist_3_joint",
|
||||
"finger_joint"]
|
||||
jointTypeList = ["REVOLUTE", "PRISMATIC", "SPHERICAL", "PLANAR", "FIXED"]
|
||||
numJoints = p.getNumJoints(robotID)
|
||||
jointInfo = namedtuple("jointInfo",
|
||||
["id", "name", "type", "lowerLimit", "upperLimit", "maxForce", "maxVelocity",
|
||||
"controllable", "jointAxis", "parentFramePos", "parentFrameOrn"])
|
||||
joints = AttrDict()
|
||||
for i in range(numJoints):
|
||||
info = p.getJointInfo(robotID, i)
|
||||
jointID = info[0]
|
||||
jointName = info[1].decode("utf-8")
|
||||
jointType = jointTypeList[info[2]]
|
||||
jointLowerLimit = info[8]
|
||||
jointUpperLimit = info[9]
|
||||
jointMaxForce = info[10]
|
||||
jointMaxVelocity = info[11]
|
||||
jointAxis = info[13]
|
||||
parentFramePos = info[14]
|
||||
parentFrameOrn = info[15]
|
||||
controllable = True if jointName in controlJoints else False
|
||||
info = jointInfo(jointID, jointName, jointType, jointLowerLimit,
|
||||
jointUpperLimit, jointMaxForce, jointMaxVelocity, controllable,
|
||||
jointAxis, parentFramePos, parentFrameOrn)
|
||||
if info.type == "REVOLUTE": # set revolute joint to static
|
||||
p.setJointMotorControl2(
|
||||
robotID, info.id, p.VELOCITY_CONTROL, targetVelocity=0, force=0)
|
||||
joints[info.name] = info
|
||||
for j in joints:
|
||||
print(joints[j])
|
||||
# explicitly deal with mimic joints
|
||||
|
||||
def controlGripper(robotID, parent, children, mul, **kwargs):
|
||||
controlMode = kwargs.pop("controlMode")
|
||||
if controlMode == p.POSITION_CONTROL:
|
||||
pose = kwargs.pop("targetPosition")
|
||||
# move parent joint
|
||||
p.setJointMotorControl2(robotID, parent.id, controlMode, targetPosition=pose,
|
||||
force=parent.maxForce, maxVelocity=parent.maxVelocity)
|
||||
# p.setJointMotorControl2(robotID, parent.id, p.TORQUE_CONTROL,
|
||||
# force=10, maxVelocity=parent.maxVelocity)
|
||||
return
|
||||
# move child joints
|
||||
for name in children:
|
||||
child = children[name]
|
||||
childPose = pose * mul[child.name]
|
||||
p.setJointMotorControl2(robotID, child.id, controlMode, targetPosition=childPose,
|
||||
force=child.maxForce, maxVelocity=child.maxVelocity)
|
||||
else:
|
||||
raise NotImplementedError(
|
||||
"controlGripper does not support \"{}\" control mode".format(controlMode))
|
||||
# check if there
|
||||
if len(kwargs) is not 0:
|
||||
raise KeyError("No keys {} in controlGripper".format(
|
||||
", ".join(kwargs.keys())))
|
||||
|
||||
assert gripper_type in ['85', '140']
|
||||
mimicParentName = "finger_joint"
|
||||
if gripper_type == '85':
|
||||
mimicChildren = {"right_outer_knuckle_joint": 1,
|
||||
"left_inner_knuckle_joint": 1,
|
||||
"right_inner_knuckle_joint": 1,
|
||||
"left_inner_finger_joint": -1,
|
||||
"right_inner_finger_joint": -1}
|
||||
else:
|
||||
mimicChildren = {
|
||||
"right_outer_knuckle_joint": -1,
|
||||
"left_inner_knuckle_joint": -1,
|
||||
"right_inner_knuckle_joint": -1,
|
||||
"left_inner_finger_joint": 1,
|
||||
"right_inner_finger_joint": 1}
|
||||
parent = joints[mimicParentName]
|
||||
children = AttrDict((j, joints[j])
|
||||
for j in joints if j in mimicChildren.keys())
|
||||
# Create all the gear constraint
|
||||
for name in children:
|
||||
child = children[name]
|
||||
c = p.createConstraint(robotID, parent.id, robotID, child.id, p.JOINT_GEAR, child.jointAxis,
|
||||
# child.parentFramePos, (0, 0, 0), child.parentFrameOrn, (0, 0, 0))
|
||||
(0, 0, 0), (0, 0, 0), (0, 0, 0), (0, 0, 0))
|
||||
p.changeConstraint(c, gearRatio=-mimicChildren[name], maxForce=10000)
|
||||
controlRobotiqC2 = functools.partial(
|
||||
controlGripper, robotID, parent, children, mimicChildren)
|
||||
|
||||
return joints, controlRobotiqC2, controlJoints, mimicParentName
|
||||
|
||||
|
||||
class Camera:
|
||||
def __init__(self, cam_pos, cam_target, near, far, size, fov):
|
||||
self.x, self.y, self.z = cam_pos
|
||||
self.x_t, self.y_t, self.z_t = cam_target
|
||||
self.width, self.height = size
|
||||
self.near, self.far = near, far
|
||||
self.fov = fov
|
||||
|
||||
aspect = self.width / self.height
|
||||
self.projection_matrix = p.computeProjectionMatrixFOV(
|
||||
fov, aspect, near, far)
|
||||
self.view_matrix = p.computeViewMatrix(cam_pos, cam_target, [0, 1, 0])
|
||||
|
||||
self.rec_id = None
|
||||
|
||||
def get_cam_img(self):
|
||||
"""
|
||||
Method to get images from camera
|
||||
return:
|
||||
rgb
|
||||
depth
|
||||
segmentation mask
|
||||
"""
|
||||
# Get depth values using the OpenGL renderer
|
||||
_w, _h, rgb, depth, seg = p.getCameraImage(self.width, self.height,
|
||||
self.view_matrix, self.projection_matrix,
|
||||
)
|
||||
return rgb[:, :, 0:3], depth, seg
|
||||
|
||||
def start_recording(self, save_dir):
|
||||
if not os.path.exists(save_dir):
|
||||
os.mkdir(save_dir)
|
||||
now = datetime.now().strftime('%Y-%m-%d_%H-%M-%S')
|
||||
file = f'{save_dir}/{now}.mp4'
|
||||
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
|
||||
self.rec_id = p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, file)
|
||||
|
||||
def stop_recording(self):
|
||||
p.stopStateLogging(self.rec_id)
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 1)
|
||||
|
|
@ -0,0 +1,196 @@
|
|||
from grasp_generator import GraspGenerator
|
||||
from environment.utilities import Camera
|
||||
from environment.env import Environment
|
||||
from utils import YcbObjects, PackPileData, IsolatedObjData, summarize
|
||||
import pybullet as p
|
||||
import os
|
||||
import sys
|
||||
sys.path.append('network')
|
||||
|
||||
|
||||
def isolated_obj_scenario(n, vis, output, debug):
|
||||
|
||||
objects = YcbObjects('objects/ycb_objects',
|
||||
mod_orn=['ChipsCan', 'MustardBottle',
|
||||
'TomatoSoupCan'],
|
||||
mod_stiffness=['Strawberry'])
|
||||
data = IsolatedObjData(objects.obj_names, n, 'results')
|
||||
|
||||
center_x, center_y = 0.05, -0.52
|
||||
network_path = 'network/trained-models/cornell-randsplit-rgbd-grconvnet3-drop1-ch32/epoch_19_iou_0.98'
|
||||
camera = Camera((center_x, center_y, 1.9), (center_x,
|
||||
center_y, 0.785), 0.2, 2.0, (224, 224), 40)
|
||||
env = Environment(camera, vis=vis, debug=debug)
|
||||
generator = GraspGenerator(network_path, camera, 5)
|
||||
|
||||
for obj_name in objects.obj_names:
|
||||
print(obj_name)
|
||||
for _ in range(n):
|
||||
|
||||
path, mod_orn, mod_stiffness = objects.get_obj_info(obj_name)
|
||||
env.load_isolated_obj(path, mod_orn, mod_stiffness)
|
||||
env.move_away_arm()
|
||||
|
||||
rgb, depth, _ = camera.get_cam_img()
|
||||
grasps, save_name = generator.predict_grasp(
|
||||
rgb, depth, n_grasps=3, show_output=output)
|
||||
for i, grasp in enumerate(grasps):
|
||||
data.add_try(obj_name)
|
||||
x, y, z, roll, opening_len, obj_height = grasp
|
||||
if vis:
|
||||
debug_id = p.addUserDebugLine(
|
||||
[x, y, z], [x, y, 1.2], [0, 0, 1])
|
||||
|
||||
succes_grasp, succes_target = env.grasp(
|
||||
(x, y, z), roll, opening_len, obj_height)
|
||||
if vis:
|
||||
p.removeUserDebugItem(debug_id)
|
||||
if succes_grasp:
|
||||
data.add_succes_grasp(obj_name)
|
||||
if succes_target:
|
||||
data.add_succes_target(obj_name)
|
||||
if save_name is not None:
|
||||
os.rename(save_name + '.png', save_name +
|
||||
f'_SUCCESS_grasp{i}.png')
|
||||
break
|
||||
env.reset_all_obj()
|
||||
env.remove_all_obj()
|
||||
|
||||
data.write_json()
|
||||
summarize(data.save_dir, n)
|
||||
|
||||
|
||||
def pile_scenario(n, vis, output, debug):
|
||||
|
||||
data = PackPileData(5, n, 'results', 'pile')
|
||||
objects = YcbObjects('objects/ycb_objects',
|
||||
mod_orn=['ChipsCan', 'MustardBottle',
|
||||
'TomatoSoupCan'],
|
||||
mod_stiffness=['Strawberry'],
|
||||
exclude=['CrackerBox', 'Hammer'])
|
||||
center_x, center_y = 0.05, -0.52
|
||||
network_path = 'network/trained-models/cornell-randsplit-rgbd-grconvnet3-drop1-ch32/epoch_19_iou_0.98'
|
||||
camera = Camera((center_x, center_y, 1.9), (center_x,
|
||||
center_y, 0.785), 0.2, 2.0, (224, 224), 40)
|
||||
env = Environment(camera, vis=vis, debug=debug, finger_length=0.06)
|
||||
generator = GraspGenerator(network_path, camera, 5)
|
||||
|
||||
for i in range(n):
|
||||
print(f'Trial {i}')
|
||||
straight_fails = 0
|
||||
objects.shuffle_objects()
|
||||
|
||||
env.move_away_arm()
|
||||
info = objects.get_n_first_obj_info(5)
|
||||
env.create_pile(info)
|
||||
|
||||
straight_fails = 0
|
||||
while len(env.obj_ids) != 0 and straight_fails < 3:
|
||||
print(f'N objs:{len(env.obj_ids)} straight fails:{straight_fails}')
|
||||
|
||||
env.move_away_arm()
|
||||
env.reset_all_obj()
|
||||
rgb, depth, _ = camera.get_cam_img()
|
||||
grasps, save_name = generator.predict_grasp(
|
||||
rgb, depth, n_grasps=3, show_output=output)
|
||||
|
||||
for i, grasp in enumerate(grasps):
|
||||
data.add_try()
|
||||
x, y, z, roll, opening_len, obj_height = grasp
|
||||
|
||||
if vis:
|
||||
debugID = p.addUserDebugLine(
|
||||
[x, y, z], [x, y, 1.2], [0, 0, 1])
|
||||
|
||||
succes_grasp, succes_target = env.grasp(
|
||||
(x, y, z), roll, opening_len, obj_height)
|
||||
if vis:
|
||||
p.removeUserDebugItem(debugID)
|
||||
if succes_grasp:
|
||||
data.add_succes_grasp()
|
||||
if succes_target:
|
||||
data.add_succes_target()
|
||||
straight_fails = 0
|
||||
if save_name is not None:
|
||||
os.rename(save_name + '.png', save_name +
|
||||
f'_SUCCESS_grasp{i}.png')
|
||||
break
|
||||
else:
|
||||
straight_fails += 1
|
||||
|
||||
if straight_fails == 3 or len(env.obj_ids) == 0:
|
||||
break
|
||||
|
||||
env.reset_all_obj()
|
||||
env.remove_all_obj()
|
||||
data.summarize()
|
||||
|
||||
|
||||
def pack_scenario(n, vis, output, debug):
|
||||
vis = vis
|
||||
output = output
|
||||
debug = debug
|
||||
|
||||
data = PackPileData(5, n, 'results', 'pack')
|
||||
objects = YcbObjects('objects/ycb_objects',
|
||||
mod_orn=['ChipsCan', 'MustardBottle',
|
||||
'TomatoSoupCan'],
|
||||
mod_stiffness=['Strawberry'])
|
||||
center_x, center_y = 0.05, -0.52
|
||||
network_path = 'network/trained-models/cornell-randsplit-rgbd-grconvnet3-drop1-ch32/epoch_19_iou_0.98'
|
||||
camera = Camera((center_x, center_y, 1.9), (center_x,
|
||||
center_y, 0.785), 0.2, 2.0, (224, 224), 40)
|
||||
env = Environment(camera, vis=vis, debug=debug, finger_length=0.06)
|
||||
generator = GraspGenerator(network_path, camera, 5)
|
||||
|
||||
for i in range(n):
|
||||
print(f'Trial {i}')
|
||||
straight_fails = 0
|
||||
objects.shuffle_objects()
|
||||
info = objects.get_n_first_obj_info(5)
|
||||
env.create_packed(info)
|
||||
|
||||
straight_fails = 0
|
||||
while len(env.obj_ids) != 0 and straight_fails < 3:
|
||||
env.move_away_arm()
|
||||
env.reset_all_obj()
|
||||
rgb, depth, _ = camera.get_cam_img()
|
||||
grasps, save_name = generator.predict_grasp(
|
||||
rgb, depth, n_grasps=3, show_output=output)
|
||||
|
||||
for i, grasp in enumerate(grasps):
|
||||
data.add_try()
|
||||
x, y, z, roll, opening_len, obj_height = grasp
|
||||
|
||||
if vis:
|
||||
debugID = p.addUserDebugLine(
|
||||
[x, y, z], [x, y, 1.2], [0, 0, 1])
|
||||
|
||||
succes_grasp, succes_target = env.grasp(
|
||||
(x, y, z), roll, opening_len, obj_height)
|
||||
if vis:
|
||||
p.removeUserDebugItem(debugID)
|
||||
if succes_grasp:
|
||||
data.add_succes_grasp()
|
||||
if succes_target:
|
||||
data.add_succes_target()
|
||||
straight_fails = 0
|
||||
if save_name is not None:
|
||||
os.rename(save_name + '.png', save_name +
|
||||
f'_SUCCESS_grasp{i}.png')
|
||||
break
|
||||
else:
|
||||
straight_fails += 1
|
||||
|
||||
if straight_fails == 3 or len(env.obj_ids) == 0:
|
||||
break
|
||||
|
||||
env.reset_all_obj()
|
||||
env.remove_all_obj()
|
||||
data.summarize()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
# isolated_obj_scenario(100, vis=False, output=True, debug=False)
|
||||
# pack_scenario(100, vis=False, output=True, debug=False)
|
||||
pile_scenario(100, vis=False, output=True, debug=False)
|
||||
|
|
@ -0,0 +1,135 @@
|
|||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
from numpy.lib.npyio import save
|
||||
import torch.utils.data
|
||||
from PIL import Image
|
||||
from datetime import datetime
|
||||
|
||||
from network.hardware.device import get_device
|
||||
from network.inference.post_process import post_process_output
|
||||
from network.utils.data.camera_data import CameraData
|
||||
from network.utils.visualisation.plot import plot_results, save_results
|
||||
from network.utils.dataset_processing.grasp import detect_grasps
|
||||
import os
|
||||
|
||||
|
||||
class GraspGenerator:
|
||||
IMG_WIDTH = 224
|
||||
IMG_ROTATION = -np.pi * 0.5
|
||||
CAM_ROTATION = 0
|
||||
PIX_CONVERSION = 277
|
||||
DIST_BACKGROUND = 1.115
|
||||
MAX_GRASP = 0.085
|
||||
|
||||
def __init__(self, net_path, camera, depth_radius):
|
||||
self.net = torch.load(net_path, map_location='cpu')
|
||||
self.device = get_device(force_cpu=True)
|
||||
|
||||
self.near = camera.near
|
||||
self.far = camera.far
|
||||
self.depth_r = depth_radius
|
||||
|
||||
# Get rotation matrix
|
||||
img_center = self.IMG_WIDTH / 2 - 0.5
|
||||
self.img_to_cam = self.get_transform_matrix(-img_center/self.PIX_CONVERSION,
|
||||
img_center/self.PIX_CONVERSION,
|
||||
0,
|
||||
self.IMG_ROTATION)
|
||||
self.cam_to_robot_base = self.get_transform_matrix(
|
||||
camera.x, camera.y, camera.z, self.CAM_ROTATION)
|
||||
|
||||
def get_transform_matrix(self, x, y, z, rot):
|
||||
return np.array([
|
||||
[np.cos(rot), -np.sin(rot), 0, x],
|
||||
[np.sin(rot), np.cos(rot), 0, y],
|
||||
[0, 0, 1, z],
|
||||
[0, 0, 0, 1]
|
||||
])
|
||||
|
||||
def grasp_to_robot_frame(self, grasp, depth_img):
|
||||
"""
|
||||
return: x, y, z, roll, opening length gripper, object height
|
||||
"""
|
||||
# Get x, y, z of center pixel
|
||||
x_p, y_p = grasp.center[0], grasp.center[1]
|
||||
|
||||
# Get area of depth values around center pixel
|
||||
x_min = np.clip(x_p-self.depth_r, 0, self.IMG_WIDTH)
|
||||
x_max = np.clip(x_p+self.depth_r, 0, self.IMG_WIDTH)
|
||||
y_min = np.clip(y_p-self.depth_r, 0, self.IMG_WIDTH)
|
||||
y_max = np.clip(y_p+self.depth_r, 0, self.IMG_WIDTH)
|
||||
depth_values = depth_img[x_min:x_max, y_min:y_max]
|
||||
|
||||
# Get minimum depth value from selected area
|
||||
z_p = np.amin(depth_values)
|
||||
|
||||
# Convert pixels to meters
|
||||
x_p /= self.PIX_CONVERSION
|
||||
y_p /= self.PIX_CONVERSION
|
||||
z_p = self.far * self.near / (self.far - (self.far - self.near) * z_p)
|
||||
|
||||
# Convert image space to camera's 3D space
|
||||
img_xyz = np.array([x_p, y_p, -z_p, 1])
|
||||
cam_space = np.matmul(self.img_to_cam, img_xyz)
|
||||
|
||||
# Convert camera's 3D space to robot frame of reference
|
||||
robot_frame_ref = np.matmul(self.cam_to_robot_base, cam_space)
|
||||
|
||||
# Change direction of the angle and rotate by alpha rad
|
||||
roll = grasp.angle * -1 + (self.IMG_ROTATION)
|
||||
if roll < -np.pi / 2:
|
||||
roll += np.pi
|
||||
|
||||
# Covert pixel width to gripper width
|
||||
opening_length = (grasp.length / int(self.MAX_GRASP *
|
||||
self.PIX_CONVERSION)) * self.MAX_GRASP
|
||||
|
||||
obj_height = self.DIST_BACKGROUND - z_p
|
||||
|
||||
# return x, y, z, roll, opening length gripper
|
||||
return robot_frame_ref[0], robot_frame_ref[1], robot_frame_ref[2], roll, opening_length, obj_height
|
||||
|
||||
def predict(self, rgb, depth, n_grasps=1, show_output=False):
|
||||
depth = np.expand_dims(np.array(depth), axis=2)
|
||||
img_data = CameraData(width=self.IMG_WIDTH, height=self.IMG_WIDTH)
|
||||
x, depth_img, rgb_img = img_data.get_data(rgb=rgb, depth=depth)
|
||||
|
||||
with torch.no_grad():
|
||||
xc = x.to(self.device)
|
||||
pred = self.net.predict(xc)
|
||||
pixels_max_grasp = int(self.MAX_GRASP * self.PIX_CONVERSION)
|
||||
q_img, ang_img, width_img = post_process_output(pred['pos'],
|
||||
pred['cos'],
|
||||
pred['sin'],
|
||||
pred['width'],
|
||||
pixels_max_grasp)
|
||||
save_name = None
|
||||
if show_output:
|
||||
fig = plt.figure(figsize=(10, 10))
|
||||
plot_results(fig=fig,
|
||||
rgb_img=img_data.get_rgb(rgb, False),
|
||||
grasp_q_img=q_img,
|
||||
grasp_angle_img=ang_img,
|
||||
no_grasps=3,
|
||||
grasp_width_img=width_img)
|
||||
|
||||
if not os.path.exists('network_output'):
|
||||
os.mkdir('network_output')
|
||||
time = datetime.now().strftime('%Y-%m-%d %H:%M:%S')
|
||||
save_name = 'network_output/{}'.format(time)
|
||||
fig.savefig(save_name + '.png')
|
||||
|
||||
grasps = detect_grasps(
|
||||
q_img, ang_img, width_img=width_img, no_grasps=n_grasps)
|
||||
return grasps, save_name
|
||||
|
||||
def predict_grasp(self, rgb, depth, n_grasps=1, show_output=False):
|
||||
predictions, save_name = self.predict(
|
||||
rgb, depth, n_grasps=n_grasps, show_output=show_output)
|
||||
grasps = []
|
||||
for grasp in predictions:
|
||||
x, y, z, roll, opening_len, obj_height = self.grasp_to_robot_frame(
|
||||
grasp, depth)
|
||||
grasps.append((x, y, z, roll, opening_len, obj_height))
|
||||
|
||||
return grasps, save_name
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 421 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 532 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 420 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 421 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 125 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 131 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 323 KiB |
|
|
@ -0,0 +1,44 @@
|
|||
BSD 3-Clause License
|
||||
|
||||
Copyright (c) 2019, Sulabh Kumra, Multi-Agent Bio-Robotics Laboratory (MABL), Rochester Institute of Technology
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
|
||||
1. Redistributions of source code must retain the above copyright notice, this
|
||||
list of conditions and the following disclaimer.
|
||||
|
||||
2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
this list of conditions and the following disclaimer in the documentation
|
||||
and/or other materials provided with the distribution.
|
||||
|
||||
3. Neither the name of the copyright holder nor the names of its
|
||||
contributors may be used to endorse or promote products derived from
|
||||
this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
|
||||
|
||||
Copyright (c) 2018, Douglas Morrison, ARC Centre of Excellence for Robotic Vision (ACRV), Queensland University of Technology
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
|
||||
|
||||
1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
|
||||
|
||||
2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
|
||||
|
||||
3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
|
||||
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
|
|
@ -0,0 +1,104 @@
|
|||
# Antipodal Robotic Grasping
|
||||
We present a novel generative residual convolutional neural network based model architecture which detects objects in the camera’s field of view and predicts a suitable antipodal grasp configuration for the objects in the image.
|
||||
|
||||
This repository contains the implementation of the Generative Residual Convolutional Neural Network (GR-ConvNet) from the paper:
|
||||
|
||||
#### Antipodal Robotic Grasping using Generative Residual Convolutional Neural Network
|
||||
|
||||
Sulabh Kumra, Shirin Joshi, Ferat Sahin
|
||||
|
||||
[arxiv](https://arxiv.org/abs/1909.04810) | [video](https://youtu.be/cwlEhdoxY4U)
|
||||
|
||||
[](https://paperswithcode.com/sota/robotic-grasping-on-cornell-grasp-dataset?p=antipodal-robotic-grasping-using-generative)
|
||||
|
||||
If you use this project in your research or wish to refer to the baseline results published in the paper, please use the following BibTeX entry:
|
||||
|
||||
```
|
||||
@inproceedings{kumra2019antipodal,
|
||||
title={Antipodal Robotic Grasping using Generative Residual Convolutional Neural Network},
|
||||
author={Kumra, Sulabh and Joshi, Shirin and Sahin, Ferat},
|
||||
booktitle={2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)},
|
||||
year={2020},
|
||||
organization={IEEE}
|
||||
}
|
||||
```
|
||||
|
||||
## Requirements
|
||||
|
||||
- numpy
|
||||
- opencv-python
|
||||
- matplotlib
|
||||
- scikit-image
|
||||
- imageio
|
||||
- torch
|
||||
- torchvision
|
||||
- torchsummary
|
||||
- tensorboardX
|
||||
- pyrealsense2
|
||||
- Pillow
|
||||
|
||||
## Installation
|
||||
- Checkout the robotic grasping package
|
||||
```bash
|
||||
$ git clone https://github.com/skumra/robotic-grasping.git
|
||||
```
|
||||
|
||||
- Create a virtual environment
|
||||
```bash
|
||||
$ python3.6 -m venv --system-site-packages venv
|
||||
```
|
||||
|
||||
- Source the virtual environment
|
||||
```bash
|
||||
$ source venv/bin/activate
|
||||
```
|
||||
|
||||
- Install the requirements
|
||||
```bash
|
||||
$ cd robotic-grasping
|
||||
$ pip install -r requirements.txt
|
||||
```
|
||||
|
||||
## Datasets
|
||||
|
||||
This repository supports both the [Cornell Grasping Dataset](http://pr.cs.cornell.edu/grasping/rect_data/data.php) and
|
||||
[Jacquard Dataset](https://jacquard.liris.cnrs.fr/).
|
||||
|
||||
#### Cornell Grasping Dataset
|
||||
|
||||
1. Download the and extract [Cornell Grasping Dataset](http://pr.cs.cornell.edu/grasping/rect_data/data.php).
|
||||
2. Convert the PCD files to depth images by running `python -m utils.dataset_processing.generate_cornell_depth <Path To Dataset>`
|
||||
|
||||
#### Jacquard Dataset
|
||||
|
||||
1. Download and extract the [Jacquard Dataset](https://jacquard.liris.cnrs.fr/).
|
||||
|
||||
|
||||
## Model Training
|
||||
|
||||
A model can be trained using the `train_network.py` script. Run `train_network.py --help` to see a full list of options.
|
||||
|
||||
For example:
|
||||
|
||||
```bash
|
||||
python train_network.py --dataset cornell --dataset-path <Path To Dataset> --description training_cornell
|
||||
```
|
||||
|
||||
## Model Evaluation
|
||||
|
||||
The trained network can be evaluated using the `evaluate.py` script. Run `evaluate.py --help` for a full set of options.
|
||||
|
||||
For example:
|
||||
|
||||
```bash
|
||||
python evaluate.py --network <Path to Trained Network> --dataset cornell --dataset-path <Path to Dataset> --iou-eval
|
||||
```
|
||||
|
||||
## Run Tasks
|
||||
A task can be executed using the relevant run script. All task scripts are named as `run_<task name>.py`. For example, to run the grasp generator run:
|
||||
```bash
|
||||
python run_grasp_generator.py
|
||||
```
|
||||
|
||||
## Run on a Robot
|
||||
To run the grasp generator with a robot, please use our ROS implementation for Baxter robot. It is available at: https://github.com/skumra/baxter-pnp
|
||||
|
|
@ -0,0 +1 @@
|
|||
theme: jekyll-theme-cayman
|
||||
|
|
@ -0,0 +1,3 @@
|
|||
#!/usr/bin/env bash
|
||||
|
||||
find logs/ -maxdepth 1 -type d | grep -v ^\\.$ | xargs -n 1 du -s | while read size name ; do if [ $size -le 10485 ] ; then echo rm -rf $name ; fi done
|
||||
|
|
@ -0,0 +1,169 @@
|
|||
import argparse
|
||||
import logging
|
||||
import time
|
||||
|
||||
import numpy as np
|
||||
import torch.utils.data
|
||||
|
||||
from hardware.device import get_device
|
||||
from inference.post_process import post_process_output
|
||||
from utils.data import get_dataset
|
||||
from utils.dataset_processing import evaluation, grasp
|
||||
from utils.visualisation.plot import save_results
|
||||
|
||||
logging.basicConfig(level=logging.INFO)
|
||||
|
||||
|
||||
def parse_args():
|
||||
parser = argparse.ArgumentParser(description='Evaluate networks')
|
||||
|
||||
# Network
|
||||
parser.add_argument('--network', metavar='N', type=str, nargs='+',
|
||||
help='Path to saved networks to evaluate')
|
||||
|
||||
# Dataset
|
||||
parser.add_argument('--dataset', type=str,
|
||||
help='Dataset Name ("cornell" or "jaquard")')
|
||||
parser.add_argument('--dataset-path', type=str,
|
||||
help='Path to dataset')
|
||||
parser.add_argument('--use-depth', type=int, default=1,
|
||||
help='Use Depth image for evaluation (1/0)')
|
||||
parser.add_argument('--use-rgb', type=int, default=1,
|
||||
help='Use RGB image for evaluation (1/0)')
|
||||
parser.add_argument('--augment', action='store_true',
|
||||
help='Whether data augmentation should be applied')
|
||||
parser.add_argument('--split', type=float, default=0.9,
|
||||
help='Fraction of data for training (remainder is validation)')
|
||||
parser.add_argument('--ds-shuffle', action='store_true', default=False,
|
||||
help='Shuffle the dataset')
|
||||
parser.add_argument('--ds-rotate', type=float, default=0.0,
|
||||
help='Shift the start point of the dataset to use a different test/train split')
|
||||
parser.add_argument('--num-workers', type=int, default=8,
|
||||
help='Dataset workers')
|
||||
|
||||
# Evaluation
|
||||
parser.add_argument('--n-grasps', type=int, default=1,
|
||||
help='Number of grasps to consider per image')
|
||||
parser.add_argument('--iou-threshold', type=float, default=0.25,
|
||||
help='Threshold for IOU matching')
|
||||
parser.add_argument('--iou-eval', action='store_true',
|
||||
help='Compute success based on IoU metric.')
|
||||
parser.add_argument('--jacquard-output', action='store_true',
|
||||
help='Jacquard-dataset style output')
|
||||
|
||||
# Misc.
|
||||
parser.add_argument('--vis', action='store_true',
|
||||
help='Visualise the network output')
|
||||
parser.add_argument('--cpu', dest='force_cpu', action='store_true', default=False,
|
||||
help='Force code to run in CPU mode')
|
||||
parser.add_argument('--random-seed', type=int, default=123,
|
||||
help='Random seed for numpy')
|
||||
|
||||
args = parser.parse_args()
|
||||
|
||||
if args.jacquard_output and args.dataset != 'jacquard':
|
||||
raise ValueError('--jacquard-output can only be used with the --dataset jacquard option.')
|
||||
if args.jacquard_output and args.augment:
|
||||
raise ValueError('--jacquard-output can not be used with data augmentation.')
|
||||
|
||||
return args
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
args = parse_args()
|
||||
|
||||
# Get the compute device
|
||||
device = get_device(args.force_cpu)
|
||||
|
||||
# Load Dataset
|
||||
logging.info('Loading {} Dataset...'.format(args.dataset.title()))
|
||||
Dataset = get_dataset(args.dataset)
|
||||
test_dataset = Dataset(args.dataset_path,
|
||||
ds_rotate=args.ds_rotate,
|
||||
random_rotate=args.augment,
|
||||
random_zoom=args.augment,
|
||||
include_depth=args.use_depth,
|
||||
include_rgb=args.use_rgb)
|
||||
|
||||
indices = list(range(test_dataset.length))
|
||||
split = int(np.floor(args.split * test_dataset.length))
|
||||
if args.ds_shuffle:
|
||||
np.random.seed(args.random_seed)
|
||||
np.random.shuffle(indices)
|
||||
val_indices = indices[split:]
|
||||
val_sampler = torch.utils.data.sampler.SubsetRandomSampler(val_indices)
|
||||
logging.info('Validation size: {}'.format(len(val_indices)))
|
||||
|
||||
test_data = torch.utils.data.DataLoader(
|
||||
test_dataset,
|
||||
batch_size=1,
|
||||
num_workers=args.num_workers,
|
||||
sampler=val_sampler
|
||||
)
|
||||
logging.info('Done')
|
||||
|
||||
for network in args.network:
|
||||
logging.info('\nEvaluating model {}'.format(network))
|
||||
|
||||
# Load Network
|
||||
net = torch.load(network)
|
||||
|
||||
results = {'correct': 0, 'failed': 0}
|
||||
|
||||
if args.jacquard_output:
|
||||
jo_fn = network + '_jacquard_output.txt'
|
||||
with open(jo_fn, 'w') as f:
|
||||
pass
|
||||
|
||||
start_time = time.time()
|
||||
|
||||
with torch.no_grad():
|
||||
for idx, (x, y, didx, rot, zoom) in enumerate(test_data):
|
||||
xc = x.to(device)
|
||||
yc = [yi.to(device) for yi in y]
|
||||
lossd = net.compute_loss(xc, yc)
|
||||
|
||||
q_img, ang_img, width_img = post_process_output(lossd['pred']['pos'], lossd['pred']['cos'],
|
||||
lossd['pred']['sin'], lossd['pred']['width'])
|
||||
|
||||
if args.iou_eval:
|
||||
s = evaluation.calculate_iou_match(q_img, ang_img, test_data.dataset.get_gtbb(didx, rot, zoom),
|
||||
no_grasps=args.n_grasps,
|
||||
grasp_width=width_img,
|
||||
threshold=args.iou_threshold
|
||||
)
|
||||
if s:
|
||||
results['correct'] += 1
|
||||
else:
|
||||
results['failed'] += 1
|
||||
|
||||
if args.jacquard_output:
|
||||
grasps = grasp.detect_grasps(q_img, ang_img, width_img=width_img, no_grasps=1)
|
||||
with open(jo_fn, 'a') as f:
|
||||
for g in grasps:
|
||||
f.write(test_data.dataset.get_jname(didx) + '\n')
|
||||
f.write(g.to_jacquard(scale=1024 / 300) + '\n')
|
||||
|
||||
if args.vis:
|
||||
save_results(
|
||||
rgb_img=test_data.dataset.get_rgb(didx, rot, zoom, normalise=False),
|
||||
depth_img=test_data.dataset.get_depth(didx, rot, zoom),
|
||||
grasp_q_img=q_img,
|
||||
grasp_angle_img=ang_img,
|
||||
no_grasps=args.n_grasps,
|
||||
grasp_width_img=width_img
|
||||
)
|
||||
|
||||
avg_time = (time.time() - start_time) / len(test_data)
|
||||
logging.info('Average evaluation time per image: {}ms'.format(avg_time * 1000))
|
||||
|
||||
if args.iou_eval:
|
||||
logging.info('IOU Results: %d/%d = %f' % (results['correct'],
|
||||
results['correct'] + results['failed'],
|
||||
results['correct'] / (results['correct'] + results['failed'])))
|
||||
|
||||
if args.jacquard_output:
|
||||
logging.info('Jacquard output saved to {}'.format(jo_fn))
|
||||
|
||||
del net
|
||||
torch.cuda.empty_cache()
|
||||
|
|
@ -0,0 +1,186 @@
|
|||
import logging
|
||||
import os
|
||||
import time
|
||||
|
||||
import cv2
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
from scipy import optimize
|
||||
# This import registers the 3D projection, but is otherwise unused.
|
||||
from mpl_toolkits.mplot3d import Axes3D # noqa: F401 unused import
|
||||
|
||||
from hardware.camera import RealSenseCamera
|
||||
|
||||
|
||||
class Calibration:
|
||||
def __init__(self,
|
||||
cam_id,
|
||||
calib_grid_step,
|
||||
checkerboard_offset_from_tool,
|
||||
workspace_limits
|
||||
):
|
||||
self.calib_grid_step = calib_grid_step
|
||||
self.checkerboard_offset_from_tool = checkerboard_offset_from_tool
|
||||
|
||||
# Cols: min max, Rows: x y z (define workspace limits in robot coordinates)
|
||||
self.workspace_limits = workspace_limits
|
||||
|
||||
self.camera = RealSenseCamera(device_id=cam_id)
|
||||
|
||||
self.measured_pts = []
|
||||
self.observed_pts = []
|
||||
self.observed_pix = []
|
||||
self.world2camera = np.eye(4)
|
||||
|
||||
homedir = os.path.join(os.path.expanduser('~'), "grasp-comms")
|
||||
self.move_completed = os.path.join(homedir, "move_completed.npy")
|
||||
self.tool_position = os.path.join(homedir, "tool_position.npy")
|
||||
|
||||
@staticmethod
|
||||
def _get_rigid_transform(A, B):
|
||||
"""
|
||||
Estimate rigid transform with SVD (from Nghia Ho)
|
||||
"""
|
||||
assert len(A) == len(B)
|
||||
|
||||
N = A.shape[0] # Total points
|
||||
centroid_A = np.mean(A, axis=0)
|
||||
centroid_B = np.mean(B, axis=0)
|
||||
AA = A - np.tile(centroid_A, (N, 1)) # Centre the points
|
||||
BB = B - np.tile(centroid_B, (N, 1))
|
||||
H = np.dot(np.transpose(AA), BB) # Dot is matrix multiplication for array
|
||||
U, S, Vt = np.linalg.svd(H)
|
||||
R = np.dot(Vt.T, U.T)
|
||||
if np.linalg.det(R) < 0: # Special reflection case
|
||||
Vt[2, :] *= -1
|
||||
R = np.dot(Vt.T, U.T)
|
||||
t = np.dot(-R, centroid_A.T) + centroid_B.T
|
||||
return R, t
|
||||
|
||||
def _get_rigid_transform_error(self, z_scale):
|
||||
"""
|
||||
Calculate the rigid transform RMS error
|
||||
|
||||
:return RMS error
|
||||
"""
|
||||
# Apply z offset and compute new observed points using camera intrinsics
|
||||
observed_z = np.squeeze(self.observed_pts[:, 2:] * z_scale)
|
||||
observed_x = np.multiply(np.squeeze(self.observed_pix[:, [0]]) - self.camera.intrinsics.ppx,
|
||||
observed_z / self.camera.intrinsics.fx)
|
||||
observed_y = np.multiply(np.squeeze(self.observed_pix[:, [1]]) - self.camera.intrinsics.ppy,
|
||||
observed_z / self.camera.intrinsics.fy)
|
||||
|
||||
new_observed_pts = np.asarray([observed_x, observed_y, observed_z]).T
|
||||
|
||||
# Estimate rigid transform between measured points and new observed points
|
||||
R, t = self._get_rigid_transform(np.asarray(self.measured_pts), np.asarray(new_observed_pts))
|
||||
t.shape = (3, 1)
|
||||
self.world2camera = np.concatenate((np.concatenate((R, t), axis=1), np.array([[0, 0, 0, 1]])), axis=0)
|
||||
|
||||
# Compute rigid transform error
|
||||
registered_pts = np.dot(R, np.transpose(self.measured_pts)) + np.tile(t, (1, self.measured_pts.shape[0]))
|
||||
error = np.transpose(registered_pts) - new_observed_pts
|
||||
error = np.sum(np.multiply(error, error))
|
||||
rmse = np.sqrt(error / self.measured_pts.shape[0])
|
||||
return rmse
|
||||
|
||||
def _generate_grid(self):
|
||||
"""
|
||||
Construct 3D calibration grid across workspace
|
||||
|
||||
:return calibration grid points
|
||||
"""
|
||||
gridspace_x = np.linspace(self.workspace_limits[0][0], self.workspace_limits[0][1],
|
||||
1 + (self.workspace_limits[0][1] - self.workspace_limits[0][
|
||||
0]) / self.calib_grid_step)
|
||||
gridspace_y = np.linspace(self.workspace_limits[1][0], self.workspace_limits[1][1],
|
||||
1 + (self.workspace_limits[1][1] - self.workspace_limits[1][
|
||||
0]) / self.calib_grid_step)
|
||||
gridspace_z = np.linspace(self.workspace_limits[2][0], self.workspace_limits[2][1],
|
||||
1 + (self.workspace_limits[2][1] - self.workspace_limits[2][
|
||||
0]) / self.calib_grid_step)
|
||||
calib_grid_x, calib_grid_y, calib_grid_z = np.meshgrid(gridspace_x, gridspace_y, gridspace_z)
|
||||
num_calib_grid_pts = calib_grid_x.shape[0] * calib_grid_x.shape[1] * calib_grid_x.shape[2]
|
||||
calib_grid_x.shape = (num_calib_grid_pts, 1)
|
||||
calib_grid_y.shape = (num_calib_grid_pts, 1)
|
||||
calib_grid_z.shape = (num_calib_grid_pts, 1)
|
||||
calib_grid_pts = np.concatenate((calib_grid_x, calib_grid_y, calib_grid_z), axis=1)
|
||||
return calib_grid_pts
|
||||
|
||||
def run(self):
|
||||
# Connect to camera
|
||||
self.camera.connect()
|
||||
logging.debug(self.camera.intrinsics)
|
||||
|
||||
logging.info('Collecting data...')
|
||||
|
||||
calib_grid_pts = self._generate_grid()
|
||||
|
||||
logging.info('Total grid points: ', calib_grid_pts.shape[0])
|
||||
|
||||
for tool_position in calib_grid_pts:
|
||||
logging.info('Requesting move to tool position: ', tool_position)
|
||||
np.save(self.tool_position, tool_position)
|
||||
np.save(self.move_completed, 0)
|
||||
while not np.load(self.move_completed):
|
||||
time.sleep(0.1)
|
||||
# Wait for robot to be stable
|
||||
time.sleep(2)
|
||||
|
||||
# Find checkerboard center
|
||||
checkerboard_size = (3, 3)
|
||||
refine_criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
|
||||
image_bundle = self.camera.get_image_bundle()
|
||||
camera_color_img = image_bundle['rgb']
|
||||
camera_depth_img = image_bundle['aligned_depth']
|
||||
bgr_color_data = cv2.cvtColor(camera_color_img, cv2.COLOR_RGB2BGR)
|
||||
gray_data = cv2.cvtColor(bgr_color_data, cv2.COLOR_RGB2GRAY)
|
||||
checkerboard_found, corners = cv2.findChessboardCorners(gray_data, checkerboard_size, None,
|
||||
cv2.CALIB_CB_ADAPTIVE_THRESH)
|
||||
if checkerboard_found:
|
||||
corners_refined = cv2.cornerSubPix(gray_data, corners, (3, 3), (-1, -1), refine_criteria)
|
||||
|
||||
# Get observed checkerboard center 3D point in camera space
|
||||
checkerboard_pix = np.round(corners_refined[4, 0, :]).astype(int)
|
||||
checkerboard_z = camera_depth_img[checkerboard_pix[1]][checkerboard_pix[0]]
|
||||
checkerboard_x = np.multiply(checkerboard_pix[0] - self.camera.intrinsics.ppx,
|
||||
checkerboard_z / self.camera.intrinsics.fx)
|
||||
checkerboard_y = np.multiply(checkerboard_pix[1] - self.camera.intrinsics.ppy,
|
||||
checkerboard_z / self.camera.intrinsics.fy)
|
||||
if checkerboard_z == 0:
|
||||
continue
|
||||
|
||||
# Save calibration point and observed checkerboard center
|
||||
self.observed_pts.append([checkerboard_x, checkerboard_y, checkerboard_z])
|
||||
# tool_position[2] += self.checkerboard_offset_from_tool
|
||||
tool_position = tool_position + self.checkerboard_offset_from_tool
|
||||
|
||||
self.measured_pts.append(tool_position)
|
||||
self.observed_pix.append(checkerboard_pix)
|
||||
|
||||
# Draw and display the corners
|
||||
vis = cv2.drawChessboardCorners(bgr_color_data, (1, 1), corners_refined[4, :, :], checkerboard_found)
|
||||
# cv2.imwrite('%06d.png' % len(self.measured_pts), vis)
|
||||
cv2.imshow('Calibration', vis)
|
||||
cv2.waitKey(10)
|
||||
else:
|
||||
logging.info('Checker board not found')
|
||||
|
||||
self.measured_pts = np.asarray(self.measured_pts)
|
||||
self.observed_pts = np.asarray(self.observed_pts)
|
||||
self.observed_pix = np.asarray(self.observed_pix)
|
||||
|
||||
# Optimize z scale w.r.t. rigid transform error
|
||||
logging.info('Calibrating...')
|
||||
z_scale_init = 1
|
||||
optim_result = optimize.minimize(self._get_rigid_transform_error, np.asarray(z_scale_init), method='Nelder-Mead')
|
||||
camera_depth_offset = optim_result.x
|
||||
|
||||
# Save camera optimized offset and camera pose
|
||||
logging.info('Saving...')
|
||||
np.savetxt('saved_data/camera_depth_scale.txt', camera_depth_offset, delimiter=' ')
|
||||
rmse = self._get_rigid_transform_error(camera_depth_offset)
|
||||
logging.info('RMSE: ', rmse)
|
||||
camera_pose = np.linalg.inv(self.world2camera)
|
||||
np.savetxt('saved_data/camera_pose.txt', camera_pose, delimiter=' ')
|
||||
logging.info('Done.')
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue