release repository
This commit is contained in:
Balakumar Sundaralingam
52
src/curobo/__init__.py
Normal file
52
src/curobo/__init__.py
Normal file
@@ -0,0 +1,52 @@
|
||||
#
|
||||
# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
#
|
||||
# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
# property and proprietary rights in and to this material, related
|
||||
# documentation and any modifications thereto. Any use, reproduction,
|
||||
# disclosure or distribution of this material and related documentation
|
||||
# without an express license agreement from NVIDIA CORPORATION or
|
||||
# its affiliates is strictly prohibited.
|
||||
#
|
||||
|
||||
"""CuRobo package."""
|
||||
|
||||
|
||||
# NOTE (roflaherty): This is inspired by how matplotlib does creates its version value.
|
||||
# https://github.com/matplotlib/matplotlib/blob/master/lib/matplotlib/__init__.py#L161
|
||||
def _get_version():
|
||||
"""Return the version string used for __version__."""
|
||||
# Standard Library
|
||||
import pathlib
|
||||
|
||||
root = pathlib.Path(__file__).resolve().parent.parent.parent
|
||||
if (root / ".git").exists() and not (root / ".git/shallow").exists():
|
||||
# Third Party
|
||||
import setuptools_scm
|
||||
|
||||
# See the `setuptools_scm` documentation for the description of the schemes used below.
|
||||
# https://pypi.org/project/setuptools-scm/
|
||||
# NOTE: If these values are updated, they need to be also updated in `pyproject.toml`.
|
||||
return setuptools_scm.get_version(
|
||||
root=root,
|
||||
version_scheme="no-guess-dev",
|
||||
local_scheme="dirty-tag",
|
||||
)
|
||||
else: # Get the version from the _version.py setuptools_scm file.
|
||||
try:
|
||||
# Standard Library
|
||||
from importlib.metadata import version
|
||||
except ModuleNotFoundError:
|
||||
# NOTE: `importlib.resources` is part of the standard library in Python 3.9.
|
||||
# `importlib_metadata` is the back ported library for older versions of python.
|
||||
# Third Party
|
||||
from importlib_metadata import version
|
||||
|
||||
return version("nvidia_curobo")
|
||||
|
||||
|
||||
# Set `__version__` attribute
|
||||
__version__ = _get_version()
|
||||
|
||||
# Remove `_get_version` so it is not added as an attribute
|
||||
del _get_version
|
||||
@@ -0,0 +1,279 @@
|
||||
<?xml version="1.0" ?>
|
||||
<robot name="panda" xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<link name="base_link"/>
|
||||
<joint name="panda_fixed" type="fixed">
|
||||
<origin rpy="0 0 0.0" xyz="0 0 0.0"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="panda_link0"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<link name="panda_link0">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link0.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link0.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="panda_link1">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link1.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link1.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint1" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.333"/>
|
||||
<parent link="panda_link0"/>
|
||||
<child link="panda_link1"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
|
||||
</joint>
|
||||
<link name="panda_link2">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link2.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link2.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint2" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
|
||||
<origin rpy="-1.57079632679 0 0" xyz="0 0 0"/>
|
||||
<parent link="panda_link1"/>
|
||||
<child link="panda_link2"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="87" lower="-1.7628" upper="1.7628" velocity="2.1750"/>
|
||||
</joint>
|
||||
<link name="panda_link3">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link3.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link3.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint3" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
|
||||
<origin rpy="1.57079632679 0 0" xyz="0 -0.316 0"/>
|
||||
<parent link="panda_link2"/>
|
||||
<child link="panda_link3"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
|
||||
</joint>
|
||||
<link name="panda_link4">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link4.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link4.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint4" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
|
||||
<origin rpy="1.57079632679 0 0" xyz="0.0825 0 0"/>
|
||||
<parent link="panda_link3"/>
|
||||
<child link="panda_link4"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="87" lower="-3.0718" upper="-0.0698" velocity="2.1750"/>
|
||||
<!-- something is weird with this joint limit config
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="87" lower="-3.0" upper="0.087" velocity="2.1750"/> -->
|
||||
</joint>
|
||||
<link name="panda_link5">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link5.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link5.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint5" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
|
||||
<origin rpy="-1.57079632679 0 0" xyz="-0.0825 0.384 0"/>
|
||||
<parent link="panda_link4"/>
|
||||
<child link="panda_link5"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
|
||||
</joint>
|
||||
<link name="panda_link6">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link6.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link6.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint6" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
|
||||
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
|
||||
<parent link="panda_link5"/>
|
||||
<child link="panda_link6"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="12" lower="-0.0175" upper="3.7525" velocity="2.6100"/>
|
||||
<!-- <dynamics damping="10.0"/>
|
||||
<limit effort="12" lower="-0.0873" upper="3.0" velocity="2.6100"/> -->
|
||||
</joint>
|
||||
<link name="panda_link7">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link7.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link7.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint7" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
|
||||
<origin rpy="1.57079632679 0 0" xyz="0.088 0 0"/>
|
||||
<parent link="panda_link6"/>
|
||||
<child link="panda_link7"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
|
||||
</joint>
|
||||
<!--
|
||||
<link name="panda_link8"/>
|
||||
<joint name="panda_joint8" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0 0.107"/>
|
||||
<parent link="panda_link7"/>
|
||||
<child link="panda_link8"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
Removing this joint seems to help with some stability things
|
||||
-->
|
||||
<joint name="panda_hand_joint" type="fixed">
|
||||
<!--
|
||||
<parent link="panda_link8"/>
|
||||
-->
|
||||
<parent link="panda_link7"/>
|
||||
<child link="panda_hand"/>
|
||||
<origin rpy="0 0 -0.785398163397" xyz="0 0 0.107"/>
|
||||
<!--
|
||||
<origin rpy="0 0 -0.785398163397" xyz="0 0 0"/>
|
||||
-->
|
||||
</joint>
|
||||
<link name="panda_hand">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/hand.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/hand.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="panda_leftfinger">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/finger.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/finger.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="panda_rightfinger">
|
||||
<visual>
|
||||
<origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/finger.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/finger.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_finger_joint1" type="prismatic">
|
||||
<parent link="panda_hand"/>
|
||||
<child link="panda_leftfinger"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.0584"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="20" lower="0.0" upper="0.04" velocity="0.2"/>
|
||||
</joint>
|
||||
<joint name="panda_finger_joint2" type="prismatic">
|
||||
<parent link="panda_hand"/>
|
||||
<child link="panda_rightfinger"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.0584"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="20" lower="-0.04" upper="0.0" velocity="0.2"/>
|
||||
<mimic joint="panda_finger_joint1"/>
|
||||
</joint>
|
||||
|
||||
<link name="ee_link"/>
|
||||
|
||||
<joint name="ee_fixed_joint" type="fixed">
|
||||
<parent link="panda_hand"/>
|
||||
<child link="ee_link"/>
|
||||
<origin rpy="0 0.0 0" xyz="0.0 0. 0.1"/>
|
||||
</joint>
|
||||
<link name="right_gripper">
|
||||
<inertial>
|
||||
<!-- Dummy inertial parameters to avoid link lumping-->
|
||||
<mass value="0.01"/>
|
||||
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="right_gripper" type="fixed">
|
||||
<!--<origin rpy="0 0 2.35619449019" xyz="0 0 0.1"/>-->
|
||||
<origin rpy="0 0 2.35619449019" xyz="0 0 0.207"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<parent link="panda_link7"/>
|
||||
<!--<parent link="panda_link8"/>-->
|
||||
<child link="right_gripper"/>
|
||||
</joint>
|
||||
|
||||
|
||||
</robot>
|
||||
|
||||
|
||||
@@ -0,0 +1,325 @@
|
||||
<?xml version="1.0" ?>
|
||||
<robot name="panda" xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<link name="base_link"/>
|
||||
<link name="base_link_x"/>
|
||||
<link name="base_link_y"/>
|
||||
<link name="base_link_z"/>
|
||||
|
||||
|
||||
<joint name="base_x" type="prismatic">
|
||||
<origin rpy="0 0 0.0" xyz="0 0 0.0"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="base_link_x"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="87" lower="-1.0" upper="1.0" velocity="2.1750"/>
|
||||
</joint>
|
||||
|
||||
<joint name="base_y" type="prismatic">
|
||||
<origin rpy="0 0 0.0" xyz="0 0 0.0"/>
|
||||
<parent link="base_link_x"/>
|
||||
<child link="base_link_y"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="87" lower="-1.0" upper="1.0" velocity="2.1750"/>
|
||||
</joint>
|
||||
|
||||
<joint name="base_z" type="prismatic">
|
||||
<origin rpy="0 0 0.0" xyz="0 0 0.0"/>
|
||||
<parent link="base_link_y"/>
|
||||
<child link="base_link_z"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="87" lower="0.1" upper="1.0" velocity="2.1750"/>
|
||||
</joint>
|
||||
|
||||
|
||||
|
||||
<joint name="base_fixed" type="fixed">
|
||||
<origin rpy="0 0 0.0" xyz="0 0 0.0"/>
|
||||
<parent link="base_link_z"/>
|
||||
<child link="panda_link0"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
|
||||
|
||||
|
||||
<link name="panda_link0">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link0.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link0.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
</link>
|
||||
<link name="panda_link1">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link1.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link1.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint1" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.333"/>
|
||||
<parent link="panda_link0"/>
|
||||
<child link="panda_link1"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<!--limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/-->
|
||||
<limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
|
||||
</joint>
|
||||
<link name="panda_link2">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link2.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link2.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint2" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
|
||||
<origin rpy="-1.57079632679 0 0" xyz="0 0 0"/>
|
||||
<parent link="panda_link1"/>
|
||||
<child link="panda_link2"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="87" lower="-1.7628" upper="1.7628" velocity="2.1750"/>
|
||||
</joint>
|
||||
<link name="panda_link3">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link3.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link3.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint3" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
|
||||
<origin rpy="1.57079632679 0 0" xyz="0 -0.316 0"/>
|
||||
<parent link="panda_link2"/>
|
||||
<child link="panda_link3"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
|
||||
</joint>
|
||||
<link name="panda_link4">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link4.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link4.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint4" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
|
||||
<origin rpy="1.57079632679 0 0" xyz="0.0825 0 0"/>
|
||||
<parent link="panda_link3"/>
|
||||
<child link="panda_link4"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="87" lower="-3.0718" upper="-0.0698" velocity="2.1750"/>
|
||||
<!-- something is weird with this joint limit config
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="87" lower="-3.0" upper="0.087" velocity="2.1750"/> -->
|
||||
</joint>
|
||||
<link name="panda_link5">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link5.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link5.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint5" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
|
||||
<origin rpy="-1.57079632679 0 0" xyz="-0.0825 0.384 0"/>
|
||||
<parent link="panda_link4"/>
|
||||
<child link="panda_link5"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
|
||||
</joint>
|
||||
<link name="panda_link6">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link6.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link6.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint6" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
|
||||
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
|
||||
<parent link="panda_link5"/>
|
||||
<child link="panda_link6"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="12" lower="0.5" upper="3.7525" velocity="2.6100"/>
|
||||
<!-- <dynamics damping="10.0"/>
|
||||
<limit effort="12" lower="-0.0873" upper="3.0" velocity="2.6100"/> -->
|
||||
</joint>
|
||||
<link name="panda_link7">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link7.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link7.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint7" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
|
||||
<origin rpy="1.57079632679 0 0" xyz="0.088 0 0"/>
|
||||
<parent link="panda_link6"/>
|
||||
<child link="panda_link7"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
|
||||
</joint>
|
||||
<link name="panda_link8"/>
|
||||
<joint name="panda_joint8" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0 0.107"/>
|
||||
<parent link="panda_link7"/>
|
||||
<child link="panda_link8"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
<joint name="panda_hand_joint" type="fixed">
|
||||
<!--
|
||||
<parent link="panda_link8"/>
|
||||
-->
|
||||
<parent link="panda_link8"/>
|
||||
<child link="panda_hand"/>
|
||||
<origin rpy="0 0 -0.785398163397" xyz="0 0 0.0"/>
|
||||
<!--
|
||||
<origin rpy="0 0 -0.785398163397" xyz="0 0 0"/>
|
||||
-->
|
||||
</joint>
|
||||
<link name="panda_hand">
|
||||
<visual>
|
||||
<origin rpy="1.57 0.0 0.0" xyz="0 0 0.0"/>
|
||||
<!--origin rpy="-1.57 0.0 0.0" xyz="0 0 0.0"/-->
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/hand.obj"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0 0 0.0"/>
|
||||
<geometry>
|
||||
<!--box size="0.01 0.01 0.01"/-->
|
||||
<!--mesh filename="meshes/visual/hand.obj"/-->
|
||||
<mesh filename="meshes/collision/hand_gripper.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="ee_fixed_joint" type="fixed">
|
||||
<parent link="panda_hand"/>
|
||||
<child link="grasp_frame"/>
|
||||
<!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0922 0.0"/-->
|
||||
<!--origin rpy="-1.57 0.0 1.57" xyz="0.0 0. 0.0"/-->
|
||||
<origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0 0.11"/>
|
||||
</joint>
|
||||
<link name="grasp_frame">
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.01 0.01 0.01"/>
|
||||
</geometry>
|
||||
|
||||
<origin rpy="0 0 0" xyz="0.0 0 0"/>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="ee_fixed_t" type="fixed">
|
||||
<parent link="panda_hand"/>
|
||||
<child link="ee_link"/>
|
||||
<!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0922 0.0"/-->
|
||||
<origin rpy="0 0.0 0" xyz="0.0 0. 0.1"/>
|
||||
<!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0 0.1"/-->
|
||||
</joint>
|
||||
<link name="ee_link"/>
|
||||
<link name="ee_grasp_frame"/>
|
||||
<joint name="ee_grasp_joint" type="fixed">
|
||||
<parent link="panda_hand"/>
|
||||
<child link="ee_grasp_frame"/>
|
||||
<!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0922 0.0"/-->
|
||||
<origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0 0.0"/>
|
||||
<!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0 0.1"/-->
|
||||
</joint>
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
<link name="panda_leftfinger">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/finger.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/finger.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="panda_rightfinger">
|
||||
<visual>
|
||||
<origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/finger.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/finger.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_finger_joint1" type="fixed">
|
||||
<parent link="panda_hand"/>
|
||||
<child link="panda_leftfinger"/>
|
||||
<origin rpy="0 0 0" xyz="0 0.04 0.0584"/>
|
||||
</joint>
|
||||
<joint name="panda_finger_joint2" type="fixed">
|
||||
<parent link="panda_hand"/>
|
||||
<child link="panda_rightfinger"/>
|
||||
<origin rpy="0 0 0" xyz="0 -0.04 0.0584"/>
|
||||
</joint>
|
||||
|
||||
</robot>
|
||||
|
||||
|
||||
@@ -0,0 +1,52 @@
|
||||
<?xml version="1.0" ?>
|
||||
<robot name="panda" xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<link name="base_link"/>
|
||||
<link name="base_link_x"/>
|
||||
<link name="base_link_y"/>
|
||||
|
||||
<joint name="base_x" type="prismatic">
|
||||
<origin rpy="0 0 0.0" xyz="0 0 0.0"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="base_link_x"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="87" lower="-5.0" upper="5.0" velocity="1.0"/>
|
||||
</joint>
|
||||
|
||||
<joint name="base_y" type="prismatic">
|
||||
<origin rpy="0 0 0.0" xyz="0 0 0.0"/>
|
||||
<parent link="base_link_x"/>
|
||||
<child link="base_link_y"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="87" lower="-5.0" upper="5.0" velocity="1.0"/>
|
||||
</joint>
|
||||
|
||||
<joint name="base_z" type="revolute">
|
||||
<origin rpy="0 0 0.0" xyz="0 0 0.0"/>
|
||||
<parent link="base_link_y"/>
|
||||
<child link="panda_link0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="87" lower="-6.0" upper="6.0" velocity="1.50"/>
|
||||
</joint>
|
||||
|
||||
|
||||
|
||||
|
||||
<link name="panda_link0">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link0.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link0.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
</link>
|
||||
</robot>
|
||||
|
||||
|
||||
@@ -0,0 +1,303 @@
|
||||
<?xml version="1.0" ?>
|
||||
<robot name="panda" xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<link name="base_link"/>
|
||||
<link name="base_link_x"/>
|
||||
|
||||
|
||||
<joint name="base_x" type="prismatic">
|
||||
<origin rpy="0 0 0.0" xyz="0 0 0.0"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="base_link_x"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="87" lower="-5.0" upper="5.0" velocity="1.0"/>
|
||||
</joint>
|
||||
|
||||
<joint name="base_fixed" type="fixed">
|
||||
<origin rpy="0 0 0.0" xyz="0 0 0.1"/>
|
||||
<parent link="base_link_x"/>
|
||||
<child link="panda_link0"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
|
||||
|
||||
|
||||
<link name="panda_link0">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link0.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link0.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
</link>
|
||||
<link name="panda_link1">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link1.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link1.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint1" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.333"/>
|
||||
<parent link="panda_link0"/>
|
||||
<child link="panda_link1"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<!--limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/-->
|
||||
<limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
|
||||
</joint>
|
||||
<link name="panda_link2">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link2.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link2.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint2" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
|
||||
<origin rpy="-1.57079632679 0 0" xyz="0 0 0"/>
|
||||
<parent link="panda_link1"/>
|
||||
<child link="panda_link2"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="87" lower="-1.7628" upper="1.7628" velocity="2.1750"/>
|
||||
</joint>
|
||||
<link name="panda_link3">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link3.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link3.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint3" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
|
||||
<origin rpy="1.57079632679 0 0" xyz="0 -0.316 0"/>
|
||||
<parent link="panda_link2"/>
|
||||
<child link="panda_link3"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
|
||||
</joint>
|
||||
<link name="panda_link4">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link4.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link4.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint4" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
|
||||
<origin rpy="1.57079632679 0 0" xyz="0.0825 0 0"/>
|
||||
<parent link="panda_link3"/>
|
||||
<child link="panda_link4"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="87" lower="-3.0718" upper="-0.0698" velocity="2.1750"/>
|
||||
<!-- something is weird with this joint limit config
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="87" lower="-3.0" upper="0.087" velocity="2.1750"/> -->
|
||||
</joint>
|
||||
<link name="panda_link5">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link5.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link5.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint5" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
|
||||
<origin rpy="-1.57079632679 0 0" xyz="-0.0825 0.384 0"/>
|
||||
<parent link="panda_link4"/>
|
||||
<child link="panda_link5"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
|
||||
</joint>
|
||||
<link name="panda_link6">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link6.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link6.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint6" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
|
||||
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
|
||||
<parent link="panda_link5"/>
|
||||
<child link="panda_link6"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="12" lower="0.5" upper="3.7525" velocity="2.6100"/>
|
||||
<!-- <dynamics damping="10.0"/>
|
||||
<limit effort="12" lower="-0.0873" upper="3.0" velocity="2.6100"/> -->
|
||||
</joint>
|
||||
<link name="panda_link7">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link7.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link7.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint7" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
|
||||
<origin rpy="1.57079632679 0 0" xyz="0.088 0 0"/>
|
||||
<parent link="panda_link6"/>
|
||||
<child link="panda_link7"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
|
||||
</joint>
|
||||
<link name="panda_link8"/>
|
||||
<joint name="panda_joint8" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0 0.107"/>
|
||||
<parent link="panda_link7"/>
|
||||
<child link="panda_link8"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
<joint name="panda_hand_joint" type="fixed">
|
||||
<!--
|
||||
<parent link="panda_link8"/>
|
||||
-->
|
||||
<parent link="panda_link8"/>
|
||||
<child link="panda_hand"/>
|
||||
<origin rpy="0 0 -0.785398163397" xyz="0 0 0.0"/>
|
||||
<!--
|
||||
<origin rpy="0 0 -0.785398163397" xyz="0 0 0"/>
|
||||
-->
|
||||
</joint>
|
||||
<link name="panda_hand">
|
||||
<visual>
|
||||
<origin rpy="1.57 0.0 0.0" xyz="0 0 0.0"/>
|
||||
<!--origin rpy="-1.57 0.0 0.0" xyz="0 0 0.0"/-->
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/hand.obj"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0 0 0.0"/>
|
||||
<geometry>
|
||||
<!--box size="0.01 0.01 0.01"/-->
|
||||
<!--mesh filename="meshes/visual/hand.obj"/-->
|
||||
<mesh filename="meshes/collision/hand_gripper.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="ee_fixed_joint" type="fixed">
|
||||
<parent link="panda_hand"/>
|
||||
<child link="grasp_frame"/>
|
||||
<!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0922 0.0"/-->
|
||||
<!--origin rpy="-1.57 0.0 1.57" xyz="0.0 0. 0.0"/-->
|
||||
<origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0 0.11"/>
|
||||
</joint>
|
||||
<link name="grasp_frame">
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.01 0.01 0.01"/>
|
||||
</geometry>
|
||||
|
||||
<origin rpy="0 0 0" xyz="0.0 0 0"/>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="ee_fixed_t" type="fixed">
|
||||
<parent link="panda_hand"/>
|
||||
<child link="ee_link"/>
|
||||
<!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0922 0.0"/-->
|
||||
<origin rpy="0 0.0 0" xyz="0.0 0. 0.1"/>
|
||||
<!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0 0.1"/-->
|
||||
</joint>
|
||||
<link name="ee_link"/>
|
||||
<link name="ee_grasp_frame"/>
|
||||
<joint name="ee_grasp_joint" type="fixed">
|
||||
<parent link="panda_hand"/>
|
||||
<child link="ee_grasp_frame"/>
|
||||
<!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0922 0.0"/-->
|
||||
<origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0 0.0"/>
|
||||
<!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0 0.1"/-->
|
||||
</joint>
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
<link name="panda_leftfinger">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/finger.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/finger.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="panda_rightfinger">
|
||||
<visual>
|
||||
<origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/finger.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/finger.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_finger_joint1" type="fixed">
|
||||
<parent link="panda_hand"/>
|
||||
<child link="panda_leftfinger"/>
|
||||
<origin rpy="0 0 0" xyz="0 0.04 0.0584"/>
|
||||
</joint>
|
||||
<joint name="panda_finger_joint2" type="fixed">
|
||||
<parent link="panda_hand"/>
|
||||
<child link="panda_rightfinger"/>
|
||||
<origin rpy="0 0 0" xyz="0 -0.04 0.0584"/>
|
||||
</joint>
|
||||
|
||||
</robot>
|
||||
|
||||
|
||||
@@ -0,0 +1,316 @@
|
||||
<?xml version="1.0" ?>
|
||||
<robot name="panda" xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<link name="base_link"/>
|
||||
<link name="base_link_x"/>
|
||||
<link name="base_link_y"/>
|
||||
|
||||
|
||||
<joint name="base_x" type="prismatic">
|
||||
<origin rpy="0 0 0.0" xyz="0 0 0.0"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="base_link_x"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="87" lower="-1.0" upper="1.0" velocity="3.0"/>
|
||||
</joint>
|
||||
|
||||
<joint name="base_y" type="prismatic">
|
||||
<origin rpy="0 0 0.0" xyz="0 0 0.0"/>
|
||||
<parent link="base_link_x"/>
|
||||
<child link="base_link_y"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="87" lower="-1.0" upper="1.0" velocity="3.0"/>
|
||||
</joint>
|
||||
|
||||
|
||||
|
||||
|
||||
<joint name="base_fixed" type="fixed">
|
||||
<origin rpy="0 0 0.0" xyz="0 0 0.1"/>
|
||||
<parent link="base_link_y"/>
|
||||
<child link="panda_link0"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
|
||||
|
||||
|
||||
<link name="panda_link0">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link0.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link0.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
</link>
|
||||
<link name="panda_link1">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link1.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link1.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint1" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.333"/>
|
||||
<parent link="panda_link0"/>
|
||||
<child link="panda_link1"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<!--limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/-->
|
||||
<limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
|
||||
</joint>
|
||||
<link name="panda_link2">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link2.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link2.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint2" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
|
||||
<origin rpy="-1.57079632679 0 0" xyz="0 0 0"/>
|
||||
<parent link="panda_link1"/>
|
||||
<child link="panda_link2"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="87" lower="-1.7628" upper="1.7628" velocity="2.1750"/>
|
||||
</joint>
|
||||
<link name="panda_link3">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link3.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link3.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint3" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
|
||||
<origin rpy="1.57079632679 0 0" xyz="0 -0.316 0"/>
|
||||
<parent link="panda_link2"/>
|
||||
<child link="panda_link3"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
|
||||
</joint>
|
||||
<link name="panda_link4">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link4.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link4.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint4" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
|
||||
<origin rpy="1.57079632679 0 0" xyz="0.0825 0 0"/>
|
||||
<parent link="panda_link3"/>
|
||||
<child link="panda_link4"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="87" lower="-3.0718" upper="-0.0698" velocity="2.1750"/>
|
||||
<!-- something is weird with this joint limit config
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="87" lower="-3.0" upper="0.087" velocity="2.1750"/> -->
|
||||
</joint>
|
||||
<link name="panda_link5">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link5.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link5.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint5" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
|
||||
<origin rpy="-1.57079632679 0 0" xyz="-0.0825 0.384 0"/>
|
||||
<parent link="panda_link4"/>
|
||||
<child link="panda_link5"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
|
||||
</joint>
|
||||
<link name="panda_link6">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link6.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link6.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint6" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
|
||||
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
|
||||
<parent link="panda_link5"/>
|
||||
<child link="panda_link6"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="12" lower="0.5" upper="3.7525" velocity="2.6100"/>
|
||||
<!-- <dynamics damping="10.0"/>
|
||||
<limit effort="12" lower="-0.0873" upper="3.0" velocity="2.6100"/> -->
|
||||
</joint>
|
||||
<link name="panda_link7">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link7.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link7.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint7" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
|
||||
<origin rpy="1.57079632679 0 0" xyz="0.088 0 0"/>
|
||||
<parent link="panda_link6"/>
|
||||
<child link="panda_link7"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
|
||||
</joint>
|
||||
<link name="panda_link8"/>
|
||||
<joint name="panda_joint8" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0 0.107"/>
|
||||
<parent link="panda_link7"/>
|
||||
<child link="panda_link8"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
<joint name="panda_hand_joint" type="fixed">
|
||||
<!--
|
||||
<parent link="panda_link8"/>
|
||||
-->
|
||||
<parent link="panda_link8"/>
|
||||
<child link="panda_hand"/>
|
||||
<origin rpy="0 0 -0.785398163397" xyz="0 0 0.0"/>
|
||||
<!--
|
||||
<origin rpy="0 0 -0.785398163397" xyz="0 0 0"/>
|
||||
-->
|
||||
</joint>
|
||||
<link name="panda_hand">
|
||||
<visual>
|
||||
<!--origin rpy="0.0 0.0 0.0" xyz="0 0 0.0"/-->
|
||||
<origin rpy="1.57 0.0 0.0" xyz="0 0 0.0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/hand.obj"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0 0 0.0"/>
|
||||
<geometry>
|
||||
<!--box size="0.01 0.01 0.01"/-->
|
||||
<!--mesh filename="meshes/visual/hand.obj"/-->
|
||||
<mesh filename="meshes/collision/hand_gripper.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="ee_fixed_joint" type="fixed">
|
||||
<parent link="panda_hand"/>
|
||||
<child link="grasp_frame"/>
|
||||
<!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0922 0.0"/-->
|
||||
<!--origin rpy="-1.57 0.0 1.57" xyz="0.0 0. 0.0"/-->
|
||||
<origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0 0.11"/>
|
||||
</joint>
|
||||
<link name="grasp_frame">
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.01 0.01 0.01"/>
|
||||
</geometry>
|
||||
|
||||
<origin rpy="0 0 0" xyz="0.0 0 0"/>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="ee_fixed_t" type="fixed">
|
||||
<parent link="panda_hand"/>
|
||||
<child link="ee_link"/>
|
||||
<!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0922 0.0"/-->
|
||||
<origin rpy="0 0.0 0" xyz="0.0 0. 0.1"/>
|
||||
<!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0 0.1"/-->
|
||||
</joint>
|
||||
<link name="ee_link"/>
|
||||
<link name="ee_grasp_frame"/>
|
||||
<joint name="ee_grasp_joint" type="fixed">
|
||||
<parent link="panda_hand"/>
|
||||
<child link="ee_grasp_frame"/>
|
||||
<!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0922 0.0"/-->
|
||||
<origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0 0.0"/>
|
||||
<!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0 0.1"/-->
|
||||
</joint>
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
<link name="panda_leftfinger">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/finger.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/finger.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="panda_rightfinger">
|
||||
<visual>
|
||||
<origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/finger.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/finger.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_finger_joint1" type="fixed">
|
||||
<parent link="panda_hand"/>
|
||||
<child link="panda_leftfinger"/>
|
||||
<origin rpy="0 0 0" xyz="0 0.04 0.0584"/>
|
||||
</joint>
|
||||
<joint name="panda_finger_joint2" type="fixed">
|
||||
<parent link="panda_hand"/>
|
||||
<child link="panda_rightfinger"/>
|
||||
<origin rpy="0 0 0" xyz="0 -0.04 0.0584"/>
|
||||
</joint>
|
||||
|
||||
</robot>
|
||||
|
||||
|
||||
@@ -0,0 +1,332 @@
|
||||
<?xml version="1.0" ?>
|
||||
<robot name="panda" xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<link name="base_link"/>
|
||||
<link name="base_link_x"/>
|
||||
<link name="base_link_y"/>
|
||||
<link name="base_link_z"/>
|
||||
|
||||
|
||||
<joint name="base_x" type="prismatic">
|
||||
<origin rpy="0 0 0.0" xyz="0 0 0.0"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="base_link_x"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="87" lower="-5.0" upper="5.0" velocity="1.0"/>
|
||||
</joint>
|
||||
|
||||
<joint name="base_y" type="prismatic">
|
||||
<origin rpy="0 0 0.0" xyz="0 0 0.0"/>
|
||||
<parent link="base_link_x"/>
|
||||
<child link="base_link_y"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="87" lower="-5.0" upper="5.0" velocity="1.0"/>
|
||||
</joint>
|
||||
|
||||
<joint name="base_z" type="revolute">
|
||||
<origin rpy="0 0 0.0" xyz="0 0 0.0"/>
|
||||
<parent link="base_link_y"/>
|
||||
<child link="base_link_z"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="87" lower="-6.0" upper="6.0" velocity="1.50"/>
|
||||
</joint>
|
||||
|
||||
|
||||
|
||||
<joint name="base_fixed" type="fixed">
|
||||
<origin rpy="0 0 0.0" xyz="0 0 0.1"/>
|
||||
<parent link="base_link_z"/>
|
||||
<child link="panda_link0"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
|
||||
|
||||
|
||||
<link name="panda_link0">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link0.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link0.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
</link>
|
||||
<link name="panda_link1">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link1.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link1.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint1" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.333"/>
|
||||
<parent link="panda_link0"/>
|
||||
<child link="panda_link1"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<!--limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/-->
|
||||
<limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
|
||||
</joint>
|
||||
<link name="panda_link2">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link2.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link2.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint2" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
|
||||
<origin rpy="-1.57079632679 0 0" xyz="0 0 0"/>
|
||||
<parent link="panda_link1"/>
|
||||
<child link="panda_link2"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="87" lower="-1.7628" upper="1.7628" velocity="2.1750"/>
|
||||
</joint>
|
||||
<link name="panda_link3">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link3.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link3.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint3" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
|
||||
<origin rpy="1.57079632679 0 0" xyz="0 -0.316 0"/>
|
||||
<parent link="panda_link2"/>
|
||||
<child link="panda_link3"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
|
||||
</joint>
|
||||
<link name="panda_link4">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link4.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link4.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint4" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
|
||||
<origin rpy="1.57079632679 0 0" xyz="0.0825 0 0"/>
|
||||
<parent link="panda_link3"/>
|
||||
<child link="panda_link4"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="87" lower="-3.0718" upper="-0.0698" velocity="2.1750"/>
|
||||
<!-- something is weird with this joint limit config
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="87" lower="-3.0" upper="0.087" velocity="2.1750"/> -->
|
||||
</joint>
|
||||
<link name="panda_link5">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link5.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link5.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint5" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
|
||||
<origin rpy="-1.57079632679 0 0" xyz="-0.0825 0.384 0"/>
|
||||
<parent link="panda_link4"/>
|
||||
<child link="panda_link5"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
|
||||
</joint>
|
||||
<link name="panda_link6">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link6.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link6.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint6" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
|
||||
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
|
||||
<parent link="panda_link5"/>
|
||||
<child link="panda_link6"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="12" lower="0.5" upper="3.7525" velocity="2.6100"/>
|
||||
<!-- <dynamics damping="10.0"/>
|
||||
<limit effort="12" lower="-0.0873" upper="3.0" velocity="2.6100"/> -->
|
||||
</joint>
|
||||
<link name="panda_link7">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link7.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link7.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint7" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
|
||||
<origin rpy="1.57079632679 0 0" xyz="0.088 0 0"/>
|
||||
<parent link="panda_link6"/>
|
||||
<child link="panda_link7"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
|
||||
</joint>
|
||||
<link name="panda_link8"/>
|
||||
<joint name="panda_joint8" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0 0.107"/>
|
||||
<parent link="panda_link7"/>
|
||||
<child link="panda_link8"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
<joint name="panda_hand_joint" type="fixed">
|
||||
<!--
|
||||
<parent link="panda_link8"/>
|
||||
-->
|
||||
<parent link="panda_link8"/>
|
||||
<child link="panda_hand"/>
|
||||
<origin rpy="0 0 -0.785398163397" xyz="0 0 0.0"/>
|
||||
<!--
|
||||
<origin rpy="0 0 -0.785398163397" xyz="0 0 0"/>
|
||||
-->
|
||||
</joint>
|
||||
<link name="panda_hand">
|
||||
<visual>
|
||||
<origin rpy="1.57 0.0 0.0" xyz="0 0 0.0"/>
|
||||
<!--origin rpy="-1.57 0.0 0.0" xyz="0 0 0.0"/-->
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/hand.obj"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0 0 0.0"/>
|
||||
<geometry>
|
||||
<!--box size="0.01 0.01 0.01"/-->
|
||||
<!--mesh filename="meshes/visual/hand.obj"/-->
|
||||
<mesh filename="meshes/collision/hand_gripper.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="ee_fixed_joint" type="fixed">
|
||||
<parent link="panda_hand"/>
|
||||
<child link="grasp_frame"/>
|
||||
<!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0922 0.0"/-->
|
||||
<!--origin rpy="-1.57 0.0 1.57" xyz="0.0 0. 0.0"/-->
|
||||
<origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0 0.11"/>
|
||||
</joint>
|
||||
<link name="grasp_frame">
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.01 0.01 0.01"/>
|
||||
</geometry>
|
||||
|
||||
<origin rpy="0 0 0" xyz="0.0 0 0"/>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="ee_fixed_t" type="fixed">
|
||||
<parent link="panda_hand"/>
|
||||
<child link="ee_link"/>
|
||||
<!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0922 0.0"/-->
|
||||
<origin rpy="0 0.0 0" xyz="0.0 0. 0.1"/>
|
||||
<!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0 0.1"/-->
|
||||
</joint>
|
||||
<link name="ee_link"/>
|
||||
<link name="ee_grasp_frame"/>
|
||||
<joint name="ee_grasp_joint" type="fixed">
|
||||
<parent link="panda_hand"/>
|
||||
<child link="ee_grasp_frame"/>
|
||||
<!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0922 0.0"/-->
|
||||
<origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0 0.0"/>
|
||||
<!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0 0.1"/-->
|
||||
</joint>
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
<link name="panda_leftfinger">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/finger.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/finger.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="panda_rightfinger">
|
||||
<visual>
|
||||
<origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/finger.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/finger.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_finger_joint1" type="prismatic">
|
||||
<parent link="panda_hand"/>
|
||||
<child link="panda_leftfinger"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.0584"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="20" lower="0.0" upper="0.04" velocity="0.2"/>
|
||||
</joint>
|
||||
<joint name="panda_finger_joint2" type="prismatic">
|
||||
<parent link="panda_hand"/>
|
||||
<child link="panda_rightfinger"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.0584"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="20" lower="-0.04" upper="0.0" velocity="0.2"/>
|
||||
<mimic joint="panda_finger_joint1"/>
|
||||
</joint>
|
||||
|
||||
</robot>
|
||||
|
||||
|
||||
@@ -0,0 +1,302 @@
|
||||
<?xml version="1.0" ?>
|
||||
<robot name="panda" xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<link name="base_link"/>
|
||||
<joint name="panda_fixed" type="fixed">
|
||||
<origin rpy="0 0 0.0" xyz="0 0 0.0"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="panda_link0"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<link name="panda_link0">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link0.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link0.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
</link>
|
||||
<link name="panda_link1">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link1.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link1.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint1" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.333"/>
|
||||
<parent link="panda_link0"/>
|
||||
<child link="panda_link1"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<!--limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/-->
|
||||
<limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
|
||||
</joint>
|
||||
<link name="panda_link2">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link2.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link2.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint2" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
|
||||
<origin rpy="-1.57079632679 0 0" xyz="0 0 0"/>
|
||||
<parent link="panda_link1"/>
|
||||
<child link="panda_link2"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="87" lower="-1.7628" upper="1.7628" velocity="2.1750"/>
|
||||
</joint>
|
||||
<link name="panda_link3">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link3.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link3.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint3" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
|
||||
<origin rpy="1.57079632679 0 0" xyz="0 -0.316 0"/>
|
||||
<parent link="panda_link2"/>
|
||||
<child link="panda_link3"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
|
||||
</joint>
|
||||
<link name="panda_link4">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link4.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link4.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint4" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
|
||||
<origin rpy="1.57079632679 0 0" xyz="0.0825 0 0"/>
|
||||
<parent link="panda_link3"/>
|
||||
<child link="panda_link4"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="87" lower="-3.0718" upper="-0.0698" velocity="2.1750"/>
|
||||
<!-- something is weird with this joint limit config
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="87" lower="-3.0" upper="0.087" velocity="2.1750"/> -->
|
||||
</joint>
|
||||
<link name="panda_link5">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link5.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link5.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint5" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
|
||||
<origin rpy="-1.57079632679 0 0" xyz="-0.0825 0.384 0"/>
|
||||
<parent link="panda_link4"/>
|
||||
<child link="panda_link5"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
|
||||
</joint>
|
||||
<link name="panda_link6">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link6.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link6.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint6" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
|
||||
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
|
||||
<parent link="panda_link5"/>
|
||||
<child link="panda_link6"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="12" lower="0.5" upper="3.7525" velocity="2.6100"/>
|
||||
<!-- <dynamics damping="10.0"/>
|
||||
<limit effort="12" lower="-0.0873" upper="3.0" velocity="2.6100"/> -->
|
||||
</joint>
|
||||
<link name="panda_link7">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link7.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link7.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint7" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
|
||||
<origin rpy="1.57079632679 0 0" xyz="0.088 0 0"/>
|
||||
<parent link="panda_link6"/>
|
||||
<child link="panda_link7"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
|
||||
</joint>
|
||||
<link name="panda_link8"/>
|
||||
<joint name="panda_joint8" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0 0.107"/>
|
||||
<parent link="panda_link7"/>
|
||||
<child link="panda_link8"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
<joint name="panda_hand_joint" type="fixed">
|
||||
<!--
|
||||
<parent link="panda_link8"/>
|
||||
-->
|
||||
<parent link="panda_link8"/>
|
||||
<child link="panda_hand"/>
|
||||
<origin rpy="0 0 -0.785398163397" xyz="0 0 0.0"/>
|
||||
<!--
|
||||
<origin rpy="0 0 -0.785398163397" xyz="0 0 0"/>
|
||||
-->
|
||||
</joint>
|
||||
<link name="panda_hand">
|
||||
<visual>
|
||||
<!--origin rpy="0.0 0.0 0.0" xyz="0 0 0.0"/-->
|
||||
<origin rpy="1.57 0.0 0.0" xyz="0 0 0.0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/hand.obj"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0 0 0.0"/>
|
||||
<geometry>
|
||||
<!--box size="0.01 0.01 0.01"/-->
|
||||
<!--mesh filename="meshes/visual/hand.obj"/-->
|
||||
<mesh filename="meshes/collision/hand_gripper.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="ee_fixed_joint" type="fixed">
|
||||
<parent link="panda_hand"/>
|
||||
<child link="grasp_frame"/>
|
||||
<!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0922 0.0"/-->
|
||||
<!--origin rpy="-1.57 0.0 1.57" xyz="0.0 0. 0.0"/-->
|
||||
<origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0 0.11"/>
|
||||
</joint>
|
||||
<link name="grasp_frame">
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.01 0.01 0.01"/>
|
||||
</geometry>
|
||||
|
||||
<origin rpy="0 0 0" xyz="0.0 0 0"/>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="ee_fixed_t" type="fixed">
|
||||
<parent link="panda_hand"/>
|
||||
<child link="ee_link"/>
|
||||
<!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0922 0.0"/-->
|
||||
<origin rpy="0 0.0 0" xyz="0.0 0. 0.1"/>
|
||||
<!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0 0.1"/-->
|
||||
</joint>
|
||||
<link name="ee_link"/>
|
||||
<link name="ee_grasp_frame"/>
|
||||
<joint name="ee_grasp_joint" type="fixed">
|
||||
<parent link="panda_hand"/>
|
||||
<child link="ee_grasp_frame"/>
|
||||
<!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0922 0.0"/-->
|
||||
<origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0 0.0"/>
|
||||
<!--origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0 0.1"/-->
|
||||
</joint>
|
||||
|
||||
|
||||
|
||||
<link name="panda_leftfinger">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/finger.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/finger.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="panda_rightfinger">
|
||||
<visual>
|
||||
<origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/finger.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/finger.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_finger_joint1" type="fixed">
|
||||
<parent link="panda_hand"/>
|
||||
<child link="panda_leftfinger"/>
|
||||
<origin rpy="0 0 0" xyz="0 0.04 0.0584"/>
|
||||
</joint>
|
||||
<joint name="panda_finger_joint2" type="fixed">
|
||||
<parent link="panda_hand"/>
|
||||
<child link="panda_rightfinger"/>
|
||||
<origin rpy="0 0 0" xyz="0 -0.04 0.0584"/>
|
||||
</joint>
|
||||
<link name="right_gripper">
|
||||
<inertial>
|
||||
<!-- Dummy inertial parameters to avoid link lumping-->
|
||||
<mass value="0.01"/>
|
||||
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="right_gripper" type="fixed">
|
||||
<!--<origin rpy="0 0 2.35619449019" xyz="0 0 0.1"/>-->
|
||||
<origin rpy="0 0 2.35619449019" xyz="0 0 0.207"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<parent link="panda_link7"/>
|
||||
<!--<parent link="panda_link8"/>-->
|
||||
<child link="right_gripper"/>
|
||||
</joint>
|
||||
|
||||
</robot>
|
||||
|
||||
|
||||
@@ -0,0 +1,312 @@
|
||||
<?xml version="1.0" ?>
|
||||
<!-- =================================================================================== -->
|
||||
<!-- | This document was autogenerated by xacro from panda_arm_hand.urdf.xacro | -->
|
||||
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||
<!-- =================================================================================== -->
|
||||
<robot name="panda" xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<link name="panda_link0">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.05"/>
|
||||
<mass value="2.9"/>
|
||||
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://meshes/collision/link0.obj"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://meshes/collision/link0.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="panda_link1">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 -0.04 -0.05"/>
|
||||
<mass value="2.7"/>
|
||||
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://meshes/visual/link1.obj"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://meshes/collision/link1.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint1" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.333"/>
|
||||
<parent link="panda_link0"/>
|
||||
<child link="panda_link1"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="87" lower="-2.9671" upper="2.9671" velocity="2.1750"/>
|
||||
</joint>
|
||||
<link name="panda_link2">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 -0.04 0.06"/>
|
||||
<mass value="2.73"/>
|
||||
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://meshes/visual/link2.obj"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://meshes/collision/link2.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint2" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
|
||||
<origin rpy="-1.57079632679 0 0" xyz="0 0 0"/>
|
||||
<parent link="panda_link1"/>
|
||||
<child link="panda_link2"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="87" lower="-1.8326" upper="1.8326" velocity="2.1750"/>
|
||||
</joint>
|
||||
<link name="panda_link3">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.01 0.01 -0.05"/>
|
||||
<mass value="2.04"/>
|
||||
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://meshes/visual/link3.obj"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://meshes/collision/link3.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint3" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
|
||||
<origin rpy="1.57079632679 0 0" xyz="0 -0.316 0"/>
|
||||
<parent link="panda_link2"/>
|
||||
<child link="panda_link3"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="87" lower="-2.9671" upper="2.9671" velocity="2.1750"/>
|
||||
</joint>
|
||||
<link name="panda_link4">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="-0.03 0.03 0.02"/>
|
||||
<mass value="2.08"/>
|
||||
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://meshes/visual/link4.obj"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://meshes/collision/link4.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint4" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
|
||||
<origin rpy="1.57079632679 0 0" xyz="0.0825 0 0"/>
|
||||
<parent link="panda_link3"/>
|
||||
<child link="panda_link4"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="87" lower="-3.1416" upper="0.0" velocity="2.1750"/>
|
||||
</joint>
|
||||
<link name="panda_link5">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0.04 -0.12"/>
|
||||
<mass value="3"/>
|
||||
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://meshes/visual/link5.obj"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://meshes/collision/link5.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint5" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
|
||||
<origin rpy="-1.57079632679 0 0" xyz="-0.0825 0.384 0"/>
|
||||
<parent link="panda_link4"/>
|
||||
<child link="panda_link5"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="12" lower="-2.9671" upper="2.9671" velocity="2.6100"/>
|
||||
</joint>
|
||||
<link name="panda_link6">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.04 0 0"/>
|
||||
<mass value="1.3"/>
|
||||
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://meshes/visual/link6.obj"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://meshes/collision/link6.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint6" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
|
||||
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
|
||||
<parent link="panda_link5"/>
|
||||
<child link="panda_link6"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="12" lower="-0.0873" upper="3.8223" velocity="2.6100"/>
|
||||
</joint>
|
||||
<link name="panda_link7">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.08"/>
|
||||
<mass value=".2"/>
|
||||
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://meshes/collision/link7.obj"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://meshes/collision/link7.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint7" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
|
||||
<origin rpy="1.57079632679 0 0" xyz="0.088 0 0"/>
|
||||
<parent link="panda_link6"/>
|
||||
<child link="panda_link7"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="12" lower="-2.9671" upper="2.9671" velocity="2.6100"/>
|
||||
</joint>
|
||||
<link name="panda_link8">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value="0.0"/>
|
||||
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="panda_joint8" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0 0.107"/>
|
||||
<parent link="panda_link7"/>
|
||||
<child link="panda_link8"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
<joint name="panda_hand_joint" type="fixed">
|
||||
<parent link="panda_link8"/>
|
||||
<child link="panda_hand"/>
|
||||
<origin rpy="0 0 -0.785398163397" xyz="0 0 0"/>
|
||||
</joint>
|
||||
<link name="panda_hand">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.04"/>
|
||||
<mass value=".81"/>
|
||||
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://meshes/visual/hand.obj"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://meshes/collision/hand.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="panda_leftfinger">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0.01 0.02"/>
|
||||
<mass value="0.1"/>
|
||||
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://meshes/visual/finger.obj"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://meshes/collision/finger.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="panda_rightfinger">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 -0.01 0.02"/>
|
||||
<mass value="0.1"/>
|
||||
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://meshes/visual/finger.obj"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://meshes/collision/finger.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_finger_joint1" type="fixed">
|
||||
<parent link="panda_hand"/>
|
||||
<child link="panda_leftfinger"/>
|
||||
<origin rpy="0 0 0" xyz="0 0.04 0.0584"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit effort="20" lower="0.0" upper="0.04" velocity="0.2"/>
|
||||
</joint>
|
||||
<joint name="panda_finger_joint2" type="fixed">
|
||||
<parent link="panda_hand"/>
|
||||
<child link="panda_rightfinger"/>
|
||||
<origin rpy="0 0 0" xyz="0 -0.04 0.0584"/>
|
||||
<axis xyz="0 -1 0"/>
|
||||
<limit effort="20" lower="0.0" upper="0.04" velocity="0.2"/>
|
||||
<mimic joint="panda_finger_joint1"/>
|
||||
</joint>
|
||||
<link name="right_gripper">
|
||||
<inertial>
|
||||
<!-- Dummy inertial parameters to avoid link lumping-->
|
||||
<mass value="0.01"/>
|
||||
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="right_gripper" type="fixed">
|
||||
<origin rpy="0 0 2.35619449019" xyz="0 0 0.1"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<parent link="panda_link8"/>
|
||||
<child link="right_gripper"/>
|
||||
</joint>
|
||||
<link name="panda_grasptarget">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value="0.0"/>
|
||||
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="panda_grasptarget_hand" type="fixed">
|
||||
<parent link="panda_hand"/>
|
||||
<child link="panda_grasptarget"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.105"/>
|
||||
</joint>
|
||||
|
||||
</robot>
|
||||
@@ -0,0 +1,71 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot name="mug">
|
||||
<link name="base"/>
|
||||
<link name="base1"/>
|
||||
<link name="base2"/>
|
||||
<link name="base3"/>
|
||||
<link name="base4"/>
|
||||
<link name="base5"/>
|
||||
<joint name="x_joint" type="prismatic">
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<parent link="base"/>
|
||||
<child link="base1"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
|
||||
</joint>
|
||||
|
||||
<joint name="y_joint" type="prismatic">
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<parent link="base1"/>
|
||||
<child link="base2"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
|
||||
</joint>
|
||||
|
||||
<joint name="z_joint" type="prismatic">
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<parent link="base2"/>
|
||||
<child link="base3"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
|
||||
</joint>
|
||||
<joint name="rx_joint" type="revolute">
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<parent link="base3"/>
|
||||
<child link="base4"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
|
||||
</joint>
|
||||
<joint name="ry_joint" type="revolute">
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<parent link="base4"/>
|
||||
<child link="base5"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
|
||||
</joint>
|
||||
<joint name="rz_joint" type="revolute">
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<parent link="base5"/>
|
||||
<child link="gripper"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
|
||||
</joint>
|
||||
<link name="gripper">
|
||||
<visual>
|
||||
<origin xyz="0.0 0.0 0.0" rpy="0.0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/hand.obj"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0.0 0.0 0.0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/hand.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.0"/>
|
||||
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||
</inertial>
|
||||
</link>
|
||||
</robot>
|
||||
342
src/curobo/content/assets/robot/franka_description/panda.urdf
Normal file
342
src/curobo/content/assets/robot/franka_description/panda.urdf
Normal file
@@ -0,0 +1,342 @@
|
||||
<?xml version="1.0" ?>
|
||||
<!-- =================================================================================== -->
|
||||
<!-- | This document was autogenerated by xacro from panda_arm_hand.urdf.xacro | -->
|
||||
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||
<!-- =================================================================================== -->
|
||||
<robot name="panda" xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<link name="panda_link0">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link0.dae"/>
|
||||
</geometry>
|
||||
<material name="panda_white">
|
||||
<color rgba="1. 1. 1. 1."/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link0.stl"/>
|
||||
</geometry>
|
||||
<material name="panda_white"/>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="panda_link1">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 -0.04 -0.05"/>
|
||||
<mass value="2.7"/>
|
||||
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link1.dae"/>
|
||||
</geometry>
|
||||
<material name="panda_white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link1.stl"/>
|
||||
</geometry>
|
||||
<material name="panda_white"/>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint1" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.333"/>
|
||||
<parent link="panda_link0"/>
|
||||
<child link="panda_link1"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
|
||||
</joint>
|
||||
<link name="panda_link2">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 -0.04 0.06"/>
|
||||
<mass value="2.73"/>
|
||||
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link2.dae"/>
|
||||
</geometry>
|
||||
<material name="panda_white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link2.stl"/>
|
||||
</geometry>
|
||||
<material name="panda_white"/>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint2" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
|
||||
<origin rpy="-1.57079632679 0 0" xyz="0 0 0"/>
|
||||
<parent link="panda_link1"/>
|
||||
<child link="panda_link2"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="87" lower="-1.7628" upper="1.7628" velocity="2.1750"/>
|
||||
</joint>
|
||||
<link name="panda_link3">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.01 0.01 -0.05"/>
|
||||
<mass value="2.04"/>
|
||||
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link3.dae"/>
|
||||
</geometry>
|
||||
<material name="panda_red">
|
||||
<color rgba="1. 1. 1. 1."/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link3.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint3" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
|
||||
<origin rpy="1.57079632679 0 0" xyz="0 -0.316 0"/>
|
||||
<parent link="panda_link2"/>
|
||||
<child link="panda_link3"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
|
||||
</joint>
|
||||
<link name="panda_link4">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="-0.03 0.03 0.02"/>
|
||||
<mass value="2.08"/>
|
||||
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link4.dae"/>
|
||||
</geometry>
|
||||
<material name="panda_white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link4.stl"/>
|
||||
</geometry>
|
||||
<material name="panda_white"/>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint4" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
|
||||
<origin rpy="1.57079632679 0 0" xyz="0.0825 0 0"/>
|
||||
<parent link="panda_link3"/>
|
||||
<child link="panda_link4"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="87" lower="-3.0718" upper="-0.0698" velocity="2.1750"/>
|
||||
</joint>
|
||||
<link name="panda_link5">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0.04 -0.12"/>
|
||||
<mass value="3"/>
|
||||
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link5.dae"/>
|
||||
</geometry>
|
||||
<material name="panda_white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link5.stl"/>
|
||||
</geometry>
|
||||
<material name="panda_white"/>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint5" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8975" soft_upper_limit="2.8975"/>
|
||||
<origin rpy="-1.57079632679 0 0" xyz="-0.0825 0.384 0"/>
|
||||
<parent link="panda_link4"/>
|
||||
<child link="panda_link5"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="12" lower="-2.8975" upper="2.8975" velocity="2.6100"/>
|
||||
</joint>
|
||||
<link name="panda_link6">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.04 0 0"/>
|
||||
<mass value="1.3"/>
|
||||
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link6.dae"/>
|
||||
</geometry>
|
||||
<material name="panda_white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link6.stl"/>
|
||||
</geometry>
|
||||
<material name="panda_white"/>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint6" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
|
||||
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
|
||||
<parent link="panda_link5"/>
|
||||
<child link="panda_link6"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="12" lower="-0.0175" upper="3.7525" velocity="2.6100"/>
|
||||
</joint>
|
||||
<link name="panda_link7">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.08"/>
|
||||
<mass value=".2"/>
|
||||
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/link7.dae"/>
|
||||
</geometry>
|
||||
<material name="panda_white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/link7.stl"/>
|
||||
</geometry>
|
||||
<material name="panda_white"/>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_joint7" type="revolute">
|
||||
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
|
||||
<origin rpy="1.57079632679 0 0" xyz="0.088 0 0"/>
|
||||
<parent link="panda_link6"/>
|
||||
<child link="panda_link7"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
|
||||
</joint>
|
||||
<link name="panda_link8">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value="0.0"/>
|
||||
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="panda_joint8" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0 0.107"/>
|
||||
<parent link="panda_link7"/>
|
||||
<child link="panda_link8"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
<joint name="panda_hand_joint" type="fixed">
|
||||
<parent link="panda_link8"/>
|
||||
<child link="panda_hand"/>
|
||||
<origin rpy="0 0 -0.785398163397" xyz="0 0 0"/>
|
||||
</joint>
|
||||
<link name="panda_hand">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.04"/>
|
||||
<mass value=".81"/>
|
||||
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/hand.dae"/>
|
||||
</geometry>
|
||||
<material name="panda_white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/hand.stl"/>
|
||||
</geometry>
|
||||
<material name="panda_white"/>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="panda_leftfinger">
|
||||
<contact>
|
||||
<friction_anchor/>
|
||||
<stiffness value="30000.0"/>
|
||||
<damping value="1000.0"/>
|
||||
<spinning_friction value="0.1"/>
|
||||
<lateral_friction value="1.0"/>
|
||||
</contact>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0.01 0.02"/>
|
||||
<mass value="0.1"/>
|
||||
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/finger.dae"/>
|
||||
</geometry>
|
||||
<material name="panda_white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/finger.stl"/>
|
||||
</geometry>
|
||||
<material name="panda_white"/>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="panda_rightfinger">
|
||||
<contact>
|
||||
<friction_anchor/>
|
||||
<stiffness value="30000.0"/>
|
||||
<damping value="1000.0"/>
|
||||
<spinning_friction value="0.1"/>
|
||||
<lateral_friction value="1.0"/>
|
||||
</contact>
|
||||
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 -0.01 0.02"/>
|
||||
<mass value="0.1"/>
|
||||
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/finger.dae"/>
|
||||
</geometry>
|
||||
<material name="panda_white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/collision/finger.stl"/>
|
||||
</geometry>
|
||||
<material name="panda_white"/>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="panda_finger_joint1" type="fixed">
|
||||
<parent link="panda_hand"/>
|
||||
<child link="panda_leftfinger"/>
|
||||
<origin rpy="0 0 0" xyz="0 0.04 0.0584"/>
|
||||
</joint>
|
||||
<joint name="panda_finger_joint2" type="fixed">
|
||||
<parent link="panda_hand"/>
|
||||
<child link="panda_rightfinger"/>
|
||||
<origin rpy="0 0 0" xyz="0 -0.04 0.0584"/>
|
||||
</joint>
|
||||
<link name="right_gripper">
|
||||
<inertial>
|
||||
<!-- Dummy inertial parameters to avoid link lumping-->
|
||||
<mass value="0.01"/>
|
||||
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="right_gripper" type="fixed">
|
||||
<origin rpy="0 0 2.35619449019" xyz="0 0 0.1"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<parent link="panda_link8"/>
|
||||
<child link="right_gripper"/>
|
||||
</joint>
|
||||
<link name="panda_grasptarget">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value="0.0"/>
|
||||
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="panda_grasptarget_hand" type="fixed">
|
||||
<parent link="panda_hand"/>
|
||||
<child link="panda_grasptarget"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.105"/>
|
||||
</joint>
|
||||
|
||||
</robot>
|
||||
@@ -0,0 +1,26 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot name="panda_gripper">
|
||||
<link name="gripper">
|
||||
<visual>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0 0 0.0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/visual/graspnet_panda_mesh.obj"/>
|
||||
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0 0 0.0"/>
|
||||
<geometry>
|
||||
<!--box size="0.01 0.01 0.01"/-->
|
||||
|
||||
<mesh filename="meshes/visual/graspnet_panda_mesh.obj"/>
|
||||
<!--mesh filename="meshes/collision/hand_gripper.obj"/-->
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<inertial>
|
||||
<mass value="0.102"/>
|
||||
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||
</inertial>
|
||||
</link>
|
||||
</robot>
|
||||
@@ -0,0 +1,26 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot name="panda_gripper">
|
||||
<link name="gripper">
|
||||
<visual>
|
||||
<origin rpy="0 0.0 0.0" xyz="0 0 0.0"/>
|
||||
<geometry>
|
||||
<!--mesh filename="meshes/visual/graspnet_panda_mesh.obj"/-->
|
||||
<mesh filename="meshes/visual/hand.obj"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0 0 0.0"/>
|
||||
<geometry>
|
||||
<!--box size="0.01 0.01 0.01"/-->
|
||||
<mesh filename="meshes/visual/hand.obj"/>
|
||||
<!--mesh filename="meshes/visual/graspnet_panda_mesh.obj"/-->
|
||||
<!--mesh filename="meshes/collision/hand_gripper.obj"/-->
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<inertial>
|
||||
<mass value="0.102"/>
|
||||
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||
</inertial>
|
||||
</link>
|
||||
</robot>
|
||||
@@ -0,0 +1,546 @@
|
||||
<?xml version="1.0" ?>
|
||||
<!-- =================================================================================== -->
|
||||
<!-- | This document was autogenerated by xacro from lula_kuka_allegro.urdf.xacro | -->
|
||||
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||
<!-- =================================================================================== -->
|
||||
<robot name="kuka_allegro" xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<!-- ======================== BASE PARAMS ========================= -->
|
||||
<!-- ======================== FINGER PARAMS ======================== -->
|
||||
<!-- full height from joint to tip. when used,
|
||||
the radius of the finger tip sphere will be subtracted
|
||||
and one fixed link will be added for the tip. -->
|
||||
<!-- ========================= THUMB PARAMS ========================= -->
|
||||
<!-- ========================= LIMITS ========================= -->
|
||||
<!-- ============================================================================= -->
|
||||
<!-- BASE -->
|
||||
<link name="allegro_mount">
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="1e-4" ixy="0" ixz="0" iyy="1e-4" iyz="0" izz="1e-4"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="kuka_allegro_description/meshes/mounts/allegro_mount.obj"/>
|
||||
</geometry>
|
||||
<material name="color_j7"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="kuka_allegro_description/meshes/mounts/allegro_mount.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="allegro_mount_joint" type="fixed">
|
||||
<origin rpy="0 -1.5708 0.785398" xyz="-0.008219 -0.02063 0.08086"/>
|
||||
<parent link="allegro_mount"/>
|
||||
<child link="palm_link"/>
|
||||
</joint>
|
||||
<link name="palm_link">
|
||||
<inertial>
|
||||
<mass value="0.4154"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="1e-4" ixy="0" ixz="0" iyy="1e-4" iyz="0" izz="1e-4"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="kuka_allegro_description/meshes/allegro/base_link.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0 0 0 "/>
|
||||
<material name="Grey">
|
||||
<color rgba="0.2 0.2 0.2 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="kuka_allegro_description/meshes/allegro/base_link.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<gazebo reference="palm_link">
|
||||
<material value="Gazebo/Grey"/>
|
||||
</gazebo>
|
||||
<link name="index_link_0">
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="kuka_allegro_description/meshes/allegro/primary_base.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.005"/>
|
||||
<inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="kuka_allegro_description/meshes/allegro/primary_base.obj"/>
|
||||
</geometry>
|
||||
<material name="Grey">
|
||||
<color rgba="0.2 0.2 0.2 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="index_link_1">
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="kuka_allegro_description/meshes/allegro/primary_proximal.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.005"/>
|
||||
<inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="kuka_allegro_description/meshes/allegro/primary_proximal.obj"/>
|
||||
</geometry>
|
||||
<material name="Grey">
|
||||
<color rgba="0.2 0.2 0.2 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="index_link_2">
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="kuka_allegro_description/meshes/allegro/primary_medial.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="kuka_allegro_description/meshes/allegro/primary_medial.obj"/>
|
||||
</geometry>
|
||||
<material name="Grey">
|
||||
<color rgba="0.2 0.2 0.2 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="index_link_3">
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="kuka_allegro_description/meshes/biotac/biotac_sensor.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.11"/>
|
||||
<inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="kuka_allegro_description/meshes/biotac/biotac_sensor.obj"/>
|
||||
</geometry>
|
||||
<material name="Green">
|
||||
<color rgba="0. 0.5 0. 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="index_biotac_tip">
|
||||
<inertial>
|
||||
<mass value="0.04"/>
|
||||
<inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="middle_link_0">
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="kuka_allegro_description/meshes/allegro/primary_base.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.1"/>
|
||||
<inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="kuka_allegro_description/meshes/allegro/primary_base.obj"/>
|
||||
</geometry>
|
||||
<material name="Grey">
|
||||
<color rgba="0.2 0.2 0.2 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="middle_link_1">
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="kuka_allegro_description/meshes/allegro/primary_proximal.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.1"/>
|
||||
<inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="kuka_allegro_description/meshes/allegro/primary_proximal.obj"/>
|
||||
</geometry>
|
||||
<material name="Grey">
|
||||
<color rgba="0.2 0.2 0.2 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="middle_link_2">
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="kuka_allegro_description/meshes/allegro/primary_medial.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.1"/>
|
||||
<inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="kuka_allegro_description/meshes/allegro/primary_medial.obj"/>
|
||||
</geometry>
|
||||
<material name="Grey">
|
||||
<color rgba="0.2 0.2 0.2 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="middle_link_3">
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="kuka_allegro_description/meshes/biotac/biotac_sensor.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.1"/>
|
||||
<inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="kuka_allegro_description/meshes/biotac/biotac_sensor.obj"/>
|
||||
</geometry>
|
||||
<material name="Green">
|
||||
<color rgba="0 0.5 0 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="middle_biotac_tip">
|
||||
<inertial>
|
||||
<mass value="0.1"/>
|
||||
<inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
|
||||
</inertial>
|
||||
</link>
|
||||
-->
|
||||
<link name="ring_link_0">
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="kuka_allegro_description/meshes/allegro/primary_base.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.1"/>
|
||||
<inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="kuka_allegro_description/meshes/allegro/primary_base.obj"/>
|
||||
</geometry>
|
||||
<material name="Grey">
|
||||
<color rgba="0.2 0.2 0.2 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="ring_link_1">
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="kuka_allegro_description/meshes/allegro/primary_proximal.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.1"/>
|
||||
<inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="kuka_allegro_description/meshes/allegro/primary_proximal.obj"/>
|
||||
</geometry>
|
||||
<material name="Grey">
|
||||
<color rgba="0.2 0.2 0.2 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="ring_link_2">
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="kuka_allegro_description/meshes/allegro/primary_medial.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.1"/>
|
||||
<inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="kuka_allegro_description/meshes/allegro/primary_medial.obj"/>
|
||||
</geometry>
|
||||
<material name="Grey">
|
||||
<color rgba="0.2 0.2 0.2 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="ring_link_3">
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="kuka_allegro_description/meshes/biotac/biotac_sensor.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.1"/>
|
||||
<inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="kuka_allegro_description/meshes/biotac/biotac_sensor.obj"/>
|
||||
</geometry>
|
||||
<material name="Green">
|
||||
<color rgba="0 0.5 0 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<link name="ring_biotac_tip">
|
||||
<inertial>
|
||||
<mass value="0.1"/>
|
||||
<inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="thumb_link_0">
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="kuka_allegro_description/meshes/allegro/thumb_base.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.1"/>
|
||||
<inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="kuka_allegro_description/meshes/allegro/thumb_base.obj"/>
|
||||
</geometry>
|
||||
<material name="Grey">
|
||||
<color rgba="0.2 0.2 0.2 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="thumb_link_1">
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="kuka_allegro_description/meshes/allegro/thumb_proximal.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.1"/>
|
||||
<inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="kuka_allegro_description/meshes/allegro/thumb_proximal.obj"/>
|
||||
</geometry>
|
||||
<material name="Grey">
|
||||
<color rgba="0.2 0.2 0.2 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="thumb_link_2">
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="kuka_allegro_description/meshes/allegro/thumb_medial.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.1"/>
|
||||
<inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="kuka_allegro_description/meshes/allegro/thumb_medial.obj"/>
|
||||
</geometry>
|
||||
<material name="Grey">
|
||||
<color rgba="0.2 0.2 0.2 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="thumb_link_3">
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="kuka_allegro_description/meshes/biotac/biotac_sensor_thumb.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.1"/>
|
||||
<inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="kuka_allegro_description/meshes/biotac/biotac_sensor_thumb.obj"/>
|
||||
</geometry>
|
||||
<material name="Green">
|
||||
<color rgba="0 0.5 0 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="thumb_biotac_tip">
|
||||
<inertial>
|
||||
<mass value="0.1"/>
|
||||
<inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="index_joint_0" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="0.35" lower="-0.558488888889" upper="0.558488888889" velocity="6.283"/>
|
||||
<origin rpy="3.1415 -1.57075 -0.0872638888889" xyz="0.0514302 -0.03632 -0.0113"/>
|
||||
<parent link="palm_link"/>
|
||||
<child link="index_link_0"/>
|
||||
<dynamics damping="0.025" friction="0.035"/>
|
||||
</joint>
|
||||
<joint name="index_joint_1" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="0.35" lower="-0.279244444444" upper="1.727825" velocity="6.283"/>
|
||||
<origin rpy="3.1415 -1.57075 1.57075" xyz="0.0 0.0 0.0"/>
|
||||
<parent link="index_link_0"/>
|
||||
<child link="index_link_1"/>
|
||||
<dynamics damping="0.025" friction="0.035"/>
|
||||
</joint>
|
||||
<joint name="index_joint_2" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="0.35" lower="-0.279244444444" upper="1.727825" velocity="6.283"/>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.054 0.0 0.0"/>
|
||||
<parent link="index_link_1"/>
|
||||
<child link="index_link_2"/>
|
||||
<dynamics damping="0.025" friction="0.035"/>
|
||||
</joint>
|
||||
<joint name="index_joint_3" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="0.35" lower="-0.279244444444" upper="1.727825" velocity="6.283"/>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0384 0.0 0.0"/>
|
||||
<parent link="index_link_2"/>
|
||||
<child link="index_link_3"/>
|
||||
<dynamics damping="0.025" friction="0.035"/>
|
||||
</joint>
|
||||
<joint name="middle_joint_0" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="0.35" lower="-0.558488888889" upper="0.558488888889" velocity="6.283"/>
|
||||
<origin rpy="3.1415 -1.57075 0" xyz="0.0537375 0.0087771 -0.0113"/>
|
||||
<parent link="palm_link"/>
|
||||
<child link="middle_link_0"/>
|
||||
<dynamics friction="0.035"/>
|
||||
</joint>
|
||||
|
||||
<joint name="middle_joint_1" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="0.35" lower="-0.279244444444" upper="1.727825" velocity="6.283"/>
|
||||
<origin rpy="3.1415 -1.57075 1.57075" xyz="0.0 0.0 0.0"/>
|
||||
<parent link="middle_link_0"/>
|
||||
<child link="middle_link_1"/>
|
||||
<dynamics friction="0.035"/>
|
||||
</joint>
|
||||
<joint name="middle_joint_2" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="0.35" lower="-0.279244444444" upper="1.727825" velocity="6.283"/>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.054 0.0 0.0"/>
|
||||
<parent link="middle_link_1"/>
|
||||
<child link="middle_link_2"/>
|
||||
<dynamics friction="0.035"/>
|
||||
</joint>
|
||||
<joint name="middle_joint_3" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="0.35" lower="-0.279244444444" upper="1.727825" velocity="6.283"/>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0384 0.0 0.0"/>
|
||||
<parent link="middle_link_2"/>
|
||||
<child link="middle_link_3"/>
|
||||
<dynamics friction="0.035"/>
|
||||
</joint>
|
||||
<joint name="middle_biotac_tip_joint" type="fixed">
|
||||
<origin rpy="0 0 0.436319444444 " xyz="0.055 0.015 0"/>
|
||||
<parent link="middle_link_3"/>
|
||||
<child link="middle_biotac_tip"/>
|
||||
</joint>
|
||||
|
||||
<joint name="ring_joint_0" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="0.35" lower="-0.558488888889" upper="0.558488888889" velocity="6.283"/>
|
||||
<origin rpy="3.1415 -1.57075 0.0872638888889" xyz="0.0514302 0.0538749 -0.0113"/>
|
||||
<parent link="palm_link"/>
|
||||
<child link="ring_link_0"/>
|
||||
<dynamics friction="0.035"/>
|
||||
</joint>
|
||||
<joint name="ring_joint_1" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="0.35" lower="-0.279244444444" upper="1.727825" velocity="6.283"/>
|
||||
<origin rpy="3.1415 -1.57075 1.57075" xyz="0.0 0.0 0.0"/>
|
||||
<parent link="ring_link_0"/>
|
||||
<child link="ring_link_1"/>
|
||||
<dynamics friction="0.035"/>
|
||||
</joint>
|
||||
<joint name="ring_joint_2" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="0.35" lower="-0.279244444444" upper="1.727825" velocity="6.283"/>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.054 0.0 0.0"/>
|
||||
<parent link="ring_link_1"/>
|
||||
<child link="ring_link_2"/>
|
||||
<dynamics friction="0.035"/>
|
||||
</joint>
|
||||
<joint name="ring_joint_3" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="0.35" lower="-0.279244444444" upper="1.727825" velocity="6.283"/>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0384 0.0 0.0"/>
|
||||
<parent link="ring_link_2"/>
|
||||
<child link="ring_link_3"/>
|
||||
<dynamics friction="0.035"/>
|
||||
</joint>
|
||||
|
||||
<joint name="ring_biotac_tip_joint" type="fixed">
|
||||
<origin rpy="0 0 0.436319444444 " xyz="0.055 0.015 0"/>
|
||||
<parent link="ring_link_3"/>
|
||||
<child link="ring_biotac_tip"/>
|
||||
</joint>
|
||||
|
||||
<joint name="thumb_joint_0" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="0.35" lower="0.279244444444" upper="1.57075" velocity="6.283"/>
|
||||
<origin rpy="-1.57075 -1.57075 1.48348611111" xyz="-0.0367482 -0.0081281 -0.0295"/>
|
||||
<parent link="palm_link"/>
|
||||
<child link="thumb_link_0"/>
|
||||
<dynamics friction="0.035"/>
|
||||
</joint>
|
||||
<joint name="thumb_joint_1" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="0.35" lower="-0.331602777778" upper="1.15188333333" velocity="6.283"/>
|
||||
<origin rpy="1.57075 0 0" xyz="0.005 0.0 0.0"/>
|
||||
<parent link="thumb_link_0"/>
|
||||
<child link="thumb_link_1"/>
|
||||
<dynamics friction="0.035"/>
|
||||
</joint>
|
||||
<joint name="thumb_joint_2" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="0.35" lower="-0.279244444444" upper="1.727825" velocity="6.283"/>
|
||||
<origin rpy="3.1415 -1.57075 0.0" xyz="0 0 0.0554"/>
|
||||
<parent link="thumb_link_1"/>
|
||||
<child link="thumb_link_2"/>
|
||||
<dynamics friction="0.035"/>
|
||||
</joint>
|
||||
<joint name="thumb_joint_3" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="0.35" lower="-0.279244444444" upper="1.76273055556" velocity="6.283"/>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0514 0.0 0.0"/>
|
||||
<parent link="thumb_link_2"/>
|
||||
<child link="thumb_link_3"/>
|
||||
<dynamics friction="0.035"/>
|
||||
</joint>
|
||||
<joint name="thumb_biotac_tip_joint" type="fixed">
|
||||
<origin rpy="0 0 0.436319444444 " xyz="0.07 0.01 0"/>
|
||||
<parent link="thumb_link_3"/>
|
||||
<child link="thumb_biotac_tip"/>
|
||||
</joint>
|
||||
|
||||
</robot>
|
||||
@@ -0,0 +1,414 @@
|
||||
<?xml version="1.0" ?>
|
||||
<!-- =================================================================================== -->
|
||||
<!-- | This document was autogenerated by xacro from lula_kuka_allegro.urdf.xacro | -->
|
||||
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||
<!-- =================================================================================== -->
|
||||
<robot name="kuka_iiwa" xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<material name="Black">
|
||||
<color rgba="0.0 0.0 0.0 1.0"/>
|
||||
</material>
|
||||
<material name="Blue">
|
||||
<color rgba="0.0 0.0 0.8 1.0"/>
|
||||
</material>
|
||||
<material name="Green">
|
||||
<color rgba="0.0 0.8 0.0 1.0"/>
|
||||
</material>
|
||||
<material name="Grey">
|
||||
<color rgba="0.4 0.4 0.4 1.0"/>
|
||||
</material>
|
||||
<material name="Orange">
|
||||
<color rgba="1.0 0.423529411765 0.0392156862745 1.0"/>
|
||||
</material>
|
||||
<material name="Brown">
|
||||
<color rgba="0.870588235294 0.811764705882 0.764705882353 1.0"/>
|
||||
</material>
|
||||
<material name="Red">
|
||||
<color rgba="0.8 0.0 0.0 1.0"/>
|
||||
</material>
|
||||
<material name="White">
|
||||
<color rgba="1.0 1.0 1.0 1.0"/>
|
||||
</material>
|
||||
<link name="base_link"/>
|
||||
<!--joint between {parent} and link_0-->
|
||||
<joint name="iiwa7_base_link_iiwa7_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="iiwa7_link_0"/>
|
||||
</joint>
|
||||
<link name="iiwa7_link_0">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="-0.1 0 0.07"/>
|
||||
<mass value="5"/>
|
||||
<inertia ixx="0.05" ixy="0" ixz="0" iyy="0.06" iyz="0" izz="0.03"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/iiwa7/visual/link_0.obj"/>
|
||||
</geometry>
|
||||
<material name="Grey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/iiwa7/collision/link_0.obj"/>
|
||||
</geometry>
|
||||
<material name="Grey"/>
|
||||
</collision>
|
||||
<self_collision_checking>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<capsule length="0.25" radius="0.15"/>
|
||||
</geometry>
|
||||
</self_collision_checking>
|
||||
</link>
|
||||
<!-- joint between link_0 and link_1 -->
|
||||
<joint name="iiwa7_joint_1" type="revolute">
|
||||
<parent link="iiwa7_link_0"/>
|
||||
<child link="iiwa7_link_1"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.15"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="300" lower="-2.96705972839" upper="2.96705972839" velocity="10"/>
|
||||
<safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.93215314335" soft_upper_limit="2.93215314335"/>
|
||||
<dynamics damping="0.5"/>
|
||||
</joint>
|
||||
<link name="iiwa7_link_1">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 -0.03 0.12"/>
|
||||
<mass value="3.4525"/>
|
||||
<inertia ixx="0.02183" ixy="0" ixz="0" iyy="0.007703" iyz="-0.003887" izz="0.02083"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.0075"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/iiwa7/visual/link_1.obj"/>
|
||||
</geometry>
|
||||
<material name="Orange"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.0075"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/iiwa7/collision/link_1.obj"/>
|
||||
</geometry>
|
||||
<material name="Orange"/>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="iiwa7_joint_2" type="revolute">
|
||||
<parent link="iiwa7_link_1"/>
|
||||
<child link="iiwa7_link_2"/>
|
||||
<origin rpy="1.57079632679 0 3.14159265359" xyz="0 0 0.19"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="300" lower="-2.09439510239" upper="2.09439510239" velocity="10"/>
|
||||
<safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.05948851735" soft_upper_limit="2.05948851735"/>
|
||||
<dynamics damping="0.5"/>
|
||||
</joint>
|
||||
<link name="iiwa7_link_2">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.0003 0.059 0.042"/>
|
||||
<mass value="3.4821"/>
|
||||
<inertia ixx="0.02076" ixy="0" ixz="-0.003626" iyy="0.02179" iyz="0" izz="0.00779"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/iiwa7/visual/link_2.obj"/>
|
||||
</geometry>
|
||||
<material name="Orange"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/iiwa7/collision/link_2.obj"/>
|
||||
</geometry>
|
||||
<material name="Orange"/>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="iiwa7_joint_3" type="revolute">
|
||||
<parent link="iiwa7_link_2"/>
|
||||
<child link="iiwa7_link_3"/>
|
||||
<origin rpy="1.57079632679 0 3.14159265359" xyz="0 0.21 0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="300" lower="-2.96705972839" upper="2.96705972839" velocity="10"/>
|
||||
<safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.93215314335" soft_upper_limit="2.93215314335"/>
|
||||
<dynamics damping="0.5"/>
|
||||
</joint>
|
||||
<link name="iiwa7_link_3">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0.03 0.13"/>
|
||||
<mass value="4.05623"/>
|
||||
<inertia ixx="0.03204" ixy="0" ixz="0" iyy="0.00972" iyz="0.006227" izz="0.03042"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 -0.026"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/iiwa7/visual/link_3.obj"/>
|
||||
</geometry>
|
||||
<material name="Orange"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 -0.026"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/iiwa7/collision/link_3.obj"/>
|
||||
</geometry>
|
||||
<material name="Orange"/>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="iiwa7_joint_4" type="revolute">
|
||||
<parent link="iiwa7_link_3"/>
|
||||
<child link="iiwa7_link_4"/>
|
||||
<origin rpy="1.57079632679 0 0" xyz="0 0 0.19"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="300" lower="-2.09439510239" upper="2.09439510239" velocity="10"/>
|
||||
<safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.05948851735" soft_upper_limit="2.05948851735"/>
|
||||
<dynamics damping="0.5"/>
|
||||
</joint>
|
||||
<link name="iiwa7_link_4">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0.067 0.034"/>
|
||||
<mass value="3.4822"/>
|
||||
<inertia ixx="0.02178" ixy="0" ixz="0" iyy="0.02075" iyz="-0.003625" izz="0.007785"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/iiwa7/visual/link_4.obj"/>
|
||||
</geometry>
|
||||
<material name="Orange"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/iiwa7/collision/link_4.obj"/>
|
||||
</geometry>
|
||||
<material name="Orange"/>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="iiwa7_joint_5" type="revolute">
|
||||
<parent link="iiwa7_link_4"/>
|
||||
<child link="iiwa7_link_5"/>
|
||||
<origin rpy="-1.57079632679 3.14159265359 0" xyz="0 0.21 0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="300" lower="-2.96705972839" upper="2.96705972839" velocity="10"/>
|
||||
<safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.93215314335" soft_upper_limit="2.93215314335"/>
|
||||
<dynamics damping="0.5"/>
|
||||
</joint>
|
||||
<link name="iiwa7_link_5">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.0001 0.021 0.076"/>
|
||||
<mass value="2.1633"/>
|
||||
<inertia ixx="0.01287" ixy="0" ixz="0" iyy="0.005708" iyz="-0.003946" izz="0.01112"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 -0.026"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/iiwa7/visual/link_5.obj"/>
|
||||
</geometry>
|
||||
<material name="Orange"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 -0.026"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/iiwa7/collision/link_5.obj"/>
|
||||
</geometry>
|
||||
<material name="Orange"/>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="iiwa7_joint_6" type="revolute">
|
||||
<parent link="iiwa7_link_5"/>
|
||||
<child link="iiwa7_link_6"/>
|
||||
<origin rpy="1.57079632679 0 0" xyz="0 0.06070 0.19"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="300" lower="-2.09439510239" upper="2.09439510239" velocity="10"/>
|
||||
<safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.05948851735" soft_upper_limit="2.05948851735"/>
|
||||
<dynamics damping="0.5"/>
|
||||
</joint>
|
||||
<link name="iiwa7_link_6">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0.0006 0.0004"/>
|
||||
<mass value="2.3466"/>
|
||||
<inertia ixx="0.006509" ixy="0" ixz="0" iyy="0.006259" iyz="0.00031891" izz="0.004527"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/iiwa7/visual/link_6.obj"/>
|
||||
</geometry>
|
||||
<material name="Orange"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/iiwa7/collision/link_6.obj"/>
|
||||
</geometry>
|
||||
<material name="Orange"/>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="iiwa7_joint_7" type="revolute">
|
||||
<parent link="iiwa7_link_6"/>
|
||||
<child link="iiwa7_link_7"/>
|
||||
<origin rpy="-1.57079632679 3.14159265359 0" xyz="0 0.081 0.06070"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="300" lower="-3.05432619099" upper="3.05432619099" velocity="10"/>
|
||||
<safety_controller k_position="100" k_velocity="2" soft_lower_limit="-3.01941960595" soft_upper_limit="3.01941960595"/>
|
||||
<dynamics damping="0.5"/>
|
||||
</joint>
|
||||
<link name="iiwa7_link_7">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.02"/>
|
||||
<mass value="3.129"/>
|
||||
<inertia ixx="0.01464" ixy="0.0005912" ixz="0" iyy="0.01465" iyz="0" izz="0.002872"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 -0.0005"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/iiwa7/visual/link_7.obj"/>
|
||||
</geometry>
|
||||
<material name="Grey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 -0.0005"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/iiwa7/collision/link_7.obj"/>
|
||||
</geometry>
|
||||
<material name="Grey"/>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="iiwa7_joint_ee" type="fixed">
|
||||
<parent link="iiwa7_link_7"/>
|
||||
<child link="iiwa7_link_ee"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.071"/>
|
||||
</joint>
|
||||
<link name="iiwa7_link_ee">
|
||||
</link>
|
||||
<!-- Load Gazebo lib and set the robot namespace -->
|
||||
<!-- <gazebo>
|
||||
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
|
||||
<robotNamespace>/${robot_name}</robotNamespace>
|
||||
</plugin>
|
||||
</gazebo> -->
|
||||
<!-- Link0 -->
|
||||
<gazebo reference="iiwa7_link_0">
|
||||
<material>Gazebo/Grey</material>
|
||||
<mu1>0.2</mu1>
|
||||
<mu2>0.2</mu2>
|
||||
</gazebo>
|
||||
<!-- Link1 -->
|
||||
<gazebo reference="iiwa7_link_1">
|
||||
<material>Gazebo/Orange</material>
|
||||
<mu1>0.2</mu1>
|
||||
<mu2>0.2</mu2>
|
||||
</gazebo>
|
||||
<!-- Link2 -->
|
||||
<gazebo reference="iiwa7_link_2">
|
||||
<material>Gazebo/Orange</material>
|
||||
<mu1>0.2</mu1>
|
||||
<mu2>0.2</mu2>
|
||||
</gazebo>
|
||||
<!-- Link3 -->
|
||||
<gazebo reference="iiwa7_link_3">
|
||||
<material>Gazebo/Orange</material>
|
||||
<mu1>0.2</mu1>
|
||||
<mu2>0.2</mu2>
|
||||
</gazebo>
|
||||
<!-- Link4 -->
|
||||
<gazebo reference="iiwa7_link_4">
|
||||
<material>Gazebo/Orange</material>
|
||||
<mu1>0.2</mu1>
|
||||
<mu2>0.2</mu2>
|
||||
</gazebo>
|
||||
<!-- Link5 -->
|
||||
<gazebo reference="iiwa7_link_5">
|
||||
<material>Gazebo/Orange</material>
|
||||
<mu1>0.2</mu1>
|
||||
<mu2>0.2</mu2>
|
||||
</gazebo>
|
||||
<!-- Link6 -->
|
||||
<gazebo reference="iiwa7_link_6">
|
||||
<material>Gazebo/Orange</material>
|
||||
<mu1>0.2</mu1>
|
||||
<mu2>0.2</mu2>
|
||||
</gazebo>
|
||||
<!-- Link7 -->
|
||||
<gazebo reference="iiwa7_link_7">
|
||||
<material>Gazebo/Grey</material>
|
||||
<mu1>0.2</mu1>
|
||||
<mu2>0.2</mu2>
|
||||
</gazebo>
|
||||
<transmission name="iiwa7_tran_1">
|
||||
<robotNamespace>/iiwa7</robotNamespace>
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="iiwa7_joint_1">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="iiwa7_motor_1">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="iiwa7_tran_2">
|
||||
<robotNamespace>/iiwa7</robotNamespace>
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="iiwa7_joint_2">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="iiwa7_motor_2">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="iiwa7_tran_3">
|
||||
<robotNamespace>/iiwa7</robotNamespace>
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="iiwa7_joint_3">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="iiwa7_motor_3">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="iiwa7_tran_4">
|
||||
<robotNamespace>/iiwa7</robotNamespace>
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="iiwa7_joint_4">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="iiwa7_motor_4">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="iiwa7_tran_5">
|
||||
<robotNamespace>/iiwa7</robotNamespace>
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="iiwa7_joint_5">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="iiwa7_motor_5">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="iiwa7_tran_6">
|
||||
<robotNamespace>/iiwa7</robotNamespace>
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="iiwa7_joint_6">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="iiwa7_motor_6">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="iiwa7_tran_7">
|
||||
<robotNamespace>/iiwa7</robotNamespace>
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="iiwa7_joint_7">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="iiwa7_motor_7">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
</robot>
|
||||
@@ -0,0 +1,973 @@
|
||||
<?xml version="1.0" ?>
|
||||
<!-- This file was autogenerated but we have modified quite a bit
|
||||
Authors: Karl, Ankur
|
||||
|
||||
Karl did the dynamics compensation and obtained the following values for the index finger
|
||||
|
||||
Noise: 0.111576
|
||||
Masses: 0.005, 0.125164, 0.131691, 0.0211922,
|
||||
Rotor Inertia: 0.00386572, 0.00346965, 0.00433775, 0.00366413,
|
||||
Viscous Friction: 0.0414019, 0.00587541, 0.010638, 0.0226948,
|
||||
Coulomb Friction: 0.0523963, 0.0150275, 0.00616359, 0.0227036,
|
||||
Mass Inertia:
|
||||
5.1458e-05, 5.1458e-05, 6.125e-05, 0, 0, 0,
|
||||
6.39979e-06, 8.88687e-05, 9.13751e-05, -3.26531e-06, 1.23963e-05, 2.07384e-05,
|
||||
7.04217e-05, 3.95744e-05, 6.61125e-05, -9.64342e-05, 5.8796e-05, -3.62996e-05,
|
||||
2.93743e-05, 7.21391e-05, 7.59731e-05, -3.51896e-05, -6.31225e-05, -9.25392e-07,
|
||||
|
||||
Center of Mass:
|
||||
0, 0, 0,
|
||||
0.027, 0, 0,
|
||||
0.038, 0, 0,
|
||||
0.029, 0, 0,
|
||||
|
||||
-->
|
||||
|
||||
<robot name="iiwa_allegro" xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<material name="Black">
|
||||
<color rgba="0.0 0.0 0.0 1.0"/>
|
||||
</material>
|
||||
<material name="Blue">
|
||||
<color rgba="0.0 0.0 0.8 1.0"/>
|
||||
</material>
|
||||
<material name="Green">
|
||||
<color rgba="0.0 0.8 0.0 1.0"/>
|
||||
</material>
|
||||
<material name="Grey">
|
||||
<color rgba="0.4 0.4 0.4 1.0"/>
|
||||
</material>
|
||||
<material name="Orange">
|
||||
<color rgba="1.0 0.423529411765 0.0392156862745 1.0"/>
|
||||
</material>
|
||||
<material name="Brown">
|
||||
<color rgba="0.870588235294 0.811764705882 0.764705882353 1.0"/>
|
||||
</material>
|
||||
<material name="Red">
|
||||
<color rgba="0.8 0.0 0.0 1.0"/>
|
||||
</material>
|
||||
<material name="White">
|
||||
<color rgba="1.0 1.0 1.0 1.0"/>
|
||||
</material>
|
||||
<link name="base_link"/>
|
||||
<!--joint between {parent} and link_0-->
|
||||
<joint name="iiwa7_base_link_iiwa7_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="iiwa7_link_0"/>
|
||||
</joint>
|
||||
<link name="iiwa7_link_0">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="-0.1 0 0.07"/>
|
||||
<mass value="5"/>
|
||||
<inertia ixx="0.05" ixy="0" ixz="0" iyy="0.06" iyz="0" izz="0.03"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/iiwa7/visual/link_0.obj"/>
|
||||
</geometry>
|
||||
<material name="Grey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/iiwa7/collision/link_0.obj"/>
|
||||
</geometry>
|
||||
<material name="Grey"/>
|
||||
</collision>
|
||||
<self_collision_checking>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<capsule length="0.25" radius="0.15"/>
|
||||
</geometry>
|
||||
</self_collision_checking>
|
||||
</link>
|
||||
<!-- joint between link_0 and link_1 -->
|
||||
<joint name="iiwa7_joint_1" type="revolute">
|
||||
<parent link="iiwa7_link_0"/>
|
||||
<child link="iiwa7_link_1"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.15"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="300" lower="-2.96705972839" upper="2.96705972839" velocity="10"/>
|
||||
<safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.93215314335" soft_upper_limit="2.93215314335"/>
|
||||
<dynamics damping="5.5" friction="0.025"/>
|
||||
</joint>
|
||||
<link name="iiwa7_link_1">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 -0.03 0.12"/>
|
||||
<mass value="3.4525"/>
|
||||
<inertia ixx="0.02183" ixy="0" ixz="0" iyy="0.007703" iyz="-0.003887" izz="0.02083"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.0075"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/iiwa7/visual/link_1.obj"/>
|
||||
</geometry>
|
||||
<material name="Orange"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.0075"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/iiwa7/collision/link_1.obj"/>
|
||||
</geometry>
|
||||
<material name="Orange"/>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="iiwa7_joint_2" type="revolute">
|
||||
<parent link="iiwa7_link_1"/>
|
||||
<child link="iiwa7_link_2"/>
|
||||
<origin rpy="1.57079632679 0 3.14159265359" xyz="0 0 0.19"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="300" lower="-2.09439510239" upper="2.09439510239" velocity="10"/>
|
||||
<safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.05948851735" soft_upper_limit="2.05948851735"/>
|
||||
<dynamics damping="5.55" friction="0.025"/>
|
||||
</joint>
|
||||
<link name="iiwa7_link_2">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.0003 0.059 0.042"/>
|
||||
<mass value="3.4821"/>
|
||||
<inertia ixx="0.02076" ixy="0" ixz="-0.003626" iyy="0.02179" iyz="0" izz="0.00779"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/iiwa7/visual/link_2.obj"/>
|
||||
</geometry>
|
||||
<material name="Orange"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/iiwa7/collision/link_2.obj"/>
|
||||
</geometry>
|
||||
<material name="Orange"/>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="iiwa7_joint_3" type="revolute">
|
||||
<parent link="iiwa7_link_2"/>
|
||||
<child link="iiwa7_link_3"/>
|
||||
<origin rpy="1.57079632679 0 3.14159265359" xyz="0 0.21 0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="300" lower="-2.96705972839" upper="2.96705972839" velocity="10"/>
|
||||
<safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.93215314335" soft_upper_limit="2.93215314335"/>
|
||||
<dynamics damping="5.5" friction="0.025"/>
|
||||
</joint>
|
||||
<link name="iiwa7_link_3">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0.03 0.13"/>
|
||||
<mass value="4.05623"/>
|
||||
<inertia ixx="0.03204" ixy="0" ixz="0" iyy="0.00972" iyz="0.006227" izz="0.03042"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 -0.026"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/iiwa7/visual/link_3.obj"/>
|
||||
</geometry>
|
||||
<material name="Orange"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 -0.026"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/iiwa7/collision/link_3.obj"/>
|
||||
</geometry>
|
||||
<material name="Orange"/>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="iiwa7_joint_4" type="revolute">
|
||||
<parent link="iiwa7_link_3"/>
|
||||
<child link="iiwa7_link_4"/>
|
||||
<origin rpy="1.57079632679 0 0" xyz="0 0 0.19"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="300" lower="-2.09439510239" upper="2.09439510239" velocity="10"/>
|
||||
<safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.05948851735" soft_upper_limit="2.05948851735"/>
|
||||
<dynamics damping="5.5" friction="0.025"/>
|
||||
</joint>
|
||||
<link name="iiwa7_link_4">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0.067 0.034"/>
|
||||
<mass value="3.4822"/>
|
||||
<inertia ixx="0.02178" ixy="0" ixz="0" iyy="0.02075" iyz="-0.003625" izz="0.007785"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/iiwa7/visual/link_4.obj"/>
|
||||
</geometry>
|
||||
<material name="Orange"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/iiwa7/collision/link_4.obj"/>
|
||||
</geometry>
|
||||
<material name="Orange"/>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="iiwa7_joint_5" type="revolute">
|
||||
<parent link="iiwa7_link_4"/>
|
||||
<child link="iiwa7_link_5"/>
|
||||
<origin rpy="-1.57079632679 3.14159265359 0" xyz="0 0.21 0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="300" lower="-2.96705972839" upper="2.96705972839" velocity="10"/>
|
||||
<safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.93215314335" soft_upper_limit="2.93215314335"/>
|
||||
<dynamics damping="5.5" friction="0.025"/>
|
||||
</joint>
|
||||
<link name="iiwa7_link_5">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.0001 0.021 0.076"/>
|
||||
<mass value="2.1633"/>
|
||||
<inertia ixx="0.01287" ixy="0" ixz="0" iyy="0.005708" iyz="-0.003946" izz="0.01112"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 -0.026"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/iiwa7/visual/link_5.obj"/>
|
||||
</geometry>
|
||||
<material name="Orange"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 -0.026"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/iiwa7/collision/link_5.obj"/>
|
||||
</geometry>
|
||||
<material name="Orange"/>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="iiwa7_joint_6" type="revolute">
|
||||
<parent link="iiwa7_link_5"/>
|
||||
<child link="iiwa7_link_6"/>
|
||||
<origin rpy="1.57079632679 0 0" xyz="0 0.06070 0.19"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="300" lower="-2.09439510239" upper="2.09439510239" velocity="10"/>
|
||||
<safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.05948851735" soft_upper_limit="2.05948851735"/>
|
||||
<dynamics damping="0.5" friction="0.025"/>
|
||||
</joint>
|
||||
<link name="iiwa7_link_6">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0.0006 0.0004"/>
|
||||
<mass value="2.3466"/>
|
||||
<inertia ixx="0.006509" ixy="0" ixz="0" iyy="0.006259" iyz="0.00031891" izz="0.004527"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/iiwa7/visual/link_6.obj"/>
|
||||
</geometry>
|
||||
<material name="Orange"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/iiwa7/collision/link_6.obj"/>
|
||||
</geometry>
|
||||
<material name="Orange"/>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="iiwa7_joint_7" type="revolute">
|
||||
<parent link="iiwa7_link_6"/>
|
||||
<child link="iiwa7_link_7"/>
|
||||
<origin rpy="-1.57079632679 3.14159265359 0" xyz="0 0.081 0.06070"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="300" lower="-3.05432619099" upper="3.05432619099" velocity="10"/>
|
||||
<safety_controller k_position="100" k_velocity="2" soft_lower_limit="-3.01941960595" soft_upper_limit="3.01941960595"/>
|
||||
<dynamics damping="5.5" friction="0.025"/>
|
||||
</joint>
|
||||
<link name="iiwa7_link_7">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.02"/>
|
||||
<mass value="3.129"/>
|
||||
<inertia ixx="0.01464" ixy="0.0005912" ixz="0" iyy="0.01465" iyz="0" izz="0.002872"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 -0.0005"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/iiwa7/visual/link_7.obj"/>
|
||||
</geometry>
|
||||
<material name="Grey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 -0.0005"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/iiwa7/collision/link_7.obj"/>
|
||||
</geometry>
|
||||
<material name="Grey"/>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="iiwa7_joint_ee" type="fixed">
|
||||
<parent link="iiwa7_link_7"/>
|
||||
<child link="iiwa7_link_ee"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.071"/>
|
||||
<dynamics damping="5.5" friction="0.025"/>
|
||||
</joint>
|
||||
<link name="iiwa7_link_ee">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.0"/>
|
||||
<mass value="0.1"/>
|
||||
<inertia ixx="1e-3" ixy="0.0" ixz="0" iyy="1e-3" iyz="0" izz="1e-3"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<!-- Load Gazebo lib and set the robot namespace -->
|
||||
<!-- <gazebo>
|
||||
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
|
||||
<robotNamespace>/${robot_name}</robotNamespace>
|
||||
</plugin>
|
||||
</gazebo> -->
|
||||
<!-- Link0 -->
|
||||
<gazebo reference="iiwa7_link_0">
|
||||
<material>Gazebo/Grey</material>
|
||||
<mu1>0.2</mu1>
|
||||
<mu2>0.2</mu2>
|
||||
</gazebo>
|
||||
<!-- Link1 -->
|
||||
<gazebo reference="iiwa7_link_1">
|
||||
<material>Gazebo/Orange</material>
|
||||
<mu1>0.2</mu1>
|
||||
<mu2>0.2</mu2>
|
||||
</gazebo>
|
||||
<!-- Link2 -->
|
||||
<gazebo reference="iiwa7_link_2">
|
||||
<material>Gazebo/Orange</material>
|
||||
<mu1>0.2</mu1>
|
||||
<mu2>0.2</mu2>
|
||||
</gazebo>
|
||||
<!-- Link3 -->
|
||||
<gazebo reference="iiwa7_link_3">
|
||||
<material>Gazebo/Orange</material>
|
||||
<mu1>0.2</mu1>
|
||||
<mu2>0.2</mu2>
|
||||
</gazebo>
|
||||
<!-- Link4 -->
|
||||
<gazebo reference="iiwa7_link_4">
|
||||
<material>Gazebo/Orange</material>
|
||||
<mu1>0.2</mu1>
|
||||
<mu2>0.2</mu2>
|
||||
</gazebo>
|
||||
<!-- Link5 -->
|
||||
<gazebo reference="iiwa7_link_5">
|
||||
<material>Gazebo/Orange</material>
|
||||
<mu1>0.2</mu1>
|
||||
<mu2>0.2</mu2>
|
||||
</gazebo>
|
||||
<!-- Link6 -->
|
||||
<gazebo reference="iiwa7_link_6">
|
||||
<material>Gazebo/Orange</material>
|
||||
<mu1>0.2</mu1>
|
||||
<mu2>0.2</mu2>
|
||||
</gazebo>
|
||||
<!-- Link7 -->
|
||||
<gazebo reference="iiwa7_link_7">
|
||||
<material>Gazebo/Grey</material>
|
||||
<mu1>0.2</mu1>
|
||||
<mu2>0.2</mu2>
|
||||
</gazebo>
|
||||
<transmission name="iiwa7_tran_1">
|
||||
<robotNamespace>/iiwa7</robotNamespace>
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="iiwa7_joint_1">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="iiwa7_motor_1">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="iiwa7_tran_2">
|
||||
<robotNamespace>/iiwa7</robotNamespace>
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="iiwa7_joint_2">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="iiwa7_motor_2">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="iiwa7_tran_3">
|
||||
<robotNamespace>/iiwa7</robotNamespace>
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="iiwa7_joint_3">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="iiwa7_motor_3">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="iiwa7_tran_4">
|
||||
<robotNamespace>/iiwa7</robotNamespace>
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="iiwa7_joint_4">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="iiwa7_motor_4">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="iiwa7_tran_5">
|
||||
<robotNamespace>/iiwa7</robotNamespace>
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="iiwa7_joint_5">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="iiwa7_motor_5">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="iiwa7_tran_6">
|
||||
<robotNamespace>/iiwa7</robotNamespace>
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="iiwa7_joint_6">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="iiwa7_motor_6">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="iiwa7_tran_7">
|
||||
<robotNamespace>/iiwa7</robotNamespace>
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="iiwa7_joint_7">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="iiwa7_motor_7">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<!-- ======================== BASE PARAMS ========================= -->
|
||||
<!-- ======================== FINGER PARAMS ======================== -->
|
||||
<!-- full height from joint to tip. when used,
|
||||
the radius of the finger tip sphere will be subtracted
|
||||
and one fixed link will be added for the tip. -->
|
||||
<!-- ========================= THUMB PARAMS ========================= -->
|
||||
<!-- ========================= LIMITS ========================= -->
|
||||
<!-- ============================================================================= -->
|
||||
<!-- BASE -->
|
||||
|
||||
<link name="allegro_mount">
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="1e-4" ixy="0" ixz="0" iyy="1e-4" iyz="0" izz="1e-4"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<!-- <origin xyz="-0.0425 -0.0425 0" rpy="0 0 0" /> -->
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mounts/allegro_mount.obj"/>
|
||||
</geometry>
|
||||
<material name="color_j7"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<!-- <origin xyz="-0.0425 -0.0425 0" rpy="0 0 0" /> -->
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/mounts/allegro_mount.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<!-- <joint name="allegro_mount_joint1" type="fixed">
|
||||
|
||||
<origin rpy="0 0 0" xyz="-0.00 -0.0 0.0"/>
|
||||
<parent link="iiwa7_link_ee"/>
|
||||
<child link="allegro_mount"/>
|
||||
|
||||
</joint> -->
|
||||
<joint name="allegro_mount_joint" type="fixed">
|
||||
<!-- <origin xyz="0.065 0 0.0275" rpy="0 1.57 0" /> -->
|
||||
<origin rpy="0 -1.5708 0.785398" xyz="0.008219 -0.02063 0.08086"/>
|
||||
<parent link="allegro_mount"/>
|
||||
<child link="palm_link"/>
|
||||
|
||||
</joint>
|
||||
|
||||
<!-- BASE -->
|
||||
<link name="palm_link">
|
||||
<inertial>
|
||||
<mass value="0.4154"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="1e-4" ixy="0" ixz="0" iyy="1e-4" iyz="0" izz="1e-4"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/allegro/base_link.obj"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0 0 0 "/>
|
||||
<material name="Grey">
|
||||
<color rgba="0.2 0.2 0.2 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0.0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/allegro/base_link.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<gazebo reference="palm_link">
|
||||
<material value="Gazebo/Grey"/>
|
||||
</gazebo>
|
||||
<!-- ============================================================================= -->
|
||||
<!-- FINGERS -->
|
||||
<!-- RIGHT HAND due to which finger is number 0 -->
|
||||
<!-- for LEFT HAND switch the sign of the **offset_origin_y** and **finger_angle_r** parameters-->
|
||||
<!-- [LINK 0, 4, 8] -->
|
||||
<link name="index_link_0">
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/allegro/primary_base.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.005"/>
|
||||
<origin xyz="0 0 0"/>
|
||||
<inertia ixx="5.1458e-05" iyy="5.1458e-05" izz="6.125e-05" ixy="0" ixz="0" iyz="0"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/allegro/primary_base.obj"/>
|
||||
</geometry>
|
||||
<material name="Grey">
|
||||
<color rgba="0.2 0.2 0.2 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="index_link_1">
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/allegro/primary_proximal.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.125164"/>
|
||||
<origin xyz="0.027 0 0"/>
|
||||
<inertia ixx="6.39979e-06" iyy="8.88687e-05" izz="9.13751e-05" ixy="-3.26531e-06" ixz="1.23963e-05" iyz="2.07384e-05"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/allegro/primary_proximal.obj"/>
|
||||
</geometry>
|
||||
<material name="Grey">
|
||||
<color rgba="0.2 0.2 0.2 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="index_link_2">
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/allegro/primary_medial.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.131691"/>
|
||||
<origin xyz="0.039 0 0"/>
|
||||
<inertia ixx="7.04217e-05" iyy="3.95744e-05" izz="6.61125e-05" ixy="-9.64342e-05" ixz="5.8796e-05" iyz="-3.62996e-05"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/allegro/primary_medial.obj"/>
|
||||
</geometry>
|
||||
<material name="Grey">
|
||||
<color rgba="0.2 0.2 0.2 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="index_link_3">
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/biotac/biotac_sensor.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.0211922"/>
|
||||
<origin xyz="0.029 0 0"/>
|
||||
<inertia ixx="2.93743e-05" iyy="7.21391e-05" izz="7.59731e-05" ixy="-3.51896e-05" ixz="-6.31225e-05" iyz="-9.25392e-07"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/biotac/biotac_sensor.obj"/>
|
||||
</geometry>
|
||||
<material name="Green">
|
||||
<color rgba="0. 0.5 0. 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="middle_link_0">
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/allegro/primary_base.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.005"/>
|
||||
<origin xyz="0 0 0"/>
|
||||
<inertia ixx="5.1458e-05" iyy="5.1458e-05" izz="6.125e-05" ixy="0" ixz="0" iyz="0"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/allegro/primary_base.obj"/>
|
||||
</geometry>
|
||||
<material name="Grey">
|
||||
<color rgba="0.2 0.2 0.2 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="middle_link_1">
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/allegro/primary_proximal.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.125164"/>
|
||||
<origin xyz="0.027 0 0"/>
|
||||
<inertia ixx="6.39979e-06" iyy="8.88687e-05" izz="9.13751e-05" ixy="-3.26531e-06" ixz="1.23963e-05" iyz="2.07384e-05"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/allegro/primary_proximal.obj"/>
|
||||
</geometry>
|
||||
<material name="Grey">
|
||||
<color rgba="0.2 0.2 0.2 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="middle_link_2">
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/allegro/primary_medial.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.131691"/>
|
||||
<origin xyz="0.039 0 0"/>
|
||||
<inertia ixx="7.04217e-05" iyy="3.95744e-05" izz="6.61125e-05" ixy="-9.64342e-05" ixz="5.8796e-05" iyz="-3.62996e-05"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/allegro/primary_medial.obj"/>
|
||||
</geometry>
|
||||
<material name="Grey">
|
||||
<color rgba="0.2 0.2 0.2 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="middle_link_3">
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/biotac/biotac_sensor.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.0211922"/>
|
||||
<origin xyz="0.029 0 0"/>
|
||||
<inertia ixx="2.93743e-05" iyy="7.21391e-05" izz="7.59731e-05" ixy="-3.51896e-05" ixz="-6.31225e-05" iyz="-9.25392e-07"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/biotac/biotac_sensor.obj"/>
|
||||
</geometry>
|
||||
<material name="Green">
|
||||
<color rgba="0 0.5 0 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="ring_link_0">
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/allegro/primary_base.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.005"/>
|
||||
<origin xyz="0 0 0"/>
|
||||
<inertia ixx="5.1458e-05" iyy="5.1458e-05" izz="6.125e-05" ixy="0" ixz="0" iyz="0"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/allegro/primary_base.obj"/>
|
||||
</geometry>
|
||||
<material name="Grey">
|
||||
<color rgba="0.2 0.2 0.2 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="ring_link_1">
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/allegro/primary_proximal.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.125164"/>
|
||||
<origin xyz="0.027 0 0"/>
|
||||
<inertia ixx="6.39979e-06" iyy="8.88687e-05" izz="9.13751e-05" ixy="-3.26531e-06" ixz="1.23963e-05" iyz="2.07384e-05"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/allegro/primary_proximal.obj"/>
|
||||
</geometry>
|
||||
<material name="Grey">
|
||||
<color rgba="0.2 0.2 0.2 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="ring_link_2">
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/allegro/primary_medial.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.131691"/>
|
||||
<origin xyz="0.039 0 0"/>
|
||||
<inertia ixx="7.04217e-05" iyy="3.95744e-05" izz="6.61125e-05" ixy="-9.64342e-05" ixz="5.8796e-05" iyz="-3.62996e-05"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/allegro/primary_medial.obj"/>
|
||||
</geometry>
|
||||
<material name="Grey">
|
||||
<color rgba="0.2 0.2 0.2 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="ring_link_3">
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/biotac/biotac_sensor.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.0211922"/>
|
||||
<origin xyz="0.029 0 0"/>
|
||||
<inertia ixx="2.93743e-05" iyy="7.21391e-05" izz="7.59731e-05" ixy="-3.51896e-05" ixz="-6.31225e-05" iyz="-9.25392e-07"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/biotac/biotac_sensor.obj"/>
|
||||
</geometry>
|
||||
<material name="Green">
|
||||
<color rgba="0 0.5 0 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="thumb_link_0">
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/allegro/thumb_base.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.1"/>
|
||||
<inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/allegro/thumb_base.obj"/>
|
||||
</geometry>
|
||||
<material name="Grey">
|
||||
<color rgba="0.2 0.2 0.2 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="thumb_link_1">
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/allegro/thumb_proximal.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.1"/>
|
||||
<inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/allegro/thumb_proximal.obj"/>
|
||||
</geometry>
|
||||
<material name="Grey">
|
||||
<color rgba="0.2 0.2 0.2 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="thumb_link_2">
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/allegro/thumb_medial.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.1"/>
|
||||
<inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/allegro/thumb_medial.obj"/>
|
||||
</geometry>
|
||||
<material name="Grey">
|
||||
<color rgba="0.2 0.2 0.2 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="thumb_link_3">
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/biotac/biotac_sensor_thumb.obj"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.1"/>
|
||||
<inertia ixx="5.1458e-5" ixy="0" ixz="0" iyy="5.1458e-5" iyz="0" izz="6.125e-5"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/biotac/biotac_sensor_thumb.obj"/>
|
||||
</geometry>
|
||||
<material name="Green">
|
||||
<color rgba="0 0.5 0 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<joint name="index_joint_0" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="0.35" lower="-0.558488888889" upper="0.558488888889" velocity="6.283"/>
|
||||
<origin rpy="3.1415 -1.57075 -0.0872638888889" xyz="0.0514302 -0.03632 -0.0113"/>
|
||||
<parent link="palm_link"/>
|
||||
<child link="index_link_0"/>
|
||||
<dynamics damping="0.0414019" friction="0.0523963"/>
|
||||
</joint>
|
||||
<joint name="index_joint_1" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="0.35" lower="-0.279244444444" upper="1.727825" velocity="6.283"/>
|
||||
<origin rpy="3.1415 -1.57075 1.57075" xyz="0.0 0.0 0.0"/>
|
||||
<parent link="index_link_0"/>
|
||||
<child link="index_link_1"/>
|
||||
<dynamics damping="0.00587541" friction="0.0150275"/>
|
||||
</joint>
|
||||
<joint name="index_joint_2" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="0.35" lower="-0.279244444444" upper="1.727825" velocity="6.283"/>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.054 0.0 0.0"/>
|
||||
<parent link="index_link_1"/>
|
||||
<child link="index_link_2"/>
|
||||
<dynamics damping="0.010638" friction="0.00616359"/>
|
||||
</joint>
|
||||
<joint name="index_joint_3" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="0.35" lower="-0.279244444444" upper="1.727825" velocity="6.283"/>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0384 0.0 0.0"/>
|
||||
<parent link="index_link_2"/>
|
||||
<child link="index_link_3"/>
|
||||
<dynamics damping="0.0226948" friction="0.0227036"/>
|
||||
</joint>
|
||||
<joint name="middle_joint_0" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="0.35" lower="-0.558488888889" upper="0.558488888889" velocity="6.283"/>
|
||||
<origin rpy="3.1415 -1.57075 0" xyz="0.0537375 0.0087771 -0.0113"/>
|
||||
<parent link="palm_link"/>
|
||||
<child link="middle_link_0"/>
|
||||
<dynamics damping="0.0414019" friction="0.0523963"/>
|
||||
</joint>
|
||||
<joint name="middle_joint_1" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="0.35" lower="-0.279244444444" upper="1.727825" velocity="6.283"/>
|
||||
<origin rpy="3.1415 -1.57075 1.57075" xyz="0.0 0.0 0.0"/>
|
||||
<parent link="middle_link_0"/>
|
||||
<child link="middle_link_1"/>
|
||||
<dynamics damping="0.00587541" friction="0.0150275"/>
|
||||
</joint>
|
||||
<joint name="middle_joint_2" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="0.35" lower="-0.279244444444" upper="1.727825" velocity="6.283"/>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.054 0.0 0.0"/>
|
||||
<parent link="middle_link_1"/>
|
||||
<child link="middle_link_2"/>
|
||||
<dynamics damping="0.010638" friction="0.00616359"/>
|
||||
</joint>
|
||||
<joint name="middle_joint_3" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="0.35" lower="-0.279244444444" upper="1.727825" velocity="6.283"/>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0384 0.0 0.0"/>
|
||||
<parent link="middle_link_2"/>
|
||||
<child link="middle_link_3"/>
|
||||
<dynamics damping="0.0226948" friction="0.0227036"/>
|
||||
</joint>
|
||||
<joint name="ring_joint_0" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="0.35" lower="-0.558488888889" upper="0.558488888889" velocity="6.283"/>
|
||||
<origin rpy="3.1415 -1.57075 0.0872638888889" xyz="0.0514302 0.0538749 -0.0113"/>
|
||||
<parent link="palm_link"/>
|
||||
<child link="ring_link_0"/>
|
||||
<dynamics damping="0.0414019" friction="0.0523963"/>
|
||||
</joint>
|
||||
<joint name="ring_joint_1" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="0.35" lower="-0.279244444444" upper="1.727825" velocity="6.283"/>
|
||||
<origin rpy="3.1415 -1.57075 1.57075" xyz="0.0 0.0 0.0"/>
|
||||
<parent link="ring_link_0"/>
|
||||
<child link="ring_link_1"/>
|
||||
<dynamics damping="0.00587541" friction="0.0150275"/>
|
||||
</joint>
|
||||
<joint name="ring_joint_2" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="0.35" lower="-0.279244444444" upper="1.727825" velocity="6.283"/>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.054 0.0 0.0"/>
|
||||
<parent link="ring_link_1"/>
|
||||
<child link="ring_link_2"/>
|
||||
<dynamics damping="0.010638" friction="0.00616359"/>
|
||||
</joint>
|
||||
<joint name="ring_joint_3" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="0.35" lower="-0.279244444444" upper="1.727825" velocity="6.283"/>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0384 0.0 0.0"/>
|
||||
<parent link="ring_link_2"/>
|
||||
<child link="ring_link_3"/>
|
||||
<dynamics damping="0.0226948" friction="0.0227036"/>
|
||||
</joint>
|
||||
<joint name="thumb_joint_0" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="0.35" lower="0.279244444444" upper="1.57075" velocity="6.283"/>
|
||||
<origin rpy="-1.57075 -1.57075 1.48348611111" xyz="-0.0367482 -0.0081281 -0.0295"/>
|
||||
<parent link="palm_link"/>
|
||||
<child link="thumb_link_0"/>
|
||||
<dynamics friction="0.035"/>
|
||||
</joint>
|
||||
<joint name="thumb_joint_1" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="0.35" lower="-0.331602777778" upper="1.15188333333" velocity="6.283"/>
|
||||
<origin rpy="1.57075 0 0" xyz="0.005 0.0 0.0"/>
|
||||
<parent link="thumb_link_0"/>
|
||||
<child link="thumb_link_1"/>
|
||||
<dynamics friction="0.035"/>
|
||||
</joint>
|
||||
<joint name="thumb_joint_2" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="0.35" lower="-0.279244444444" upper="1.727825" velocity="6.283"/>
|
||||
<origin rpy="3.1415 -1.57075 0.0" xyz="0 0 0.0554"/>
|
||||
<parent link="thumb_link_1"/>
|
||||
<child link="thumb_link_2"/>
|
||||
<dynamics friction="0.035"/>
|
||||
</joint>
|
||||
<joint name="thumb_joint_3" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="0.35" lower="-0.279244444444" upper="1.76273055556" velocity="6.283"/>
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0514 0.0 0.0"/>
|
||||
<parent link="thumb_link_2"/>
|
||||
<child link="thumb_link_3"/>
|
||||
<dynamics friction="0.035"/>
|
||||
</joint>
|
||||
<!-- ============================================================================= -->
|
||||
|
||||
<!-- Create a different *root* for the allegro hand -->
|
||||
<!-- Note: this offset is just eyeballed... -->
|
||||
<joint name="iiwa7_allegro" type="fixed">
|
||||
<parent link="iiwa7_link_ee"/>
|
||||
<child link="allegro_mount"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
</robot>
|
||||
693
src/curobo/content/assets/robot/jaco/jaco_7n.urdf
Normal file
693
src/curobo/content/assets/robot/jaco/jaco_7n.urdf
Normal file
@@ -0,0 +1,693 @@
|
||||
<?xml version="1.0" ?>
|
||||
<!-- =================================================================================== -->
|
||||
<!-- | This document was autogenerated by xacro from j2n7s300_standalone.xacro | -->
|
||||
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||
<!-- =================================================================================== -->
|
||||
<!-- j2n7s300 refers to jaco v2 7DOF non-spherical service 3fingers -->
|
||||
<robot name="j2n7s300" xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz" xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint" xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model" xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics" xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable" xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:xi="http://www.w3.org/2001/XInclude">
|
||||
<!-- links mesh_no
|
||||
base 0
|
||||
shoulder 1
|
||||
arm 2
|
||||
forearm 3
|
||||
wrist 4
|
||||
arm_mico 5
|
||||
arm_half1 (7dof) 6
|
||||
arm_half2 (7dof) 7
|
||||
wrist_spherical_1 8
|
||||
wrist_spherical_2 9
|
||||
|
||||
hand 3 finger 55
|
||||
hand_2finger 56
|
||||
finger_proximal 57
|
||||
finger_distal 58
|
||||
-->
|
||||
<!-- links mesh_no
|
||||
base 0
|
||||
shoulder 1
|
||||
arm 2
|
||||
forearm 3
|
||||
wrist 4
|
||||
arm_mico 5
|
||||
arm_half1 (7dof) 6
|
||||
arm_half2 (7dof) 7
|
||||
wrist_spherical_1 8
|
||||
wrist_spherical_2 9
|
||||
|
||||
hand 3 finger 55
|
||||
hand_2finger 56
|
||||
finger_proximal 57
|
||||
finger_distal 58
|
||||
-->
|
||||
<link name="root"/>
|
||||
<!-- for gazebo -->
|
||||
<link name="world"/>
|
||||
<joint name="connect_root_and_world" type="fixed">
|
||||
<child link="root"/>
|
||||
<parent link="world"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
</joint>
|
||||
<!-- ros_control plugin -->
|
||||
<gazebo>
|
||||
<plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
|
||||
<robotNamespace>j2n7s300</robotNamespace>
|
||||
<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
|
||||
<legacyModeNS>true</legacyModeNS>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
<link name="j2n7s300_link_base">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://kinova_description/meshes/base.dae"/>
|
||||
</geometry>
|
||||
<material name="carbon_fiber">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://kinova_description/meshes/base.dae"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.46784"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.1255"/>
|
||||
<inertia ixx="0.000951270861568" ixy="0" ixz="0" iyy="0.000951270861568" iyz="0" izz="0.00037427200000000004"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="j2n7s300_joint_base" type="fixed">
|
||||
<parent link="root"/>
|
||||
<child link="j2n7s300_link_base"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<link name="j2n7s300_link_1">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://kinova_description/meshes/shoulder.dae"/>
|
||||
</geometry>
|
||||
<material name="carbon_fiber">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://kinova_description/meshes/ring_big.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://kinova_description/meshes/shoulder.dae"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.7477"/>
|
||||
<origin xyz="0 -0.002 -0.0605"/>
|
||||
<inertia ixx="0.0015203172520400004" ixy="0" ixz="0" iyy="0.0015203172520400004" iyz="0" izz="0.00059816"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="j2n7s300_joint_1" type="continuous">
|
||||
<parent link="j2n7s300_link_base"/>
|
||||
<child link="j2n7s300_link_1"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="40" velocity="0.6283185307179586"/>
|
||||
<origin rpy="0 3.141592653589793 0" xyz="0 0 0.15675"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<transmission name="j2n7s300_joint_1_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="j2n7s300_joint_1">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="j2n7s300_joint_1_actuator">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>160</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<!--For torque sensing in simulation-->
|
||||
<gazebo reference="j2n7s300_joint_1">
|
||||
<provideFeedback>true</provideFeedback>
|
||||
</gazebo>
|
||||
<gazebo>
|
||||
<plugin filename="libgazebo_ros_ft_sensor.so" name="ft_sensor">
|
||||
<updateRate>30.0</updateRate>
|
||||
<topicName>j2n7s300_joint_1_ft_sensor_topic</topicName>
|
||||
<jointName>j2n7s300_joint_1</jointName>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
<link name="j2n7s300_link_2">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://kinova_description/meshes/arm_half_1.dae"/>
|
||||
</geometry>
|
||||
<material name="carbon_fiber">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://kinova_description/meshes/ring_big.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://kinova_description/meshes/arm_half_1.dae"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.8447"/>
|
||||
<origin xyz="0 -0.103563213 0"/>
|
||||
<inertia ixx="0.00247073761701" ixy="0" ixz="0" iyy="0.000380115" iyz="0" izz="0.00247073761701"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="j2n7s300_joint_2" type="revolute">
|
||||
<parent link="j2n7s300_link_1"/>
|
||||
<child link="j2n7s300_link_2"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="80" lower="0.5235987755982988" upper="5.759586531581287" velocity="0.6283185307179586"/>
|
||||
<origin rpy="-1.5707963267948966 0 3.141592653589793" xyz="0 0.0016 -0.11875"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<transmission name="j2n7s300_joint_2_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="j2n7s300_joint_2">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="j2n7s300_joint_2_actuator">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>160</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<!--For torque sensing in simulation-->
|
||||
<gazebo reference="j2n7s300_joint_2">
|
||||
<provideFeedback>true</provideFeedback>
|
||||
</gazebo>
|
||||
<gazebo>
|
||||
<plugin filename="libgazebo_ros_ft_sensor.so" name="ft_sensor">
|
||||
<updateRate>30.0</updateRate>
|
||||
<topicName>j2n7s300_joint_2_ft_sensor_topic</topicName>
|
||||
<jointName>j2n7s300_joint_2</jointName>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
<link name="j2n7s300_link_3">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://kinova_description/meshes/arm_half_2.dae"/>
|
||||
</geometry>
|
||||
<material name="carbon_fiber">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://kinova_description/meshes/ring_big.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://kinova_description/meshes/arm_half_2.dae"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.8447"/>
|
||||
<origin xyz="0 0 -0.1022447445"/>
|
||||
<inertia ixx="0.00247073761701" ixy="0" ixz="0" iyy="0.00247073761701" iyz="0" izz="0.000380115"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="j2n7s300_joint_3" type="continuous">
|
||||
<parent link="j2n7s300_link_2"/>
|
||||
<child link="j2n7s300_link_3"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="40" velocity="0.6283185307179586"/>
|
||||
<origin rpy="1.5707963267948966 0 3.141592653589793" xyz="0 -0.205 0"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<transmission name="j2n7s300_joint_3_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="j2n7s300_joint_3">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="j2n7s300_joint_3_actuator">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>160</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<!--For torque sensing in simulation-->
|
||||
<gazebo reference="j2n7s300_joint_3">
|
||||
<provideFeedback>true</provideFeedback>
|
||||
</gazebo>
|
||||
<gazebo>
|
||||
<plugin filename="libgazebo_ros_ft_sensor.so" name="ft_sensor">
|
||||
<updateRate>30.0</updateRate>
|
||||
<topicName>j2n7s300_joint_3_ft_sensor_topic</topicName>
|
||||
<jointName>j2n7s300_joint_3</jointName>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
<link name="j2n7s300_link_4">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://kinova_description/meshes/forearm.dae"/>
|
||||
</geometry>
|
||||
<material name="carbon_fiber">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://kinova_description/meshes/ring_big.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://kinova_description/meshes/forearm.dae"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.6763"/>
|
||||
<origin xyz="0 0.081 -0.0086"/>
|
||||
<inertia ixx="0.0014202243190800001" ixy="0" ixz="0" iyy="0.000304335" iyz="0" izz="0.0014202243190800001"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="j2n7s300_joint_4" type="revolute">
|
||||
<parent link="j2n7s300_link_3"/>
|
||||
<child link="j2n7s300_link_4"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="40" lower="0.5235987755982988" upper="5.759586531581287" velocity="0.6283185307179586"/>
|
||||
<origin rpy="1.5707963267948966 0 3.141592653589793" xyz="0 0 -0.205"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<transmission name="j2n7s300_joint_4_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="j2n7s300_joint_4">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="j2n7s300_joint_4_actuator">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>160</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<!--For torque sensing in simulation-->
|
||||
<gazebo reference="j2n7s300_joint_4">
|
||||
<provideFeedback>true</provideFeedback>
|
||||
</gazebo>
|
||||
<gazebo>
|
||||
<plugin filename="libgazebo_ros_ft_sensor.so" name="ft_sensor">
|
||||
<updateRate>30.0</updateRate>
|
||||
<topicName>j2n7s300_joint_4_ft_sensor_topic</topicName>
|
||||
<jointName>j2n7s300_joint_4</jointName>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
<link name="j2n7s300_link_5">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://kinova_description/meshes/wrist.dae"/>
|
||||
</geometry>
|
||||
<material name="carbon_fiber">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://kinova_description/meshes/ring_small.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://kinova_description/meshes/wrist.dae"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.426367"/>
|
||||
<origin xyz="0 -0.037 -0.0642"/>
|
||||
<inertia ixx="7.734969059999999e-05" ixy="0" ixz="0" iyy="7.734969059999999e-05" iyz="0" izz="0.0001428"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="j2n7s300_joint_5" type="continuous">
|
||||
<parent link="j2n7s300_link_4"/>
|
||||
<child link="j2n7s300_link_5"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="20" velocity="0.8377580409572781"/>
|
||||
<origin rpy="-1.5707963267948966 0 3.141592653589793" xyz="0 0.2073 -0.0114"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<transmission name="j2n7s300_joint_5_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="j2n7s300_joint_5">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="j2n7s300_joint_5_actuator">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>160</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<!--For torque sensing in simulation-->
|
||||
<gazebo reference="j2n7s300_joint_5">
|
||||
<provideFeedback>true</provideFeedback>
|
||||
</gazebo>
|
||||
<gazebo>
|
||||
<plugin filename="libgazebo_ros_ft_sensor.so" name="ft_sensor">
|
||||
<updateRate>30.0</updateRate>
|
||||
<topicName>j2n7s300_joint_5_ft_sensor_topic</topicName>
|
||||
<jointName>j2n7s300_joint_5</jointName>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
<link name="j2n7s300_link_6">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://kinova_description/meshes/wrist.dae"/>
|
||||
</geometry>
|
||||
<material name="carbon_fiber">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://kinova_description/meshes/ring_small.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://kinova_description/meshes/wrist.dae"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.426367"/>
|
||||
<origin xyz="0 -0.037 -0.0642"/>
|
||||
<inertia ixx="7.734969059999999e-05" ixy="0" ixz="0" iyy="7.734969059999999e-05" iyz="0" izz="0.0001428"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="j2n7s300_joint_6" type="continuous">
|
||||
<parent link="j2n7s300_link_5"/>
|
||||
<child link="j2n7s300_link_6"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="20" velocity="0.8377580409572781"/>
|
||||
<origin rpy="1.0471975511965976 0 3.141592653589793" xyz="0 -0.03703 -0.06414"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<transmission name="j2n7s300_joint_6_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="j2n7s300_joint_6">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="j2n7s300_joint_6_actuator">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>160</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<!--For torque sensing in simulation-->
|
||||
<gazebo reference="j2n7s300_joint_6">
|
||||
<provideFeedback>true</provideFeedback>
|
||||
</gazebo>
|
||||
<gazebo>
|
||||
<plugin filename="libgazebo_ros_ft_sensor.so" name="ft_sensor">
|
||||
<updateRate>30.0</updateRate>
|
||||
<topicName>j2n7s300_joint_6_ft_sensor_topic</topicName>
|
||||
<jointName>j2n7s300_joint_6</jointName>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
<link name="j2n7s300_link_7">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://kinova_description/meshes/hand_3finger.dae"/>
|
||||
</geometry>
|
||||
<material name="carbon_fiber">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://kinova_description/meshes/ring_small.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://kinova_description/meshes/hand_3finger.dae"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.99"/>
|
||||
<origin xyz="0 0 -0.06"/>
|
||||
<inertia ixx="0.00034532361869999995" ixy="0" ixz="0" iyy="0.00034532361869999995" iyz="0" izz="0.0005815999999999999"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="j2n7s300_joint_7" type="continuous">
|
||||
<parent link="j2n7s300_link_6"/>
|
||||
<child link="j2n7s300_link_7"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="20" velocity="0.8377580409572781"/>
|
||||
<origin rpy="1.0471975511965976 0 3.141592653589793" xyz="0 -0.03703 -0.06414"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<transmission name="j2n7s300_joint_7_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="j2n7s300_joint_7">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="j2n7s300_joint_7_actuator">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>160</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<!--For torque sensing in simulation-->
|
||||
<gazebo reference="j2n7s300_joint_7">
|
||||
<provideFeedback>true</provideFeedback>
|
||||
</gazebo>
|
||||
<gazebo>
|
||||
<plugin filename="libgazebo_ros_ft_sensor.so" name="ft_sensor">
|
||||
<updateRate>30.0</updateRate>
|
||||
<topicName>j2n7s300_joint_7_ft_sensor_topic</topicName>
|
||||
<jointName>j2n7s300_joint_7</jointName>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
<link name="j2n7s300_end_effector"/>
|
||||
<joint name="j2n7s300_joint_end_effector" type="fixed">
|
||||
<parent link="j2n7s300_link_7"/>
|
||||
<child link="j2n7s300_end_effector"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
<origin rpy="3.141592653589793 0 1.5707963267948966" xyz="0 0 -0.1600"/>
|
||||
</joint>
|
||||
<link name="j2n7s300_link_finger_1">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://kinova_description/meshes/finger_proximal.dae"/>
|
||||
</geometry>
|
||||
<material name="carbon_fiber">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://kinova_description/meshes/finger_proximal.dae"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.01"/>
|
||||
<origin xyz="0.022 0 0"/>
|
||||
<inertia ixx="7.8999684e-07" ixy="0" ixz="0" iyy="7.8999684e-07" iyz="0" izz="8e-08"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="j2n7s300_joint_finger_1" type="revolute">
|
||||
<parent link="j2n7s300_link_7"/>
|
||||
<child link="j2n7s300_link_finger_1"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<origin rpy="-1.7047873384941834 0.6476144647144773 1.67317415161155" xyz="0.00279 0.03126 -0.11467"/>
|
||||
<limit effort="2" lower="0" upper="1.51" velocity="1"/>
|
||||
</joint>
|
||||
<transmission name="j2n7s300_joint_finger_1_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="j2n7s300_joint_finger_1">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="j2n7s300_joint_finger_1_actuator">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<link name="j2n7s300_link_finger_tip_1">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://kinova_description/meshes/finger_distal.dae"/>
|
||||
</geometry>
|
||||
<material name="carbon_fiber">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://kinova_description/meshes/finger_distal.dae"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.01"/>
|
||||
<origin xyz="0.022 0 0"/>
|
||||
<inertia ixx="7.8999684e-07" ixy="0" ixz="0" iyy="7.8999684e-07" iyz="0" izz="8e-08"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="j2n7s300_joint_finger_tip_1" type="revolute">
|
||||
<parent link="j2n7s300_link_finger_1"/>
|
||||
<child link="j2n7s300_link_finger_tip_1"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<origin rpy="0 0 0" xyz="0.044 -0.003 0"/>
|
||||
<limit effort="2" lower="0" upper="2" velocity="1"/>
|
||||
</joint>
|
||||
<transmission name="j2n7s300_joint_finger_tip_1_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="j2n7s300_joint_finger_tip_1">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="j2n7s300_joint_finger_tip_1_actuator">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<link name="j2n7s300_link_finger_2">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://kinova_description/meshes/finger_proximal.dae"/>
|
||||
</geometry>
|
||||
<material name="carbon_fiber">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://kinova_description/meshes/finger_proximal.dae"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.01"/>
|
||||
<origin xyz="0.022 0 0"/>
|
||||
<inertia ixx="7.8999684e-07" ixy="0" ixz="0" iyy="7.8999684e-07" iyz="0" izz="8e-08"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="j2n7s300_joint_finger_2" type="revolute">
|
||||
<parent link="j2n7s300_link_7"/>
|
||||
<child link="j2n7s300_link_finger_2"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<origin rpy="-1.570796327 .649262481663582 -1.38614049188413" xyz="0.02226 -0.02707 -0.11482"/>
|
||||
<limit effort="2" lower="0" upper="1.51" velocity="1"/>
|
||||
</joint>
|
||||
<transmission name="j2n7s300_joint_finger_2_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="j2n7s300_joint_finger_2">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="j2n7s300_joint_finger_2_actuator">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<link name="j2n7s300_link_finger_tip_2">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://kinova_description/meshes/finger_distal.dae"/>
|
||||
</geometry>
|
||||
<material name="carbon_fiber">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://kinova_description/meshes/finger_distal.dae"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.01"/>
|
||||
<origin xyz="0.022 0 0"/>
|
||||
<inertia ixx="7.8999684e-07" ixy="0" ixz="0" iyy="7.8999684e-07" iyz="0" izz="8e-08"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="j2n7s300_joint_finger_tip_2" type="revolute">
|
||||
<parent link="j2n7s300_link_finger_2"/>
|
||||
<child link="j2n7s300_link_finger_tip_2"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<origin rpy="0 0 0" xyz="0.044 -0.003 0"/>
|
||||
<limit effort="2" lower="0" upper="2" velocity="1"/>
|
||||
</joint>
|
||||
<transmission name="j2n7s300_joint_finger_tip_2_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="j2n7s300_joint_finger_tip_2">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="j2n7s300_joint_finger_tip_2_actuator">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<link name="j2n7s300_link_finger_3">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://kinova_description/meshes/finger_proximal.dae"/>
|
||||
</geometry>
|
||||
<material name="carbon_fiber">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://kinova_description/meshes/finger_proximal.dae"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.01"/>
|
||||
<origin xyz="0.022 0 0"/>
|
||||
<inertia ixx="7.8999684e-07" ixy="0" ixz="0" iyy="7.8999684e-07" iyz="0" izz="8e-08"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="j2n7s300_joint_finger_3" type="revolute">
|
||||
<parent link="j2n7s300_link_7"/>
|
||||
<child link="j2n7s300_link_finger_3"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<origin rpy="-1.570796327 .649262481663582 -1.75545216211587" xyz="-0.02226 -0.02707 -0.11482"/>
|
||||
<limit effort="2" lower="0" upper="1.51" velocity="1"/>
|
||||
</joint>
|
||||
<transmission name="j2n7s300_joint_finger_3_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="j2n7s300_joint_finger_3">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="j2n7s300_joint_finger_3_actuator">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<link name="j2n7s300_link_finger_tip_3">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://kinova_description/meshes/finger_distal.dae"/>
|
||||
</geometry>
|
||||
<material name="carbon_fiber">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://kinova_description/meshes/finger_distal.dae"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.01"/>
|
||||
<origin xyz="0.022 0 0"/>
|
||||
<inertia ixx="7.8999684e-07" ixy="0" ixz="0" iyy="7.8999684e-07" iyz="0" izz="8e-08"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="j2n7s300_joint_finger_tip_3" type="revolute">
|
||||
<parent link="j2n7s300_link_finger_3"/>
|
||||
<child link="j2n7s300_link_finger_tip_3"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<origin rpy="0 0 0" xyz="0.044 -0.003 0"/>
|
||||
<limit effort="2" lower="0" upper="2" velocity="1"/>
|
||||
</joint>
|
||||
<transmission name="j2n7s300_joint_finger_tip_3_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="j2n7s300_joint_finger_tip_3">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="j2n7s300_joint_finger_tip_3_actuator">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
</robot>
|
||||
|
||||
693
src/curobo/content/assets/robot/jaco/jaco_7s.urdf
Normal file
693
src/curobo/content/assets/robot/jaco/jaco_7s.urdf
Normal file
@@ -0,0 +1,693 @@
|
||||
<?xml version="1.0" ?>
|
||||
<!-- =================================================================================== -->
|
||||
<!-- | This document was autogenerated by xacro from j2s7s300_standalone.xacro | -->
|
||||
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||
<!-- =================================================================================== -->
|
||||
<!-- j2s7s300 refers to jaco v2 7DOF spherical 3fingers -->
|
||||
<robot name="jaco" xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz" xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint" xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model" xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics" xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable" xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:xi="http://www.w3.org/2001/XInclude">
|
||||
<!-- links mesh_no
|
||||
base 0
|
||||
shoulder 1
|
||||
arm 2
|
||||
forearm 3
|
||||
wrist 4
|
||||
arm_mico 5
|
||||
arm_half1 (7dof) 6
|
||||
arm_half2 (7dof) 7
|
||||
wrist_spherical_1 8
|
||||
wrist_spherical_2 9
|
||||
|
||||
hand 3 finger 55
|
||||
hand_2finger 56
|
||||
finger_proximal 57
|
||||
finger_distal 58
|
||||
-->
|
||||
<!-- links mesh_no
|
||||
base 0
|
||||
shoulder 1
|
||||
arm 2
|
||||
forearm 3
|
||||
wrist 4
|
||||
arm_mico 5
|
||||
arm_half1 (7dof) 6
|
||||
arm_half2 (7dof) 7
|
||||
wrist_spherical_1 8
|
||||
wrist_spherical_2 9
|
||||
|
||||
hand 3 finger 55
|
||||
hand_2finger 56
|
||||
finger_proximal 57
|
||||
finger_distal 58
|
||||
-->
|
||||
<link name="root"/>
|
||||
<!-- for gazebo -->
|
||||
<link name="world"/>
|
||||
<joint name="connect_root_and_world" type="fixed">
|
||||
<child link="root"/>
|
||||
<parent link="world"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
</joint>
|
||||
<!-- ros_control plugin -->
|
||||
<gazebo>
|
||||
<plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
|
||||
<robotNamespace>j2s7s300</robotNamespace>
|
||||
<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
|
||||
<legacyModeNS>true</legacyModeNS>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
<link name="j2s7s300_link_base">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/base.dae"/>
|
||||
</geometry>
|
||||
<material name="carbon_fiber">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/base.dae"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.46784"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.1255"/>
|
||||
<inertia ixx="0.000951270861568" ixy="0" ixz="0" iyy="0.000951270861568" iyz="0" izz="0.00037427200000000004"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="j2s7s300_joint_base" type="fixed">
|
||||
<parent link="root"/>
|
||||
<child link="j2s7s300_link_base"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<link name="j2s7s300_link_1">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/shoulder.dae"/>
|
||||
</geometry>
|
||||
<material name="carbon_fiber">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ring_big.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/shoulder.dae"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.7477"/>
|
||||
<origin xyz="0 -0.002 -0.0605"/>
|
||||
<inertia ixx="0.0015203172520400004" ixy="0" ixz="0" iyy="0.0015203172520400004" iyz="0" izz="0.00059816"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="j2s7s300_joint_1" type="continuous">
|
||||
<parent link="j2s7s300_link_base"/>
|
||||
<child link="j2s7s300_link_1"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="40" velocity="0.6283185307179586"/>
|
||||
<origin rpy="0 3.141592653589793 0" xyz="0 0 0.15675"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<transmission name="j2s7s300_joint_1_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="j2s7s300_joint_1">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="j2s7s300_joint_1_actuator">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>160</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<!--For torque sensing in simulation-->
|
||||
<gazebo reference="j2s7s300_joint_1">
|
||||
<provideFeedback>true</provideFeedback>
|
||||
</gazebo>
|
||||
<gazebo>
|
||||
<plugin filename="libgazebo_ros_ft_sensor.so" name="ft_sensor">
|
||||
<updateRate>30.0</updateRate>
|
||||
<topicName>j2s7s300_joint_1_ft_sensor_topic</topicName>
|
||||
<jointName>j2s7s300_joint_1</jointName>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
<link name="j2s7s300_link_2">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/arm_half_1.dae"/>
|
||||
</geometry>
|
||||
<material name="carbon_fiber">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ring_big.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/arm_half_1.dae"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.8447"/>
|
||||
<origin xyz="0 -0.103563213 0"/>
|
||||
<inertia ixx="0.00247073761701" ixy="0" ixz="0" iyy="0.000380115" iyz="0" izz="0.00247073761701"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="j2s7s300_joint_2" type="revolute">
|
||||
<parent link="j2s7s300_link_1"/>
|
||||
<child link="j2s7s300_link_2"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="80" lower="0.8203047484373349" upper="5.462880558742252" velocity="0.6283185307179586"/>
|
||||
<origin rpy="-1.5707963267948966 0 3.141592653589793" xyz="0 0.0016 -0.11875"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<transmission name="j2s7s300_joint_2_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="j2s7s300_joint_2">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="j2s7s300_joint_2_actuator">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>160</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<!--For torque sensing in simulation-->
|
||||
<gazebo reference="j2s7s300_joint_2">
|
||||
<provideFeedback>true</provideFeedback>
|
||||
</gazebo>
|
||||
<gazebo>
|
||||
<plugin filename="libgazebo_ros_ft_sensor.so" name="ft_sensor">
|
||||
<updateRate>30.0</updateRate>
|
||||
<topicName>j2s7s300_joint_2_ft_sensor_topic</topicName>
|
||||
<jointName>j2s7s300_joint_2</jointName>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
<link name="j2s7s300_link_3">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/arm_half_2.dae"/>
|
||||
</geometry>
|
||||
<material name="carbon_fiber">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ring_big.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/arm_half_2.dae"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.8447"/>
|
||||
<origin xyz="0 0 -0.1022447445"/>
|
||||
<inertia ixx="0.00247073761701" ixy="0" ixz="0" iyy="0.00247073761701" iyz="0" izz="0.000380115"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="j2s7s300_joint_3" type="continuous">
|
||||
<parent link="j2s7s300_link_2"/>
|
||||
<child link="j2s7s300_link_3"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="40" velocity="0.6283185307179586"/>
|
||||
<origin rpy="-1.5707963267948966 0 0" xyz="0 -0.205 0"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<transmission name="j2s7s300_joint_3_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="j2s7s300_joint_3">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="j2s7s300_joint_3_actuator">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>160</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<!--For torque sensing in simulation-->
|
||||
<gazebo reference="j2s7s300_joint_3">
|
||||
<provideFeedback>true</provideFeedback>
|
||||
</gazebo>
|
||||
<gazebo>
|
||||
<plugin filename="libgazebo_ros_ft_sensor.so" name="ft_sensor">
|
||||
<updateRate>30.0</updateRate>
|
||||
<topicName>j2s7s300_joint_3_ft_sensor_topic</topicName>
|
||||
<jointName>j2s7s300_joint_3</jointName>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
<link name="j2s7s300_link_4">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/forearm.dae"/>
|
||||
</geometry>
|
||||
<material name="carbon_fiber">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ring_big.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/forearm.dae"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.6763"/>
|
||||
<origin xyz="0 0.081 -0.0086"/>
|
||||
<inertia ixx="0.0014202243190800001" ixy="0" ixz="0" iyy="0.000304335" iyz="0" izz="0.0014202243190800001"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="j2s7s300_joint_4" type="revolute">
|
||||
<parent link="j2s7s300_link_3"/>
|
||||
<child link="j2s7s300_link_4"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="40" lower="0.5235987755982988" upper="5.759586531581287" velocity="0.6283185307179586"/>
|
||||
<origin rpy="1.5707963267948966 0 3.141592653589793" xyz="0 0 -0.205"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<transmission name="j2s7s300_joint_4_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="j2s7s300_joint_4">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="j2s7s300_joint_4_actuator">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>160</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<!--For torque sensing in simulation-->
|
||||
<gazebo reference="j2s7s300_joint_4">
|
||||
<provideFeedback>true</provideFeedback>
|
||||
</gazebo>
|
||||
<gazebo>
|
||||
<plugin filename="libgazebo_ros_ft_sensor.so" name="ft_sensor">
|
||||
<updateRate>30.0</updateRate>
|
||||
<topicName>j2s7s300_joint_4_ft_sensor_topic</topicName>
|
||||
<jointName>j2s7s300_joint_4</jointName>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
<link name="j2s7s300_link_5">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/wrist_spherical_1.dae"/>
|
||||
</geometry>
|
||||
<material name="carbon_fiber">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ring_small.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/wrist_spherical_1.dae"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.463"/>
|
||||
<origin xyz="0 0.0028848942 -0.0541932613"/>
|
||||
<inertia ixx="0.0004321316048000001" ixy="0" ixz="0" iyy="0.0004321316048000001" iyz="0" izz="9.260000000000001e-05"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="j2s7s300_joint_5" type="continuous">
|
||||
<parent link="j2s7s300_link_4"/>
|
||||
<child link="j2s7s300_link_5"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="20" velocity="0.8377580409572781"/>
|
||||
<origin rpy="-1.5707963267948966 0 3.141592653589793" xyz="0 0.2073 -0.0114"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<transmission name="j2s7s300_joint_5_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="j2s7s300_joint_5">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="j2s7s300_joint_5_actuator">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>160</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<!--For torque sensing in simulation-->
|
||||
<gazebo reference="j2s7s300_joint_5">
|
||||
<provideFeedback>true</provideFeedback>
|
||||
</gazebo>
|
||||
<gazebo>
|
||||
<plugin filename="libgazebo_ros_ft_sensor.so" name="ft_sensor">
|
||||
<updateRate>30.0</updateRate>
|
||||
<topicName>j2s7s300_joint_5_ft_sensor_topic</topicName>
|
||||
<jointName>j2s7s300_joint_5</jointName>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
<link name="j2s7s300_link_6">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/wrist_spherical_2.dae"/>
|
||||
</geometry>
|
||||
<material name="carbon_fiber">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ring_small.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/wrist_spherical_2.dae"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.463"/>
|
||||
<origin xyz="0 0.0497208855 -0.0028562765"/>
|
||||
<inertia ixx="0.0004321316048000001" ixy="0" ixz="0" iyy="9.260000000000001e-05" iyz="0" izz="0.0004321316048000001"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="j2s7s300_joint_6" type="revolute">
|
||||
<parent link="j2s7s300_link_5"/>
|
||||
<child link="j2s7s300_link_6"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="20" lower="1.1344640137963142" upper="5.148721293383272" velocity="0.8377580409572781"/>
|
||||
<origin rpy="1.5707963267948966 0 3.141592653589793" xyz="0 0 -0.10375"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<transmission name="j2s7s300_joint_6_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="j2s7s300_joint_6">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="j2s7s300_joint_6_actuator">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>160</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<!--For torque sensing in simulation-->
|
||||
<gazebo reference="j2s7s300_joint_6">
|
||||
<provideFeedback>true</provideFeedback>
|
||||
</gazebo>
|
||||
<gazebo>
|
||||
<plugin filename="libgazebo_ros_ft_sensor.so" name="ft_sensor">
|
||||
<updateRate>30.0</updateRate>
|
||||
<topicName>j2s7s300_joint_6_ft_sensor_topic</topicName>
|
||||
<jointName>j2s7s300_joint_6</jointName>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
<link name="j2s7s300_link_7">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/hand_3finger.dae"/>
|
||||
</geometry>
|
||||
<material name="carbon_fiber">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ring_small.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/hand_3finger.dae"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.99"/>
|
||||
<origin xyz="0 0 -0.06"/>
|
||||
<inertia ixx="0.00034532361869999995" ixy="0" ixz="0" iyy="0.00034532361869999995" iyz="0" izz="0.0005815999999999999"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="j2s7s300_joint_7" type="continuous">
|
||||
<parent link="j2s7s300_link_6"/>
|
||||
<child link="j2s7s300_link_7"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="20" velocity="0.8377580409572781"/>
|
||||
<origin rpy="-1.5707963267948966 0 3.141592653589793" xyz="0 0.10375 0"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<transmission name="j2s7s300_joint_7_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="j2s7s300_joint_7">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="j2s7s300_joint_7_actuator">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>160</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<!--For torque sensing in simulation-->
|
||||
<gazebo reference="j2s7s300_joint_7">
|
||||
<provideFeedback>true</provideFeedback>
|
||||
</gazebo>
|
||||
<gazebo>
|
||||
<plugin filename="libgazebo_ros_ft_sensor.so" name="ft_sensor">
|
||||
<updateRate>30.0</updateRate>
|
||||
<topicName>j2s7s300_joint_7_ft_sensor_topic</topicName>
|
||||
<jointName>j2s7s300_joint_7</jointName>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
<link name="j2s7s300_end_effector"/>
|
||||
<joint name="j2s7s300_joint_end_effector" type="fixed">
|
||||
<parent link="j2s7s300_link_7"/>
|
||||
<child link="j2s7s300_end_effector"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
<origin rpy="3.141592653589793 0 1.5707963267948966" xyz="0 0 -0.1600"/>
|
||||
</joint>
|
||||
<link name="j2s7s300_link_finger_1">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/finger_proximal.dae"/>
|
||||
</geometry>
|
||||
<material name="carbon_fiber">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/finger_proximal.dae"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.01"/>
|
||||
<origin xyz="0.022 0 0"/>
|
||||
<inertia ixx="7.8999684e-07" ixy="0" ixz="0" iyy="7.8999684e-07" iyz="0" izz="8e-08"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="j2s7s300_joint_finger_1" type="revolute">
|
||||
<parent link="j2s7s300_link_7"/>
|
||||
<child link="j2s7s300_link_finger_1"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<origin rpy="-1.7047873384941834 0.6476144647144773 1.67317415161155" xyz="0.00279 0.03126 -0.11467"/>
|
||||
<limit effort="2" lower="0" upper="1.51" velocity="1"/>
|
||||
</joint>
|
||||
<transmission name="j2s7s300_joint_finger_1_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="j2s7s300_joint_finger_1">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="j2s7s300_joint_finger_1_actuator">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<link name="j2s7s300_link_finger_tip_1">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/finger_distal.dae"/>
|
||||
</geometry>
|
||||
<material name="carbon_fiber">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/finger_distal.dae"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.01"/>
|
||||
<origin xyz="0.022 0 0"/>
|
||||
<inertia ixx="7.8999684e-07" ixy="0" ixz="0" iyy="7.8999684e-07" iyz="0" izz="8e-08"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="j2s7s300_joint_finger_tip_1" type="revolute">
|
||||
<parent link="j2s7s300_link_finger_1"/>
|
||||
<child link="j2s7s300_link_finger_tip_1"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<origin rpy="0 0 0" xyz="0.044 -0.003 0"/>
|
||||
<limit effort="2" lower="0" upper="2" velocity="1"/>
|
||||
</joint>
|
||||
<transmission name="j2s7s300_joint_finger_tip_1_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="j2s7s300_joint_finger_tip_1">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="j2s7s300_joint_finger_tip_1_actuator">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<link name="j2s7s300_link_finger_2">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/finger_proximal.dae"/>
|
||||
</geometry>
|
||||
<material name="carbon_fiber">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/finger_proximal.dae"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.01"/>
|
||||
<origin xyz="0.022 0 0"/>
|
||||
<inertia ixx="7.8999684e-07" ixy="0" ixz="0" iyy="7.8999684e-07" iyz="0" izz="8e-08"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="j2s7s300_joint_finger_2" type="revolute">
|
||||
<parent link="j2s7s300_link_7"/>
|
||||
<child link="j2s7s300_link_finger_2"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<origin rpy="-1.570796327 .649262481663582 -1.38614049188413" xyz="0.02226 -0.02707 -0.11482"/>
|
||||
<limit effort="2" lower="0" upper="1.51" velocity="1"/>
|
||||
</joint>
|
||||
<transmission name="j2s7s300_joint_finger_2_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="j2s7s300_joint_finger_2">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="j2s7s300_joint_finger_2_actuator">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<link name="j2s7s300_link_finger_tip_2">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/finger_distal.dae"/>
|
||||
</geometry>
|
||||
<material name="carbon_fiber">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/finger_distal.dae"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.01"/>
|
||||
<origin xyz="0.022 0 0"/>
|
||||
<inertia ixx="7.8999684e-07" ixy="0" ixz="0" iyy="7.8999684e-07" iyz="0" izz="8e-08"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="j2s7s300_joint_finger_tip_2" type="revolute">
|
||||
<parent link="j2s7s300_link_finger_2"/>
|
||||
<child link="j2s7s300_link_finger_tip_2"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<origin rpy="0 0 0" xyz="0.044 -0.003 0"/>
|
||||
<limit effort="2" lower="0" upper="2" velocity="1"/>
|
||||
</joint>
|
||||
<transmission name="j2s7s300_joint_finger_tip_2_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="j2s7s300_joint_finger_tip_2">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="j2s7s300_joint_finger_tip_2_actuator">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<link name="j2s7s300_link_finger_3">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/finger_proximal.dae"/>
|
||||
</geometry>
|
||||
<material name="carbon_fiber">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/finger_proximal.dae"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.01"/>
|
||||
<origin xyz="0.022 0 0"/>
|
||||
<inertia ixx="7.8999684e-07" ixy="0" ixz="0" iyy="7.8999684e-07" iyz="0" izz="8e-08"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="j2s7s300_joint_finger_3" type="revolute">
|
||||
<parent link="j2s7s300_link_7"/>
|
||||
<child link="j2s7s300_link_finger_3"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<origin rpy="-1.570796327 .649262481663582 -1.75545216211587" xyz="-0.02226 -0.02707 -0.11482"/>
|
||||
<limit effort="2" lower="0" upper="1.51" velocity="1"/>
|
||||
</joint>
|
||||
<transmission name="j2s7s300_joint_finger_3_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="j2s7s300_joint_finger_3">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="j2s7s300_joint_finger_3_actuator">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<link name="j2s7s300_link_finger_tip_3">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/finger_distal.dae"/>
|
||||
</geometry>
|
||||
<material name="carbon_fiber">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/finger_distal.dae"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.01"/>
|
||||
<origin xyz="0.022 0 0"/>
|
||||
<inertia ixx="7.8999684e-07" ixy="0" ixz="0" iyy="7.8999684e-07" iyz="0" izz="8e-08"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="j2s7s300_joint_finger_tip_3" type="revolute">
|
||||
<parent link="j2s7s300_link_finger_3"/>
|
||||
<child link="j2s7s300_link_finger_tip_3"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<origin rpy="0 0 0" xyz="0.044 -0.003 0"/>
|
||||
<limit effort="2" lower="0" upper="2" velocity="1"/>
|
||||
</joint>
|
||||
<transmission name="j2s7s300_joint_finger_tip_3_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="j2s7s300_joint_finger_tip_3">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="j2s7s300_joint_finger_tip_3_actuator">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
</robot>
|
||||
|
||||
2
src/curobo/content/assets/robot/kinova/README.md
Normal file
2
src/curobo/content/assets/robot/kinova/README.md
Normal file
@@ -0,0 +1,2 @@
|
||||
The files in this directory come from:
|
||||
https://github.com/Kinovarobotics/ros_kortex/tree/noetic-devel/kortex_description
|
||||
577
src/curobo/content/assets/robot/kinova/kinova_gen3_7dof.urdf
Normal file
577
src/curobo/content/assets/robot/kinova/kinova_gen3_7dof.urdf
Normal file
@@ -0,0 +1,577 @@
|
||||
<?xml version="1.0" ?>
|
||||
<!-- =================================================================================== -->
|
||||
<!-- | This document was autogenerated by xacro from robots/gen3_robotiq_2f_85.xacro | -->
|
||||
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||
<!-- =================================================================================== -->
|
||||
<robot name="gen3_robotiq_2f_85">
|
||||
<!-- Run the macros -->
|
||||
<!-- For gazebo and DART -->
|
||||
|
||||
<link name="base_link">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="-0.000648 -0.000166 0.084487"/>
|
||||
<mass value="1.697"/>
|
||||
<inertia ixx="0.004622" ixy="9E-06" ixz="6E-05" iyy="0.004495" iyz="9E-06" izz="0.002079"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="kortex_description/arms/gen3/7dof/meshes/base_link.STL"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.75294 0.75294 0.75294 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="kortex_description/arms/gen3/7dof/meshes/base_link.STL"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="shoulder_link">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="-2.3E-05 -0.010364 -0.07336"/>
|
||||
<mass value="1.3773"/>
|
||||
<inertia ixx="0.00457" ixy="1E-06" ixz="2E-06" iyy="0.004831" iyz="0.000448" izz="0.001409"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="kortex_description/arms/gen3/7dof/meshes/shoulder_link.STL"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.75294 0.75294 0.75294 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="kortex_description/arms/gen3/7dof/meshes/shoulder_link.STL"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint_1" type="revolute">
|
||||
<origin rpy="3.1416 2.7629E-18 -4.9305E-36" xyz="0 0 0.15643"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="shoulder_link"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="39" lower="-6.0" upper="6.0" velocity="1.3963"/>
|
||||
</joint>
|
||||
<link name="half_arm_1_link">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="-4.4E-05 -0.09958 -0.013278"/>
|
||||
<mass value="1.1636"/>
|
||||
<inertia ixx="0.011088" ixy="5E-06" ixz="0" iyy="0.001072" iyz="-0.000691" izz="0.011255"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="kortex_description/arms/gen3/7dof/meshes/half_arm_1_link.STL"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.75294 0.75294 0.75294 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="kortex_description/arms/gen3/7dof/meshes/half_arm_1_link.STL"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint_2" type="revolute">
|
||||
<origin rpy="1.5708 2.1343E-17 -1.1102E-16" xyz="0 0.005375 -0.12838"/>
|
||||
<parent link="shoulder_link"/>
|
||||
<child link="half_arm_1_link"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="39" lower="-2.41" upper="2.41" velocity="1.3963"/>
|
||||
</joint>
|
||||
<link name="half_arm_2_link">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="-4.4E-05 -0.006641 -0.117892"/>
|
||||
<mass value="1.1636"/>
|
||||
<inertia ixx="0.010932" ixy="0" ixz="-7E-06" iyy="0.011127" iyz="0.000606" izz="0.001043"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="kortex_description/arms/gen3/7dof/meshes/half_arm_2_link.STL"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.75294 0.75294 0.75294 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="kortex_description/arms/gen3/7dof/meshes/half_arm_2_link.STL"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint_3" type="revolute">
|
||||
<origin rpy="-1.5708 1.2326E-32 -2.9122E-16" xyz="0 -0.21038 -0.006375"/>
|
||||
<parent link="half_arm_1_link"/>
|
||||
<child link="half_arm_2_link"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="39" lower="-6.0" upper="6.0" velocity="1.3963"/>
|
||||
</joint>
|
||||
<link name="forearm_link">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="-1.8E-05 -0.075478 -0.015006"/>
|
||||
<mass value="0.9302"/>
|
||||
<inertia ixx="0.008147" ixy="-1E-06" ixz="0" iyy="0.000631" iyz="-0.0005" izz="0.008316"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="kortex_description/arms/gen3/7dof/meshes/forearm_link.STL"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.75294 0.75294 0.75294 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="kortex_description/arms/gen3/7dof/meshes/forearm_link.STL"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint_4" type="revolute">
|
||||
<origin rpy="1.5708 -6.6954E-17 -1.6653E-16" xyz="0 0.006375 -0.21038"/>
|
||||
<parent link="half_arm_2_link"/>
|
||||
<child link="forearm_link"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="39" lower="-2.66" upper="2.66" velocity="1.3963"/>
|
||||
</joint>
|
||||
<link name="spherical_wrist_1_link">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="1E-06 -0.009432 -0.063883"/>
|
||||
<mass value="0.6781"/>
|
||||
<inertia ixx="0.001596" ixy="0" ixz="0" iyy="0.001607" iyz="0.000256" izz="0.000399"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="kortex_description/arms/gen3/7dof/meshes/spherical_wrist_1_link.STL"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.75294 0.75294 0.75294 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="kortex_description/arms/gen3/7dof/meshes/spherical_wrist_1_link.STL"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint_5" type="revolute">
|
||||
<origin rpy="-1.5708 2.2204E-16 -6.373E-17" xyz="0 -0.20843 -0.006375"/>
|
||||
<parent link="forearm_link"/>
|
||||
<child link="spherical_wrist_1_link"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="9" lower="-6.0" upper="6.0" velocity="1.2218"/>
|
||||
</joint>
|
||||
<link name="spherical_wrist_2_link">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="1E-06 -0.045483 -0.00965"/>
|
||||
<mass value="0.6781"/>
|
||||
<inertia ixx="0.001641" ixy="0" ixz="0" iyy="0.00041" iyz="-0.000278" izz="0.001641"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="kortex_description/arms/gen3/7dof/meshes/spherical_wrist_2_link.STL"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.75294 0.75294 0.75294 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="kortex_description/arms/gen3/7dof/meshes/spherical_wrist_2_link.STL"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint_6" type="revolute">
|
||||
<origin rpy="1.5708 9.2076E-28 -8.2157E-15" xyz="0 0.00017505 -0.10593"/>
|
||||
<parent link="spherical_wrist_1_link"/>
|
||||
<child link="spherical_wrist_2_link"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="9" lower="-2.23" upper="2.23" velocity="1.2218"/>
|
||||
</joint>
|
||||
<link name="bracelet_link">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.000281 0.011402 -0.029798"/>
|
||||
<mass value="0.5"/>
|
||||
<inertia ixx="0.000587" ixy="3E-06" ixz="3E-06" iyy="0.000369" iyz="-0.000118" izz="0.000609"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="kortex_description/arms/gen3/7dof/meshes/bracelet_with_vision_link.STL"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.75294 0.75294 0.75294 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="kortex_description/arms/gen3/7dof/meshes/bracelet_with_vision_link.STL"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint_7" type="revolute">
|
||||
<origin rpy="-1.5708 -5.5511E-17 9.6396E-17" xyz="0 -0.10593 -0.00017505"/>
|
||||
<parent link="spherical_wrist_2_link"/>
|
||||
<child link="bracelet_link"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="9" lower="-6.0" upper="6.0" velocity="1.2218"/>
|
||||
</joint>
|
||||
<link name="end_effector_link"/>
|
||||
<joint name="end_effector" type="fixed">
|
||||
<origin rpy="3.14159265358979 1.09937075168372E-32 0" xyz="0 0 -0.0615250000000001"/>
|
||||
<parent link="bracelet_link"/>
|
||||
<child link="end_effector_link"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
<link name="camera_link"/>
|
||||
<joint name="camera_module" type="fixed">
|
||||
<origin rpy="3.14159265358979 3.14159265358979 0" xyz="0 0.05639 -0.00305"/>
|
||||
<parent link="end_effector_link"/>
|
||||
<child link="camera_link"/>
|
||||
</joint>
|
||||
<link name="camera_depth_frame"/>
|
||||
<joint name="depth_module" type="fixed">
|
||||
<origin rpy="3.14159265358979 3.14159265358979 0" xyz="0.0275 0.066 -0.00305"/>
|
||||
<parent link="end_effector_link"/>
|
||||
<child link="camera_depth_frame"/>
|
||||
</joint>
|
||||
<link name="camera_color_frame"/>
|
||||
<joint name="color_module" type="fixed">
|
||||
<origin rpy="3.14159265358979 3.14159265358979 0" xyz="0 0.05639 -0.00305"/>
|
||||
<parent link="end_effector_link"/>
|
||||
<child link="camera_color_frame"/>
|
||||
</joint>
|
||||
<!-- This line was removed by Kinova because we replaced the transmission file with our own
|
||||
<xacro:include filename="$(find robotiq_2f_85_gripper_visualization)/urdf/robotiq_arg2f_transmission.xacro" /> -->
|
||||
<!-- Tool frame used by the arm -->
|
||||
<link name="tool_frame"/>
|
||||
<joint name="tool_frame_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0 0.120"/>
|
||||
<parent link="end_effector_link"/>
|
||||
<child link="tool_frame"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
<!-- This joint was added by Kinova -->
|
||||
<joint name="gripper_base_joint" type="fixed">
|
||||
<parent link="end_effector_link"/>
|
||||
<child link="robotiq_arg2f_base_link"/>
|
||||
<origin rpy="0.0 0.0 1.57" xyz="0.0 0.0 0.0"/>
|
||||
</joint>
|
||||
<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>
|
||||
<!-- The DAE file doesn't show well in Gazebo so we're using the STL instead -->
|
||||
<!-- <mesh filename="kortex_description/grippers/robotiq_2f_85/meshes/visual/robotiq_arg2f_85_base_link.dae" scale="0.001 0.001 0.001"/> -->
|
||||
<mesh filename="kortex_description/grippers/robotiq_2f_85/meshes/collision/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="kortex_description/grippers/robotiq_2f_85/meshes/collision/robotiq_arg2f_base_link.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<gazebo reference="robotiq_arg2f_base_link">
|
||||
<material>Gazebo/Black</material>
|
||||
</gazebo>
|
||||
<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="kortex_description/grippers/robotiq_2f_85/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="kortex_description/grippers/robotiq_2f_85/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 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="kortex_description/grippers/robotiq_2f_85/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="kortex_description/grippers/robotiq_2f_85/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 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="kortex_description/grippers/robotiq_2f_85/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="kortex_description/grippers/robotiq_2f_85/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 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="kortex_description/grippers/robotiq_2f_85/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="kortex_description/grippers/robotiq_2f_85/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 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="kortex_description/grippers/robotiq_2f_85/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="kortex_description/grippers/robotiq_2f_85/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 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="kortex_description/grippers/robotiq_2f_85/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="kortex_description/grippers/robotiq_2f_85/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 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="kortex_description/grippers/robotiq_2f_85/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="kortex_description/grippers/robotiq_2f_85/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 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="kortex_description/grippers/robotiq_2f_85/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="kortex_description/grippers/robotiq_2f_85/meshes/collision/robotiq_arg2f_85_inner_knuckle.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="left_outer_knuckle_joint" type="fixed">
|
||||
<origin rpy="0 0 3.1415" 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.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"/>
|
||||
</joint>
|
||||
<joint name="left_inner_knuckle_joint" type="fixed">
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 -0.0127 0.06142"/>
|
||||
<parent link="robotiq_arg2f_base_link"/>
|
||||
<child link="left_inner_knuckle"/>
|
||||
|
||||
</joint>
|
||||
<joint name="left_inner_finger_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0.0061 0.0471"/>
|
||||
<parent link="left_outer_finger"/>
|
||||
<child link="left_inner_finger"/>
|
||||
|
||||
</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"/>
|
||||
</joint>
|
||||
<joint name="right_outer_knuckle_joint" type="fixed">
|
||||
<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.8" velocity="2.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"/>
|
||||
</joint>
|
||||
<joint name="right_inner_knuckle_joint" type="fixed">
|
||||
<origin rpy="0 0 0.0" xyz="0 0.0127 0.06142"/>
|
||||
<parent link="robotiq_arg2f_base_link"/>
|
||||
<child link="right_inner_knuckle"/>
|
||||
|
||||
</joint>
|
||||
<joint name="right_inner_finger_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0.0061 0.0471"/>
|
||||
<parent link="right_outer_finger"/>
|
||||
<child link="right_inner_finger"/>
|
||||
|
||||
</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"/>
|
||||
</joint>
|
||||
</robot>
|
||||
@@ -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,13 @@
|
||||
This is the README file from Robotiq's package for the 2F-85 gripper. The package's license file can also be found in this folder.
|
||||
We used commit `4e3196c1c6a83f54acfa3dd8cf02b575ebba3e53` from [Robotiq's ROS Industrial Github repository](https://github.com/ros-industrial/robotiq)
|
||||
|
||||
# Robotiq 85mm 2-Finger-Adaptive-Gripper
|
||||
|
||||
This package contains the URDF files describing the 85mm stroke gripper from robotiq, also known as series **C3**.
|
||||
|
||||
## Robot Visual
|
||||
|
||||
|
||||
## Robot Collision
|
||||
|
||||
|
||||
@@ -0,0 +1,331 @@
|
||||
<?xml version="1.0" ?>
|
||||
<!-- =================================================================================== -->
|
||||
<!-- | This document was autogenerated by xacro from tm12.urdf.xacro | -->
|
||||
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||
<!-- =================================================================================== -->
|
||||
<robot name="tm12">
|
||||
<gazebo reference="link_1">
|
||||
<selfCollide>true</selfCollide>
|
||||
</gazebo>
|
||||
<gazebo reference="link_2">
|
||||
<selfCollide>true</selfCollide>
|
||||
</gazebo>
|
||||
<gazebo reference="link_3">
|
||||
<selfCollide>true</selfCollide>
|
||||
</gazebo>
|
||||
<gazebo reference="link_4">
|
||||
<selfCollide>true</selfCollide>
|
||||
</gazebo>
|
||||
<gazebo reference="link_5">
|
||||
<selfCollide>true</selfCollide>
|
||||
</gazebo>
|
||||
<gazebo reference="link_6">
|
||||
<selfCollide>true</selfCollide>
|
||||
</gazebo>
|
||||
<gazebo>
|
||||
<plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
|
||||
<robotNamespace>/</robotNamespace>
|
||||
<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
<transmission name="trans_1">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="shoulder_1_joint">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="motor_1">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="trans_2">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="shoulder_2_joint">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="motor_2">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="trans_3">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="elbow_joint">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="motor_3">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="trans_4">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="wrist_1_joint">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="motor_4">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="trans_5">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="wrist_2_joint">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="motor_5">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="trans_6">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="wrist_3_joint">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="motor_6">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<material name="none">
|
||||
<color rgba="0.0 0.0 0.0 1.0"/>
|
||||
</material>
|
||||
<material name="lightgrey">
|
||||
<color rgba="0.5 0.5 0.5 1.0"/>
|
||||
</material>
|
||||
<material name="grey">
|
||||
<color rgba="0.75 0.75 0.75 1.0"/>
|
||||
</material>
|
||||
<material name="white">
|
||||
<color rgba="0.95 0.95 0.95 1.0"/>
|
||||
</material>
|
||||
<material name="red">
|
||||
<color rgba="0.95 0.0 0.0 1.0"/>
|
||||
</material>
|
||||
<material name="orange">
|
||||
<color rgba="1.0 0.5 0.0 1.0"/>
|
||||
</material>
|
||||
<material name="yellow">
|
||||
<color rgba="1.0 1.0 0.0 1.0"/>
|
||||
</material>
|
||||
<material name="green">
|
||||
<color rgba="0.0 1.0 0.0 1.0"/>
|
||||
</material>
|
||||
<material name="blue">
|
||||
<color rgba="0.0 0.0 1.0 1.0"/>
|
||||
</material>
|
||||
<material name="indigo">
|
||||
<color rgba="0.3 0.3 0.6 1.0"/>
|
||||
</material>
|
||||
<material name="violet">
|
||||
<color rgba="0.6 0.0 1.0 1.0"/>
|
||||
</material>
|
||||
<material name="black">
|
||||
<color rgba="0.0 0.0 0.0 1.0"/>
|
||||
</material>
|
||||
<material name="darkolive">
|
||||
<color rgba="0.3 0.3 0.25 1.0"/>
|
||||
</material>
|
||||
<!--LinkDescription-->
|
||||
<link name="link_0">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/tm12/visual/tm12-base.obj"/>
|
||||
</geometry>
|
||||
<material name="none"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/tm12/collision/tm12-base_c.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1.0"/>
|
||||
<origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
|
||||
<inertia ixx="0.00110833289" ixy="0.0" ixz="0.0" iyy="0.00110833289" iyz="0.0" izz="0.0018"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="shoulder_1_joint" type="revolute">
|
||||
<parent link="link_0"/>
|
||||
<child link="link_1"/>
|
||||
<origin rpy="0.000000 -0.000000 0.000000" xyz="0.000000 0.000000 0.165200"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<!--limit-->
|
||||
<limit effort="353" lower="-4.71238898038469" upper="4.71238898038469" velocity="2.0943951023931953"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<link name="link_1">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/tm12/visual/tmr_750w_01.obj"/>
|
||||
</geometry>
|
||||
<material name="none"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/tm12/collision/tmr_750w_01_c.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
|
||||
<mass value="7.6"/>
|
||||
<inertia ixx="0.020289334" ixy="0.000000" ixz="0.000000" iyy="0.020289334" iyz="0.000000" izz="0.021396270999999998"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="shoulder_2_joint" type="revolute">
|
||||
<parent link="link_1"/>
|
||||
<child link="link_2"/>
|
||||
<origin rpy="-1.570796 -1.570796 0.000000" xyz="0.000000 0.000000 0.000000"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<!--limit-->
|
||||
<limit effort="353" lower="-3.141592653589793" upper="3.141592653589793" velocity="2.0943951023931953"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<link name="link_2">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/tm12/visual/tm12-arm1.obj"/>
|
||||
</geometry>
|
||||
<material name="none"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/tm12/collision/tm12-arm1_c.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
|
||||
<mass value="14.0239"/>
|
||||
<inertia ixx="0.071505715" ixy="0.000000" ixz="0.000000" iyy="1.1758788999999998" iyz="0.000000" izz="1.2033932999999999"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="elbow_joint" type="revolute">
|
||||
<parent link="link_2"/>
|
||||
<child link="link_3"/>
|
||||
<origin rpy="0.000000 -0.000000 0.000000" xyz="0.636100 0.000000 0.000000"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<!--limit-->
|
||||
<limit effort="157" lower="-2.897246558310587" upper="2.897246558310587" velocity="3.141592653589793"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<link name="link_3">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/tm12/visual/tm12-arm2.obj"/>
|
||||
</geometry>
|
||||
<material name="none"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/tm12/collision/tm12-arm2_c.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
|
||||
<mass value="3.3577"/>
|
||||
<inertia ixx="0.009755469" ixy="0.000000" ixz="0.000000" iyy="0.16334719" iyz="0.000000" izz="0.16656678"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="wrist_1_joint" type="revolute">
|
||||
<parent link="link_3"/>
|
||||
<child link="link_4"/>
|
||||
<origin rpy="0.000000 -0.000000 1.570796" xyz="0.557900 0.000000 -0.156300"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<!--limit-->
|
||||
<limit effort="54" lower="-3.141592653589793" upper="3.141592653589793" velocity="3.141592653589793"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<link name="link_4">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/tm5-900/visual/tmr_100w_01.obj"/>
|
||||
</geometry>
|
||||
<material name="none"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/tm5-900/collision/tmr_100w_01_c.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
|
||||
<mass value="1.576"/>
|
||||
<inertia ixx="0.002058405" ixy="0.000000" ixz="0.000000" iyy="0.0025630790000000002" iyz="0.000000" izz="0.00264321"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="wrist_2_joint" type="revolute">
|
||||
<parent link="link_4"/>
|
||||
<child link="link_5"/>
|
||||
<origin rpy="1.570796 -0.000000 0.000000" xyz="0.000000 -0.106000 0.000000"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<!--limit-->
|
||||
<limit effort="54" lower="-3.141592653589793" upper="3.141592653589793" velocity="3.141592653589793"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<link name="link_5">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/tm5-900/visual/tmr_100w_02.obj"/>
|
||||
</geometry>
|
||||
<material name="none"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/tm5-900/collision/tmr_100w_02_c.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
|
||||
<mass value="1.576"/>
|
||||
<inertia ixx="0.002058405" ixy="0.000000" ixz="0.000000" iyy="0.0025630790000000002" iyz="0.000000" izz="0.00264321"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="wrist_3_joint" type="revolute">
|
||||
<parent link="link_5"/>
|
||||
<child link="link_6"/>
|
||||
<origin rpy="1.570796 -0.000000 0.000000" xyz="0.000000 -0.113150 0.000000"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<!--limit-->
|
||||
<limit effort="54" lower="-4.71238898038469" upper="4.71238898038469" velocity="3.141592653589793"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<link name="link_6">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/tm5-900/visual/tmr_ee.obj"/>
|
||||
</geometry>
|
||||
<material name="none"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/tm5-900/collision/tmr_ee_c.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
|
||||
<mass value="0.65"/>
|
||||
<inertia ixx="0.000774544" ixy="0.000000" ixz="0.000000" iyy="0.001383811" iyz="0.000000" izz="0.001559496"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="flange_fixed_joint" type="fixed">
|
||||
<parent link="link_6"/>
|
||||
<child link="flange_link"/>
|
||||
<origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
|
||||
</joint>
|
||||
<link name="flange_link"/>
|
||||
<link name="base"/>
|
||||
<joint name="base_fixed_joint" type="fixed">
|
||||
<parent link="base"/>
|
||||
<child link="link_0"/>
|
||||
<origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
|
||||
</joint>
|
||||
<link name="tool0"/>
|
||||
<joint name="flange_link-tool0" type="fixed">
|
||||
<parent link="flange_link"/>
|
||||
<child link="tool0"/>
|
||||
<origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
|
||||
</joint>
|
||||
<!--LinkDescription-->
|
||||
<link name="world"/>
|
||||
<joint name="world_joint" type="fixed">
|
||||
<parent link="world"/>
|
||||
<child link="base"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
</joint>
|
||||
</robot>
|
||||
|
||||
331
src/curobo/content/assets/robot/techman/tm_description/urdf/tm12x-nominal.urdf
Executable file
331
src/curobo/content/assets/robot/techman/tm_description/urdf/tm12x-nominal.urdf
Executable file
@@ -0,0 +1,331 @@
|
||||
<?xml version="1.0" ?>
|
||||
<!-- =================================================================================== -->
|
||||
<!-- | This document was autogenerated by xacro from tm12x.urdf.xacro | -->
|
||||
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||
<!-- =================================================================================== -->
|
||||
<robot name="tm12x">
|
||||
<gazebo reference="link_1">
|
||||
<selfCollide>true</selfCollide>
|
||||
</gazebo>
|
||||
<gazebo reference="link_2">
|
||||
<selfCollide>true</selfCollide>
|
||||
</gazebo>
|
||||
<gazebo reference="link_3">
|
||||
<selfCollide>true</selfCollide>
|
||||
</gazebo>
|
||||
<gazebo reference="link_4">
|
||||
<selfCollide>true</selfCollide>
|
||||
</gazebo>
|
||||
<gazebo reference="link_5">
|
||||
<selfCollide>true</selfCollide>
|
||||
</gazebo>
|
||||
<gazebo reference="link_6">
|
||||
<selfCollide>true</selfCollide>
|
||||
</gazebo>
|
||||
<gazebo>
|
||||
<plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
|
||||
<robotNamespace>/</robotNamespace>
|
||||
<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
<transmission name="trans_1">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="shoulder_1_joint">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="motor_1">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="trans_2">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="shoulder_2_joint">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="motor_2">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="trans_3">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="elbow_joint">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="motor_3">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="trans_4">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="wrist_1_joint">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="motor_4">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="trans_5">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="wrist_2_joint">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="motor_5">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="trans_6">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="wrist_3_joint">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="motor_6">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<material name="none">
|
||||
<color rgba="0.0 0.0 0.0 1.0"/>
|
||||
</material>
|
||||
<material name="lightgrey">
|
||||
<color rgba="0.5 0.5 0.5 1.0"/>
|
||||
</material>
|
||||
<material name="grey">
|
||||
<color rgba="0.75 0.75 0.75 1.0"/>
|
||||
</material>
|
||||
<material name="white">
|
||||
<color rgba="0.95 0.95 0.95 1.0"/>
|
||||
</material>
|
||||
<material name="red">
|
||||
<color rgba="0.95 0.0 0.0 1.0"/>
|
||||
</material>
|
||||
<material name="orange">
|
||||
<color rgba="1.0 0.5 0.0 1.0"/>
|
||||
</material>
|
||||
<material name="yellow">
|
||||
<color rgba="1.0 1.0 0.0 1.0"/>
|
||||
</material>
|
||||
<material name="green">
|
||||
<color rgba="0.0 1.0 0.0 1.0"/>
|
||||
</material>
|
||||
<material name="blue">
|
||||
<color rgba="0.0 0.0 1.0 1.0"/>
|
||||
</material>
|
||||
<material name="indigo">
|
||||
<color rgba="0.3 0.3 0.6 1.0"/>
|
||||
</material>
|
||||
<material name="violet">
|
||||
<color rgba="0.6 0.0 1.0 1.0"/>
|
||||
</material>
|
||||
<material name="black">
|
||||
<color rgba="0.0 0.0 0.0 1.0"/>
|
||||
</material>
|
||||
<material name="darkolive">
|
||||
<color rgba="0.3 0.3 0.25 1.0"/>
|
||||
</material>
|
||||
<!--LinkDescription-->
|
||||
<link name="link_0">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/tm12/visual/tm12-base.obj"/>
|
||||
</geometry>
|
||||
<material name="none"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/tm12/collision/tm12-base_c.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1.0"/>
|
||||
<origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
|
||||
<inertia ixx="0.00110833289" ixy="0.0" ixz="0.0" iyy="0.00110833289" iyz="0.0" izz="0.0018"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="shoulder_1_joint" type="revolute">
|
||||
<parent link="link_0"/>
|
||||
<child link="link_1"/>
|
||||
<origin rpy="0.000000 -0.000000 0.000000" xyz="0.000000 0.000000 0.165200"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<!--limit-->
|
||||
<limit effort="353" lower="-6.283185307179586" upper="6.283185307179586" velocity="2.0943951023931953"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<link name="link_1">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/tm12/visual/tmr_750w_01.obj"/>
|
||||
</geometry>
|
||||
<material name="none"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/tm12/collision/tmr_750w_01_c.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
|
||||
<mass value="7.6"/>
|
||||
<inertia ixx="0.020289334" ixy="0.000000" ixz="0.000000" iyy="0.020289334" iyz="0.000000" izz="0.021396270999999998"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="shoulder_2_joint" type="revolute">
|
||||
<parent link="link_1"/>
|
||||
<child link="link_2"/>
|
||||
<origin rpy="-1.570796 -1.570796 0.000000" xyz="0.000000 0.000000 0.000000"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<!--limit-->
|
||||
<limit effort="353" lower="-6.283185307179586" upper="6.283185307179586" velocity="2.0943951023931953"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<link name="link_2">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/tm12/visual/tm12-arm1.obj"/>
|
||||
</geometry>
|
||||
<material name="none"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/tm12/collision/tm12-arm1_c.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
|
||||
<mass value="14.0239"/>
|
||||
<inertia ixx="0.071505715" ixy="0.000000" ixz="0.000000" iyy="1.1758788999999998" iyz="0.000000" izz="1.2033932999999999"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="elbow_joint" type="revolute">
|
||||
<parent link="link_2"/>
|
||||
<child link="link_3"/>
|
||||
<origin rpy="0.000000 -0.000000 0.000000" xyz="0.636100 0.000000 0.000000"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<!--limit-->
|
||||
<limit effort="157" lower="-2.897246558310587" upper="2.897246558310587" velocity="3.141592653589793"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<link name="link_3">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/tm12/visual/tm12-arm2.obj"/>
|
||||
</geometry>
|
||||
<material name="none"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/tm12/collision/tm12-arm2_c.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
|
||||
<mass value="3.3577"/>
|
||||
<inertia ixx="0.009755469" ixy="0.000000" ixz="0.000000" iyy="0.16334719" iyz="0.000000" izz="0.16656678"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="wrist_1_joint" type="revolute">
|
||||
<parent link="link_3"/>
|
||||
<child link="link_4"/>
|
||||
<origin rpy="0.000000 -0.000000 1.570796" xyz="0.557900 0.000000 -0.156300"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<!--limit-->
|
||||
<limit effort="54" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<link name="link_4">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/tm5-900/visual/tmr_100w_01.obj"/>
|
||||
</geometry>
|
||||
<material name="none"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/tm5-900/collision/tmr_100w_01_c.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
|
||||
<mass value="1.576"/>
|
||||
<inertia ixx="0.002058405" ixy="0.000000" ixz="0.000000" iyy="0.0025630790000000002" iyz="0.000000" izz="0.00264321"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="wrist_2_joint" type="revolute">
|
||||
<parent link="link_4"/>
|
||||
<child link="link_5"/>
|
||||
<origin rpy="1.570796 -0.000000 0.000000" xyz="0.000000 -0.106000 0.000000"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<!--limit-->
|
||||
<limit effort="54" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<link name="link_5">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/tm5-900/visual/tmr_100w_02.obj"/>
|
||||
</geometry>
|
||||
<material name="none"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/tm5-900/collision/tmr_100w_02_c.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
|
||||
<mass value="1.576"/>
|
||||
<inertia ixx="0.002058405" ixy="0.000000" ixz="0.000000" iyy="0.0025630790000000002" iyz="0.000000" izz="0.00264321"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="wrist_3_joint" type="revolute">
|
||||
<parent link="link_5"/>
|
||||
<child link="link_6"/>
|
||||
<origin rpy="1.570796 -0.000000 0.000000" xyz="0.000000 -0.113150 0.000000"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<!--limit-->
|
||||
<limit effort="54" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
<link name="link_6">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="meshes/tm5-900/visual/tmr_iox.obj"/>
|
||||
</geometry>
|
||||
<material name="none"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="meshes/tm5-900/collision/tmr_iox_c.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
|
||||
<mass value="0.31"/>
|
||||
<inertia ixx="0.000226642" ixy="0.000000" ixz="0.000000" iyy="0.000226642" iyz="0.000000" izz="0.000286595"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="flange_fixed_joint" type="fixed">
|
||||
<parent link="link_6"/>
|
||||
<child link="flange_link"/>
|
||||
<origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
|
||||
</joint>
|
||||
<link name="flange_link"/>
|
||||
<link name="base"/>
|
||||
<joint name="base_fixed_joint" type="fixed">
|
||||
<parent link="base"/>
|
||||
<child link="link_0"/>
|
||||
<origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
|
||||
</joint>
|
||||
<link name="tool0"/>
|
||||
<joint name="flange_link-tool0" type="fixed">
|
||||
<parent link="flange_link"/>
|
||||
<child link="tool0"/>
|
||||
<origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
|
||||
</joint>
|
||||
<!--LinkDescription-->
|
||||
<link name="world"/>
|
||||
<joint name="world_joint" type="fixed">
|
||||
<parent link="world"/>
|
||||
<child link="base"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
</joint>
|
||||
</robot>
|
||||
|
||||
556
src/curobo/content/assets/robot/ur_description/dual_ur10e.urdf
Normal file
556
src/curobo/content/assets/robot/ur_description/dual_ur10e.urdf
Normal file
@@ -0,0 +1,556 @@
|
||||
<?xml version="1.0" ?>
|
||||
<!-- =================================================================================== -->
|
||||
<!-- | This document was autogenerated by xacro from ur_description/urdf/ur10e.xacro | -->
|
||||
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||
<!-- =================================================================================== -->
|
||||
<robot name="ur10e_robot">
|
||||
<!--
|
||||
Base UR robot series xacro macro.
|
||||
|
||||
NOTE: this is NOT a URDF. It cannot directly be loaded by consumers
|
||||
expecting a flattened '.urdf' file. See the top-level '.xacro' for that
|
||||
(but note: that .xacro must still be processed by the xacro command).
|
||||
|
||||
For use in '.launch' files: use one of the 'load_urX.launch' convenience
|
||||
launch files.
|
||||
|
||||
This file models the base kinematic chain of a UR robot, which then gets
|
||||
parameterised by various configuration files to convert it into a UR3(e),
|
||||
UR5(e), UR10(e) or UR16e.
|
||||
|
||||
NOTE: the default kinematic parameters (ie: link lengths, frame locations,
|
||||
offets, etc) do not correspond to any particular robot. They are defaults
|
||||
only. There WILL be non-zero offsets between the Forward Kinematics results
|
||||
in TF (ie: robot_state_publisher) and the values reported by the Teach
|
||||
Pendant.
|
||||
|
||||
For accurate (and robot-specific) transforms, the 'kinematics_parameters_file'
|
||||
parameter MUST point to a .yaml file containing the appropriate values for
|
||||
the targetted robot.
|
||||
|
||||
If using the UniversalRobots/Universal_Robots_ROS_Driver, follow the steps
|
||||
described in the readme of that repository to extract the kinematic
|
||||
calibration from the controller and generate the required .yaml file.
|
||||
|
||||
Main author of the migration to yaml configs: Ludovic Delval.
|
||||
|
||||
Contributors to previous versions (in no particular order):
|
||||
|
||||
- Felix Messmer
|
||||
- Kelsey Hawkins
|
||||
- Wim Meeussen
|
||||
- Shaun Edwards
|
||||
- Nadia Hammoudeh Garcia
|
||||
- Dave Hershberger
|
||||
- G. vd. Hoorn
|
||||
- Philip Long
|
||||
- Dave Coleman
|
||||
- Miguel Prada
|
||||
- Mathias Luedtke
|
||||
- Marcel Schnirring
|
||||
- Felix von Drigalski
|
||||
- Felix Exner
|
||||
- Jimmy Da Silva
|
||||
- Ajit Krisshna N L
|
||||
- Muhammad Asif Rana
|
||||
-->
|
||||
<!--
|
||||
NOTE: the macro defined in this file is NOT part of the public API of this
|
||||
package. Users CANNOT rely on this file being available, or stored in
|
||||
this location. Nor can they rely on the existence of the macro.
|
||||
-->
|
||||
|
||||
<!-- links: main serial chain -->
|
||||
<link name="base_fixture_link"/>
|
||||
<link name="base_link"/>
|
||||
|
||||
<link name="base_link_1"/>
|
||||
|
||||
|
||||
|
||||
<!-- joints: main serial chain -->
|
||||
<joint name="base_fixture_j_base_link" type="fixed">
|
||||
<parent link="base_fixture_link"/>
|
||||
<child link="base_link"/>
|
||||
<origin rpy="0 0 0" xyz="0.75 0.75 0"/>
|
||||
</joint>
|
||||
|
||||
|
||||
<!-- joints: main serial chain -->
|
||||
<joint name="base_fixture_j_base_link_1" type="fixed">
|
||||
<parent link="base_fixture_link"/>
|
||||
<child link="base_link_1"/>
|
||||
<origin rpy="0 0 0.0" xyz="-0.75 0.75 0"/>
|
||||
</joint>
|
||||
|
||||
|
||||
|
||||
<link name="base_link_inertia">
|
||||
<visual>
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/base.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/base.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="4.0"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="0.0061063308908" ixy="0.0" ixz="0.0" iyy="0.0061063308908" iyz="0.0" izz="0.01125"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="base_link_base_link_inertia" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="base_link_inertia"/>
|
||||
<!-- 'base_link' is REP-103 aligned (so X+ forward), while the internal
|
||||
frames of the robot/controller have X+ pointing backwards.
|
||||
Use the joint between 'base_link' and 'base_link_inertia' (a dummy
|
||||
link/frame) to introduce the necessary rotation over Z (of pi rad).
|
||||
-->
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<link name="shoulder_link">
|
||||
<visual>
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/shoulder.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/shoulder.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="7.778"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="0.03147431257693659" ixy="0.0" ixz="0.0" iyy="0.03147431257693659" iyz="0.0" izz="0.021875624999999996"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="upper_arm_link">
|
||||
<visual>
|
||||
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.1762"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/upperarm.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.1762"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/upperarm.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="12.93"/>
|
||||
<origin rpy="0 1.5707963267948966 0" xyz="-0.306 0.0 0.175"/>
|
||||
<inertia ixx="0.42175380379841093" ixy="0.0" ixz="0.0" iyy="0.42175380379841093" iyz="0.0" izz="0.03636562499999999"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="forearm_link">
|
||||
<visual>
|
||||
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.0393"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/forearm.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.0393"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/forearm.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="3.87"/>
|
||||
<origin rpy="0 1.5707963267948966 0" xyz="-0.285775 0.0 0.0393"/>
|
||||
<inertia ixx="0.11079302548902206" ixy="0.0" ixz="0.0" iyy="0.11079302548902206" iyz="0.0" izz="0.010884375"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="wrist_1_link">
|
||||
<visual>
|
||||
<!-- TODO: Move this to a parameter -->
|
||||
<origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.135"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/wrist1.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.135"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/wrist1.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1.96"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="0.005108247956699999" ixy="0.0" ixz="0.0" iyy="0.005108247956699999" iyz="0.0" izz="0.005512499999999999"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="wrist_2_link">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 -0.12"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/wrist2.dae"/>
|
||||
</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.12"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/wrist2.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1.96"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="0.005108247956699999" ixy="0.0" ixz="0.0" iyy="0.005108247956699999" iyz="0.0" izz="0.005512499999999999"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="wrist_3_link">
|
||||
<visual>
|
||||
<origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.1168"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/wrist3.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.1168"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/wrist3.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.202"/>
|
||||
<origin rpy="0 0 0" xyz="0.0 0.0 -0.025"/>
|
||||
<inertia ixx="0.00014434577559500002" ixy="0.0" ixz="0.0" iyy="0.00014434577559500002" iyz="0.0" izz="0.00020452500000000002"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
|
||||
<joint name="shoulder_pan_joint" type="revolute">
|
||||
<parent link="base_link_inertia"/>
|
||||
<child link="shoulder_link"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.1807"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="330.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="2.0943951023931953"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
<joint name="shoulder_lift_joint" type="revolute">
|
||||
<parent link="shoulder_link"/>
|
||||
<child link="upper_arm_link"/>
|
||||
<origin rpy="1.570796327 0 0" xyz="0 0 0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="330.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="2.0943951023931953"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
<joint name="elbow_joint" type="revolute">
|
||||
<parent link="upper_arm_link"/>
|
||||
<child link="forearm_link"/>
|
||||
<origin rpy="0 0 0" xyz="-0.6127 0 0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="150.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="3.141592653589793"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
<joint name="wrist_1_joint" type="revolute">
|
||||
<parent link="forearm_link"/>
|
||||
<child link="wrist_1_link"/>
|
||||
<origin rpy="0 0 0" xyz="-0.57155 0 0.17415"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
<joint name="wrist_2_joint" type="revolute">
|
||||
<parent link="wrist_1_link"/>
|
||||
<child link="wrist_2_link"/>
|
||||
<origin rpy="1.570796327 0 0" xyz="0 -0.11985 -2.458164590756244e-11"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
<joint name="wrist_3_joint" type="revolute">
|
||||
<parent link="wrist_2_link"/>
|
||||
<child link="wrist_3_link"/>
|
||||
<origin rpy="1.570796326589793 3.141592653589793 3.141592653589793" xyz="0 0.11655 -2.390480459346185e-11"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
|
||||
<link name="flange"/>
|
||||
<joint name="wrist_3-flange" type="fixed">
|
||||
<parent link="wrist_3_link"/>
|
||||
<child link="flange"/>
|
||||
<origin rpy="0 -1.5707963267948966 -1.5707963267948966" xyz="0 0 0"/>
|
||||
</joint>
|
||||
<!-- ROS-Industrial 'tool0' frame: all-zeros tool frame -->
|
||||
<link name="tool0"/>
|
||||
<joint name="flange-tool0" type="fixed">
|
||||
<!-- default toolframe: X+ left, Y+ up, Z+ front -->
|
||||
<origin rpy="1.5707963267948966 0 1.5707963267948966" xyz="0 0 0"/>
|
||||
<parent link="flange"/>
|
||||
<child link="tool0"/>
|
||||
</joint>
|
||||
<!-- ROS-Industrial 'base' frame: base_link to UR 'Base' Coordinates transform -->
|
||||
|
||||
|
||||
|
||||
<link name="base_link_inertia_link_1">
|
||||
<visual>
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/base.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/base.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="4.0"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="0.0061063308908" ixy="0.0" ixz="0.0" iyy="0.0061063308908" iyz="0.0" izz="0.01125"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="shoulder_link_1">
|
||||
<visual>
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/shoulder.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/shoulder.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="7.778"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="0.03147431257693659" ixy="0.0" ixz="0.0" iyy="0.03147431257693659" iyz="0.0" izz="0.021875624999999996"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="upper_arm_link_1">
|
||||
<visual>
|
||||
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.1762"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/upperarm.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.1762"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/upperarm.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="12.93"/>
|
||||
<origin rpy="0 1.5707963267948966 0" xyz="-0.306 0.0 0.175"/>
|
||||
<inertia ixx="0.42175380379841093" ixy="0.0" ixz="0.0" iyy="0.42175380379841093" iyz="0.0" izz="0.03636562499999999"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="forearm_link_1">
|
||||
<visual>
|
||||
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.0393"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/forearm.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.0393"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/forearm.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="3.87"/>
|
||||
<origin rpy="0 1.5707963267948966 0" xyz="-0.285775 0.0 0.0393"/>
|
||||
<inertia ixx="0.11079302548902206" ixy="0.0" ixz="0.0" iyy="0.11079302548902206" iyz="0.0" izz="0.010884375"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="wrist_1_link_1">
|
||||
<visual>
|
||||
<!-- TODO: Move this to a parameter -->
|
||||
<origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.135"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/wrist1.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.135"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/wrist1.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1.96"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="0.005108247956699999" ixy="0.0" ixz="0.0" iyy="0.005108247956699999" iyz="0.0" izz="0.005512499999999999"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="wrist_2_link_1">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 -0.12"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/wrist2.dae"/>
|
||||
</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.12"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/wrist2.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1.96"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="0.005108247956699999" ixy="0.0" ixz="0.0" iyy="0.005108247956699999" iyz="0.0" izz="0.005512499999999999"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="wrist_3_link_1">
|
||||
<visual>
|
||||
<origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.1168"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/wrist3.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.1168"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/wrist3.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.202"/>
|
||||
<origin rpy="0 0 0" xyz="0.0 0.0 -0.025"/>
|
||||
<inertia ixx="0.00014434577559500002" ixy="0.0" ixz="0.0" iyy="0.00014434577559500002" iyz="0.0" izz="0.00020452500000000002"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="base_link_base_link_inertia_joint_1" type="fixed">
|
||||
<parent link="base_link_1"/>
|
||||
<child link="base_link_inertia_link_1"/>
|
||||
<!-- 'base_link' is REP-103 aligned (so X+ forward), while the internal
|
||||
frames of the robot/controller have X+ pointing backwards.
|
||||
Use the joint between 'base_link' and 'base_link_inertia' (a dummy
|
||||
link/frame) to introduce the necessary rotation over Z (of pi rad).
|
||||
-->
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<joint name="shoulder_pan_joint_1" type="revolute">
|
||||
<parent link="base_link_inertia_link_1"/>
|
||||
<child link="shoulder_link_1"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.1807"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="330.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="2.0943951023931953"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
<joint name="shoulder_lift_joint_1" type="revolute">
|
||||
<parent link="shoulder_link_1"/>
|
||||
<child link="upper_arm_link_1"/>
|
||||
<origin rpy="1.570796327 0 0" xyz="0 0 0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="330.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="2.0943951023931953"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
<joint name="elbow_joint_1" type="revolute">
|
||||
<parent link="upper_arm_link_1"/>
|
||||
<child link="forearm_link_1"/>
|
||||
<origin rpy="0 0 0" xyz="-0.6127 0 0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="150.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="3.141592653589793"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
<joint name="wrist_1_joint_1" type="revolute">
|
||||
<parent link="forearm_link_1"/>
|
||||
<child link="wrist_1_link_1"/>
|
||||
<origin rpy="0 0 0" xyz="-0.57155 0 0.17415"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
<joint name="wrist_2_joint_1" type="revolute">
|
||||
<parent link="wrist_1_link_1"/>
|
||||
<child link="wrist_2_link_1"/>
|
||||
<origin rpy="1.570796327 0 0" xyz="0 -0.11985 -2.458164590756244e-11"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
<joint name="wrist_3_joint_1" type="revolute">
|
||||
<parent link="wrist_2_link_1"/>
|
||||
<child link="wrist_3_link_1"/>
|
||||
<origin rpy="1.570796326589793 3.141592653589793 3.141592653589793" xyz="0 0.11655 -2.390480459346185e-11"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- ROS-Industrial 'flange' frame: attachment point for EEF models -->
|
||||
<link name="flange_1"/>
|
||||
<joint name="wrist_3_flange_1" type="fixed">
|
||||
<parent link="wrist_3_link_1"/>
|
||||
<child link="flange_1"/>
|
||||
<origin rpy="0 -1.5707963267948966 -1.5707963267948966" xyz="0 0 0"/>
|
||||
</joint>
|
||||
<!-- ROS-Industrial 'tool0' frame: all-zeros tool frame -->
|
||||
<link name="tool1"/>
|
||||
<joint name="flange_tool_1" type="fixed">
|
||||
<!-- default toolframe: X+ left, Y+ up, Z+ front -->
|
||||
<origin rpy="1.5707963267948966 0 1.5707963267948966" xyz="0 0 0"/>
|
||||
<parent link="flange_1"/>
|
||||
<child link="tool1"/>
|
||||
</joint>
|
||||
|
||||
|
||||
</robot>
|
||||
1035
src/curobo/content/assets/robot/ur_description/quad_ur10e.urdf
Normal file
1035
src/curobo/content/assets/robot/ur_description/quad_ur10e.urdf
Normal file
File diff suppressed because it is too large
Load Diff
953
src/curobo/content/assets/robot/ur_description/tri_ur10e.urdf
Normal file
953
src/curobo/content/assets/robot/ur_description/tri_ur10e.urdf
Normal file
@@ -0,0 +1,953 @@
|
||||
<?xml version="1.0" ?>
|
||||
<!-- =================================================================================== -->
|
||||
<!-- | This document was autogenerated by xacro from ur_description/urdf/ur10e.xacro | -->
|
||||
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||
<!-- =================================================================================== -->
|
||||
<robot name="ur10e_robot">
|
||||
<!--
|
||||
Base UR robot series xacro macro.
|
||||
|
||||
NOTE: this is NOT a URDF. It cannot directly be loaded by consumers
|
||||
expecting a flattened '.urdf' file. See the top-level '.xacro' for that
|
||||
(but note: that .xacro must still be processed by the xacro command).
|
||||
|
||||
For use in '.launch' files: use one of the 'load_urX.launch' convenience
|
||||
launch files.
|
||||
|
||||
This file models the base kinematic chain of a UR robot, which then gets
|
||||
parameterised by various configuration files to convert it into a UR3(e),
|
||||
UR5(e), UR10(e) or UR16e.
|
||||
|
||||
NOTE: the default kinematic parameters (ie: link lengths, frame locations,
|
||||
offets, etc) do not correspond to any particular robot. They are defaults
|
||||
only. There WILL be non-zero offsets between the Forward Kinematics results
|
||||
in TF (ie: robot_state_publisher) and the values reported by the Teach
|
||||
Pendant.
|
||||
|
||||
For accurate (and robot-specific) transforms, the 'kinematics_parameters_file'
|
||||
parameter MUST point to a .yaml file containing the appropriate values for
|
||||
the targetted robot.
|
||||
|
||||
If using the UniversalRobots/Universal_Robots_ROS_Driver, follow the steps
|
||||
described in the readme of that repository to extract the kinematic
|
||||
calibration from the controller and generate the required .yaml file.
|
||||
|
||||
Main author of the migration to yaml configs: Ludovic Delval.
|
||||
|
||||
Contributors to previous versions (in no particular order):
|
||||
|
||||
- Felix Messmer
|
||||
- Kelsey Hawkins
|
||||
- Wim Meeussen
|
||||
- Shaun Edwards
|
||||
- Nadia Hammoudeh Garcia
|
||||
- Dave Hershberger
|
||||
- G. vd. Hoorn
|
||||
- Philip Long
|
||||
- Dave Coleman
|
||||
- Miguel Prada
|
||||
- Mathias Luedtke
|
||||
- Marcel Schnirring
|
||||
- Felix von Drigalski
|
||||
- Felix Exner
|
||||
- Jimmy Da Silva
|
||||
- Ajit Krisshna N L
|
||||
- Muhammad Asif Rana
|
||||
-->
|
||||
<!--
|
||||
NOTE: the macro defined in this file is NOT part of the public API of this
|
||||
package. Users CANNOT rely on this file being available, or stored in
|
||||
this location. Nor can they rely on the existence of the macro.
|
||||
-->
|
||||
|
||||
<!-- links: main serial chain -->
|
||||
<link name="base_fixture_link"/>
|
||||
<link name="base_link"/>
|
||||
|
||||
<link name="base_link_1"/>
|
||||
<link name="base_link_2"/>
|
||||
|
||||
|
||||
<!-- joints: main serial chain -->
|
||||
<joint name="base_fixture_j_base_link" type="fixed">
|
||||
<parent link="base_fixture_link"/>
|
||||
<child link="base_link"/>
|
||||
<origin rpy="0 0 0" xyz="0.75 0.75 0"/>
|
||||
</joint>
|
||||
|
||||
|
||||
<!-- joints: main serial chain -->
|
||||
<joint name="base_fixture_j_base_link_1" type="fixed">
|
||||
<parent link="base_fixture_link"/>
|
||||
<child link="base_link_1"/>
|
||||
<origin rpy="0 0 0.0" xyz="-0.75 0.75 0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joints: main serial chain -->
|
||||
<joint name="base_fixture_j_base_link_2" type="fixed">
|
||||
<parent link="base_fixture_link"/>
|
||||
<child link="base_link_2"/>
|
||||
<origin rpy="0 0 3.14" xyz="0.0 -0.75 0"/>
|
||||
</joint>
|
||||
|
||||
|
||||
|
||||
|
||||
<link name="base_link_inertia">
|
||||
<visual>
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/base.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/base.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="4.0"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="0.0061063308908" ixy="0.0" ixz="0.0" iyy="0.0061063308908" iyz="0.0" izz="0.01125"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="base_link_base_link_inertia" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="base_link_inertia"/>
|
||||
<!-- 'base_link' is REP-103 aligned (so X+ forward), while the internal
|
||||
frames of the robot/controller have X+ pointing backwards.
|
||||
Use the joint between 'base_link' and 'base_link_inertia' (a dummy
|
||||
link/frame) to introduce the necessary rotation over Z (of pi rad).
|
||||
-->
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<link name="shoulder_link">
|
||||
<visual>
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/shoulder.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/shoulder.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="7.778"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="0.03147431257693659" ixy="0.0" ixz="0.0" iyy="0.03147431257693659" iyz="0.0" izz="0.021875624999999996"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="upper_arm_link">
|
||||
<visual>
|
||||
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.1762"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/upperarm.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.1762"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/upperarm.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="12.93"/>
|
||||
<origin rpy="0 1.5707963267948966 0" xyz="-0.306 0.0 0.175"/>
|
||||
<inertia ixx="0.42175380379841093" ixy="0.0" ixz="0.0" iyy="0.42175380379841093" iyz="0.0" izz="0.03636562499999999"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="forearm_link">
|
||||
<visual>
|
||||
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.0393"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/forearm.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.0393"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/forearm.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="3.87"/>
|
||||
<origin rpy="0 1.5707963267948966 0" xyz="-0.285775 0.0 0.0393"/>
|
||||
<inertia ixx="0.11079302548902206" ixy="0.0" ixz="0.0" iyy="0.11079302548902206" iyz="0.0" izz="0.010884375"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="wrist_1_link">
|
||||
<visual>
|
||||
<!-- TODO: Move this to a parameter -->
|
||||
<origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.135"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/wrist1.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.135"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/wrist1.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1.96"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="0.005108247956699999" ixy="0.0" ixz="0.0" iyy="0.005108247956699999" iyz="0.0" izz="0.005512499999999999"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="wrist_2_link">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 -0.12"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/wrist2.dae"/>
|
||||
</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.12"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/wrist2.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1.96"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="0.005108247956699999" ixy="0.0" ixz="0.0" iyy="0.005108247956699999" iyz="0.0" izz="0.005512499999999999"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="wrist_3_link">
|
||||
<visual>
|
||||
<origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.1168"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/wrist3.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.1168"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/wrist3.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.202"/>
|
||||
<origin rpy="0 0 0" xyz="0.0 0.0 -0.025"/>
|
||||
<inertia ixx="0.00014434577559500002" ixy="0.0" ixz="0.0" iyy="0.00014434577559500002" iyz="0.0" izz="0.00020452500000000002"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
|
||||
<joint name="shoulder_pan_joint" type="revolute">
|
||||
<parent link="base_link_inertia"/>
|
||||
<child link="shoulder_link"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.1807"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="330.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="2.0943951023931953"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
<joint name="shoulder_lift_joint" type="revolute">
|
||||
<parent link="shoulder_link"/>
|
||||
<child link="upper_arm_link"/>
|
||||
<origin rpy="1.570796327 0 0" xyz="0 0 0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="330.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="2.0943951023931953"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
<joint name="elbow_joint" type="revolute">
|
||||
<parent link="upper_arm_link"/>
|
||||
<child link="forearm_link"/>
|
||||
<origin rpy="0 0 0" xyz="-0.6127 0 0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="150.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="3.141592653589793"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
<joint name="wrist_1_joint" type="revolute">
|
||||
<parent link="forearm_link"/>
|
||||
<child link="wrist_1_link"/>
|
||||
<origin rpy="0 0 0" xyz="-0.57155 0 0.17415"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
<joint name="wrist_2_joint" type="revolute">
|
||||
<parent link="wrist_1_link"/>
|
||||
<child link="wrist_2_link"/>
|
||||
<origin rpy="1.570796327 0 0" xyz="0 -0.11985 -2.458164590756244e-11"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
<joint name="wrist_3_joint" type="revolute">
|
||||
<parent link="wrist_2_link"/>
|
||||
<child link="wrist_3_link"/>
|
||||
<origin rpy="1.570796326589793 3.141592653589793 3.141592653589793" xyz="0 0.11655 -2.390480459346185e-11"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
|
||||
<link name="flange"/>
|
||||
<joint name="wrist_3-flange" type="fixed">
|
||||
<parent link="wrist_3_link"/>
|
||||
<child link="flange"/>
|
||||
<origin rpy="0 -1.5707963267948966 -1.5707963267948966" xyz="0 0 0"/>
|
||||
</joint>
|
||||
<!-- ROS-Industrial 'tool0' frame: all-zeros tool frame -->
|
||||
<link name="tool0"/>
|
||||
<joint name="flange-tool0" type="fixed">
|
||||
<!-- default toolframe: X+ left, Y+ up, Z+ front -->
|
||||
<origin rpy="1.5707963267948966 0 1.5707963267948966" xyz="0 0 0"/>
|
||||
<parent link="flange"/>
|
||||
<child link="tool0"/>
|
||||
</joint>
|
||||
<!-- ROS-Industrial 'base' frame: base_link to UR 'Base' Coordinates transform -->
|
||||
|
||||
|
||||
|
||||
<link name="base_link_inertia_link_1">
|
||||
<visual>
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/base.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/base.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="4.0"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="0.0061063308908" ixy="0.0" ixz="0.0" iyy="0.0061063308908" iyz="0.0" izz="0.01125"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="shoulder_link_1">
|
||||
<visual>
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/shoulder.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/shoulder.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="7.778"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="0.03147431257693659" ixy="0.0" ixz="0.0" iyy="0.03147431257693659" iyz="0.0" izz="0.021875624999999996"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="upper_arm_link_1">
|
||||
<visual>
|
||||
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.1762"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/upperarm.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.1762"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/upperarm.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="12.93"/>
|
||||
<origin rpy="0 1.5707963267948966 0" xyz="-0.306 0.0 0.175"/>
|
||||
<inertia ixx="0.42175380379841093" ixy="0.0" ixz="0.0" iyy="0.42175380379841093" iyz="0.0" izz="0.03636562499999999"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="forearm_link_1">
|
||||
<visual>
|
||||
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.0393"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/forearm.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.0393"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/forearm.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="3.87"/>
|
||||
<origin rpy="0 1.5707963267948966 0" xyz="-0.285775 0.0 0.0393"/>
|
||||
<inertia ixx="0.11079302548902206" ixy="0.0" ixz="0.0" iyy="0.11079302548902206" iyz="0.0" izz="0.010884375"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="wrist_1_link_1">
|
||||
<visual>
|
||||
<!-- TODO: Move this to a parameter -->
|
||||
<origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.135"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/wrist1.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.135"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/wrist1.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1.96"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="0.005108247956699999" ixy="0.0" ixz="0.0" iyy="0.005108247956699999" iyz="0.0" izz="0.005512499999999999"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="wrist_2_link_1">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 -0.12"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/wrist2.dae"/>
|
||||
</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.12"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/wrist2.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1.96"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="0.005108247956699999" ixy="0.0" ixz="0.0" iyy="0.005108247956699999" iyz="0.0" izz="0.005512499999999999"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="wrist_3_link_1">
|
||||
<visual>
|
||||
<origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.1168"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/wrist3.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.1168"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/wrist3.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.202"/>
|
||||
<origin rpy="0 0 0" xyz="0.0 0.0 -0.025"/>
|
||||
<inertia ixx="0.00014434577559500002" ixy="0.0" ixz="0.0" iyy="0.00014434577559500002" iyz="0.0" izz="0.00020452500000000002"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="base_link_base_link_inertia_joint_1" type="fixed">
|
||||
<parent link="base_link_1"/>
|
||||
<child link="base_link_inertia_link_1"/>
|
||||
<!-- 'base_link' is REP-103 aligned (so X+ forward), while the internal
|
||||
frames of the robot/controller have X+ pointing backwards.
|
||||
Use the joint between 'base_link' and 'base_link_inertia' (a dummy
|
||||
link/frame) to introduce the necessary rotation over Z (of pi rad).
|
||||
-->
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<joint name="shoulder_pan_joint_1" type="revolute">
|
||||
<parent link="base_link_inertia_link_1"/>
|
||||
<child link="shoulder_link_1"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.1807"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="330.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="2.0943951023931953"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
<joint name="shoulder_lift_joint_1" type="revolute">
|
||||
<parent link="shoulder_link_1"/>
|
||||
<child link="upper_arm_link_1"/>
|
||||
<origin rpy="1.570796327 0 0" xyz="0 0 0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="330.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="2.0943951023931953"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
<joint name="elbow_joint_1" type="revolute">
|
||||
<parent link="upper_arm_link_1"/>
|
||||
<child link="forearm_link_1"/>
|
||||
<origin rpy="0 0 0" xyz="-0.6127 0 0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="150.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="3.141592653589793"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
<joint name="wrist_1_joint_1" type="revolute">
|
||||
<parent link="forearm_link_1"/>
|
||||
<child link="wrist_1_link_1"/>
|
||||
<origin rpy="0 0 0" xyz="-0.57155 0 0.17415"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
<joint name="wrist_2_joint_1" type="revolute">
|
||||
<parent link="wrist_1_link_1"/>
|
||||
<child link="wrist_2_link_1"/>
|
||||
<origin rpy="1.570796327 0 0" xyz="0 -0.11985 -2.458164590756244e-11"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
<joint name="wrist_3_joint_1" type="revolute">
|
||||
<parent link="wrist_2_link_1"/>
|
||||
<child link="wrist_3_link_1"/>
|
||||
<origin rpy="1.570796326589793 3.141592653589793 3.141592653589793" xyz="0 0.11655 -2.390480459346185e-11"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- ROS-Industrial 'flange' frame: attachment point for EEF models -->
|
||||
<link name="flange_1"/>
|
||||
<joint name="wrist_3_flange_1" type="fixed">
|
||||
<parent link="wrist_3_link_1"/>
|
||||
<child link="flange_1"/>
|
||||
<origin rpy="0 -1.5707963267948966 -1.5707963267948966" xyz="0 0 0"/>
|
||||
</joint>
|
||||
<!-- ROS-Industrial 'tool0' frame: all-zeros tool frame -->
|
||||
<link name="tool1"/>
|
||||
<joint name="flange_tool_1" type="fixed">
|
||||
<!-- default toolframe: X+ left, Y+ up, Z+ front -->
|
||||
<origin rpy="1.5707963267948966 0 1.5707963267948966" xyz="0 0 0"/>
|
||||
<parent link="flange_1"/>
|
||||
<child link="tool1"/>
|
||||
</joint>
|
||||
|
||||
|
||||
<link name="base_link_inertia_link_2">
|
||||
<visual>
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/base.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/base.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="4.0"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="0.0061063308908" ixy="0.0" ixz="0.0" iyy="0.0061063308908" iyz="0.0" izz="0.01125"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="shoulder_link_2">
|
||||
<visual>
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/shoulder.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/shoulder.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="7.778"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="0.03147431257693659" ixy="0.0" ixz="0.0" iyy="0.03147431257693659" iyz="0.0" izz="0.021875624999999996"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="upper_arm_link_2">
|
||||
<visual>
|
||||
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.1762"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/upperarm.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.1762"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/upperarm.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="12.93"/>
|
||||
<origin rpy="0 1.5707963267948966 0" xyz="-0.306 0.0 0.175"/>
|
||||
<inertia ixx="0.42175380379841093" ixy="0.0" ixz="0.0" iyy="0.42175380379841093" iyz="0.0" izz="0.03636562499999999"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="forearm_link_2">
|
||||
<visual>
|
||||
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.0393"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/forearm.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.0393"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/forearm.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="3.87"/>
|
||||
<origin rpy="0 1.5707963267948966 0" xyz="-0.285775 0.0 0.0393"/>
|
||||
<inertia ixx="0.11079302548902206" ixy="0.0" ixz="0.0" iyy="0.11079302548902206" iyz="0.0" izz="0.010884375"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="wrist_1_link_2">
|
||||
<visual>
|
||||
<!-- TODO: Move this to a parameter -->
|
||||
<origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.135"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/wrist1.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.135"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/wrist1.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1.96"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="0.005108247956699999" ixy="0.0" ixz="0.0" iyy="0.005108247956699999" iyz="0.0" izz="0.005512499999999999"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="wrist_2_link_2">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 -0.12"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/wrist2.dae"/>
|
||||
</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.12"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/wrist2.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1.96"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="0.005108247956699999" ixy="0.0" ixz="0.0" iyy="0.005108247956699999" iyz="0.0" izz="0.005512499999999999"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="wrist_3_link_2">
|
||||
<visual>
|
||||
<origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.1168"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/wrist3.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.1168"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/wrist3.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.202"/>
|
||||
<origin rpy="0 0 0" xyz="0.0 0.0 -0.025"/>
|
||||
<inertia ixx="0.00014434577559500002" ixy="0.0" ixz="0.0" iyy="0.00014434577559500002" iyz="0.0" izz="0.00020452500000000002"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="base_link_base_link_inertia_joint_2" type="fixed">
|
||||
<parent link="base_link_2"/>
|
||||
<child link="base_link_inertia_link_2"/>
|
||||
<!-- 'base_link' is REP-103 aligned (so X+ forward), while the internal
|
||||
frames of the robot/controller have X+ pointing backwards.
|
||||
Use the joint between 'base_link' and 'base_link_inertia' (a dummy
|
||||
link/frame) to introduce the necessary rotation over Z (of pi rad).
|
||||
-->
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<joint name="shoulder_pan_joint_2" type="revolute">
|
||||
<parent link="base_link_inertia_link_2"/>
|
||||
<child link="shoulder_link_2"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.1807"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="330.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="2.0943951023931953"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
<joint name="shoulder_lift_joint_2" type="revolute">
|
||||
<parent link="shoulder_link_2"/>
|
||||
<child link="upper_arm_link_2"/>
|
||||
<origin rpy="1.570796327 0 0" xyz="0 0 0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="330.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="2.0943951023931953"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
<joint name="elbow_joint_2" type="revolute">
|
||||
<parent link="upper_arm_link_2"/>
|
||||
<child link="forearm_link_2"/>
|
||||
<origin rpy="0 0 0" xyz="-0.6127 0 0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="150.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="3.141592653589793"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
<joint name="wrist_1_joint_2" type="revolute">
|
||||
<parent link="forearm_link_2"/>
|
||||
<child link="wrist_1_link_2"/>
|
||||
<origin rpy="0 0 0" xyz="-0.57155 0 0.17415"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
<joint name="wrist_2_joint_2" type="revolute">
|
||||
<parent link="wrist_1_link_2"/>
|
||||
<child link="wrist_2_link_2"/>
|
||||
<origin rpy="1.570796327 0 0" xyz="0 -0.11985 -2.458164590756244e-11"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
<joint name="wrist_3_joint_2" type="revolute">
|
||||
<parent link="wrist_2_link_2"/>
|
||||
<child link="wrist_3_link_2"/>
|
||||
<origin rpy="1.570796326589793 3.141592653589793 3.141592653589793" xyz="0 0.11655 -2.390480459346185e-11"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- ROS-Industrial 'flange' frame: attachment point for EEF models -->
|
||||
<link name="flange_2"/>
|
||||
<joint name="wrist_3_flange_joint_2" type="fixed">
|
||||
<parent link="wrist_3_link_2"/>
|
||||
<child link="flange_2"/>
|
||||
<origin rpy="0 -1.5707963267948966 -1.5707963267948966" xyz="0 0 0"/>
|
||||
</joint>
|
||||
<!-- ROS-Industrial 'tool0' frame: all-zeros tool frame -->
|
||||
<link name="tool2"/>
|
||||
<joint name="flange_tool_joint_2" type="fixed">
|
||||
<!-- default toolframe: X+ left, Y+ up, Z+ front -->
|
||||
<origin rpy="1.5707963267948966 0 1.5707963267948966" xyz="0 0 0"/>
|
||||
<parent link="flange_2"/>
|
||||
<child link="tool2"/>
|
||||
</joint>
|
||||
|
||||
|
||||
<link name="base_link_inertia_link_3">
|
||||
<visual>
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/base.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/base.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="4.0"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="0.0061063308908" ixy="0.0" ixz="0.0" iyy="0.0061063308908" iyz="0.0" izz="0.01125"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="shoulder_link_3">
|
||||
<visual>
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/shoulder.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/shoulder.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="7.778"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="0.03147431257693659" ixy="0.0" ixz="0.0" iyy="0.03147431257693659" iyz="0.0" izz="0.021875624999999996"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="upper_arm_link_3">
|
||||
<visual>
|
||||
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.1762"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/upperarm.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.1762"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/upperarm.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="12.93"/>
|
||||
<origin rpy="0 1.5707963267948966 0" xyz="-0.306 0.0 0.175"/>
|
||||
<inertia ixx="0.42175380379841093" ixy="0.0" ixz="0.0" iyy="0.42175380379841093" iyz="0.0" izz="0.03636562499999999"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="forearm_link_3">
|
||||
<visual>
|
||||
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.0393"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/forearm.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.0393"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/forearm.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="3.87"/>
|
||||
<origin rpy="0 1.5707963267948966 0" xyz="-0.285775 0.0 0.0393"/>
|
||||
<inertia ixx="0.11079302548902206" ixy="0.0" ixz="0.0" iyy="0.11079302548902206" iyz="0.0" izz="0.010884375"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="wrist_1_link_3">
|
||||
<visual>
|
||||
<!-- TODO: Move this to a parameter -->
|
||||
<origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.135"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/wrist1.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.135"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/wrist1.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1.96"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="0.005108247956699999" ixy="0.0" ixz="0.0" iyy="0.005108247956699999" iyz="0.0" izz="0.005512499999999999"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="wrist_2_link_3">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 -0.12"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/wrist2.dae"/>
|
||||
</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.12"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/wrist2.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1.96"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="0.005108247956699999" ixy="0.0" ixz="0.0" iyy="0.005108247956699999" iyz="0.0" izz="0.005512499999999999"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="wrist_3_link_3">
|
||||
<visual>
|
||||
<origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.1168"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/wrist3.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.1168"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/wrist3.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.202"/>
|
||||
<origin rpy="0 0 0" xyz="0.0 0.0 -0.025"/>
|
||||
<inertia ixx="0.00014434577559500002" ixy="0.0" ixz="0.0" iyy="0.00014434577559500002" iyz="0.0" izz="0.00020452500000000002"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
</robot>
|
||||
359
src/curobo/content/assets/robot/ur_description/ur10e.urdf
Normal file
359
src/curobo/content/assets/robot/ur_description/ur10e.urdf
Normal file
@@ -0,0 +1,359 @@
|
||||
<?xml version="1.0" ?>
|
||||
<!-- =================================================================================== -->
|
||||
<!-- | This document was autogenerated by xacro from ur_description/urdf/ur10e.xacro | -->
|
||||
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||
<!-- =================================================================================== -->
|
||||
<robot name="ur10e_robot">
|
||||
<!--
|
||||
Base UR robot series xacro macro.
|
||||
|
||||
NOTE: this is NOT a URDF. It cannot directly be loaded by consumers
|
||||
expecting a flattened '.urdf' file. See the top-level '.xacro' for that
|
||||
(but note: that .xacro must still be processed by the xacro command).
|
||||
|
||||
For use in '.launch' files: use one of the 'load_urX.launch' convenience
|
||||
launch files.
|
||||
|
||||
This file models the base kinematic chain of a UR robot, which then gets
|
||||
parameterised by various configuration files to convert it into a UR3(e),
|
||||
UR5(e), UR10(e) or UR16e.
|
||||
|
||||
NOTE: the default kinematic parameters (ie: link lengths, frame locations,
|
||||
offets, etc) do not correspond to any particular robot. They are defaults
|
||||
only. There WILL be non-zero offsets between the Forward Kinematics results
|
||||
in TF (ie: robot_state_publisher) and the values reported by the Teach
|
||||
Pendant.
|
||||
|
||||
For accurate (and robot-specific) transforms, the 'kinematics_parameters_file'
|
||||
parameter MUST point to a .yaml file containing the appropriate values for
|
||||
the targetted robot.
|
||||
|
||||
If using the UniversalRobots/Universal_Robots_ROS_Driver, follow the steps
|
||||
described in the readme of that repository to extract the kinematic
|
||||
calibration from the controller and generate the required .yaml file.
|
||||
|
||||
Main author of the migration to yaml configs: Ludovic Delval.
|
||||
|
||||
Contributors to previous versions (in no particular order):
|
||||
|
||||
- Felix Messmer
|
||||
- Kelsey Hawkins
|
||||
- Wim Meeussen
|
||||
- Shaun Edwards
|
||||
- Nadia Hammoudeh Garcia
|
||||
- Dave Hershberger
|
||||
- G. vd. Hoorn
|
||||
- Philip Long
|
||||
- Dave Coleman
|
||||
- Miguel Prada
|
||||
- Mathias Luedtke
|
||||
- Marcel Schnirring
|
||||
- Felix von Drigalski
|
||||
- Felix Exner
|
||||
- Jimmy Da Silva
|
||||
- Ajit Krisshna N L
|
||||
- Muhammad Asif Rana
|
||||
-->
|
||||
<!--
|
||||
NOTE: the macro defined in this file is NOT part of the public API of this
|
||||
package. Users CANNOT rely on this file being available, or stored in
|
||||
this location. Nor can they rely on the existence of the macro.
|
||||
-->
|
||||
<transmission name="shoulder_pan_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="shoulder_pan_joint">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="shoulder_pan_motor">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="shoulder_lift_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="shoulder_lift_joint">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="shoulder_lift_motor">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="elbow_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="elbow_joint">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="elbow_motor">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="wrist_1_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="wrist_1_joint">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="wrist_1_motor">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="wrist_2_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="wrist_2_joint">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="wrist_2_motor">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="wrist_3_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="wrist_3_joint">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="wrist_3_motor">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<!-- links: main serial chain -->
|
||||
<link name="base_link"/>
|
||||
<link name="base_link_inertia">
|
||||
<visual>
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/base.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/base.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="4.0"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="0.0061063308908" ixy="0.0" ixz="0.0" iyy="0.0061063308908" iyz="0.0" izz="0.01125"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="shoulder_link">
|
||||
<visual>
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/shoulder.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/shoulder.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="7.778"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="0.03147431257693659" ixy="0.0" ixz="0.0" iyy="0.03147431257693659" iyz="0.0" izz="0.021875624999999996"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="upper_arm_link">
|
||||
<visual>
|
||||
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.1762"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/upperarm.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.1762"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/upperarm.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="12.93"/>
|
||||
<origin rpy="0 1.5707963267948966 0" xyz="-0.306 0.0 0.175"/>
|
||||
<inertia ixx="0.42175380379841093" ixy="0.0" ixz="0.0" iyy="0.42175380379841093" iyz="0.0" izz="0.03636562499999999"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="forearm_link">
|
||||
<visual>
|
||||
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.0393"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/forearm.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.0393"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/forearm.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="3.87"/>
|
||||
<origin rpy="0 1.5707963267948966 0" xyz="-0.285775 0.0 0.0393"/>
|
||||
<inertia ixx="0.11079302548902206" ixy="0.0" ixz="0.0" iyy="0.11079302548902206" iyz="0.0" izz="0.010884375"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="wrist_1_link">
|
||||
<visual>
|
||||
<!-- TODO: Move this to a parameter -->
|
||||
<origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.135"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/wrist1.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.135"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/wrist1.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1.96"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="0.005108247956699999" ixy="0.0" ixz="0.0" iyy="0.005108247956699999" iyz="0.0" izz="0.005512499999999999"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="wrist_2_link">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 -0.12"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/wrist2.dae"/>
|
||||
</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.12"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/wrist2.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1.96"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="0.005108247956699999" ixy="0.0" ixz="0.0" iyy="0.005108247956699999" iyz="0.0" izz="0.005512499999999999"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="wrist_3_link">
|
||||
<visual>
|
||||
<origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.1168"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/visual/wrist3.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.1168"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur10e/collision/wrist3.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.202"/>
|
||||
<origin rpy="0 0 0" xyz="0.0 0.0 -0.025"/>
|
||||
<inertia ixx="0.00014434577559500002" ixy="0.0" ixz="0.0" iyy="0.00014434577559500002" iyz="0.0" izz="0.00020452500000000002"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<!-- joints: main serial chain -->
|
||||
<joint name="base_link-base_link_inertia" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="base_link_inertia"/>
|
||||
<!-- 'base_link' is REP-103 aligned (so X+ forward), while the internal
|
||||
frames of the robot/controller have X+ pointing backwards.
|
||||
Use the joint between 'base_link' and 'base_link_inertia' (a dummy
|
||||
link/frame) to introduce the necessary rotation over Z (of pi rad).
|
||||
-->
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
</joint>
|
||||
<joint name="shoulder_pan_joint" type="revolute">
|
||||
<parent link="base_link_inertia"/>
|
||||
<child link="shoulder_link"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.1807"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="330.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="2.0943951023931953"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
<joint name="shoulder_lift_joint" type="revolute">
|
||||
<parent link="shoulder_link"/>
|
||||
<child link="upper_arm_link"/>
|
||||
<origin rpy="1.570796327 0 0" xyz="0 0 0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="330.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="2.0943951023931953"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
<joint name="elbow_joint" type="revolute">
|
||||
<parent link="upper_arm_link"/>
|
||||
<child link="forearm_link"/>
|
||||
<origin rpy="0 0 0" xyz="-0.6127 0 0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="150.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="3.141592653589793"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
<joint name="wrist_1_joint" type="revolute">
|
||||
<parent link="forearm_link"/>
|
||||
<child link="wrist_1_link"/>
|
||||
<origin rpy="0 0 0" xyz="-0.57155 0 0.17415"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
<joint name="wrist_2_joint" type="revolute">
|
||||
<parent link="wrist_1_link"/>
|
||||
<child link="wrist_2_link"/>
|
||||
<origin rpy="1.570796327 0 0" xyz="0 -0.11985 -2.458164590756244e-11"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
<joint name="wrist_3_joint" type="revolute">
|
||||
<parent link="wrist_2_link"/>
|
||||
<child link="wrist_3_link"/>
|
||||
<origin rpy="1.570796326589793 3.141592653589793 3.141592653589793" xyz="0 0.11655 -2.390480459346185e-11"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
<!-- ROS-Industrial 'base' frame: base_link to UR 'Base' Coordinates transform -->
|
||||
<link name="base"/>
|
||||
<joint name="base_link-base_fixed_joint" type="fixed">
|
||||
<!-- Note the rotation over Z of pi radians: as base_link is REP-103
|
||||
aligned (ie: has X+ forward, Y+ left and Z+ up), this is needed
|
||||
to correctly align 'base' with the 'Base' coordinate system of
|
||||
the UR controller.
|
||||
-->
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="base"/>
|
||||
</joint>
|
||||
<!-- ROS-Industrial 'flange' frame: attachment point for EEF models -->
|
||||
<link name="flange"/>
|
||||
<joint name="wrist_3-flange" type="fixed">
|
||||
<parent link="wrist_3_link"/>
|
||||
<child link="flange"/>
|
||||
<origin rpy="0 -1.5707963267948966 -1.5707963267948966" xyz="0 0 0"/>
|
||||
</joint>
|
||||
<!-- ROS-Industrial 'tool0' frame: all-zeros tool frame -->
|
||||
<link name="tool0"/>
|
||||
<joint name="flange-tool0" type="fixed">
|
||||
<!-- default toolframe: X+ left, Y+ up, Z+ front -->
|
||||
<origin rpy="1.5707963267948966 0 1.5707963267948966" xyz="0 0 0"/>
|
||||
<parent link="flange"/>
|
||||
<child link="tool0"/>
|
||||
</joint>
|
||||
</robot>
|
||||
359
src/curobo/content/assets/robot/ur_description/ur5e.urdf
Normal file
359
src/curobo/content/assets/robot/ur_description/ur5e.urdf
Normal file
@@ -0,0 +1,359 @@
|
||||
<?xml version="1.0" ?>
|
||||
<!-- =================================================================================== -->
|
||||
<!-- | This document was autogenerated by xacro from ur_description/urdf/ur5e.xacro | -->
|
||||
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||
<!-- =================================================================================== -->
|
||||
<robot name="ur5e_robot">
|
||||
<!--
|
||||
Base UR robot series xacro macro.
|
||||
|
||||
NOTE: this is NOT a URDF. It cannot directly be loaded by consumers
|
||||
expecting a flattened '.urdf' file. See the top-level '.xacro' for that
|
||||
(but note: that .xacro must still be processed by the xacro command).
|
||||
|
||||
For use in '.launch' files: use one of the 'load_urX.launch' convenience
|
||||
launch files.
|
||||
|
||||
This file models the base kinematic chain of a UR robot, which then gets
|
||||
parameterised by various configuration files to convert it into a UR3(e),
|
||||
UR5(e), UR10(e) or UR16e.
|
||||
|
||||
NOTE: the default kinematic parameters (ie: link lengths, frame locations,
|
||||
offets, etc) do not correspond to any particular robot. They are defaults
|
||||
only. There WILL be non-zero offsets between the Forward Kinematics results
|
||||
in TF (ie: robot_state_publisher) and the values reported by the Teach
|
||||
Pendant.
|
||||
|
||||
For accurate (and robot-specific) transforms, the 'kinematics_parameters_file'
|
||||
parameter MUST point to a .yaml file containing the appropriate values for
|
||||
the targetted robot.
|
||||
|
||||
If using the UniversalRobots/Universal_Robots_ROS_Driver, follow the steps
|
||||
described in the readme of that repository to extract the kinematic
|
||||
calibration from the controller and generate the required .yaml file.
|
||||
|
||||
Main author of the migration to yaml configs: Ludovic Delval.
|
||||
|
||||
Contributors to previous versions (in no particular order):
|
||||
|
||||
- Felix Messmer
|
||||
- Kelsey Hawkins
|
||||
- Wim Meeussen
|
||||
- Shaun Edwards
|
||||
- Nadia Hammoudeh Garcia
|
||||
- Dave Hershberger
|
||||
- G. vd. Hoorn
|
||||
- Philip Long
|
||||
- Dave Coleman
|
||||
- Miguel Prada
|
||||
- Mathias Luedtke
|
||||
- Marcel Schnirring
|
||||
- Felix von Drigalski
|
||||
- Felix Exner
|
||||
- Jimmy Da Silva
|
||||
- Ajit Krisshna N L
|
||||
- Muhammad Asif Rana
|
||||
-->
|
||||
<!--
|
||||
NOTE: the macro defined in this file is NOT part of the public API of this
|
||||
package. Users CANNOT rely on this file being available, or stored in
|
||||
this location. Nor can they rely on the existence of the macro.
|
||||
-->
|
||||
<transmission name="shoulder_pan_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="shoulder_pan_joint">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="shoulder_pan_motor">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="shoulder_lift_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="shoulder_lift_joint">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="shoulder_lift_motor">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="elbow_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="elbow_joint">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="elbow_motor">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="wrist_1_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="wrist_1_joint">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="wrist_1_motor">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="wrist_2_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="wrist_2_joint">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="wrist_2_motor">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="wrist_3_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="wrist_3_joint">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="wrist_3_motor">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<!-- links: main serial chain -->
|
||||
<link name="base_link"/>
|
||||
<link name="base_link_inertia">
|
||||
<visual>
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur5e/visual/base.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur5e/collision/base.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="4.0"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="0.00443333156" ixy="0.0" ixz="0.0" iyy="0.00443333156" iyz="0.0" izz="0.0072"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="shoulder_link">
|
||||
<visual>
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur5e/visual/shoulder.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur5e/collision/shoulder.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="3.7"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="0.010267495893" ixy="0.0" ixz="0.0" iyy="0.010267495893" iyz="0.0" izz="0.00666"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="upper_arm_link">
|
||||
<visual>
|
||||
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.138"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur5e/visual/upperarm.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.138"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur5e/collision/upperarm.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="8.393"/>
|
||||
<origin rpy="0 1.5707963267948966 0" xyz="-0.2125 0.0 0.138"/>
|
||||
<inertia ixx="0.1338857818623325" ixy="0.0" ixz="0.0" iyy="0.1338857818623325" iyz="0.0" izz="0.0151074"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="forearm_link">
|
||||
<visual>
|
||||
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.007"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur5e/visual/forearm.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.007"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur5e/collision/forearm.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="2.275"/>
|
||||
<origin rpy="0 1.5707963267948966 0" xyz="-0.1961 0.0 0.007"/>
|
||||
<inertia ixx="0.031209355099586295" ixy="0.0" ixz="0.0" iyy="0.031209355099586295" iyz="0.0" izz="0.004095"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="wrist_1_link">
|
||||
<visual>
|
||||
<!-- TODO: Move this to a parameter -->
|
||||
<origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.127"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur5e/visual/wrist1.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.127"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur5e/collision/wrist1.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1.219"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="0.0025598989760400002" ixy="0.0" ixz="0.0" iyy="0.0025598989760400002" iyz="0.0" izz="0.0021942"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="wrist_2_link">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 -0.0997"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur5e/visual/wrist2.dae"/>
|
||||
</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.0997"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur5e/collision/wrist2.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1.219"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="0.0025598989760400002" ixy="0.0" ixz="0.0" iyy="0.0025598989760400002" iyz="0.0" izz="0.0021942"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="wrist_3_link">
|
||||
<visual>
|
||||
<origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.0989"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur5e/visual/wrist3.dae"/>
|
||||
</geometry>
|
||||
<material name="LightGrey">
|
||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.0989"/>
|
||||
<geometry>
|
||||
<mesh filename="meshes/ur5e/collision/wrist3.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.1879"/>
|
||||
<origin rpy="0 0 0" xyz="0.0 0.0 -0.0229"/>
|
||||
<inertia ixx="9.890410052167731e-05" ixy="0.0" ixz="0.0" iyy="9.890410052167731e-05" iyz="0.0" izz="0.0001321171875"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<!-- joints: main serial chain -->
|
||||
<joint name="base_link-base_link_inertia" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="base_link_inertia"/>
|
||||
<!-- 'base_link' is REP-103 aligned (so X+ forward), while the internal
|
||||
frames of the robot/controller have X+ pointing backwards.
|
||||
Use the joint between 'base_link' and 'base_link_inertia' (a dummy
|
||||
link/frame) to introduce the necessary rotation over Z (of pi rad).
|
||||
-->
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
</joint>
|
||||
<joint name="shoulder_pan_joint" type="revolute">
|
||||
<parent link="base_link_inertia"/>
|
||||
<child link="shoulder_link"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.1625"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="150.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
<joint name="shoulder_lift_joint" type="revolute">
|
||||
<parent link="shoulder_link"/>
|
||||
<child link="upper_arm_link"/>
|
||||
<origin rpy="1.570796327 0 0" xyz="0 0 0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="150.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
<joint name="elbow_joint" type="revolute">
|
||||
<parent link="upper_arm_link"/>
|
||||
<child link="forearm_link"/>
|
||||
<origin rpy="0 0 0" xyz="-0.425 0 0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="150.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="3.141592653589793"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
<joint name="wrist_1_joint" type="revolute">
|
||||
<parent link="forearm_link"/>
|
||||
<child link="wrist_1_link"/>
|
||||
<origin rpy="0 0 0" xyz="-0.3922 0 0.1333"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="28.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
<joint name="wrist_2_joint" type="revolute">
|
||||
<parent link="wrist_1_link"/>
|
||||
<child link="wrist_2_link"/>
|
||||
<origin rpy="1.570796327 0 0" xyz="0 -0.0997 -2.044881182297852e-11"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="28.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
<joint name="wrist_3_joint" type="revolute">
|
||||
<parent link="wrist_2_link"/>
|
||||
<child link="wrist_3_link"/>
|
||||
<origin rpy="1.570796326589793 3.141592653589793 3.141592653589793" xyz="0 0.0996 -2.042830148012698e-11"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="28.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
</joint>
|
||||
<!-- ROS-Industrial 'base' frame: base_link to UR 'Base' Coordinates transform -->
|
||||
<link name="base"/>
|
||||
<joint name="base_link-base_fixed_joint" type="fixed">
|
||||
<!-- Note the rotation over Z of pi radians: as base_link is REP-103
|
||||
aligned (ie: has X+ forward, Y+ left and Z+ up), this is needed
|
||||
to correctly align 'base' with the 'Base' coordinate system of
|
||||
the UR controller.
|
||||
-->
|
||||
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="base"/>
|
||||
</joint>
|
||||
<!-- ROS-Industrial 'flange' frame: attachment point for EEF models -->
|
||||
<link name="flange"/>
|
||||
<joint name="wrist_3-flange" type="fixed">
|
||||
<parent link="wrist_3_link"/>
|
||||
<child link="flange"/>
|
||||
<origin rpy="0 -1.5707963267948966 -1.5707963267948966" xyz="0 0 0"/>
|
||||
</joint>
|
||||
<!-- ROS-Industrial 'tool0' frame: all-zeros tool frame -->
|
||||
<link name="tool0"/>
|
||||
<joint name="flange-tool0" type="fixed">
|
||||
<!-- default toolframe: X+ left, Y+ up, Z+ front -->
|
||||
<origin rpy="1.5707963267948966 0 1.5707963267948966" xyz="0 0 0"/>
|
||||
<parent link="flange"/>
|
||||
<child link="tool0"/>
|
||||
</joint>
|
||||
</robot>
|
||||
@@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:93892dddfc6a65dedc4ef311e773eba7273b29f0a2cdcb76637013fdadf22897
|
||||
size 87195648
|
||||
@@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:4b82b0a4873d7bf1796893a47f9b41ab7cd32751d8caa102cfea3f401f75dd70
|
||||
size 1773918
|
||||
63
src/curobo/content/configs/robot/dual_ur10e.yml
Normal file
63
src/curobo/content/configs/robot/dual_ur10e.yml
Normal file
@@ -0,0 +1,63 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
|
||||
robot_cfg:
|
||||
kinematics:
|
||||
use_usd_kinematics: False
|
||||
urdf_path: "robot/ur_description/dual_ur10e.urdf"
|
||||
asset_root_path: "robot/ur_description"
|
||||
isaac_usd_path: "/Isaac/Robots/UR10/ur10_long_suction.usd"
|
||||
usd_robot_root: "/ur10"
|
||||
usd_path: "robot/ur_description/dual_ur10e.usd"
|
||||
base_link: "base_fixture_link"
|
||||
ee_link: "tool0"
|
||||
link_names: [ "tool1", "tool0"] # , "tool2"] #, "tool3"]
|
||||
collision_link_names: [
|
||||
'shoulder_link','upper_arm_link', 'forearm_link', 'wrist_1_link', 'wrist_2_link' ,'wrist_3_link', 'tool0',
|
||||
'shoulder_link_1','upper_arm_link_1', 'forearm_link_1', 'wrist_1_link_1', 'wrist_2_link_1' ,'wrist_3_link_1', 'tool1']
|
||||
collision_spheres: 'spheres/dual_ur10e.yml'
|
||||
collision_sphere_buffer: 0.005
|
||||
|
||||
|
||||
self_collision_ignore: {
|
||||
"upper_arm_link": ["shoulder_link","forearm_link"],
|
||||
"forarm_link": ["wrist_1_link"],
|
||||
"wrist_1_link": ["wrist_2_link","wrist_3_link"],
|
||||
"wrist_2_link": ["wrist_3_link", "tool0"],
|
||||
"wrist_3_link": ["tool0"],
|
||||
|
||||
"upper_arm_link_1": ["shoulder_link_1","forearm_link_1"],
|
||||
"forarm_link_1": ["wrist_1_link_1"],
|
||||
"wrist_1_link_1": ["wrist_2_link_1","wrist_3_link_1"],
|
||||
"wrist_2_link_1": ["wrist_3_link_1", "tool1"],
|
||||
"wrist_3_link_1": ["tool1"],
|
||||
|
||||
}
|
||||
self_collision_buffer: {
|
||||
'shoulder_link': 0.05,
|
||||
'shoulder_link_1': 0.05,
|
||||
|
||||
}
|
||||
lock_joints: null
|
||||
|
||||
cspace:
|
||||
joint_names: [
|
||||
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint',
|
||||
'shoulder_pan_joint_1', 'shoulder_lift_joint_1', 'elbow_joint_1', 'wrist_1_joint_1', 'wrist_2_joint_1', 'wrist_3_joint_1']
|
||||
retract_config: [-1.57, -2.2, 1.9, -1.383, -1.57, 0.00,
|
||||
-1.57, -2.2, 1.9, -1.383, -1.57, 0.00]
|
||||
null_space_weight: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
|
||||
1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
|
||||
cspace_distance_weight: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
|
||||
1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
|
||||
|
||||
max_jerk: 500.0
|
||||
max_acceleration: 15.0
|
||||
113
src/curobo/content/configs/robot/franka.yml
Normal file
113
src/curobo/content/configs/robot/franka.yml
Normal file
@@ -0,0 +1,113 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
|
||||
robot_cfg:
|
||||
kinematics:
|
||||
use_usd_kinematics: False
|
||||
isaac_usd_path: "/Isaac/Robots/Franka/franka.usd"
|
||||
usd_path: "robot/non_shipping/franka/franka_panda_meters1.usda"
|
||||
usd_robot_root: "/panda"
|
||||
usd_flip_joints: ["panda_joint1","panda_joint2","panda_joint3","panda_joint4", "panda_joint5",
|
||||
"panda_joint6","panda_joint7","panda_finger_joint1", "panda_finger_joint2"]
|
||||
usd_flip_joints: {
|
||||
"panda_joint1": "Z",
|
||||
"panda_joint2": "Z",
|
||||
"panda_joint3": "Z",
|
||||
"panda_joint4": "Z",
|
||||
"panda_joint5": "Z",
|
||||
"panda_joint6": "Z",
|
||||
"panda_joint7": "Z",
|
||||
"panda_finger_joint1": "Y",
|
||||
"panda_finger_joint2": "Y",
|
||||
}
|
||||
usd_flip_joint_limits: ["panda_finger_joint2"]
|
||||
urdf_path: "robot/franka_description/franka_panda.urdf"
|
||||
asset_root_path: "robot/franka_description"
|
||||
base_link: "base_link"
|
||||
ee_link: "panda_hand"
|
||||
collision_link_names:
|
||||
[
|
||||
"panda_link0",
|
||||
"panda_link1",
|
||||
"panda_link2",
|
||||
"panda_link3",
|
||||
"panda_link4",
|
||||
"panda_link5",
|
||||
"panda_link6",
|
||||
"panda_link7",
|
||||
"panda_hand",
|
||||
"panda_leftfinger",
|
||||
"panda_rightfinger",
|
||||
"attached_object",
|
||||
]
|
||||
collision_spheres: "spheres/franka_mesh.yml"
|
||||
collision_sphere_buffer: 0.0025
|
||||
extra_collision_spheres: {"attached_object": 4}
|
||||
use_global_cumul: True
|
||||
self_collision_ignore:
|
||||
{
|
||||
"panda_link0": ["panda_link1", "panda_link2"],
|
||||
"panda_link1": ["panda_link2", "panda_link3", "panda_link4"],
|
||||
"panda_link2": ["panda_link3", "panda_link4"],
|
||||
"panda_link3": ["panda_link4", "panda_link6"],
|
||||
"panda_link4":
|
||||
["panda_link5", "panda_link6", "panda_link7", "panda_link8"],
|
||||
"panda_link5": ["panda_link6", "panda_link7", "panda_hand","panda_leftfinger", "panda_rightfinger"],
|
||||
"panda_link6": ["panda_link7", "panda_hand", "attached_object", "panda_leftfinger", "panda_rightfinger"],
|
||||
"panda_link7": ["panda_hand", "attached_object", "panda_leftfinger", "panda_rightfinger"],
|
||||
"panda_hand": ["panda_leftfinger", "panda_rightfinger","attached_object"],
|
||||
"panda_leftfinger": ["panda_rightfinger", "attached_object"],
|
||||
"panda_rightfinger": ["attached_object"],
|
||||
|
||||
}
|
||||
|
||||
self_collision_buffer:
|
||||
{
|
||||
"panda_link0": 0.1,
|
||||
"panda_link1": 0.05,
|
||||
"panda_link2": 0.0,
|
||||
"panda_link3": 0.0,
|
||||
"panda_link4": 0.0,
|
||||
"panda_link5": 0.0,
|
||||
"panda_link6": 0.0,
|
||||
"panda_link7": 0.0,
|
||||
"panda_hand": 0.02,
|
||||
"panda_leftfinger": 0.01,
|
||||
"panda_rightfinger": 0.01,
|
||||
"attached_object": 0.0,
|
||||
}
|
||||
|
||||
mesh_link_names:
|
||||
[
|
||||
"panda_link0",
|
||||
"panda_link1",
|
||||
"panda_link2",
|
||||
"panda_link3",
|
||||
"panda_link4",
|
||||
"panda_link5",
|
||||
"panda_link6",
|
||||
"panda_link7",
|
||||
"panda_hand",
|
||||
"panda_leftfinger",
|
||||
"panda_rightfinger",
|
||||
]
|
||||
lock_joints: {"panda_finger_joint1": 0.04, "panda_finger_joint2": -0.04}
|
||||
extra_links: {"attached_object":{"parent_link_name": "panda_hand" ,
|
||||
"link_name": "attached_object", "fixed_transform": [0,0,0,1,0,0,0], "joint_type":"FIXED",
|
||||
"joint_name": "attach_joint" }}
|
||||
cspace:
|
||||
joint_names: ["panda_joint1","panda_joint2","panda_joint3","panda_joint4", "panda_joint5",
|
||||
"panda_joint6","panda_joint7","panda_finger_joint1", "panda_finger_joint2"]
|
||||
retract_config: [0.0, -1.3, 0.0, -2.5, 0.0, 1.0, 0., 0.04, -0.04]
|
||||
null_space_weight: [1,1,1,1,1,1,1,1,1]
|
||||
cspace_distance_weight: [1,1,1,1,1,1,1,1,1]
|
||||
max_acceleration: 15.0
|
||||
max_jerk: 500.0
|
||||
116
src/curobo/content/configs/robot/franka_mobile.yml
Normal file
116
src/curobo/content/configs/robot/franka_mobile.yml
Normal file
@@ -0,0 +1,116 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
|
||||
robot_cfg:
|
||||
kinematics:
|
||||
use_usd_kinematics: False
|
||||
isaac_usd_path: "/Isaac/Robots/Franka/franka.usd"
|
||||
usd_path: "robot/franka_description/franka_panda_meters.usda"
|
||||
usd_robot_root: "/panda"
|
||||
usd_flip_joints: ["panda_joint1","panda_joint2","panda_joint3","panda_joint4", "panda_joint5",
|
||||
"panda_joint6","panda_joint7","panda_finger_joint1", "panda_finger_joint2"]
|
||||
usd_flip_joints: {
|
||||
"panda_joint1": "Z",
|
||||
"panda_joint2": "Z",
|
||||
"panda_joint3": "Z",
|
||||
"panda_joint4": "Z",
|
||||
"panda_joint5": "Z",
|
||||
"panda_joint6": "Z",
|
||||
"panda_joint7": "Z",
|
||||
"panda_finger_joint1": "Y",
|
||||
"panda_finger_joint2": "Y",
|
||||
}
|
||||
usd_flip_joint_limits: ["panda_finger_joint2"]
|
||||
urdf_path: "robot/franka_description/franka_panda_mobile_xy_tz.urdf"
|
||||
asset_root_path: "robot/franka_description"
|
||||
base_link: "base_link"
|
||||
ee_link: "panda_hand"
|
||||
# link_names: null
|
||||
collision_link_names:
|
||||
[
|
||||
"panda_link0",
|
||||
"panda_link1",
|
||||
"panda_link2",
|
||||
"panda_link3",
|
||||
"panda_link4",
|
||||
"panda_link5",
|
||||
"panda_link6",
|
||||
"panda_link7",
|
||||
"panda_hand",
|
||||
"panda_leftfinger",
|
||||
"panda_rightfinger",
|
||||
"attached_object",
|
||||
]
|
||||
collision_spheres: "spheres/franka_mesh.yml"
|
||||
collision_sphere_buffer: 0.002 #0.02
|
||||
extra_collision_spheres: {"attached_object": 4}
|
||||
use_global_cumul: True
|
||||
self_collision_ignore:
|
||||
{
|
||||
"panda_link0": ["panda_link1", "panda_link2"],
|
||||
"panda_link1": ["panda_link2", "panda_link3", "panda_link4"],
|
||||
"panda_link2": ["panda_link3", "panda_link4"],
|
||||
"panda_link3": ["panda_link4", "panda_link6"],
|
||||
"panda_link4":
|
||||
["panda_link5", "panda_link6", "panda_link7", "panda_link8"],
|
||||
"panda_link5": ["panda_link6", "panda_link7", "panda_hand","panda_leftfinger", "panda_rightfinger"],
|
||||
"panda_link6": ["panda_link7", "panda_hand", "attached_object", "panda_leftfinger", "panda_rightfinger"],
|
||||
"panda_link7": ["panda_hand", "attached_object", "panda_leftfinger", "panda_rightfinger"],
|
||||
"panda_hand": ["panda_leftfinger", "panda_rightfinger","attached_object"],
|
||||
"panda_leftfinger": ["panda_rightfinger", "attached_object"],
|
||||
"panda_rightfinger": ["attached_object"],
|
||||
|
||||
}
|
||||
|
||||
self_collision_buffer:
|
||||
{
|
||||
"panda_link0": 0.1,
|
||||
"panda_link1": 0.05,
|
||||
"panda_link2": 0.0,
|
||||
"panda_link3": 0.0,
|
||||
"panda_link4": 0.0,
|
||||
"panda_link5": 0.0,
|
||||
"panda_link6": 0.0,
|
||||
"panda_link7": 0.0,
|
||||
"panda_hand": 0.02,
|
||||
"panda_leftfinger": 0.01,
|
||||
"panda_rightfinger": 0.01,
|
||||
"attached_object": 0.0,
|
||||
}
|
||||
|
||||
mesh_link_names:
|
||||
[
|
||||
"panda_link0",
|
||||
"panda_link1",
|
||||
"panda_link2",
|
||||
"panda_link3",
|
||||
"panda_link4",
|
||||
"panda_link5",
|
||||
"panda_link6",
|
||||
"panda_link7",
|
||||
"panda_hand",
|
||||
"panda_leftfinger",
|
||||
"panda_rightfinger",
|
||||
]
|
||||
lock_joints: {"panda_finger_joint1": 0.04, "panda_finger_joint2": -0.04}
|
||||
extra_links: {"attached_object":{"parent_link_name": "panda_hand" ,
|
||||
"link_name": "attached_object", "fixed_transform": [0,0,0,1,0,0,0], "joint_type":"FIXED",
|
||||
"joint_name": "attach_joint" }}
|
||||
cspace:
|
||||
joint_names: [
|
||||
"base_x", "base_y", "base_z",
|
||||
"panda_joint1","panda_joint2","panda_joint3","panda_joint4", "panda_joint5",
|
||||
"panda_joint6","panda_joint7","panda_finger_joint1", "panda_finger_joint2"]
|
||||
retract_config: [0,0,0,0.0, -1.3, 0.0, -2.5, 0.0, 1.0, 0., 0.04, -0.04]
|
||||
null_space_weight: [1,1,1,1,1,1,1,1,1,1,1,1]
|
||||
cspace_distance_weight: [1,1,1,1,1,1,1,1,1,1,1,1]
|
||||
max_acceleration: 15.0 #15.0
|
||||
max_jerk: 500.0
|
||||
81
src/curobo/content/configs/robot/iiwa.yml
Normal file
81
src/curobo/content/configs/robot/iiwa.yml
Normal file
@@ -0,0 +1,81 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
robot_cfg:
|
||||
kinematics:
|
||||
base_link: base_link
|
||||
collision_link_names:
|
||||
- iiwa7_link_1
|
||||
- iiwa7_link_2
|
||||
- iiwa7_link_3
|
||||
- iiwa7_link_4
|
||||
- iiwa7_link_5
|
||||
- iiwa7_link_6
|
||||
collision_sphere_buffer: 0.005
|
||||
collision_spheres: spheres/iiwa_allegro.yml
|
||||
ee_link: iiwa7_link_ee
|
||||
self_collision_buffer:
|
||||
iiwa7_link_1: -0.02
|
||||
iiwa7_link_2: 0.0
|
||||
iiwa7_link_3: 0.0
|
||||
iiwa7_link_4: 0.0
|
||||
iiwa7_link_5: 0.0
|
||||
iiwa7_link_6: 0.0
|
||||
|
||||
self_collision_ignore:
|
||||
{
|
||||
iiwa7_link_1: [iiwa7_link_2, iiwa7_link_3],
|
||||
iiwa7_link_2: [iiwa7_link_3, iiwa7_link_4],
|
||||
iiwa7_link_3: [iiwa7_link_4, iiwa7_link_5],
|
||||
iiwa7_link_4: [iiwa7_link_5, iiwa7_link_6],
|
||||
iiwa7_link_5: [iiwa7_link_6],
|
||||
}
|
||||
urdf_path: robot/iiwa_allegro_description/iiwa.urdf
|
||||
asset_root_path: robot/iiwa_allegro_description
|
||||
|
||||
cspace:
|
||||
joint_names:
|
||||
[
|
||||
"iiwa7_joint_1",
|
||||
"iiwa7_joint_2",
|
||||
"iiwa7_joint_3",
|
||||
"iiwa7_joint_4",
|
||||
"iiwa7_joint_5",
|
||||
"iiwa7_joint_6",
|
||||
"iiwa7_joint_7",
|
||||
]
|
||||
cspace_distance_weight:
|
||||
- 1.0
|
||||
- 0.9
|
||||
- 0.8
|
||||
- 0.8
|
||||
- 0.7
|
||||
- 0.6
|
||||
- 0.5
|
||||
|
||||
null_space_weight:
|
||||
- 1.0
|
||||
- 1.0
|
||||
- 1.0
|
||||
- 1.0
|
||||
- 1.0
|
||||
- 1.0
|
||||
- 1.0
|
||||
|
||||
retract_config:
|
||||
- 0.0
|
||||
- -0.78
|
||||
- 0.0
|
||||
- -1.4
|
||||
- 0.0
|
||||
- 0.6
|
||||
- 0.0
|
||||
max_acceleration: 15.0
|
||||
max_jerk: 500.0
|
||||
195
src/curobo/content/configs/robot/iiwa_allegro.yml
Normal file
195
src/curobo/content/configs/robot/iiwa_allegro.yml
Normal file
@@ -0,0 +1,195 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
robot_cfg:
|
||||
kinematics:
|
||||
base_link: base_link
|
||||
collision_link_names:
|
||||
- iiwa7_link_1
|
||||
- iiwa7_link_2
|
||||
- iiwa7_link_3
|
||||
- iiwa7_link_4
|
||||
- iiwa7_link_5
|
||||
- iiwa7_link_6
|
||||
- palm_link
|
||||
- index_link_1
|
||||
- index_link_2
|
||||
- index_link_3
|
||||
- middle_link_1
|
||||
- middle_link_2
|
||||
- middle_link_3
|
||||
- ring_link_1
|
||||
- ring_link_2
|
||||
- ring_link_3
|
||||
- thumb_link_2
|
||||
- thumb_link_3
|
||||
collision_sphere_buffer: 0.005
|
||||
collision_spheres: spheres/iiwa_allegro.yml
|
||||
ee_link: index_link_3
|
||||
link_names:
|
||||
- palm_link
|
||||
- index_link_3
|
||||
- middle_link_3
|
||||
- ring_link_3
|
||||
- thumb_link_3
|
||||
self_collision_buffer:
|
||||
iiwa7_link_1: -0.02
|
||||
iiwa7_link_2: 0.0
|
||||
iiwa7_link_3: 0.0
|
||||
iiwa7_link_4: 0.0
|
||||
iiwa7_link_5: 0.0
|
||||
iiwa7_link_6: 0.0
|
||||
index_link_1: -0.0
|
||||
index_link_2: -0.0
|
||||
index_link_3: 0.0
|
||||
middle_link_1: 0.0
|
||||
middle_link_2: 0.0
|
||||
middle_link_3: 0.0
|
||||
palm_link: 0.0
|
||||
ring_link_1: 0.0
|
||||
ring_link_2: 0.0
|
||||
ring_link_3: 0.0
|
||||
thumb_link_2: 0.0
|
||||
thumb_link_3: 0.0
|
||||
self_collision_ignore: {
|
||||
iiwa7_link_1: [iiwa7_link_2, iiwa7_link_3],
|
||||
iiwa7_link_2: [iiwa7_link_3, iiwa7_link_4],
|
||||
iiwa7_link_3: [iiwa7_link_4, iiwa7_link_5],
|
||||
iiwa7_link_4: [iiwa7_link_5, iiwa7_link_6],
|
||||
iiwa7_link_5: [iiwa7_link_6, palm_link],
|
||||
iiwa7_link_6: [palm_link],
|
||||
palm_link:
|
||||
[
|
||||
index_link_1,
|
||||
index_link_2,
|
||||
middle_link_1,
|
||||
middle_link_2,
|
||||
ring_link_1,
|
||||
ring_link_2,
|
||||
thumb_link_2,
|
||||
],
|
||||
index_link_1: [index_link_2, middle_link_1, middle_link_2],
|
||||
index_link_2: [index_link_3],
|
||||
#index_link_3: [iiwa7_link_2],
|
||||
middle_link_1: [middle_link_2, ring_link_1, ring_link_2],
|
||||
middle_link_2: [middle_link_3],
|
||||
#middle_link_3: [],
|
||||
ring_link_1: [ring_link_2],
|
||||
ring_link_2: [ring_link_3],
|
||||
#ring_link_3: [iiwa7_link_2],
|
||||
thumb_link_2: [thumb_link_3],
|
||||
}
|
||||
#thumb_link_3: [iiwa7_link_2],
|
||||
urdf_path: robot/iiwa_allegro_description/iiwa_allegro.urdf
|
||||
asset_root_path: robot/iiwa_allegro_description
|
||||
usd_path: robot/iiwa_allegro_description/iiwa_allegro.usda
|
||||
usd_robot_root: /iiwa_allegro
|
||||
cspace:
|
||||
joint_names:
|
||||
[
|
||||
"iiwa7_joint_1",
|
||||
"iiwa7_joint_2",
|
||||
"iiwa7_joint_3",
|
||||
"iiwa7_joint_4",
|
||||
"iiwa7_joint_5",
|
||||
"iiwa7_joint_6",
|
||||
"iiwa7_joint_7",
|
||||
"index_joint_0",
|
||||
"index_joint_1",
|
||||
"index_joint_2",
|
||||
"index_joint_3",
|
||||
"middle_joint_0",
|
||||
"middle_joint_1",
|
||||
"middle_joint_2",
|
||||
"middle_joint_3",
|
||||
"ring_joint_0",
|
||||
"ring_joint_1",
|
||||
"ring_joint_2",
|
||||
"ring_joint_3",
|
||||
"thumb_joint_0",
|
||||
"thumb_joint_1",
|
||||
"thumb_joint_2",
|
||||
"thumb_joint_3",
|
||||
]
|
||||
cspace_distance_weight:
|
||||
- 1.0
|
||||
- 0.9
|
||||
- 0.8
|
||||
- 0.8
|
||||
- 0.7
|
||||
- 0.6
|
||||
- 0.5
|
||||
- 0.1
|
||||
- 0.1
|
||||
- 0.1
|
||||
- 0.1
|
||||
- 0.1
|
||||
- 0.1
|
||||
- 0.1
|
||||
- 0.1
|
||||
- 0.1
|
||||
- 0.1
|
||||
- 0.1
|
||||
- 0.1
|
||||
- 1.1
|
||||
- 1.1
|
||||
- 0.1
|
||||
- 0.1
|
||||
|
||||
null_space_weight:
|
||||
- 1.0
|
||||
- 1.0
|
||||
- 1.0
|
||||
- 1.0
|
||||
- 1.0
|
||||
- 1.0
|
||||
- 1.0
|
||||
- 1.0
|
||||
- 1.0
|
||||
- 1.0
|
||||
- 1.0
|
||||
- 1.0
|
||||
- 1.0 #
|
||||
- 1.0
|
||||
- 1.0
|
||||
- 1.0
|
||||
- 1.0
|
||||
- 1.0
|
||||
- 1.0
|
||||
- 1.0
|
||||
- 1.0
|
||||
- 1.0
|
||||
- 1.0
|
||||
retract_config:
|
||||
- 0.0
|
||||
- -0.78
|
||||
- 0.0
|
||||
- -1.4
|
||||
- 0.0
|
||||
- 0.6
|
||||
- 2.1
|
||||
- 0.0
|
||||
- 0.7242903113365173
|
||||
- 0.7242903113365173
|
||||
- 0.7242903113365173
|
||||
- 0.0
|
||||
- 0.7242903113365173
|
||||
- 0.7242903113365173
|
||||
- 0.7242903113365173
|
||||
- 0.0
|
||||
- 0.7242903113365173
|
||||
- 0.7242903113365173
|
||||
- 0.9249972105026245
|
||||
- 0.4101402759552002
|
||||
- 0.7242903113365173
|
||||
- 0.7417430877685547
|
||||
- 0.7242903113365173
|
||||
max_acceleration: 15.0
|
||||
max_jerk: 500.0
|
||||
@@ -0,0 +1,146 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
|
||||
# The robot description defines the generalized coordinates and how to map those
|
||||
# to the underlying URDF dofs.
|
||||
|
||||
api_version: 1.0
|
||||
|
||||
# Defines the generalized coordinates. Each generalized coordinate is assumed
|
||||
# to have an entry in the URDF.
|
||||
# Lula will only use these joints to control the robot position.
|
||||
cspace:
|
||||
- j2s7s300_joint_1
|
||||
- j2s7s300_joint_2
|
||||
- j2s7s300_joint_3
|
||||
- j2s7s300_joint_4
|
||||
- j2s7s300_joint_5
|
||||
- j2s7s300_joint_6
|
||||
- j2s7s300_joint_7
|
||||
default_q: [
|
||||
0.0,1.7,0.0,0.5236,0.0,1.1345,0.0
|
||||
]
|
||||
|
||||
# Most dimensions of the cspace have a direct corresponding element
|
||||
# in the URDF. This list of rules defines how unspecified coordinates
|
||||
# should be extracted or how values in the URDF should be overwritten.
|
||||
|
||||
cspace_to_urdf_rules:
|
||||
- {name: j2s7s300_joint_finger_1, rule: fixed, value: 0.0}
|
||||
- {name: j2s7s300_joint_finger_2, rule: fixed, value: 1e-04}
|
||||
- {name: j2s7s300_joint_finger_3, rule: fixed, value: 0.0}
|
||||
- {name: j2s7s300_joint_finger_tip_1, rule: fixed, value: 0.0}
|
||||
- {name: j2s7s300_joint_finger_tip_2, rule: fixed, value: 0.0}
|
||||
- {name: j2s7s300_joint_finger_tip_3, rule: fixed, value: 0.0}
|
||||
|
||||
# Lula uses collision spheres to define the robot geometry in order to avoid
|
||||
# collisions with external obstacles. If no spheres are specified, Lula will
|
||||
# not be able to avoid obstacles.
|
||||
|
||||
collision_spheres:
|
||||
- j2s7s300_link_base:
|
||||
- "center": [0.0, -0.003, 0.103]
|
||||
"radius": 0.048
|
||||
- "center": [0.0, -0.0, 0.162]
|
||||
"radius": 0.047
|
||||
- j2s7s300_link_1:
|
||||
- "center": [-0.004, -0.002, -0.056]
|
||||
"radius": 0.05
|
||||
- "center": [0.0, 0.001, -0.119]
|
||||
"radius": 0.05
|
||||
- j2s7s300_link_2:
|
||||
- "center": [0.0, -0.162, -0.001]
|
||||
"radius": 0.049
|
||||
- "center": [-0.0, -0.0, 0.0]
|
||||
"radius": 0.05
|
||||
- "center": [0.0, -0.108, -0.001]
|
||||
"radius": 0.049
|
||||
- "center": [0.0, -0.054, -0.0]
|
||||
"radius": 0.05
|
||||
- j2s7s300_link_3:
|
||||
- "center": [-0.004, -0.0, -0.202]
|
||||
"radius": 0.05
|
||||
- "center": [0.0, 0.0, 0.0]
|
||||
"radius": 0.05
|
||||
- "center": [-0.003, -0.0, -0.161]
|
||||
"radius": 0.05
|
||||
- "center": [-0.002, -0.0, -0.121]
|
||||
"radius": 0.05
|
||||
- "center": [-0.001, -0.0, -0.081]
|
||||
"radius": 0.05
|
||||
- "center": [-0.001, -0.0, -0.04]
|
||||
"radius": 0.05
|
||||
- j2s7s300_link_4:
|
||||
- "center": [0.002, 0.21, -0.013]
|
||||
"radius": 0.04
|
||||
- "center": [0.0, 0.0, 0.0]
|
||||
"radius": 0.05
|
||||
- "center": [0.001, 0.172, -0.01]
|
||||
"radius": 0.042
|
||||
- "center": [0.001, 0.132, -0.008]
|
||||
"radius": 0.044
|
||||
- "center": [0.001, 0.09, -0.005]
|
||||
"radius": 0.046
|
||||
- "center": [0.0, 0.046, -0.003]
|
||||
"radius": 0.048
|
||||
- j2s7s300_link_5:
|
||||
- "center": [-0.001, 0.013, -0.106]
|
||||
"radius": 0.04
|
||||
- "center": [-0.003, 0.003, -0.044]
|
||||
"radius": 0.04
|
||||
- "center": [-0.002, 0.008, -0.075]
|
||||
"radius": 0.04
|
||||
- j2s7s300_link_6:
|
||||
- "center": [-0.001, 0.094, 0.0]
|
||||
"radius": 0.04
|
||||
- "center": [0.0, -0.0, -0.017]
|
||||
"radius": 0.04
|
||||
- "center": [-0.001, 0.047, -0.008]
|
||||
"radius": 0.04
|
||||
- j2s7s300_link_7:
|
||||
- "center": [-0.01, -0.017, -0.069]
|
||||
"radius": 0.045
|
||||
- "center": [0.001, 0.023, -0.076]
|
||||
"radius": 0.038
|
||||
- "center": [0.016, -0.022, -0.071]
|
||||
"radius": 0.038
|
||||
- "center": [-0.0, 0.0, -0.031]
|
||||
"radius": 0.04
|
||||
- j2s7s300_link_finger_tip_1:
|
||||
- "center": [0.009, -0.004, -0.0]
|
||||
"radius": 0.017
|
||||
- "center": [0.03, -0.007, -0.002]
|
||||
"radius": 0.015
|
||||
- j2s7s300_link_finger_tip_2:
|
||||
- "center": [0.009, -0.004, -0.0]
|
||||
"radius": 0.017
|
||||
- "center": [0.032, -0.002, -0.001]
|
||||
"radius": 0.015
|
||||
- j2s7s300_link_finger_2:
|
||||
- "center": [0.011, -0.01, -0.0]
|
||||
"radius": 0.021
|
||||
- "center": [0.032, -0.01, 0.001]
|
||||
"radius": 0.02
|
||||
- j2s7s300_link_finger_3:
|
||||
- "center": [0.011, -0.01, -0.0]
|
||||
"radius": 0.021
|
||||
- "center": [0.032, -0.01, 0.001]
|
||||
"radius": 0.02
|
||||
- j2s7s300_link_finger_tip_3:
|
||||
- "center": [0.032, -0.004, 0.001]
|
||||
"radius": 0.015
|
||||
- "center": [0.015, -0.006, -0.002]
|
||||
"radius": 0.015
|
||||
- j2s7s300_link_finger_1:
|
||||
- "center": [0.011, -0.01, -0.0]
|
||||
"radius": 0.021
|
||||
- "center": [0.032, -0.01, 0.001]
|
||||
"radius": 0.02
|
||||
103
src/curobo/content/configs/robot/isaac_sim_description/tm12.yaml
Normal file
103
src/curobo/content/configs/robot/isaac_sim_description/tm12.yaml
Normal file
@@ -0,0 +1,103 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
|
||||
# The robot description defines the generalized coordinates and how to map those
|
||||
# to the underlying URDF dofs.
|
||||
|
||||
api_version: 1.0
|
||||
|
||||
# Defines the generalized coordinates. Each generalized coordinate is assumed
|
||||
# to have an entry in the URDF.
|
||||
# Lula will only use these joints to control the robot position.
|
||||
cspace:
|
||||
- shoulder_1_joint
|
||||
- shoulder_2_joint
|
||||
- elbow_joint
|
||||
- wrist_1_joint
|
||||
- wrist_2_joint
|
||||
- wrist_3_joint
|
||||
default_q: [
|
||||
-0.1,-0.8,1.5,-0.2,0.2,0.2
|
||||
]
|
||||
|
||||
# Most dimensions of the cspace have a direct corresponding element
|
||||
# in the URDF. This list of rules defines how unspecified coordinates
|
||||
# should be extracted or how values in the URDF should be overwritten.
|
||||
|
||||
cspace_to_urdf_rules:
|
||||
|
||||
# Lula uses collision spheres to define the robot geometry in order to avoid
|
||||
# collisions with external obstacles. If no spheres are specified, Lula will
|
||||
# not be able to avoid obstacles.
|
||||
|
||||
collision_spheres:
|
||||
- link_1:
|
||||
- "center": [-0.0, -0.0, 0.018]
|
||||
"radius": 0.12
|
||||
- "center": [0.0, 0.0, -0.13]
|
||||
"radius": 0.09
|
||||
- "center": [-0.017, -0.18, 0.02]
|
||||
"radius": 0.12
|
||||
- link_2:
|
||||
- "center": [0.116, 0.006, -0.182]
|
||||
"radius": 0.075
|
||||
- "center": [0.638, -0.004, -0.192]
|
||||
"radius": 0.08
|
||||
- "center": [0.19, 0.004, -0.183]
|
||||
"radius": 0.075
|
||||
- "center": [0.265, 0.003, -0.184]
|
||||
"radius": 0.075
|
||||
- "center": [0.34, 0.001, -0.186]
|
||||
"radius": 0.075
|
||||
- "center": [0.414, 0.0, -0.187]
|
||||
"radius": 0.075
|
||||
- "center": [0.489, -0.001, -0.189]
|
||||
"radius": 0.075
|
||||
- "center": [0.563, -0.003, -0.19]
|
||||
"radius": 0.075
|
||||
- link_3:
|
||||
- "center": [0.012, 0.004, -0.076]
|
||||
"radius": 0.08
|
||||
- "center": [0.55, -0.001, -0.046]
|
||||
"radius": 0.07
|
||||
- "center": [0.088, 0.003, -0.061]
|
||||
"radius": 0.06
|
||||
- "center": [0.165, 0.002, -0.056]
|
||||
"radius": 0.06
|
||||
- "center": [0.242, 0.001, -0.052]
|
||||
"radius": 0.06
|
||||
- "center": [0.319, 0.001, -0.047]
|
||||
"radius": 0.06
|
||||
- "center": [0.396, -0.0, -0.043]
|
||||
"radius": 0.06
|
||||
- "center": [0.473, -0.001, -0.038]
|
||||
"radius": 0.06
|
||||
- link_4:
|
||||
- "center": [0.0, 0.0, 0.0]
|
||||
"radius": 0.07
|
||||
- link_6:
|
||||
- "center": [0.003, -0.002, -0.028]
|
||||
"radius": 0.06
|
||||
- "center": [-0.001, 0.075, 0.009]
|
||||
"radius": 0.05
|
||||
- "center": [-0.0, 0.078, -0.028]
|
||||
"radius": 0.05
|
||||
- "center": [-0.031, 0.128, 0.008]
|
||||
"radius": 0.03
|
||||
- "center": [-0.006, 0.146, 0.0]
|
||||
"radius": 0.03
|
||||
- "center": [0.025, 0.125, 0.007]
|
||||
"radius": 0.03
|
||||
- "center": [-0.005, 0.128, 0.003]
|
||||
"radius": 0.03
|
||||
- link_5:
|
||||
- "center": [0.0, 0.0, 0.0]
|
||||
"radius": 0.08
|
||||
101
src/curobo/content/configs/robot/isaac_sim_description/ur5e.yaml
Normal file
101
src/curobo/content/configs/robot/isaac_sim_description/ur5e.yaml
Normal file
@@ -0,0 +1,101 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
|
||||
# The robot description defines the generalized coordinates and how to map those
|
||||
# to the underlying URDF dofs.
|
||||
|
||||
api_version: 1.0
|
||||
|
||||
# Defines the generalized coordinates. Each generalized coordinate is assumed
|
||||
# to have an entry in the URDF.
|
||||
# Lula will only use these joints to control the robot position.
|
||||
cspace:
|
||||
- shoulder_pan_joint
|
||||
- shoulder_lift_joint
|
||||
- elbow_joint
|
||||
- wrist_1_joint
|
||||
- wrist_2_joint
|
||||
- wrist_3_joint
|
||||
default_q: [
|
||||
0.0,0.0002,0.0,0.0002,0.0,0.0
|
||||
]
|
||||
|
||||
# Most dimensions of the cspace have a direct corresponding element
|
||||
# in the URDF. This list of rules defines how unspecified coordinates
|
||||
# should be extracted or how values in the URDF should be overwritten.
|
||||
|
||||
cspace_to_urdf_rules:
|
||||
|
||||
# Lula uses collision spheres to define the robot geometry in order to avoid
|
||||
# collisions with external obstacles. If no spheres are specified, Lula will
|
||||
# not be able to avoid obstacles.
|
||||
|
||||
collision_spheres:
|
||||
- shoulder_link:
|
||||
- "center": [0.0, 0.0, 0.0]
|
||||
"radius": 0.1
|
||||
- upper_arm_link:
|
||||
- "center": [-0.416, -0.0, 0.143]
|
||||
"radius": 0.078
|
||||
- "center": [-0.015, 0.0, 0.134]
|
||||
"radius": 0.077
|
||||
- "center": [-0.14, 0.0, 0.138]
|
||||
"radius": 0.062
|
||||
- "center": [-0.285, -0.001, 0.139]
|
||||
"radius": 0.061
|
||||
- "center": [-0.376, 0.001, 0.138]
|
||||
"radius": 0.077
|
||||
- "center": [-0.222, 0.001, 0.139]
|
||||
"radius": 0.061
|
||||
- "center": [-0.055, 0.008, 0.14]
|
||||
"radius": 0.07
|
||||
- "center": [-0.001, -0.002, 0.143]
|
||||
"radius": 0.076
|
||||
- forearm_link:
|
||||
- "center": [-0.01, 0.002, 0.031]
|
||||
"radius": 0.072
|
||||
- "center": [-0.387, 0.0, 0.014]
|
||||
"radius": 0.057
|
||||
- "center": [-0.121, -0.0, 0.006]
|
||||
"radius": 0.057
|
||||
- "center": [-0.206, 0.001, 0.007]
|
||||
"radius": 0.057
|
||||
- "center": [-0.312, -0.001, 0.006]
|
||||
"radius": 0.056
|
||||
- "center": [-0.057, 0.003, 0.008]
|
||||
"radius": 0.065
|
||||
- "center": [-0.266, 0.0, 0.006]
|
||||
"radius": 0.057
|
||||
- "center": [-0.397, -0.001, -0.018]
|
||||
"radius": 0.052
|
||||
- "center": [-0.164, -0.0, 0.007]
|
||||
"radius": 0.057
|
||||
- wrist_1_link:
|
||||
- "center": [-0.0, 0.0, -0.009]
|
||||
"radius": 0.047
|
||||
- "center": [-0.0, 0.0, -0.052]
|
||||
"radius": 0.047
|
||||
- "center": [-0.002, 0.027, -0.001]
|
||||
"radius": 0.045
|
||||
- "center": [0.001, -0.01, 0.0]
|
||||
"radius": 0.046
|
||||
- wrist_2_link:
|
||||
- "center": [0.0, -0.01, -0.001]
|
||||
"radius": 0.047
|
||||
- "center": [0.0, 0.008, -0.001]
|
||||
"radius": 0.047
|
||||
- "center": [0.001, -0.001, -0.036]
|
||||
"radius": 0.047
|
||||
- "center": [0.001, -0.03, -0.0]
|
||||
"radius": 0.047
|
||||
- wrist_3_link:
|
||||
- "center": [0.001, 0.001, -0.029]
|
||||
"radius": 0.043
|
||||
217
src/curobo/content/configs/robot/jaco7.yml
Normal file
217
src/curobo/content/configs/robot/jaco7.yml
Normal file
@@ -0,0 +1,217 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
|
||||
robot_cfg:
|
||||
|
||||
kinematics:
|
||||
|
||||
urdf_path: "robot/jaco/jaco_7s.urdf"
|
||||
asset_root_path: "robot/jaco"
|
||||
usd_path: "robot/jaco/jaco_7s.usda"
|
||||
isaac_usd_path: "/Isaac/Robots/Kinova/Jaco2/J2N7S300/j2n7s300_instanceable.usd"
|
||||
usd_robot_root: "/jaco"
|
||||
#usd_robot_root: "/robot"
|
||||
usd_flip_joints: {
|
||||
j2s7s300_joint_1: "Z",
|
||||
j2s7s300_joint_2: "Z",
|
||||
j2s7s300_joint_3: "Z",
|
||||
j2s7s300_joint_4: "Z",
|
||||
j2s7s300_joint_5: "Z",
|
||||
j2s7s300_joint_6: "Z",
|
||||
j2s7s300_joint_7 : "Z",
|
||||
j2s7s300_joint_finger_1: "Z",
|
||||
j2s7s300_joint_finger_2: "Z",
|
||||
j2s7s300_joint_finger_3: "Z",
|
||||
j2s7s300_joint_finger_tip_1: "Z",
|
||||
j2s7s300_joint_finger_tip_2: "Z",
|
||||
j2s7s300_joint_finger_tip_3: "Z",
|
||||
}
|
||||
base_link: "root"
|
||||
ee_link: "j2s7s300_end_effector"
|
||||
link_names: null
|
||||
|
||||
collision_link_names: [
|
||||
"j2s7s300_link_base", "j2s7s300_link_1", "j2s7s300_link_2",
|
||||
"j2s7s300_link_3", "j2s7s300_link_4", "j2s7s300_link_5", "j2s7s300_link_6", "j2s7s300_link_7",
|
||||
"j2s7s300_link_finger_tip_1", "j2s7s300_link_finger_tip_2",
|
||||
"j2s7s300_link_finger_tip_3",
|
||||
"j2s7s300_link_finger_3",
|
||||
"j2s7s300_link_finger_2",
|
||||
"j2s7s300_link_finger_1",
|
||||
]
|
||||
|
||||
collision_spheres:
|
||||
j2s7s300_link_base:
|
||||
- "center": [0.0, -0.003, 0.103]
|
||||
"radius": 0.048
|
||||
- "center": [0.0, -0.0, 0.162]
|
||||
"radius": 0.047
|
||||
j2s7s300_link_1:
|
||||
- "center": [-0.004, -0.002, -0.056]
|
||||
"radius": 0.05
|
||||
- "center": [0.0, 0.001, -0.119]
|
||||
"radius": 0.05
|
||||
j2s7s300_link_2:
|
||||
- "center": [0.0, -0.162, -0.001]
|
||||
"radius": 0.049
|
||||
- "center": [-0.0, -0.0, 0.0]
|
||||
"radius": 0.05
|
||||
- "center": [0.0, -0.108, -0.001]
|
||||
"radius": 0.049
|
||||
- "center": [0.0, -0.054, -0.0]
|
||||
"radius": 0.05
|
||||
j2s7s300_link_3:
|
||||
- "center": [-0.004, -0.0, -0.202]
|
||||
"radius": 0.05
|
||||
- "center": [0.0, 0.0, 0.0]
|
||||
"radius": 0.05
|
||||
- "center": [-0.003, -0.0, -0.161]
|
||||
"radius": 0.05
|
||||
- "center": [-0.002, -0.0, -0.121]
|
||||
"radius": 0.05
|
||||
- "center": [-0.001, -0.0, -0.081]
|
||||
"radius": 0.05
|
||||
- "center": [-0.001, -0.0, -0.04]
|
||||
"radius": 0.05
|
||||
j2s7s300_link_4:
|
||||
- "center": [0.002, 0.21, -0.013]
|
||||
"radius": 0.04
|
||||
- "center": [0.0, 0.0, 0.0]
|
||||
"radius": 0.05
|
||||
- "center": [0.001, 0.172, -0.01]
|
||||
"radius": 0.042
|
||||
- "center": [0.001, 0.132, -0.008]
|
||||
"radius": 0.044
|
||||
- "center": [0.001, 0.09, -0.005]
|
||||
"radius": 0.046
|
||||
- "center": [0.0, 0.046, -0.003]
|
||||
"radius": 0.048
|
||||
j2s7s300_link_5:
|
||||
- "center": [-0.001, 0.013, -0.106]
|
||||
"radius": 0.04
|
||||
- "center": [-0.003, 0.003, -0.044]
|
||||
"radius": 0.04
|
||||
- "center": [-0.002, 0.008, -0.075]
|
||||
"radius": 0.04
|
||||
j2s7s300_link_6:
|
||||
- "center": [-0.001, 0.094, 0.0]
|
||||
"radius": 0.04
|
||||
- "center": [0.0, -0.0, -0.017]
|
||||
"radius": 0.04
|
||||
- "center": [-0.001, 0.047, -0.008]
|
||||
"radius": 0.04
|
||||
j2s7s300_link_7:
|
||||
- "center": [-0.01, -0.017, -0.069]
|
||||
"radius": 0.045
|
||||
- "center": [0.001, 0.023, -0.076]
|
||||
"radius": 0.038
|
||||
- "center": [0.016, -0.022, -0.071]
|
||||
"radius": 0.038
|
||||
- "center": [-0.0, 0.0, -0.031]
|
||||
"radius": 0.04
|
||||
j2s7s300_link_finger_tip_1:
|
||||
- "center": [0.009, -0.004, -0.0]
|
||||
"radius": 0.017
|
||||
- "center": [0.03, -0.007, -0.002]
|
||||
"radius": 0.015
|
||||
j2s7s300_link_finger_tip_2:
|
||||
- "center": [0.009, -0.004, -0.0]
|
||||
"radius": 0.017
|
||||
- "center": [0.032, -0.002, -0.001]
|
||||
"radius": 0.015
|
||||
j2s7s300_link_finger_2:
|
||||
- "center": [0.011, -0.01, -0.0]
|
||||
"radius": 0.021
|
||||
- "center": [0.032, -0.01, 0.001]
|
||||
"radius": 0.02
|
||||
j2s7s300_link_finger_3:
|
||||
- "center": [0.011, -0.01, -0.0]
|
||||
"radius": 0.021
|
||||
- "center": [0.032, -0.01, 0.001]
|
||||
"radius": 0.02
|
||||
j2s7s300_link_finger_tip_3:
|
||||
- "center": [0.032, -0.004, 0.001]
|
||||
"radius": 0.015
|
||||
- "center": [0.015, -0.006, -0.002]
|
||||
"radius": 0.015
|
||||
j2s7s300_link_finger_1:
|
||||
- "center": [0.011, -0.01, -0.0]
|
||||
"radius": 0.021
|
||||
- "center": [0.032, -0.01, 0.001]
|
||||
"radius": 0.02
|
||||
|
||||
collision_sphere_buffer: 0.005
|
||||
self_collision_ignore: {
|
||||
"j2s7s300_link_base":["j2s7s300_link_1"],
|
||||
"j2s7s300_link_1":["j2s7s300_link_2"],
|
||||
"j2s7s300_link_2":["j2s7s300_link_3"],
|
||||
"j2s7s300_link_3":["j2s7s300_link_4"],
|
||||
"j2s7s300_link_4":["j2s7s300_link_5"],
|
||||
"j2s7s300_link_5":["j2s7s300_link_6"],
|
||||
"j2s7s300_link_6":["j2s7s300_link_7"],
|
||||
"j2s7s300_link_7":[
|
||||
"j2s7s300_link_finger_tip_1",
|
||||
"j2s7s300_link_finger_tip_2",
|
||||
"j2s7s300_link_finger_tip_3",
|
||||
"j2s7s300_link_finger_1",
|
||||
"j2s7s300_link_finger_2",
|
||||
"j2s7s300_link_finger_3",
|
||||
],
|
||||
|
||||
"j2s7s300_link_finger_3":["j2s7s300_link_finger_tip_3","j2s7s300_link_finger_2",
|
||||
"j2s7s300_link_finger_1"],
|
||||
"j2s7s300_link_finger_2":["j2s7s300_link_finger_tip_2", "j2s7s300_link_finger_1",
|
||||
"j2s7s300_link_finger_3"],
|
||||
"j2s7s300_link_finger_1":["j2s7s300_link_finger_tip_1", "j2s7s300_link_finger_2",
|
||||
"j2s7s300_link_finger_3"],
|
||||
|
||||
} # Dict[str, List[str]]
|
||||
self_collision_buffer: {} # Dict[str, float]
|
||||
|
||||
mesh_link_names: [
|
||||
"j2s7s300_link_base", "j2s7s300_link_1", "j2s7s300_link_2",
|
||||
"j2s7s300_link_3", "j2s7s300_link_4", "j2s7s300_link_5", "j2s7s300_link_6", "j2s7s300_link_7",
|
||||
"j2s7s300_link_finger_tip_1", "j2s7s300_link_finger_tip_2",
|
||||
"j2s7s300_link_finger_tip_3",
|
||||
"j2s7s300_link_finger_3",
|
||||
"j2s7s300_link_finger_2",
|
||||
"j2s7s300_link_finger_1",
|
||||
] # List[str]
|
||||
lock_joints: {"j2s7s300_joint_finger_1": 0,
|
||||
"j2s7s300_joint_finger_2": 0,
|
||||
"j2s7s300_joint_finger_3": 0,
|
||||
"j2s7s300_joint_finger_tip_1": 0,
|
||||
"j2s7s300_joint_finger_tip_2": 0,
|
||||
"j2s7s300_joint_finger_tip_3": 0,
|
||||
|
||||
}
|
||||
|
||||
cspace:
|
||||
joint_names:
|
||||
- j2s7s300_joint_1
|
||||
- j2s7s300_joint_2
|
||||
- j2s7s300_joint_3
|
||||
- j2s7s300_joint_4
|
||||
- j2s7s300_joint_5
|
||||
- j2s7s300_joint_6
|
||||
- j2s7s300_joint_7
|
||||
- j2s7s300_joint_finger_1
|
||||
- j2s7s300_joint_finger_2
|
||||
- j2s7s300_joint_finger_3
|
||||
- j2s7s300_joint_finger_tip_1
|
||||
- j2s7s300_joint_finger_tip_2
|
||||
- j2s7s300_joint_finger_tip_3
|
||||
retract_config: [0.0,4.0,0.0,5,0.0,3.0,0.0, 0,0,0,0,0,0]
|
||||
null_space_weight: [1,1,1,1,1,1,1, 1,1,1,1,1,1] # List[str]
|
||||
cspace_distance_weight: [1,1,1,1,1,1,1,1,1,1,1,1,1] # List[str]
|
||||
|
||||
max_acceleration: 15.0
|
||||
max_jerk: 500.0
|
||||
92
src/curobo/content/configs/robot/kinova_gen3.yml
Normal file
92
src/curobo/content/configs/robot/kinova_gen3.yml
Normal file
@@ -0,0 +1,92 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
robot_cfg:
|
||||
kinematics:
|
||||
base_link: base_link
|
||||
collision_link_names:
|
||||
- base_link
|
||||
- shoulder_link
|
||||
- half_arm_1_link
|
||||
- half_arm_2_link
|
||||
- forearm_link
|
||||
- spherical_wrist_1_link
|
||||
- spherical_wrist_2_link
|
||||
- bracelet_link
|
||||
- robotiq_arg2f_base_link
|
||||
- left_outer_finger
|
||||
- left_inner_finger_pad
|
||||
- right_outer_finger
|
||||
- right_inner_finger_pad
|
||||
collision_sphere_buffer: 0.005
|
||||
collision_spheres: spheres/kinova_gen3.yml
|
||||
ee_link: tool_frame
|
||||
self_collision_buffer:
|
||||
base_link: 0.0
|
||||
forearm_link: 0.0
|
||||
half_arm_1_link: 0.0
|
||||
half_arm_2_link: 0.0
|
||||
shoulder_link: 0.0
|
||||
spherical_wrist_1_link : 0.0
|
||||
spherical_wrist_2_link : 0.0
|
||||
bracelet_link: 0.0
|
||||
robotiq_arg2f_base_link: 0.0
|
||||
left_outer_finger: 0.0
|
||||
left_inner_finger_pad: 0.0
|
||||
right_outer_finger: 0.0
|
||||
right_inner_finger_pad: 0.0
|
||||
left_outer_knuckle: 0.0
|
||||
self_collision_ignore: {
|
||||
base_link: [shoulder_link],
|
||||
shoulder_link: [half_arm_1_link],
|
||||
half_arm_1_link: [half_arm_2_link],
|
||||
half_arm_2_link: [forearm_link],
|
||||
forearm_link: [spherical_wrist_1_link],
|
||||
spherical_wrist_1_link: [spherical_wrist_2_link],
|
||||
spherical_wrist_2_link: [bracelet_link],
|
||||
bracelet_link: [robotiq_arg2f_base_link],
|
||||
robotiq_arg2f_base_link: [left_outer_finger, right_outer_finger, left_inner_finger_pad, right_inner_finger_pad],
|
||||
left_outer_finger: [right_outer_finger, right_inner_finger_pad, left_inner_finger_pad],
|
||||
right_outer_finger: [right_inner_finger_pad, left_inner_finger_pad],
|
||||
left_inner_finger_pad: [right_inner_finger_pad],
|
||||
|
||||
}
|
||||
urdf_path: robot/kinova/kinova_gen3_7dof.urdf
|
||||
asset_root_path: robot/kinova
|
||||
|
||||
cspace:
|
||||
joint_names: ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6', 'joint_7']
|
||||
cspace_distance_weight:
|
||||
- 1.0
|
||||
- 1.0
|
||||
- 1.0
|
||||
- 1.0
|
||||
- 1.0
|
||||
- 1.0
|
||||
- 1.0
|
||||
|
||||
null_space_weight:
|
||||
- 1.0
|
||||
- 1.0
|
||||
- 1.0
|
||||
- 1.0
|
||||
- 1.0
|
||||
- 1.0
|
||||
- 1.0
|
||||
retract_config:
|
||||
- 0.0
|
||||
- -0.8
|
||||
- 0.0
|
||||
- 1.5
|
||||
- 0.0
|
||||
- 0.4
|
||||
- 0.0
|
||||
max_acceleration: 25.0
|
||||
max_jerk: 500.0
|
||||
85
src/curobo/content/configs/robot/quad_ur10e.yml
Normal file
85
src/curobo/content/configs/robot/quad_ur10e.yml
Normal file
@@ -0,0 +1,85 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
|
||||
robot_cfg:
|
||||
kinematics:
|
||||
use_usd_kinematics: False
|
||||
urdf_path: "robot/ur_description/quad_ur10e.urdf"
|
||||
asset_root_path: "robot/ur_description"
|
||||
isaac_usd_path: "/Isaac/Robots/UR10/ur10_long_suction.usd"
|
||||
usd_robot_root: "/ur10"
|
||||
usd_path: "robot/ur_description/ur10e.usd"
|
||||
base_link: "base_fixture_link"
|
||||
ee_link: "tool0"
|
||||
link_names: ["tool0", "tool1","tool2","tool3"] # , "tool2"] #, "tool3"]
|
||||
collision_link_names: [
|
||||
'shoulder_link','upper_arm_link', 'forearm_link', 'wrist_1_link', 'wrist_2_link' ,'wrist_3_link', 'tool0',
|
||||
'shoulder_link_1','upper_arm_link_1', 'forearm_link_1', 'wrist_1_link_1', 'wrist_2_link_1' ,'wrist_3_link_1', 'tool1',
|
||||
'shoulder_link_2','upper_arm_link_2', 'forearm_link_2', 'wrist_1_link_2', 'wrist_2_link_2' ,'wrist_3_link_2', 'tool2',
|
||||
'shoulder_link_3','upper_arm_link_3', 'forearm_link_3', 'wrist_1_link_3', 'wrist_2_link_3' ,'wrist_3_link_3', 'tool3']
|
||||
collision_spheres: 'spheres/quad_ur10e.yml'
|
||||
collision_sphere_buffer: 0.005
|
||||
|
||||
|
||||
self_collision_ignore: {
|
||||
"upper_arm_link": ["shoulder_link","forearm_link"],
|
||||
"forarm_link": ["wrist_1_link"],
|
||||
"wrist_1_link": ["wrist_2_link","wrist_3_link"],
|
||||
"wrist_2_link": ["wrist_3_link", "tool0"],
|
||||
"wrist_3_link": ["tool0"],
|
||||
|
||||
"upper_arm_link_1": ["shoulder_link_1","forearm_link_1"],
|
||||
"forarm_link_1": ["wrist_1_link_1"],
|
||||
"wrist_1_link_1": ["wrist_2_link_1","wrist_3_link_1"],
|
||||
"wrist_2_link_1": ["wrist_3_link_1", "tool1"],
|
||||
"wrist_3_link_1": ["tool1"],
|
||||
|
||||
"upper_arm_link_2": ["shoulder_link_2","forearm_link_2"],
|
||||
"forarm_link_2": ["wrist_1_link_2"],
|
||||
"wrist_1_link_2": ["wrist_2_link_2","wrist_3_link_2"],
|
||||
"wrist_2_link_2": ["wrist_3_link_2", "tool2"],
|
||||
"wrist_3_link_2": ["tool2"],
|
||||
|
||||
"upper_arm_link_3": ["shoulder_link_3","forearm_link_3"],
|
||||
"forarm_link_3": ["wrist_1_link_3"],
|
||||
"wrist_1_link_3": ["wrist_2_link_3","wrist_3_link_3"],
|
||||
"wrist_2_link_3": ["wrist_3_link_3", "tool3"],
|
||||
"wrist_3_link_3": ["tool3"],
|
||||
|
||||
}
|
||||
self_collision_buffer: {
|
||||
'shoulder_link': 0.05,
|
||||
'shoulder_link_1': 0.05,
|
||||
'shoulder_link_2': 0.05,
|
||||
'shoulder_link_3': 0.05,
|
||||
}
|
||||
lock_joints: null
|
||||
|
||||
cspace:
|
||||
joint_names: [
|
||||
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint',
|
||||
'shoulder_pan_joint_1', 'shoulder_lift_joint_1', 'elbow_joint_1', 'wrist_1_joint_1', 'wrist_2_joint_1', 'wrist_3_joint_1',
|
||||
'shoulder_pan_joint_2', 'shoulder_lift_joint_2', 'elbow_joint_2', 'wrist_1_joint_2', 'wrist_2_joint_2', 'wrist_3_joint_2',
|
||||
'shoulder_pan_joint_3', 'shoulder_lift_joint_3', 'elbow_joint_3', 'wrist_1_joint_3', 'wrist_2_joint_3', 'wrist_3_joint_3']
|
||||
retract_config: [-1.57, -2.2, 1.9, -1.383, -1.57, 0.00,
|
||||
-1.57, -2.2, 1.9, -1.383, -1.57, 0.00,
|
||||
-1.57, -2.2, 1.9, -1.383, -1.57, 0.00,
|
||||
-1.57, -2.2, 1.9, -1.383, -1.57, 0.00]
|
||||
null_space_weight: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
|
||||
1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
|
||||
1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
|
||||
1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
|
||||
cspace_distance_weight: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
|
||||
1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
|
||||
1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
|
||||
1.0, 1.0, 1.0, 1.0, 1.0, 1.0] #[2.5, 1.75, 1.5, 1.25, 0.7, 0.4]
|
||||
max_jerk: 500.0
|
||||
max_acceleration: 15.0
|
||||
110
src/curobo/content/configs/robot/spheres/dual_ur10e.yml
Normal file
110
src/curobo/content/configs/robot/spheres/dual_ur10e.yml
Normal file
@@ -0,0 +1,110 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
robot: 'UR10'
|
||||
collision_spheres:
|
||||
shoulder_link:
|
||||
- center: [0, 0, 0]
|
||||
radius: 0.05
|
||||
- center: [0.0, -0.18, 0]
|
||||
radius: 0.05
|
||||
upper_arm_link:
|
||||
- center: [-0.102167, 0, 0.18]
|
||||
radius: 0.05
|
||||
- center: [-0.204333, -8.88178e-19, 0.18]
|
||||
radius: 0.05
|
||||
- center: [-0.3065, -1.33227e-18, 0.18]
|
||||
radius: 0.05
|
||||
- center: [-0.408667, -1.77636e-18, 0.18]
|
||||
radius: 0.05
|
||||
- center: [-0.510833, 0, 0.18]
|
||||
radius: 0.05
|
||||
- center: [ -0.613, 0,0.18]
|
||||
radius: 0.07
|
||||
forearm_link:
|
||||
- center: [-0, 0, 0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.0951667, 0, 0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.190333, 0, 0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.2855, 0, 0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.380667, 0,0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.475833, 0,0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.571, -1.19904e-17, 0.03]
|
||||
radius: 0.05
|
||||
wrist_1_link:
|
||||
- center: [0, 0, 0]
|
||||
radius: 0.05
|
||||
wrist_2_link:
|
||||
- center: [0, 0, 0]
|
||||
radius: 0.05
|
||||
wrist_3_link:
|
||||
- center: [0, 0, 0]
|
||||
radius: 0.05
|
||||
- center: [0, 0, 0.06]
|
||||
radius: 0.08
|
||||
tool0:
|
||||
- center: [0, 0, 0.03]
|
||||
radius: 0.05
|
||||
|
||||
|
||||
shoulder_link_1:
|
||||
- center: [0, 0, 0]
|
||||
radius: 0.05
|
||||
- center: [0.0, -0.18, 0]
|
||||
radius: 0.05
|
||||
upper_arm_link_1:
|
||||
- center: [-0.102167, 0, 0.18]
|
||||
radius: 0.05
|
||||
- center: [-0.204333, -8.88178e-19, 0.18]
|
||||
radius: 0.05
|
||||
- center: [-0.3065, -1.33227e-18, 0.18]
|
||||
radius: 0.05
|
||||
- center: [-0.408667, -1.77636e-18, 0.18]
|
||||
radius: 0.05
|
||||
- center: [-0.510833, 0, 0.18]
|
||||
radius: 0.05
|
||||
- center: [ -0.613, 0,0.18]
|
||||
radius: 0.07
|
||||
forearm_link_1:
|
||||
- center: [-0, 0, 0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.0951667, 0, 0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.190333, 0, 0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.2855, 0, 0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.380667, 0,0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.475833, 0,0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.571, -1.19904e-17, 0.03]
|
||||
radius: 0.05
|
||||
wrist_1_link_1:
|
||||
- center: [0, 0, 0]
|
||||
radius: 0.05
|
||||
wrist_2_link_1:
|
||||
- center: [0, 0, 0]
|
||||
radius: 0.05
|
||||
wrist_3_link_1:
|
||||
- center: [0, 0, 0]
|
||||
radius: 0.05
|
||||
- center: [0, 0, 0.06]
|
||||
radius: 0.08
|
||||
|
||||
tool1:
|
||||
- center: [0, 0, 0.03]
|
||||
radius: 0.05
|
||||
|
||||
156
src/curobo/content/configs/robot/spheres/franka.yml
Normal file
156
src/curobo/content/configs/robot/spheres/franka.yml
Normal file
@@ -0,0 +1,156 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
robot: 'Franka Panda'
|
||||
urdf_path: "urdf/franka_description/franka_panda_dyn.urdf"
|
||||
collision_spheres:
|
||||
panda_link0:
|
||||
- "center": [-0.08, 0.0, 0.05]
|
||||
"radius": 0.06
|
||||
- "center": [-0.0, 0.0, 0.05]
|
||||
"radius": 0.08
|
||||
panda_link1:
|
||||
- "center": [0.0, -0.08, 0.0]
|
||||
"radius": 0.06
|
||||
- "center": [0.0, -0.03, 0.0]
|
||||
"radius": 0.06
|
||||
- "center": [0.0, 0.0, -0.12]
|
||||
"radius": 0.06
|
||||
- "center": [0.0, 0.0, -0.17]
|
||||
"radius": 0.06
|
||||
panda_link2:
|
||||
- "center": [0.0, 0.0, 0.03]
|
||||
"radius": 0.06
|
||||
- "center": [0.0, 0.0, 0.08]
|
||||
"radius": 0.06
|
||||
- "center": [0.0, -0.12, 0.0]
|
||||
"radius": 0.06
|
||||
- "center": [0.0, -0.17, 0.0]
|
||||
"radius": 0.06
|
||||
panda_link3:
|
||||
- "center": [0.0, 0.0, -0.06]
|
||||
"radius": 0.05
|
||||
- "center": [0.0, 0.0, -0.1]
|
||||
"radius": 0.06
|
||||
- "center": [0.08, 0.06, 0.0]
|
||||
"radius": 0.055
|
||||
- "center": [0.08, 0.02, 0.0]
|
||||
"radius": 0.055
|
||||
panda_link4:
|
||||
- "center": [0.0, 0.0, 0.02]
|
||||
"radius": 0.055
|
||||
- "center": [0.0, 0.0, 0.06]
|
||||
"radius": 0.055
|
||||
- "center": [-0.08, 0.095, 0.0]
|
||||
"radius": 0.06
|
||||
- "center": [-0.08, 0.06, 0.0]
|
||||
"radius": 0.055
|
||||
panda_link5:
|
||||
- "center": [0.0, 0.055, 0.0]
|
||||
"radius": 0.05
|
||||
- "center": [0.0, 0.085, 0.0]
|
||||
"radius": 0.055
|
||||
- "center": [0.0, 0.000, -0.22]
|
||||
"radius": 0.05
|
||||
- "center": [0.0, 0.05, -0.18]
|
||||
"radius": 0.045
|
||||
- "center": [0.015, 0.08, -0.14]
|
||||
"radius": 0.03
|
||||
- "center": [0.015, 0.085, -0.11]
|
||||
"radius": 0.03
|
||||
- "center": [0.015, 0.09, -0.08]
|
||||
"radius": 0.03
|
||||
- "center": [0.015, 0.095, -0.05]
|
||||
"radius": 0.03
|
||||
- "center": [-0.015, 0.08, -0.14]
|
||||
"radius": 0.03
|
||||
- "center": [-0.015, 0.085, -0.11]
|
||||
"radius": 0.03
|
||||
- "center": [-0.015, 0.09, -0.08]
|
||||
"radius": 0.03
|
||||
- "center": [-0.015, 0.095, -0.05]
|
||||
"radius": 0.03
|
||||
panda_link6:
|
||||
- "center": [0.0, 0.0, 0.0]
|
||||
"radius": 0.05
|
||||
- "center": [0.08, 0.035, 0.0]
|
||||
"radius": 0.05
|
||||
- "center": [0.08, -0.01, 0.0]
|
||||
"radius": 0.05
|
||||
panda_link7:
|
||||
- "center": [0.0, 0.0, 0.07]
|
||||
"radius": 0.05
|
||||
- "center": [0.02, 0.04, 0.08]
|
||||
"radius": 0.025
|
||||
- "center": [0.04, 0.02, 0.08]
|
||||
"radius": 0.025
|
||||
- "center": [0.04, 0.06, 0.085]
|
||||
"radius": 0.02
|
||||
- "center": [0.06, 0.04, 0.085]
|
||||
"radius": 0.02
|
||||
panda_hand:
|
||||
- "center": [0.0, -0.075, 0.01]
|
||||
"radius": 0.03
|
||||
- "center": [0.0, -0.045, 0.01]
|
||||
"radius": 0.03
|
||||
- "center": [0.0, -0.015, 0.01]
|
||||
"radius": 0.03
|
||||
- "center": [0.0, 0.015, 0.01]
|
||||
"radius": 0.03
|
||||
- "center": [0.0, 0.045, 0.01]
|
||||
"radius": 0.03
|
||||
- "center": [0.0, 0.075, 0.01]
|
||||
"radius": 0.03
|
||||
- "center": [0.0, 0.065, -0.02]
|
||||
"radius": 0.03
|
||||
- "center": [0.0, -0.075, 0.03]
|
||||
"radius": 0.03
|
||||
- "center": [0.0, -0.045, 0.03]
|
||||
"radius": 0.03
|
||||
- "center": [0.0, -0.015, 0.03]
|
||||
"radius": 0.03
|
||||
- "center": [0.0, 0.015, 0.03]
|
||||
"radius": 0.03
|
||||
- "center": [0.0, 0.045, 0.03]
|
||||
"radius": 0.03
|
||||
- "center": [0.0, 0.075, 0.03]
|
||||
"radius": 0.03
|
||||
- "center": [0.0, -0.075, 0.05]
|
||||
"radius": 0.02
|
||||
- "center": [0.0, -0.045, 0.05]
|
||||
"radius": 0.02
|
||||
- "center": [0.0, -0.015, 0.05]
|
||||
"radius": 0.02
|
||||
- "center": [0.0, 0.015, 0.05]
|
||||
"radius": 0.02
|
||||
- "center": [0.0, 0.045, 0.05]
|
||||
"radius": 0.02
|
||||
- "center": [0.0, 0.075, 0.05]
|
||||
"radius": 0.02
|
||||
panda_leftfinger:
|
||||
- "center": [0.0, 0.01, 0.04]
|
||||
"radius": 0.01
|
||||
- "center": [0.0, 0.01, 0.014]
|
||||
"radius": 0.01
|
||||
panda_rightfinger:
|
||||
- "center": [0.0, -0.01, 0.04]
|
||||
"radius": 0.01
|
||||
- "center": [0.0, -0.01, 0.014]
|
||||
"radius": 0.01
|
||||
attached_object:
|
||||
- "center": [0.0, 0.0, 0.0]
|
||||
"radius": -10.0
|
||||
- "center": [0.0, 0.0, 0.0]
|
||||
"radius": -10.0
|
||||
- "center": [0.0, 0.0, 0.0]
|
||||
"radius": -10.0
|
||||
- "center": [0.0, 0.0, 0.0]
|
||||
"radius": -10.0
|
||||
|
||||
@@ -0,0 +1,142 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
collision_spheres:
|
||||
panda_link0:
|
||||
- "center": [0.0, 0.0, 0.085]
|
||||
"radius": 0.03 #0.07
|
||||
- "center": [-0.1, -0.0, 0.085]
|
||||
"radius": 0.03 #0.07
|
||||
panda_link1:
|
||||
- "center": [0.0, -0.08, 0.0]
|
||||
"radius": 0.06
|
||||
- "center": [0.0, -0.03, 0.0]
|
||||
"radius": 0.06
|
||||
- "center": [0.0, 0.0, -0.12]
|
||||
"radius": 0.06
|
||||
- "center": [0.0, 0.0, -0.17]
|
||||
"radius": 0.06
|
||||
panda_link2:
|
||||
- "center": [0.0, 0.0, 0.03]
|
||||
"radius": 0.06
|
||||
- "center": [0.0, 0.0, 0.08]
|
||||
"radius": 0.06
|
||||
- "center": [0.0, -0.12, 0.0]
|
||||
"radius": 0.06
|
||||
- "center": [0.0, -0.17, 0.0]
|
||||
"radius": 0.06
|
||||
panda_link3:
|
||||
- "center": [0.0, 0.0, -0.06]
|
||||
"radius": 0.05
|
||||
- "center": [0.0, 0.0, -0.1]
|
||||
"radius": 0.06
|
||||
- "center": [0.08, 0.06, 0.0]
|
||||
"radius": 0.055
|
||||
- "center": [0.08, 0.02, 0.0]
|
||||
"radius": 0.055
|
||||
panda_link4:
|
||||
- "center": [0.0, 0.0, 0.02]
|
||||
"radius": 0.06
|
||||
- "center": [0.0, 0.0, 0.06]
|
||||
"radius": 0.06
|
||||
- "center": [-0.08, 0.095, 0.0]
|
||||
"radius": 0.06
|
||||
- "center": [-0.08, 0.06, 0.0]
|
||||
"radius": 0.06
|
||||
panda_link5:
|
||||
- "center": [0.0, 0.03, 0.0]
|
||||
"radius": 0.06
|
||||
- "center": [0.0, 0.08, 0.0]
|
||||
"radius": 0.06
|
||||
- "center": [0.0, 0.000, -0.22]
|
||||
"radius": 0.06
|
||||
- "center": [0.0, 0.05, -0.18]
|
||||
"radius": 0.06
|
||||
- "center": [0.01, 0.08, -0.14]
|
||||
"radius": 0.06
|
||||
- "center": [0.01, 0.085, -0.11]
|
||||
"radius": 0.025
|
||||
- "center": [0.01, 0.09, -0.08]
|
||||
"radius": 0.025
|
||||
- "center": [0.01, 0.095, -0.05]
|
||||
"radius": 0.025
|
||||
- "center": [-0.01, 0.08, -0.14]
|
||||
"radius": 0.025
|
||||
- "center": [-0.01, 0.085, -0.11]
|
||||
"radius": 0.025
|
||||
- "center": [-0.01, 0.09, -0.08]
|
||||
"radius": 0.025
|
||||
- "center": [-0.01, 0.095, -0.05]
|
||||
"radius": 0.025
|
||||
panda_link6:
|
||||
- "center": [0.0, 0.0, 0.012]
|
||||
"radius": 0.052
|
||||
- "center": [0.08, 0.03, 0.0]
|
||||
"radius": 0.05
|
||||
- "center": [0.08, -0.01, 0.0]
|
||||
"radius": 0.05
|
||||
panda_link7:
|
||||
- "center": [0.0, 0.0, 0.07]
|
||||
"radius": 0.045
|
||||
- "center": [0.02, 0.04, 0.08]
|
||||
"radius": 0.025
|
||||
- "center": [0.04, 0.02, 0.08]
|
||||
"radius": 0.025
|
||||
- "center": [0.04, 0.06, 0.085]
|
||||
"radius": 0.02
|
||||
- "center": [0.06, 0.04, 0.085]
|
||||
"radius": 0.02
|
||||
panda_hand:
|
||||
- "center": [0.0, -0.075, 0.01]
|
||||
"radius": 0.024
|
||||
- "center": [0.0, -0.045, 0.01]
|
||||
"radius": 0.024
|
||||
- "center": [0.0, -0.015, 0.01]
|
||||
"radius": 0.024
|
||||
- "center": [0.0, 0.015, 0.01]
|
||||
"radius": 0.024
|
||||
- "center": [0.0, 0.045, 0.01]
|
||||
"radius": 0.024
|
||||
- "center": [0.0, 0.075, 0.01]
|
||||
"radius": 0.024
|
||||
- "center": [0.0, -0.08, 0.03]
|
||||
"radius": 0.022
|
||||
- "center": [0.0, -0.045, 0.03]
|
||||
"radius": 0.022
|
||||
- "center": [0.0, -0.015, 0.03]
|
||||
"radius": 0.022
|
||||
- "center": [0.0, 0.015, 0.03]
|
||||
"radius": 0.022
|
||||
- "center": [0.0, 0.045, 0.03]
|
||||
"radius": 0.022
|
||||
- "center": [0.0, 0.08, 0.03]
|
||||
"radius": 0.022
|
||||
- "center": [0.0, -0.08, 0.045]
|
||||
"radius": 0.022
|
||||
- "center": [0.0, -0.045, 0.045]
|
||||
"radius": 0.022
|
||||
- "center": [0.0, -0.015, 0.045]
|
||||
"radius": 0.022
|
||||
- "center": [0.0, 0.015, 0.045]
|
||||
"radius": 0.022
|
||||
- "center": [0.0, 0.045, 0.045]
|
||||
"radius": 0.022
|
||||
- "center": [0.0, 0.08, 0.045]
|
||||
"radius": 0.022
|
||||
panda_leftfinger:
|
||||
- "center": [0.0, 0.01, 0.043]
|
||||
"radius": 0.0125 # 0.01
|
||||
- "center": [0.0, 0.02, 0.015]
|
||||
"radius": 0.0125
|
||||
panda_rightfinger:
|
||||
- "center": [0.0, -0.01, 0.043]
|
||||
"radius": 0.0125 # 0.01
|
||||
- "center": [0.0, -0.02, 0.015]
|
||||
"radius": 0.0125
|
||||
144
src/curobo/content/configs/robot/spheres/franka_mesh.yml
Normal file
144
src/curobo/content/configs/robot/spheres/franka_mesh.yml
Normal file
@@ -0,0 +1,144 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
collision_spheres:
|
||||
panda_link0:
|
||||
- "center": [0.0, 0.0, 0.085]
|
||||
"radius": 0.03 #0.07
|
||||
- "center": [-0.1, -0.0, 0.085]
|
||||
"radius": 0.03 #0.07
|
||||
panda_link1:
|
||||
- "center": [0.0, -0.08, 0.0]
|
||||
"radius": 0.055
|
||||
- "center": [0.0, -0.03, 0.0]
|
||||
"radius": 0.06
|
||||
- "center": [0.0, 0.0, -0.12]
|
||||
"radius": 0.06
|
||||
- "center": [0.0, 0.0, -0.17]
|
||||
"radius": 0.06
|
||||
panda_link2:
|
||||
- "center": [0.0, 0.0, 0.03]
|
||||
"radius": 0.055
|
||||
- "center": [0.0, 0.0, 0.08]
|
||||
"radius": 0.055
|
||||
- "center": [0.0, -0.12, 0.0]
|
||||
"radius": 0.055
|
||||
- "center": [0.0, -0.17, 0.0]
|
||||
"radius": 0.055
|
||||
panda_link3:
|
||||
- "center": [0.0, 0.0, -0.06]
|
||||
"radius": 0.05
|
||||
- "center": [0.0, 0.0, -0.1]
|
||||
"radius": 0.06
|
||||
- "center": [0.08, 0.06, 0.0]
|
||||
"radius": 0.052
|
||||
- "center": [0.08, 0.02, 0.0]
|
||||
"radius": 0.052
|
||||
panda_link4:
|
||||
- "center": [0.0, 0.0, 0.02]
|
||||
"radius": 0.052
|
||||
- "center": [0.0, 0.0, 0.06]
|
||||
"radius": 0.052
|
||||
- "center": [-0.08, 0.095, 0.0]
|
||||
"radius": 0.055
|
||||
- "center": [-0.08, 0.06, 0.0]
|
||||
"radius": 0.052
|
||||
panda_link5:
|
||||
- "center": [0.0, 0.03, 0.0]
|
||||
"radius": 0.05
|
||||
- "center": [0.0, 0.082, 0.0]
|
||||
"radius": 0.05
|
||||
- "center": [0.0, 0.000, -0.22]
|
||||
"radius": 0.05
|
||||
- "center": [0.0, 0.052, -0.18]
|
||||
"radius": 0.04
|
||||
- "center": [0.01, 0.08, -0.14]
|
||||
"radius": 0.022
|
||||
- "center": [0.01, 0.085, -0.11]
|
||||
"radius": 0.022
|
||||
- "center": [0.01, 0.09, -0.08]
|
||||
"radius": 0.022
|
||||
- "center": [0.01, 0.095, -0.05]
|
||||
"radius": 0.022
|
||||
- "center": [-0.01, 0.08, -0.14]
|
||||
"radius": 0.022
|
||||
- "center": [-0.01, 0.085, -0.11]
|
||||
"radius": 0.022
|
||||
- "center": [-0.01, 0.09, -0.08]
|
||||
"radius": 0.022
|
||||
- "center": [-0.01, 0.095, -0.05]
|
||||
"radius": 0.022
|
||||
- "center": [0.0, -0.009, 0.0]
|
||||
"radius": 0.05
|
||||
panda_link6:
|
||||
- "center": [0.085, 0.035, 0.0]
|
||||
"radius": 0.045
|
||||
- "center": [0.085, 0.0, 0.0]
|
||||
"radius": 0.045
|
||||
- "center": [0.085, -0.015, 0.0]
|
||||
"radius": 0.045
|
||||
panda_link7:
|
||||
- "center": [0.0, 0.0, 0.07]
|
||||
"radius": 0.045
|
||||
- "center": [0.02, 0.04, 0.08]
|
||||
"radius": 0.024
|
||||
- "center": [0.04, 0.02, 0.08]
|
||||
"radius": 0.024
|
||||
- "center": [0.04, 0.06, 0.085]
|
||||
"radius": 0.02
|
||||
- "center": [0.06, 0.04, 0.085]
|
||||
"radius": 0.02
|
||||
panda_hand:
|
||||
- "center": [0.0, -0.075, 0.01]
|
||||
"radius": 0.023
|
||||
- "center": [0.0, -0.045, 0.01]
|
||||
"radius": 0.023
|
||||
- "center": [0.0, -0.015, 0.01]
|
||||
"radius": 0.023
|
||||
- "center": [0.0, 0.015, 0.01]
|
||||
"radius": 0.023
|
||||
- "center": [0.0, 0.045, 0.01]
|
||||
"radius": 0.023
|
||||
- "center": [0.0, 0.075, 0.01]
|
||||
"radius": 0.023
|
||||
- "center": [0.0, -0.08, 0.03]
|
||||
"radius": 0.022
|
||||
- "center": [0.0, -0.045, 0.03]
|
||||
"radius": 0.022
|
||||
- "center": [0.0, -0.015, 0.03]
|
||||
"radius": 0.022
|
||||
- "center": [0.0, 0.015, 0.03]
|
||||
"radius": 0.022
|
||||
- "center": [0.0, 0.045, 0.03]
|
||||
"radius": 0.022
|
||||
- "center": [0.0, 0.08, 0.03]
|
||||
"radius": 0.022
|
||||
- "center": [0.0, -0.08, 0.045]
|
||||
"radius": 0.022
|
||||
- "center": [0.0, -0.045, 0.045]
|
||||
"radius": 0.022
|
||||
- "center": [0.0, -0.015, 0.045]
|
||||
"radius": 0.022
|
||||
- "center": [0.0, 0.015, 0.045]
|
||||
"radius": 0.022
|
||||
- "center": [0.0, 0.045, 0.045]
|
||||
"radius": 0.022
|
||||
- "center": [0.0, 0.08, 0.045]
|
||||
"radius": 0.022
|
||||
panda_leftfinger:
|
||||
- "center": [0.0, 0.01, 0.043]
|
||||
"radius": 0.011 # 25
|
||||
- "center": [0.0, 0.02, 0.015]
|
||||
"radius": 0.011 # 25
|
||||
panda_rightfinger:
|
||||
- "center": [0.0, -0.01, 0.043]
|
||||
"radius": 0.011 #25
|
||||
- "center": [0.0, -0.02, 0.015]
|
||||
"radius": 0.011 #25
|
||||
146
src/curobo/content/configs/robot/spheres/franka_mesh_inside.yml
Normal file
146
src/curobo/content/configs/robot/spheres/franka_mesh_inside.yml
Normal file
@@ -0,0 +1,146 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
collision_spheres:
|
||||
panda_link0:
|
||||
- "center": [0.0, 0.0, 0.1]
|
||||
"radius": 0.02
|
||||
panda_link1:
|
||||
- "center": [0.0, -0.08, 0.0]
|
||||
"radius": 0.06
|
||||
- "center": [0.0, -0.03, 0.0]
|
||||
"radius": 0.06
|
||||
- "center": [0.0, 0.0, -0.12]
|
||||
"radius": 0.06
|
||||
- "center": [0.0, 0.0, -0.17]
|
||||
"radius": 0.06
|
||||
panda_link2:
|
||||
- "center": [0.0, 0.0, 0.03]
|
||||
"radius": 0.06
|
||||
- "center": [0.0, 0.0, 0.08]
|
||||
"radius": 0.06
|
||||
- "center": [0.0, -0.12, 0.0]
|
||||
"radius": 0.06
|
||||
- "center": [0.0, -0.17, 0.0]
|
||||
"radius": 0.06
|
||||
panda_link3:
|
||||
- "center": [0.0, 0.0, -0.06]
|
||||
"radius": 0.05
|
||||
- "center": [0.0, 0.0, -0.1]
|
||||
"radius": 0.06
|
||||
- "center": [0.08, 0.06, 0.0]
|
||||
"radius": 0.055
|
||||
- "center": [0.08, 0.02, 0.0]
|
||||
"radius": 0.055
|
||||
panda_link4:
|
||||
- "center": [0.0, 0.0, 0.02]
|
||||
"radius": 0.055
|
||||
- "center": [0.0, 0.0, 0.06]
|
||||
"radius": 0.055
|
||||
- "center": [-0.08, 0.095, 0.0]
|
||||
"radius": 0.06
|
||||
- "center": [-0.08, 0.06, 0.0]
|
||||
"radius": 0.055
|
||||
panda_link5:
|
||||
- "center": [0.0, 0.055, 0.0]
|
||||
"radius": 0.06
|
||||
- "center": [0.0, 0.075, 0.0]
|
||||
"radius": 0.06
|
||||
- "center": [0.0, 0.000, -0.22]
|
||||
"radius": 0.06
|
||||
- "center": [0.0, 0.05, -0.18]
|
||||
"radius": 0.05
|
||||
- "center": [0.01, 0.08, -0.14]
|
||||
"radius": 0.025
|
||||
- "center": [0.01, 0.085, -0.11]
|
||||
"radius": 0.025
|
||||
- "center": [0.01, 0.09, -0.08]
|
||||
"radius": 0.025
|
||||
- "center": [0.01, 0.095, -0.05]
|
||||
"radius": 0.025
|
||||
- "center": [-0.01, 0.08, -0.14]
|
||||
"radius": 0.025
|
||||
- "center": [-0.01, 0.085, -0.11]
|
||||
"radius": 0.025
|
||||
- "center": [-0.01, 0.09, -0.08]
|
||||
"radius": 0.025
|
||||
- "center": [-0.01, 0.095, -0.05]
|
||||
"radius": 0.025
|
||||
panda_link6:
|
||||
- "center": [0.0, 0.0, 0.0]
|
||||
"radius": 0.06
|
||||
- "center": [0.08, 0.03, 0.0]
|
||||
"radius": 0.06
|
||||
- "center": [0.08, -0.01, 0.0]
|
||||
"radius": 0.06
|
||||
panda_link7:
|
||||
- "center": [0.0, 0.0, 0.07]
|
||||
"radius": 0.05
|
||||
- "center": [0.02, 0.04, 0.08]
|
||||
"radius": 0.025
|
||||
- "center": [0.04, 0.02, 0.08]
|
||||
"radius": 0.025
|
||||
- "center": [0.04, 0.06, 0.085]
|
||||
"radius": 0.02
|
||||
- "center": [0.06, 0.04, 0.085]
|
||||
"radius": 0.02
|
||||
panda_hand:
|
||||
- "center": [0.0, -0.075, 0.01]
|
||||
"radius": 0.028
|
||||
- "center": [0.0, -0.045, 0.01]
|
||||
"radius": 0.028
|
||||
- "center": [0.0, -0.015, 0.01]
|
||||
"radius": 0.028
|
||||
- "center": [0.0, 0.015, 0.01]
|
||||
"radius": 0.028
|
||||
- "center": [0.0, 0.045, 0.01]
|
||||
"radius": 0.028
|
||||
- "center": [0.0, 0.075, 0.01]
|
||||
"radius": 0.028
|
||||
- "center": [0.0, -0.075, 0.03]
|
||||
"radius": 0.026
|
||||
- "center": [0.0, -0.045, 0.03]
|
||||
"radius": 0.026
|
||||
- "center": [0.0, -0.015, 0.03]
|
||||
"radius": 0.026
|
||||
- "center": [0.0, 0.015, 0.03]
|
||||
"radius": 0.026
|
||||
- "center": [0.0, 0.045, 0.03]
|
||||
"radius": 0.026
|
||||
- "center": [0.0, 0.075, 0.03]
|
||||
"radius": 0.026
|
||||
- "center": [0.0, -0.075, 0.05]
|
||||
"radius": 0.024
|
||||
- "center": [0.0, -0.045, 0.05]
|
||||
"radius": 0.024
|
||||
- "center": [0.0, -0.015, 0.05]
|
||||
"radius": 0.024
|
||||
- "center": [0.0, 0.015, 0.05]
|
||||
"radius": 0.024
|
||||
- "center": [0.0, 0.045, 0.05]
|
||||
"radius": 0.024
|
||||
- "center": [0.0, 0.075, 0.05]
|
||||
"radius": 0.024
|
||||
#panda_leftfinger:
|
||||
# - "center": [0.0, 0.01, 0.043]
|
||||
# "radius": 0.01
|
||||
# - "center": [0.0, 0.02, 0.015]
|
||||
# "radius": 0.01
|
||||
#panda_rightfinger:
|
||||
# - "center": [0.0, -0.01, 0.043]
|
||||
# "radius": 0.01
|
||||
# - "center": [0.0, -0.02, 0.015]
|
||||
# "radius": 0.01
|
||||
panda_leftfinger:
|
||||
- "center": [0.0, 0.0075, 0.0]
|
||||
"radius": 0.0108
|
||||
panda_rightfinger:
|
||||
- "center": [0.0, -0.0075, 0.0]
|
||||
"radius": 0.0108
|
||||
159
src/curobo/content/configs/robot/spheres/franka_mesh_mb.yml
Normal file
159
src/curobo/content/configs/robot/spheres/franka_mesh_mb.yml
Normal file
@@ -0,0 +1,159 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
collision_spheres:
|
||||
panda_link0:
|
||||
- "center": [0.0, 0.0, 0.1]
|
||||
"radius": 0.03
|
||||
- "center": [-0.1, -0.0, 0.1]
|
||||
"radius": 0.03
|
||||
panda_link1:
|
||||
- "center": [0.0, -0.08, 0.0]
|
||||
"radius": 0.06
|
||||
- "center": [0.0, -0.03, 0.0]
|
||||
"radius": 0.06
|
||||
- "center": [0.0, 0.0, -0.12]
|
||||
"radius": 0.06
|
||||
- "center": [0.0, 0.0, -0.17]
|
||||
"radius": 0.06
|
||||
panda_link2:
|
||||
- "center": [0.0, 0.0, 0.03]
|
||||
"radius": 0.06
|
||||
- "center": [0.0, 0.0, 0.08]
|
||||
"radius": 0.06
|
||||
- "center": [0.0, -0.12, 0.0]
|
||||
"radius": 0.06
|
||||
- "center": [0.0, -0.17, 0.0]
|
||||
"radius": 0.06
|
||||
panda_link3:
|
||||
- "center": [0.0, 0.0, -0.06]
|
||||
"radius": 0.05
|
||||
- "center": [0.0, 0.0, -0.1]
|
||||
"radius": 0.06
|
||||
- "center": [0.08, 0.06, 0.0]
|
||||
"radius": 0.055
|
||||
- "center": [0.08, 0.02, 0.0]
|
||||
"radius": 0.055
|
||||
panda_link4:
|
||||
- "center": [0.0, 0.0, 0.02]
|
||||
"radius": 0.055
|
||||
- "center": [0.0, 0.0, 0.06]
|
||||
"radius": 0.055
|
||||
- "center": [-0.08, 0.095, 0.0]
|
||||
"radius": 0.06
|
||||
- "center": [-0.08, 0.06, 0.0]
|
||||
"radius": 0.055
|
||||
panda_link5:
|
||||
- "center": [0.0, 0.03, 0.0]
|
||||
"radius": 0.05
|
||||
- "center": [0.0, 0.08, 0.0]
|
||||
"radius": 0.05
|
||||
- "center": [0.0, 0.000, -0.22]
|
||||
"radius": 0.05
|
||||
- "center": [0.0, 0.05, -0.18]
|
||||
"radius": 0.045
|
||||
- "center": [0.01, 0.08, -0.14]
|
||||
"radius": 0.025
|
||||
- "center": [0.01, 0.085, -0.11]
|
||||
"radius": 0.025
|
||||
- "center": [0.01, 0.09, -0.08]
|
||||
"radius": 0.025
|
||||
- "center": [0.01, 0.095, -0.05]
|
||||
"radius": 0.025
|
||||
- "center": [-0.01, 0.08, -0.14]
|
||||
"radius": 0.025
|
||||
- "center": [-0.01, 0.085, -0.11]
|
||||
"radius": 0.025
|
||||
- "center": [-0.01, 0.09, -0.08]
|
||||
"radius": 0.025
|
||||
- "center": [-0.01, 0.095, -0.05]
|
||||
"radius": 0.025
|
||||
panda_link6:
|
||||
- "center": [0.0, 0.0, 0.012]
|
||||
"radius": 0.052
|
||||
- "center": [0.08, 0.03, 0.0]
|
||||
"radius": 0.05
|
||||
- "center": [0.08, -0.01, 0.0]
|
||||
"radius": 0.05
|
||||
panda_link7:
|
||||
- "center": [0.0, 0.0, 0.07]
|
||||
"radius": 0.045
|
||||
- "center": [0.02, 0.04, 0.08]
|
||||
"radius": 0.025
|
||||
- "center": [0.04, 0.02, 0.08]
|
||||
"radius": 0.025
|
||||
- "center": [0.04, 0.06, 0.085]
|
||||
"radius": 0.02
|
||||
- "center": [0.06, 0.04, 0.085]
|
||||
"radius": 0.02
|
||||
panda_hand:
|
||||
- "center": [0.0, -0.075, 0.01]
|
||||
"radius": 0.024
|
||||
- "center": [0.0, -0.045, 0.01]
|
||||
"radius": 0.024
|
||||
- "center": [0.0, -0.015, 0.01]
|
||||
"radius": 0.024
|
||||
- "center": [0.0, 0.015, 0.01]
|
||||
"radius": 0.024
|
||||
- "center": [0.0, 0.045, 0.01]
|
||||
"radius": 0.024
|
||||
- "center": [0.0, 0.073, 0.01]
|
||||
"radius": 0.02
|
||||
- "center": [0.0, -0.08, 0.03]
|
||||
"radius": 0.022
|
||||
- "center": [0.0, -0.045, 0.03]
|
||||
"radius": 0.022
|
||||
- "center": [0.0, -0.015, 0.03]
|
||||
"radius": 0.022
|
||||
- "center": [0.0, 0.015, 0.03]
|
||||
"radius": 0.022
|
||||
- "center": [0.0, 0.045, 0.03]
|
||||
"radius": 0.022
|
||||
- "center": [0.0, 0.075, 0.03]
|
||||
"radius": 0.02
|
||||
- "center": [0.0, -0.08, 0.045]
|
||||
"radius": 0.02
|
||||
- "center": [0.0, -0.045, 0.045]
|
||||
"radius": 0.02
|
||||
- "center": [0.0, -0.015, 0.045]
|
||||
"radius": 0.02
|
||||
- "center": [0.0, 0.015, 0.045]
|
||||
"radius": 0.02
|
||||
- "center": [0.0, 0.045, 0.045]
|
||||
"radius": 0.02
|
||||
- "center": [0.0, 0.078, 0.045]
|
||||
"radius": 0.02
|
||||
#- "center": [0.0, -0.02, 0.1]
|
||||
# "radius": 0.015
|
||||
#- "center": [0.0, 0.02, 0.1]
|
||||
# "radius": 0.015
|
||||
#- "center": [0.0, 0.0, 0.1]
|
||||
# "radius": 0.015
|
||||
panda_leftfinger:
|
||||
- "center": [0.0, 0.01, 0.043]
|
||||
"radius": 0.01 # 0.01
|
||||
- "center": [0.0, 0.02, 0.015]
|
||||
"radius": 0.005
|
||||
panda_rightfinger:
|
||||
|
||||
- "center": [0.0, -0.01, 0.043]
|
||||
"radius": 0.01 # 0.01
|
||||
- "center": [0.0, -0.02, 0.015]
|
||||
"radius": 0.005
|
||||
attached_object:
|
||||
- "center": [0.0, 0.0, 0.0]
|
||||
"radius": -10.0
|
||||
- "center": [0.0, 0.0, 0.0]
|
||||
"radius": -10.0
|
||||
- "center": [0.0, 0.0, 0.0]
|
||||
"radius": -10.0
|
||||
- "center": [0.0, 0.0, 0.0]
|
||||
"radius": -10.0
|
||||
|
||||
141
src/curobo/content/configs/robot/spheres/franka_real_robot.yml
Normal file
141
src/curobo/content/configs/robot/spheres/franka_real_robot.yml
Normal file
@@ -0,0 +1,141 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
robot: 'Franka Panda'
|
||||
urdf_path: "urdf/franka_description/franka_panda_dyn.urdf"
|
||||
collision_spheres:
|
||||
panda_link0:
|
||||
- "center": [-0.08, 0.0, 0.05]
|
||||
"radius": 0.06
|
||||
- "center": [-0.0, 0.0, 0.05]
|
||||
"radius": 0.08
|
||||
panda_link1:
|
||||
- "center": [0.0, -0.08, 0.0]
|
||||
"radius": 0.1
|
||||
- "center": [0.0, -0.03, 0.0]
|
||||
"radius": 0.1
|
||||
- "center": [0.0, 0.0, -0.12]
|
||||
"radius": 0.06
|
||||
- "center": [0.0, 0.0, -0.17]
|
||||
"radius": 0.06
|
||||
panda_link2:
|
||||
- "center": [0.0, 0.0, 0.03]
|
||||
"radius": 0.1
|
||||
- "center": [0.0, 0.0, 0.08]
|
||||
"radius": 0.1
|
||||
- "center": [0.0, -0.12, 0.0]
|
||||
"radius": 0.06
|
||||
- "center": [0.0, -0.17, 0.0]
|
||||
"radius": 0.06
|
||||
panda_link3:
|
||||
- "center": [0.0, 0.0, -0.06]
|
||||
"radius": 0.05
|
||||
- "center": [0.0, 0.0, -0.1]
|
||||
"radius": 0.06
|
||||
- "center": [0.08, 0.06, 0.0]
|
||||
"radius": 0.055
|
||||
- "center": [0.08, 0.02, 0.0]
|
||||
"radius": 0.055
|
||||
panda_link4:
|
||||
- "center": [0.0, 0.0, 0.02]
|
||||
"radius": 0.055
|
||||
- "center": [0.0, 0.0, 0.06]
|
||||
"radius": 0.055
|
||||
- "center": [-0.08, 0.095, 0.0]
|
||||
"radius": 0.06
|
||||
- "center": [-0.08, 0.06, 0.0]
|
||||
"radius": 0.055
|
||||
panda_link5:
|
||||
- "center": [0.0, 0.055, 0.0]
|
||||
"radius": 0.05
|
||||
- "center": [0.0, 0.085, 0.0]
|
||||
"radius": 0.055
|
||||
- "center": [0.0, 0.000, -0.22]
|
||||
"radius": 0.05
|
||||
- "center": [0.0, 0.05, -0.18]
|
||||
"radius": 0.045
|
||||
- "center": [0.015, 0.08, -0.14]
|
||||
"radius": 0.03
|
||||
- "center": [0.015, 0.085, -0.11]
|
||||
"radius": 0.03
|
||||
- "center": [0.015, 0.09, -0.08]
|
||||
"radius": 0.03
|
||||
- "center": [0.015, 0.095, -0.05]
|
||||
"radius": 0.03
|
||||
- "center": [-0.015, 0.08, -0.14]
|
||||
"radius": 0.03
|
||||
- "center": [-0.015, 0.085, -0.11]
|
||||
"radius": 0.03
|
||||
- "center": [-0.015, 0.09, -0.08]
|
||||
"radius": 0.03
|
||||
- "center": [-0.015, 0.095, -0.05]
|
||||
"radius": 0.03
|
||||
panda_link6:
|
||||
- "center": [0.0, 0.0, 0.0]
|
||||
"radius": 0.05
|
||||
- "center": [0.08, 0.035, 0.0]
|
||||
"radius": 0.052
|
||||
- "center": [0.08, -0.01, 0.0]
|
||||
"radius": 0.05
|
||||
panda_link7:
|
||||
- "center": [0.0, 0.0, 0.07]
|
||||
"radius": 0.05
|
||||
- "center": [0.02, 0.04, 0.08]
|
||||
"radius": 0.025
|
||||
- "center": [0.04, 0.02, 0.08]
|
||||
"radius": 0.025
|
||||
- "center": [0.04, 0.06, 0.085]
|
||||
"radius": 0.02
|
||||
- "center": [0.06, 0.04, 0.085]
|
||||
"radius": 0.02
|
||||
panda_hand:
|
||||
- "center": [0.0, -0.08, 0.01]
|
||||
"radius": 0.03
|
||||
- "center": [0.0, -0.045, 0.01]
|
||||
"radius": 0.03
|
||||
- "center": [0.0, -0.015, 0.01]
|
||||
"radius": 0.03
|
||||
- "center": [0.0, 0.015, 0.01]
|
||||
"radius": 0.03
|
||||
- "center": [0.0, 0.045, 0.01]
|
||||
"radius": 0.03
|
||||
- "center": [0.0, 0.08, 0.01]
|
||||
"radius": 0.03
|
||||
- "center": [0.0, 0.065, -0.02]
|
||||
"radius": 0.05
|
||||
- "center": [0.0, -0.08, 0.05]
|
||||
"radius": 0.05
|
||||
- "center": [0.0, -0.045, 0.05]
|
||||
"radius": 0.05
|
||||
- "center": [0.0, -0.015, 0.05]
|
||||
"radius": 0.05
|
||||
- "center": [0.0, 0.015, 0.05]
|
||||
"radius": 0.05
|
||||
- "center": [0.0, 0.045, 0.05]
|
||||
"radius": 0.05
|
||||
- "center": [0.0, 0.08, 0.05]
|
||||
"radius": 0.05
|
||||
- "center": [0.0, 0.08, 0.08]
|
||||
"radius": 0.05
|
||||
- "center": [0.0, -0.08, 0.08]
|
||||
"radius": 0.05
|
||||
- "center": [0.0, 0.05, 0.08]
|
||||
"radius": 0.05
|
||||
- "center": [0.0, -0.05, 0.08]
|
||||
"radius": 0.05
|
||||
- "center": [0.0, 0.0, 0.08]
|
||||
"radius": 0.05
|
||||
panda_leftfinger:
|
||||
- "center": [0.0, 0.01, 0.034]
|
||||
"radius": 0.02
|
||||
panda_rightfinger:
|
||||
- "center": [0.0, -0.01, 0.034]
|
||||
"radius": 0.02
|
||||
|
||||
157
src/curobo/content/configs/robot/spheres/iiwa_allegro.yml
Normal file
157
src/curobo/content/configs/robot/spheres/iiwa_allegro.yml
Normal file
@@ -0,0 +1,157 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
|
||||
collision_spheres:
|
||||
iiwa7_link_0:
|
||||
- "center": [0.0, 0.0, 0.05]
|
||||
"radius": 0.10
|
||||
iiwa7_link_1:
|
||||
- "center": [0.0, 0.0, 0.0]
|
||||
"radius": 0.08
|
||||
- "center": [0.0, -0.05, 0.1]
|
||||
"radius": 0.07
|
||||
- "center": [0.0, -0.05, 0.18]
|
||||
"radius": 0.08
|
||||
iiwa7_link_2:
|
||||
- "center": [0.0, 0.0, 0.0]
|
||||
"radius": 0.08
|
||||
- "center": [0.0, 0.02, 0.06]
|
||||
"radius": 0.07
|
||||
- "center": [0.0, 0.1, 0.03]
|
||||
"radius": 0.07
|
||||
- "center": [0.0, 0.18, 0.0]
|
||||
"radius": 0.08
|
||||
iiwa7_link_3:
|
||||
- "center": [0.0, 0.0, 0.08]
|
||||
"radius": 0.08
|
||||
- "center": [0.0, 0.06, 0.16]
|
||||
"radius": 0.07
|
||||
- "center": [0.0, 0.05, 0.22]
|
||||
"radius": 0.07
|
||||
iiwa7_link_4:
|
||||
- "center": [0.0, 0.0, 0.0]
|
||||
"radius": 0.08
|
||||
- "center": [0.0, 0.0, 0.05]
|
||||
"radius": 0.07
|
||||
- "center": [0.0, 0.07, 0.05]
|
||||
"radius": 0.06
|
||||
- "center": [0.0, 0.11, 0.03]
|
||||
"radius": 0.06
|
||||
- "center": [0.0, 0.15, 0.01]
|
||||
"radius": 0.07
|
||||
iiwa7_link_5:
|
||||
- "center": [0.0, 0.0, 0.02]
|
||||
"radius": 0.08
|
||||
- "center": [0.0, 0.03, 0.07]
|
||||
"radius": 0.06
|
||||
- "center": [0.0, 0.08, 0.13]
|
||||
"radius": 0.05
|
||||
iiwa7_link_6:
|
||||
- "center": [0.0, 0.0, 0.0]
|
||||
"radius": 0.06
|
||||
- "center": [0., 0., 0.05]
|
||||
"radius": 0.08
|
||||
- "center": [0., -0.04, 0.075]
|
||||
"radius": 0.06
|
||||
- "center": [0., 0.08, 0.06]
|
||||
"radius": 0.065
|
||||
- "center": [0., 0.16, 0.06]
|
||||
"radius": 0.05
|
||||
palm_link:
|
||||
- "center": [0.0, 0.01, -0.04]
|
||||
"radius": 0.07
|
||||
index_link_1:
|
||||
- "center": [0.0, 0.0, 0.0]
|
||||
"radius": 0.016
|
||||
- "center": [0.02, 0.0, 0.0]
|
||||
"radius": 0.016
|
||||
- "center": [0.04, 0.0, 0.0]
|
||||
"radius": 0.016
|
||||
index_link_2:
|
||||
- "center": [0.0, 0.0, 0.0]
|
||||
"radius": 0.016
|
||||
- "center": [0.02, 0.0, 0.0]
|
||||
"radius": 0.016
|
||||
index_link_3:
|
||||
- "center": [0.0, 0.0, 0.0]
|
||||
"radius": 0.016
|
||||
- "center": [0.02, 0.0, 0.0]
|
||||
"radius": 0.015
|
||||
- "center": [0.04, 0.0, 0.0]
|
||||
"radius": 0.01
|
||||
- "center": [0.0475, 0.0025, 0.0]
|
||||
"radius": 0.01
|
||||
- "center": [0.055, 0.005, 0.0]
|
||||
"radius": 0.01
|
||||
middle_link_1:
|
||||
- "center": [0.0, 0.0, 0.0]
|
||||
"radius": 0.016
|
||||
- "center": [0.02, 0.0, 0.0]
|
||||
"radius": 0.016
|
||||
- "center": [0.04, 0.0, 0.0]
|
||||
"radius": 0.016
|
||||
middle_link_2:
|
||||
- "center": [0.0, 0.0, 0.0]
|
||||
"radius": 0.016
|
||||
- "center": [0.02, 0.0, 0.0]
|
||||
"radius": 0.016
|
||||
middle_link_3:
|
||||
- "center": [0.0, 0.0, 0.0]
|
||||
"radius": 0.016
|
||||
- "center": [0.02, 0.0, 0.0]
|
||||
"radius": 0.015
|
||||
- "center": [0.04, 0.0, 0.0]
|
||||
"radius": 0.01
|
||||
- "center": [0.0475, 0.0025, 0.0]
|
||||
"radius": 0.01
|
||||
- "center": [0.055, 0.005, 0.0]
|
||||
"radius": 0.01
|
||||
ring_link_1:
|
||||
- "center": [0.0, 0.0, 0.0]
|
||||
"radius": 0.016
|
||||
- "center": [0.02, 0.0, 0.0]
|
||||
"radius": 0.016
|
||||
- "center": [0.04, 0.0, 0.0]
|
||||
"radius": 0.016
|
||||
ring_link_2:
|
||||
- "center": [0.0, 0.0, 0.0]
|
||||
"radius": 0.016
|
||||
- "center": [0.02, 0.0, 0.0]
|
||||
"radius": 0.016
|
||||
ring_link_3:
|
||||
- "center": [0.0, 0.0, 0.0]
|
||||
"radius": 0.016
|
||||
- "center": [0.02, 0.0, 0.0]
|
||||
"radius": 0.015
|
||||
- "center": [0.04, 0.0, 0.0]
|
||||
"radius": 0.01
|
||||
- "center": [0.0475, 0.0025, 0.0]
|
||||
"radius": 0.01
|
||||
- "center": [0.055, 0.005, 0.0]
|
||||
"radius": 0.01
|
||||
thumb_link_2:
|
||||
- "center": [0.0, 0.0, 0.0]
|
||||
"radius": 0.016
|
||||
- "center": [0.02, 0.0, 0.0]
|
||||
"radius": 0.016
|
||||
thumb_link_3:
|
||||
- "center": [0.0, 0.0, 0.0]
|
||||
"radius": 0.016
|
||||
- "center": [0.02, 0.0, 0.0]
|
||||
"radius": 0.016
|
||||
- "center": [0.04, 0.0, 0.0]
|
||||
"radius": 0.015
|
||||
- "center": [0.05, 0.0, 0.0]
|
||||
"radius": 0.01
|
||||
- "center": [0.06, 0.003, 0.0]
|
||||
"radius": 0.01
|
||||
- "center": [0.07, 0.0055, 0.0]
|
||||
"radius": 0.01
|
||||
76
src/curobo/content/configs/robot/spheres/kinova_gen3.yml
Normal file
76
src/curobo/content/configs/robot/spheres/kinova_gen3.yml
Normal file
@@ -0,0 +1,76 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
collision_spheres:
|
||||
base_link:
|
||||
- "center": [0.0, 0.0, 0.1]
|
||||
"radius": 0.05
|
||||
|
||||
shoulder_link:
|
||||
- "center": [0.0, 0.0, -0.1]
|
||||
"radius": 0.06
|
||||
- "center": [0.0, 0.0, -0.15]
|
||||
"radius": 0.05
|
||||
half_arm_1_link:
|
||||
- "center": [0.0, -0.0, 0.0]
|
||||
"radius": 0.055
|
||||
- "center": [0.0, -0.07, 0.0]
|
||||
"radius": 0.055
|
||||
- "center": [0.0, -0.15,0.0]
|
||||
"radius": 0.055
|
||||
half_arm_2_link:
|
||||
- "center": [0.0, -0.0, 0.0]
|
||||
"radius": 0.055
|
||||
- "center": [0.0, -0.0, -0.07]
|
||||
"radius": 0.055
|
||||
- "center": [0.0, -0.0,-0.15]
|
||||
"radius": 0.055
|
||||
- "center": [0.0, -0.0,-0.21]
|
||||
"radius": 0.055
|
||||
forearm_link:
|
||||
- "center": [0.0, -0.0, 0.0]
|
||||
"radius": 0.055
|
||||
- "center": [0.0, -0.07, -0.0]
|
||||
"radius": 0.055
|
||||
- "center": [0.0, -0.17,-0.0]
|
||||
"radius": 0.055
|
||||
spherical_wrist_1_link:
|
||||
- "center": [0.0, -0.0, 0.0]
|
||||
"radius": 0.055
|
||||
- "center": [0.0, -0.0, -0.085]
|
||||
"radius": 0.055
|
||||
spherical_wrist_2_link:
|
||||
- "center": [0.0, -0.0, 0.0]
|
||||
"radius": 0.05
|
||||
- "center": [0.0, -0.085, -0.0]
|
||||
"radius": 0.05
|
||||
bracelet_link:
|
||||
- "center": [0.0, -0.0, -0.05]
|
||||
"radius": 0.04
|
||||
- "center": [0.0, -0.05, -0.05]
|
||||
"radius": 0.04
|
||||
robotiq_arg2f_base_link:
|
||||
- "center": [0.0, -0.0, 0.04]
|
||||
"radius": 0.04
|
||||
left_outer_finger:
|
||||
- "center": [0.0, -0.01, 0.02]
|
||||
"radius": 0.03
|
||||
left_inner_finger_pad:
|
||||
- "center": [0.0, -0.0, 0.0]
|
||||
"radius": 0.01
|
||||
right_outer_finger:
|
||||
- "center": [0.0, -0.01, 0.02]
|
||||
"radius": 0.03
|
||||
right_inner_finger_pad:
|
||||
- "center": [0.0, -0.0, 0.0]
|
||||
"radius": 0.01
|
||||
|
||||
|
||||
|
||||
214
src/curobo/content/configs/robot/spheres/quad_ur10e.yml
Normal file
214
src/curobo/content/configs/robot/spheres/quad_ur10e.yml
Normal file
@@ -0,0 +1,214 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
robot: 'UR10'
|
||||
collision_spheres:
|
||||
shoulder_link:
|
||||
- center: [0, 0, 0]
|
||||
radius: 0.05
|
||||
- center: [0.0, -0.18, 0]
|
||||
radius: 0.05
|
||||
upper_arm_link:
|
||||
- center: [-0.102167, 0, 0.18]
|
||||
radius: 0.05
|
||||
- center: [-0.204333, -8.88178e-19, 0.18]
|
||||
radius: 0.05
|
||||
- center: [-0.3065, -1.33227e-18, 0.18]
|
||||
radius: 0.05
|
||||
- center: [-0.408667, -1.77636e-18, 0.18]
|
||||
radius: 0.05
|
||||
- center: [-0.510833, 0, 0.18]
|
||||
radius: 0.05
|
||||
- center: [ -0.613, 0,0.18]
|
||||
radius: 0.07
|
||||
forearm_link:
|
||||
- center: [-0, 0, 0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.0951667, 0, 0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.190333, 0, 0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.2855, 0, 0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.380667, 0,0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.475833, 0,0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.571, -1.19904e-17, 0.03]
|
||||
radius: 0.05
|
||||
wrist_1_link:
|
||||
- center: [0, 0, 0]
|
||||
radius: 0.05
|
||||
wrist_2_link:
|
||||
- center: [0, 0, 0]
|
||||
radius: 0.05
|
||||
wrist_3_link:
|
||||
- center: [0, 0, 0]
|
||||
radius: 0.05
|
||||
- center: [0, 0, 0.06]
|
||||
radius: 0.08
|
||||
tool0:
|
||||
- center: [0, 0, 0.03]
|
||||
radius: 0.05
|
||||
|
||||
|
||||
shoulder_link_1:
|
||||
- center: [0, 0, 0]
|
||||
radius: 0.05
|
||||
- center: [0.0, -0.18, 0]
|
||||
radius: 0.05
|
||||
upper_arm_link_1:
|
||||
- center: [-0.102167, 0, 0.18]
|
||||
radius: 0.05
|
||||
- center: [-0.204333, -8.88178e-19, 0.18]
|
||||
radius: 0.05
|
||||
- center: [-0.3065, -1.33227e-18, 0.18]
|
||||
radius: 0.05
|
||||
- center: [-0.408667, -1.77636e-18, 0.18]
|
||||
radius: 0.05
|
||||
- center: [-0.510833, 0, 0.18]
|
||||
radius: 0.05
|
||||
- center: [ -0.613, 0,0.18]
|
||||
radius: 0.07
|
||||
forearm_link_1:
|
||||
- center: [-0, 0, 0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.0951667, 0, 0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.190333, 0, 0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.2855, 0, 0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.380667, 0,0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.475833, 0,0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.571, -1.19904e-17, 0.03]
|
||||
radius: 0.05
|
||||
wrist_1_link_1:
|
||||
- center: [0, 0, 0]
|
||||
radius: 0.05
|
||||
wrist_2_link_1:
|
||||
- center: [0, 0, 0]
|
||||
radius: 0.05
|
||||
wrist_3_link_1:
|
||||
- center: [0, 0, 0]
|
||||
radius: 0.05
|
||||
- center: [0, 0, 0.06]
|
||||
radius: 0.08
|
||||
|
||||
tool1:
|
||||
- center: [0, 0, 0.03]
|
||||
radius: 0.05
|
||||
|
||||
|
||||
|
||||
shoulder_link_2:
|
||||
- center: [0, 0, 0]
|
||||
radius: 0.05
|
||||
- center: [0.0, -0.18, 0]
|
||||
radius: 0.05
|
||||
upper_arm_link_2:
|
||||
- center: [-0.102167, 0, 0.18]
|
||||
radius: 0.05
|
||||
- center: [-0.204333, -8.88178e-19, 0.18]
|
||||
radius: 0.05
|
||||
- center: [-0.3065, -1.33227e-18, 0.18]
|
||||
radius: 0.05
|
||||
- center: [-0.408667, -1.77636e-18, 0.18]
|
||||
radius: 0.05
|
||||
- center: [-0.510833, 0, 0.18]
|
||||
radius: 0.05
|
||||
- center: [ -0.613, 0,0.18]
|
||||
radius: 0.07
|
||||
forearm_link_2:
|
||||
- center: [-0, 0, 0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.0951667, 0, 0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.190333, 0, 0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.2855, 0, 0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.380667, 0,0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.475833, 0,0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.571, -1.19904e-17, 0.03]
|
||||
radius: 0.05
|
||||
wrist_1_link_2:
|
||||
- center: [0, 0, 0]
|
||||
radius: 0.05
|
||||
wrist_2_link_2:
|
||||
- center: [0, 0, 0]
|
||||
radius: 0.05
|
||||
wrist_3_link_2:
|
||||
- center: [0, 0, 0]
|
||||
radius: 0.05
|
||||
- center: [0, 0, 0.06]
|
||||
radius: 0.08
|
||||
|
||||
tool2:
|
||||
- center: [0, 0, 0.03]
|
||||
radius: 0.05
|
||||
|
||||
|
||||
|
||||
shoulder_link_3:
|
||||
- center: [0, 0, 0]
|
||||
radius: 0.05
|
||||
- center: [0.0, -0.18, 0]
|
||||
radius: 0.05
|
||||
upper_arm_link_3:
|
||||
- center: [-0.102167, 0, 0.18]
|
||||
radius: 0.05
|
||||
- center: [-0.204333, -8.88178e-19, 0.18]
|
||||
radius: 0.05
|
||||
- center: [-0.3065, -1.33227e-18, 0.18]
|
||||
radius: 0.05
|
||||
- center: [-0.408667, -1.77636e-18, 0.18]
|
||||
radius: 0.05
|
||||
- center: [-0.510833, 0, 0.18]
|
||||
radius: 0.05
|
||||
- center: [ -0.613, 0,0.18]
|
||||
radius: 0.07
|
||||
forearm_link_3:
|
||||
- center: [-0, 0, 0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.0951667, 0, 0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.190333, 0, 0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.2855, 0, 0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.380667, 0,0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.475833, 0,0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.571, -1.19904e-17, 0.03]
|
||||
radius: 0.05
|
||||
wrist_1_link_3:
|
||||
- center: [0, 0, 0]
|
||||
radius: 0.05
|
||||
wrist_2_link_3:
|
||||
- center: [0, 0, 0]
|
||||
radius: 0.05
|
||||
wrist_3_link_3:
|
||||
- center: [0, 0, 0]
|
||||
radius: 0.05
|
||||
- center: [0, 0, 0.06]
|
||||
radius: 0.08
|
||||
|
||||
tool3:
|
||||
- center: [0, 0, 0.03]
|
||||
radius: 0.05
|
||||
|
||||
|
||||
|
||||
16
src/curobo/content/configs/robot/spheres/single.yml
Normal file
16
src/curobo/content/configs/robot/spheres/single.yml
Normal file
@@ -0,0 +1,16 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
robot: 'single'
|
||||
collision_spheres:
|
||||
base:
|
||||
- "center": [-0.08, 0.0, 0.05]
|
||||
"radius": 0.06
|
||||
|
||||
63
src/curobo/content/configs/robot/spheres/ur10e.yml
Normal file
63
src/curobo/content/configs/robot/spheres/ur10e.yml
Normal file
@@ -0,0 +1,63 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
robot: 'UR10'
|
||||
collision_spheres:
|
||||
shoulder_link:
|
||||
- center: [0, 0, 0.0]
|
||||
radius: 0.02
|
||||
upper_arm_link:
|
||||
- center: [-0, -0, 0.18]
|
||||
radius: 0.09
|
||||
- center: [-0.102167, 0, 0.18]
|
||||
radius: 0.05
|
||||
- center: [-0.204333, -8.88178e-19, 0.18]
|
||||
radius: 0.05
|
||||
- center: [-0.3065, -1.33227e-18, 0.18]
|
||||
radius: 0.05
|
||||
- center: [-0.408667, -1.77636e-18, 0.18]
|
||||
radius: 0.05
|
||||
- center: [-0.510833, 0, 0.18]
|
||||
radius: 0.05
|
||||
- center: [ -0.613, 0,0.18]
|
||||
radius: 0.07
|
||||
forearm_link:
|
||||
- center: [-0, 0, 0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.0951667, 0, 0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.190333, 0, 0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.2855, 0, 0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.380667, 0,0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.475833, 0,0.03]
|
||||
radius: 0.05
|
||||
- center: [-0.571, -1.19904e-17, 0.03]
|
||||
radius: 0.05
|
||||
wrist_1_link:
|
||||
- center: [0, 0, 0]
|
||||
radius: 0.05
|
||||
wrist_2_link:
|
||||
- center: [0, 0, 0]
|
||||
radius: 0.05
|
||||
wrist_3_link:
|
||||
- center: [0, 0, 0]
|
||||
radius: 0.05
|
||||
- center: [0, 0, 0.06]
|
||||
radius: 0.08
|
||||
|
||||
tool0:
|
||||
- center: [0, 0, 0.03]
|
||||
radius: 0.05
|
||||
camera_mount:
|
||||
- center: [0, 0.11, -0.01]
|
||||
radius: 0.06
|
||||
46
src/curobo/content/configs/robot/template.yml
Normal file
46
src/curobo/content/configs/robot/template.yml
Normal file
@@ -0,0 +1,46 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
|
||||
robot_cfg:
|
||||
kinematics:
|
||||
usd_path: "FILL_THIS"
|
||||
usd_robot_root: "/robot"
|
||||
isaac_usd_path: ""
|
||||
usd_flip_joints: {}
|
||||
usd_flip_joint_limits: []
|
||||
|
||||
urdf_path: "FILL_THIS"
|
||||
asset_root_path: ""
|
||||
|
||||
base_link: "FILL_THIS"
|
||||
ee_link: "FILL_THIS"
|
||||
link_names: null
|
||||
lock_joints: null
|
||||
extra_links: null
|
||||
|
||||
|
||||
collision_link_names: null # List[str]
|
||||
collision_spheres: null #
|
||||
collision_sphere_buffer: 0.005
|
||||
extra_collision_spheres: {}
|
||||
self_collision_ignore: null # Dict[str, List[str]]
|
||||
self_collision_buffer: null # Dict[str, float]
|
||||
|
||||
use_global_cumul: True
|
||||
mesh_link_names: null # List[str]
|
||||
|
||||
cspace:
|
||||
joint_names: [] # List[str]
|
||||
retract_config: null # List[float]
|
||||
null_space_weight: null # List[str]
|
||||
cspace_distance_weight: null # List[str]
|
||||
max_jerk: 500.0
|
||||
max_acceleration: 15.0
|
||||
140
src/curobo/content/configs/robot/tm12.yml
Normal file
140
src/curobo/content/configs/robot/tm12.yml
Normal file
@@ -0,0 +1,140 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
|
||||
robot_cfg:
|
||||
kinematics:
|
||||
use_usd_kinematics: False
|
||||
usd_path: "robot/non_shipping/techman/tm12.usd"
|
||||
usd_robot_root: "/tm12"
|
||||
usd_flip_joints:
|
||||
{
|
||||
"shoulder_1_joint": "Z",
|
||||
"shoulder_2_joint": "Z",
|
||||
"elbow_joint": "Z",
|
||||
"wrist_1_joint": "Z",
|
||||
"wrist_2_joint": "Z",
|
||||
"wrist_3_joint": "Z",
|
||||
}
|
||||
urdf_path: "robot/techman/tm_description/urdf/tm12-nominal.urdf"
|
||||
asset_root_path: "robot/techman/tm_description"
|
||||
|
||||
base_link: "base"
|
||||
ee_link: "tool0"
|
||||
link_names: null
|
||||
|
||||
collision_link_names: [
|
||||
"link_1",
|
||||
"link_2",
|
||||
"link_3",
|
||||
"link_4",
|
||||
"link_5",
|
||||
"link_6",
|
||||
] # List[str]
|
||||
|
||||
collision_spheres:
|
||||
link_1:
|
||||
- "center": [-0.0, -0.0, 0.018]
|
||||
"radius": 0.1
|
||||
- "center": [-0.017, -0.18, 0.02]
|
||||
"radius": 0.1
|
||||
link_2:
|
||||
- "center": [0.116, 0.006, -0.182]
|
||||
"radius": 0.075
|
||||
- "center": [0.638, -0.004, -0.192]
|
||||
"radius": 0.08
|
||||
- "center": [0.19, 0.004, -0.183]
|
||||
"radius": 0.075
|
||||
- "center": [0.265, 0.003, -0.184]
|
||||
"radius": 0.075
|
||||
- "center": [0.34, 0.001, -0.186]
|
||||
"radius": 0.075
|
||||
- "center": [0.414, 0.0, -0.187]
|
||||
"radius": 0.075
|
||||
- "center": [0.489, -0.001, -0.189]
|
||||
"radius": 0.075
|
||||
- "center": [0.563, -0.003, -0.19]
|
||||
"radius": 0.075
|
||||
link_3:
|
||||
- "center": [0.012, 0.004, -0.076]
|
||||
"radius": 0.08
|
||||
- "center": [0.55, -0.001, -0.046]
|
||||
"radius": 0.07
|
||||
- "center": [0.088, 0.003, -0.061]
|
||||
"radius": 0.06
|
||||
- "center": [0.165, 0.002, -0.056]
|
||||
"radius": 0.06
|
||||
- "center": [0.242, 0.001, -0.052]
|
||||
"radius": 0.06
|
||||
- "center": [0.319, 0.001, -0.047]
|
||||
"radius": 0.06
|
||||
- "center": [0.396, -0.0, -0.043]
|
||||
"radius": 0.06
|
||||
- "center": [0.473, -0.001, -0.038]
|
||||
"radius": 0.06
|
||||
link_4:
|
||||
- "center": [0.0, 0.0, 0.0]
|
||||
"radius": 0.07
|
||||
|
||||
link_5:
|
||||
- "center": [0.0, 0.0, 0.0]
|
||||
"radius": 0.06
|
||||
link_6:
|
||||
- "center": [0.003, -0.002, -0.028]
|
||||
"radius": 0.06
|
||||
- "center": [-0.001, 0.075, 0.009]
|
||||
"radius": 0.05
|
||||
- "center": [-0.0, 0.078, -0.028]
|
||||
"radius": 0.05
|
||||
- "center": [-0.031, 0.128, 0.008]
|
||||
"radius": 0.03
|
||||
- "center": [-0.006, 0.146, 0.0]
|
||||
"radius": 0.03
|
||||
- "center": [0.025, 0.125, 0.007]
|
||||
"radius": 0.03
|
||||
- "center": [-0.005, 0.128, 0.003]
|
||||
"radius": 0.03
|
||||
collision_sphere_buffer: 0.0
|
||||
|
||||
self_collision_ignore: {
|
||||
"link_1": ["link_2"],
|
||||
"link_2": ["link_3"],
|
||||
"link_3": ["link_4", "link_5"],
|
||||
"link_4": ["link_5", "link_6"],
|
||||
"link_5": ["link_6", "link_4"],
|
||||
}
|
||||
self_collision_buffer: {
|
||||
link_1: -0.01,
|
||||
link_2: -0.02,
|
||||
link_3: -0.01,
|
||||
link_4: -0.02,
|
||||
link_6: -0.02,
|
||||
link_5: -0.02,
|
||||
} # Dict[str, float]
|
||||
|
||||
mesh_link_names: ["link_0", "link_1", "link_2", "link_3", "link_4", "link_5", "link_6"] # List[str]
|
||||
lock_joints: null
|
||||
add_object_link: False
|
||||
|
||||
cspace:
|
||||
joint_names: [
|
||||
"shoulder_1_joint",
|
||||
"shoulder_2_joint",
|
||||
"elbow_joint",
|
||||
"wrist_1_joint",
|
||||
"wrist_2_joint",
|
||||
"wrist_3_joint",
|
||||
] # List[str]
|
||||
retract_config: [0.0, -0.5, 1.9, -0.2, 1.25, 0.0] # List[float]
|
||||
null_space_weight: [1,1,1,1,1,1] # List[str]
|
||||
cspace_distance_weight: [1,1,1,1,1,1] # List[str]
|
||||
|
||||
max_acceleration: 13.5
|
||||
max_jerk: 2000.0
|
||||
75
src/curobo/content/configs/robot/tri_ur10e.yml
Normal file
75
src/curobo/content/configs/robot/tri_ur10e.yml
Normal file
@@ -0,0 +1,75 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
|
||||
robot_cfg:
|
||||
kinematics:
|
||||
use_usd_kinematics: False
|
||||
urdf_path: "robot/ur_description/tri_ur10e.urdf"
|
||||
asset_root_path: "robot/ur_description"
|
||||
isaac_usd_path: "/Isaac/Robots/UR10/ur10_long_suction.usd"
|
||||
usd_robot_root: "/ur10"
|
||||
usd_path: "robot/ur_description/tri_ur10e.usd"
|
||||
base_link: "base_fixture_link"
|
||||
ee_link: "tool0"
|
||||
link_names: ["tool1","tool2"]
|
||||
collision_link_names: [
|
||||
'shoulder_link','upper_arm_link', 'forearm_link', 'wrist_1_link', 'wrist_2_link' ,'wrist_3_link', 'tool0',
|
||||
'shoulder_link_1','upper_arm_link_1', 'forearm_link_1', 'wrist_1_link_1', 'wrist_2_link_1' ,'wrist_3_link_1', 'tool1',
|
||||
'shoulder_link_2','upper_arm_link_2', 'forearm_link_2', 'wrist_1_link_2', 'wrist_2_link_2' ,'wrist_3_link_2', 'tool2']
|
||||
collision_spheres: 'spheres/quad_ur10e.yml'
|
||||
collision_sphere_buffer: 0.005
|
||||
|
||||
|
||||
self_collision_ignore: {
|
||||
"upper_arm_link": ["shoulder_link","forearm_link"],
|
||||
"forarm_link": ["wrist_1_link"],
|
||||
"wrist_1_link": ["wrist_2_link","wrist_3_link"],
|
||||
"wrist_2_link": ["wrist_3_link", "tool0"],
|
||||
"wrist_3_link": ["tool0"],
|
||||
|
||||
"upper_arm_link_1": ["shoulder_link_1","forearm_link_1"],
|
||||
"forarm_link_1": ["wrist_1_link_1"],
|
||||
"wrist_1_link_1": ["wrist_2_link_1","wrist_3_link_1"],
|
||||
"wrist_2_link_1": ["wrist_3_link_1", "tool1"],
|
||||
"wrist_3_link_1": ["tool1"],
|
||||
|
||||
"upper_arm_link_2": ["shoulder_link_2","forearm_link_2"],
|
||||
"forarm_link_2": ["wrist_1_link_2"],
|
||||
"wrist_1_link_2": ["wrist_2_link_2","wrist_3_link_2"],
|
||||
"wrist_2_link_2": ["wrist_3_link_2", "tool2"],
|
||||
"wrist_3_link_2": ["tool2"],
|
||||
|
||||
|
||||
|
||||
}
|
||||
self_collision_buffer: {
|
||||
'shoulder_link': 0.05,
|
||||
'shoulder_link_1': 0.05,
|
||||
'shoulder_link_2': 0.05,
|
||||
}
|
||||
lock_joints: null
|
||||
|
||||
cspace:
|
||||
joint_names: [
|
||||
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint',
|
||||
'shoulder_pan_joint_1', 'shoulder_lift_joint_1', 'elbow_joint_1', 'wrist_1_joint_1', 'wrist_2_joint_1', 'wrist_3_joint_1',
|
||||
'shoulder_pan_joint_2', 'shoulder_lift_joint_2', 'elbow_joint_2', 'wrist_1_joint_2', 'wrist_2_joint_2', 'wrist_3_joint_2']
|
||||
retract_config: [-1.57, -2.2, 1.9, -1.383, -1.57, 0.00,
|
||||
-1.57, -2.2, 1.9, -1.383, -1.57, 0.00,
|
||||
-1.57, -2.2, 1.9, -1.383, -1.57, 0.00]
|
||||
null_space_weight: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
|
||||
1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
|
||||
1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
|
||||
cspace_distance_weight: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
|
||||
1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
|
||||
1.0, 1.0, 1.0, 1.0, 1.0, 1.0] #[2.5, 1.75, 1.5, 1.25, 0.7, 0.4]
|
||||
max_jerk: 500.0
|
||||
max_acceleration: 15.0
|
||||
51
src/curobo/content/configs/robot/ur10e.yml
Normal file
51
src/curobo/content/configs/robot/ur10e.yml
Normal file
@@ -0,0 +1,51 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
|
||||
robot_cfg:
|
||||
kinematics:
|
||||
use_usd_kinematics: False
|
||||
urdf_path: "robot/ur_description/ur10e.urdf"
|
||||
asset_root_path: "robot/ur_description"
|
||||
isaac_usd_path: "/Isaac/Robots/UR10/ur10_long_suction.usd"
|
||||
usd_robot_root: "/ur10"
|
||||
usd_path: "robot/ur_description/ur10e.usd"
|
||||
base_link: "base_link"
|
||||
ee_link: "tool0"
|
||||
link_names: null
|
||||
collision_link_names: ['shoulder_link','upper_arm_link', 'forearm_link', 'wrist_1_link', 'wrist_2_link' ,'wrist_3_link', 'tool0']
|
||||
collision_spheres: 'spheres/ur10e.yml'
|
||||
collision_sphere_buffer: 0.02
|
||||
self_collision_ignore: {
|
||||
"upper_arm_link": ["forearm_link", "shoulder_link"],
|
||||
"forarm_link": ["wrist_1_link"],
|
||||
"wrist_1_link": ["wrist_2_link","wrist_3_link"],
|
||||
"wrist_2_link": ["wrist_3_link", "tool0"],
|
||||
"wrist_3_link": ["tool0"],
|
||||
"camera_mount": ["tool0", "wrist_3_link"],
|
||||
}
|
||||
self_collision_buffer: {
|
||||
'shoulder_link': 0.07,
|
||||
'upper_arm_link': 0,
|
||||
'forearm_link': 0,
|
||||
'wrist_1_link': 0,
|
||||
'wrist_2_link': 0,
|
||||
'wrist_3_link' : 0,
|
||||
'tool0': 0,
|
||||
}
|
||||
lock_joints: null
|
||||
|
||||
cspace:
|
||||
joint_names: ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
|
||||
retract_config: [0.0, -2.2, 1.9, -1.383, -1.57, 0.00]
|
||||
null_space_weight: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
|
||||
cspace_distance_weight: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] #[2.5, 1.75, 1.5, 1.25, 0.7, 0.4]
|
||||
max_jerk: 500.0
|
||||
max_acceleration: 15.0
|
||||
117
src/curobo/content/configs/robot/ur5e.yml
Normal file
117
src/curobo/content/configs/robot/ur5e.yml
Normal file
@@ -0,0 +1,117 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
|
||||
robot_cfg:
|
||||
kinematics:
|
||||
usd_path: "robot/ur_description/ur5e.usd"
|
||||
usd_robot_root: "/robot"
|
||||
isaac_usd_path: ""
|
||||
usd_flip_joints: {}
|
||||
usd_flip_joint_limits: []
|
||||
|
||||
urdf_path: "robot/ur_description/ur5e.urdf"
|
||||
asset_root_path: "robot/ur_description"
|
||||
|
||||
base_link: "base_link"
|
||||
ee_link: "tool0"
|
||||
link_names: null
|
||||
lock_joints: null
|
||||
extra_links: null
|
||||
|
||||
|
||||
collision_link_names: ['shoulder_link','upper_arm_link', 'forearm_link', 'wrist_1_link', 'wrist_2_link' ,'wrist_3_link'] # List[str]
|
||||
collision_spheres:
|
||||
shoulder_link:
|
||||
- "center": [0.0, 0.0, 0.0]
|
||||
"radius": 0.1
|
||||
upper_arm_link:
|
||||
- "center": [-0.416, -0.0, 0.143]
|
||||
"radius": 0.078
|
||||
- "center": [-0.015, 0.0, 0.134]
|
||||
"radius": 0.077
|
||||
- "center": [-0.14, 0.0, 0.138]
|
||||
"radius": 0.062
|
||||
- "center": [-0.285, -0.001, 0.139]
|
||||
"radius": 0.061
|
||||
- "center": [-0.376, 0.001, 0.138]
|
||||
"radius": 0.077
|
||||
- "center": [-0.222, 0.001, 0.139]
|
||||
"radius": 0.061
|
||||
- "center": [-0.055, 0.008, 0.14]
|
||||
"radius": 0.07
|
||||
- "center": [-0.001, -0.002, 0.143]
|
||||
"radius": 0.076
|
||||
forearm_link:
|
||||
- "center": [-0.01, 0.002, 0.031]
|
||||
"radius": 0.072
|
||||
- "center": [-0.387, 0.0, 0.014]
|
||||
"radius": 0.057
|
||||
- "center": [-0.121, -0.0, 0.006]
|
||||
"radius": 0.057
|
||||
- "center": [-0.206, 0.001, 0.007]
|
||||
"radius": 0.057
|
||||
- "center": [-0.312, -0.001, 0.006]
|
||||
"radius": 0.056
|
||||
- "center": [-0.057, 0.003, 0.008]
|
||||
"radius": 0.065
|
||||
- "center": [-0.266, 0.0, 0.006]
|
||||
"radius": 0.057
|
||||
- "center": [-0.397, -0.001, -0.018]
|
||||
"radius": 0.052
|
||||
- "center": [-0.164, -0.0, 0.007]
|
||||
"radius": 0.057
|
||||
wrist_1_link:
|
||||
- "center": [-0.0, 0.0, -0.009]
|
||||
"radius": 0.047
|
||||
- "center": [-0.0, 0.0, -0.052]
|
||||
"radius": 0.047
|
||||
- "center": [-0.002, 0.027, -0.001]
|
||||
"radius": 0.045
|
||||
- "center": [0.001, -0.01, 0.0]
|
||||
"radius": 0.046
|
||||
wrist_2_link:
|
||||
- "center": [0.0, -0.01, -0.001]
|
||||
"radius": 0.047
|
||||
- "center": [0.0, 0.008, -0.001]
|
||||
"radius": 0.047
|
||||
- "center": [0.001, -0.001, -0.036]
|
||||
"radius": 0.047
|
||||
- "center": [0.001, -0.03, -0.0]
|
||||
"radius": 0.047
|
||||
wrist_3_link:
|
||||
- "center": [0.001, 0.001, -0.029]
|
||||
"radius": 0.043
|
||||
|
||||
collision_sphere_buffer: 0.005
|
||||
extra_collision_spheres: {}
|
||||
self_collision_ignore: {
|
||||
"upper_arm_link": ["forearm_link", "shoulder_link"],
|
||||
"forearm_link": ["wrist_1_link", "wrist_2_link", "wrist_3_link"],
|
||||
"wrist_1_link": ["wrist_2_link","wrist_3_link"],
|
||||
"wrist_2_link": ["wrist_3_link"],
|
||||
}
|
||||
self_collision_buffer: {'upper_arm_link': 0,
|
||||
'forearm_link': 0,
|
||||
'wrist_1_link': 0,
|
||||
'wrist_2_link': 0,
|
||||
'wrist_3_link' : 0,
|
||||
}
|
||||
|
||||
use_global_cumul: True
|
||||
mesh_link_names: null # List[str]
|
||||
|
||||
cspace:
|
||||
joint_names: ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
|
||||
retract_config: [0.0, -2.2, 1.9, -1.383, -1.57, 0.00]
|
||||
null_space_weight: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
|
||||
cspace_distance_weight: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
|
||||
max_jerk: 500.0
|
||||
max_acceleration: 15.0
|
||||
60
src/curobo/content/configs/task/base_cfg.yml
Normal file
60
src/curobo/content/configs/task/base_cfg.yml
Normal file
@@ -0,0 +1,60 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
|
||||
|
||||
world_collision_checker_cfg:
|
||||
cache: null #{"cube": 41, "capsule": 0, "sphere": 0}
|
||||
checker_type: "PRIMITIVE" # ["PRIMITIVE", "BLOX", "MESH"]
|
||||
max_distance: 0.1
|
||||
|
||||
|
||||
cost:
|
||||
pose_cfg:
|
||||
vec_weight: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
|
||||
weight: [0.0, 0.0]
|
||||
vec_convergence: [0.0, 0.00] # orientation, position
|
||||
terminal: False
|
||||
|
||||
|
||||
bound_cfg:
|
||||
weight: 000.0
|
||||
activation_distance: [0.0,0.0,0.0,0.0]
|
||||
|
||||
constraint:
|
||||
primitive_collision_cfg:
|
||||
weight: 2000.0
|
||||
use_sweep: False
|
||||
classify: True
|
||||
self_collision_cfg:
|
||||
weight: 1000.0
|
||||
classify: True
|
||||
bound_cfg:
|
||||
weight: [5000.0, 5000.0, 5000.0,5000.0]
|
||||
activation_distance: [0.0,0.0,0.0,0.0] # for position, velocity, acceleration and jerk
|
||||
|
||||
|
||||
convergence:
|
||||
pose_cfg:
|
||||
vec_weight: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
|
||||
weight: [1.0, 1.0]
|
||||
vec_convergence: [0.0, 0.00] # orientation, position
|
||||
terminal: False
|
||||
|
||||
cspace_cfg:
|
||||
weight: 1.0
|
||||
terminal: True
|
||||
run_weight: 1.0
|
||||
null_space_cfg:
|
||||
weight: 1.0
|
||||
terminal: True
|
||||
run_weight: 1.0
|
||||
|
||||
|
||||
97
src/curobo/content/configs/task/finetune_trajopt.yml
Normal file
97
src/curobo/content/configs/task/finetune_trajopt.yml
Normal file
@@ -0,0 +1,97 @@
|
||||
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
|
||||
model:
|
||||
horizon: 32
|
||||
state_filter_cfg:
|
||||
filter_coeff:
|
||||
position: 1.0
|
||||
velocity: 1.0
|
||||
acceleration: 1.0
|
||||
enable: False
|
||||
dt_traj_params:
|
||||
base_dt: 0.2
|
||||
base_ratio: 1.0
|
||||
max_dt: 0.2
|
||||
vel_scale: 1.0
|
||||
control_space: 'POSITION'
|
||||
state_finite_difference_mode: "CENTRAL"
|
||||
|
||||
teleport_mode: False
|
||||
return_full_act_buffer: True
|
||||
cost:
|
||||
pose_cfg:
|
||||
vec_weight: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] # orientation, position for all timesteps
|
||||
run_vec_weight: [1.00,1.0,1.0,1.0,1.0,1.0] # running weight orientation, position
|
||||
weight: [5000,30000.0,50,70] #[150.0, 2000.0, 30, 40]
|
||||
vec_convergence: [0.0,0.0] # orientation, position, orientation metric activation, position metric activation
|
||||
terminal: True
|
||||
run_weight: 0.0
|
||||
use_metric: True
|
||||
|
||||
cspace_cfg:
|
||||
weight: 10000.0
|
||||
terminal: True
|
||||
run_weight: 0.00 #1
|
||||
|
||||
bound_cfg:
|
||||
weight: [5000.0, 5000.0, 5000.0, 5000.0] # needs to be 3 values
|
||||
smooth_weight: [0.0,1000.0, 500.0, 0.0] # [vel, acc, jerk,]
|
||||
run_weight_velocity: 0.0
|
||||
run_weight_acceleration: 1.0
|
||||
run_weight_jerk: 1.0
|
||||
activation_distance: [0.1,0.1,0.1,10.0] # for position, velocity, acceleration and jerk
|
||||
null_space_weight: [1.0]
|
||||
|
||||
primitive_collision_cfg:
|
||||
weight: 50000.0
|
||||
use_sweep: True
|
||||
sweep_steps: 6
|
||||
classify: False
|
||||
use_sweep_kernel: True
|
||||
use_speed_metric: True
|
||||
speed_dt: 0.01 # used only for speed metric
|
||||
activation_distance: 0.025
|
||||
|
||||
self_collision_cfg:
|
||||
weight: 5000.0
|
||||
classify: False
|
||||
|
||||
|
||||
|
||||
lbfgs:
|
||||
n_iters: 300 #125 #@200 #250 #250 # 150 #25
|
||||
inner_iters: 25 #$25 # 25
|
||||
cold_start_n_iters: 300 #125 #200 #250 #$150 #25
|
||||
min_iters: 25
|
||||
line_search_scale: [0.01,0.3,0.7,1.0] # #
|
||||
fixed_iters: False
|
||||
cost_convergence: 0.01
|
||||
cost_delta_threshold: 10.0 #10.0 # 5.0 #2.0
|
||||
cost_relative_threshold: 0.999 #0.999
|
||||
epsilon: 0.01
|
||||
history: 4 #15
|
||||
use_cuda_graph: True
|
||||
n_envs: 1
|
||||
store_debug: False
|
||||
use_cuda_kernel: True
|
||||
stable_mode: True
|
||||
line_search_type: "approx_wolfe"
|
||||
use_cuda_line_search_kernel: True
|
||||
use_cuda_update_best_kernel: True
|
||||
use_temporal_smooth: False
|
||||
sync_cuda_time: True
|
||||
step_scale: 1.0
|
||||
last_best: 5
|
||||
use_coo_sparse: True
|
||||
debug_info:
|
||||
visual_traj : null #'ee_pos_seq'
|
||||
81
src/curobo/content/configs/task/gradient_ik.yml
Normal file
81
src/curobo/content/configs/task/gradient_ik.yml
Normal file
@@ -0,0 +1,81 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
|
||||
model:
|
||||
horizon: 1
|
||||
state_filter_cfg:
|
||||
filter_coeff:
|
||||
position: 1.0
|
||||
velocity: 1.0
|
||||
acceleration: 0.0
|
||||
|
||||
enable: False
|
||||
dt_traj_params:
|
||||
base_dt: 0.02
|
||||
base_ratio: 1.0
|
||||
max_dt: 0.25
|
||||
vel_scale: 1.0
|
||||
control_space: 'POSITION'
|
||||
teleport_mode: True
|
||||
cost:
|
||||
pose_cfg:
|
||||
vec_weight: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
|
||||
#weight: [100, 500, 10, 20]
|
||||
weight: [500, 5000, 30, 30]
|
||||
vec_convergence: [0.00, 0.000]
|
||||
terminal: False
|
||||
use_metric: True
|
||||
|
||||
cspace_cfg:
|
||||
weight: 0.000
|
||||
bound_cfg:
|
||||
weight: 50.0
|
||||
activation_distance: [0.1]
|
||||
null_space_weight: [1.0]
|
||||
primitive_collision_cfg:
|
||||
weight: 1000.0
|
||||
use_sweep: False
|
||||
classify: False
|
||||
activation_distance: 0.025
|
||||
self_collision_cfg:
|
||||
weight: 1000.0
|
||||
classify: False
|
||||
|
||||
|
||||
lbfgs:
|
||||
n_iters: 80 #60
|
||||
inner_iters: 20
|
||||
cold_start_n_iters: 80
|
||||
min_iters: 20
|
||||
line_search_scale: [0.01, 0.3, 0.7, 1.0] #[0.01,0.4, 0.9, 1.0] # #
|
||||
fixed_iters: True
|
||||
cost_convergence: 1e-7
|
||||
cost_delta_threshold: 1e-6 #0.0001
|
||||
cost_relative_threshold: 1.0
|
||||
epsilon: 0.01 # used only in stable_mode
|
||||
history: 4
|
||||
horizon: 1
|
||||
use_cuda_graph: True
|
||||
n_envs: 1
|
||||
store_debug: False
|
||||
use_cuda_kernel: True
|
||||
stable_mode: True
|
||||
line_search_type: "approx_wolfe" #"wolfe"
|
||||
use_cuda_line_search_kernel: True
|
||||
use_cuda_update_best_kernel: True
|
||||
sync_cuda_time: True
|
||||
step_scale: 1.0
|
||||
use_coo_sparse: True
|
||||
last_best: 10
|
||||
debug_info:
|
||||
visual_traj : null #'ee_pos_seq'
|
||||
|
||||
|
||||
99
src/curobo/content/configs/task/gradient_mpc.yml
Normal file
99
src/curobo/content/configs/task/gradient_mpc.yml
Normal file
@@ -0,0 +1,99 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
|
||||
model:
|
||||
horizon: 40
|
||||
state_filter_cfg:
|
||||
filter_coeff:
|
||||
position: 0.0
|
||||
velocity: 0.0
|
||||
acceleration: 0.0
|
||||
enable: True
|
||||
dt_traj_params:
|
||||
base_dt: 0.02
|
||||
base_ratio: 0.5
|
||||
max_dt: 0.02
|
||||
vel_scale: 1.0
|
||||
control_space: 'POSITION'
|
||||
teleport_mode: False
|
||||
state_finite_difference_mode: "CENTRAL"
|
||||
|
||||
cost:
|
||||
pose_cfg:
|
||||
vec_weight: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] # orientation, position for all timesteps
|
||||
run_vec_weight: [1.00,1.00,1.00,1.0,1.0,1.0] # running weight orientation, position
|
||||
weight: [500,2000.0,10,10] #[150.0, 2000.0, 30, 40]
|
||||
vec_convergence: [0.0,0.0] #[0.001,0.0001] #[0.01, 0.001] # orientation, position
|
||||
terminal: True
|
||||
run_weight: 0.1
|
||||
use_metric: True
|
||||
|
||||
cspace_cfg:
|
||||
weight: 00.0
|
||||
terminal: True
|
||||
run_weight: 1.0
|
||||
|
||||
bound_cfg:
|
||||
weight: [50.0, 0.0,0.0,0.0]
|
||||
activation_distance: [0.1,0.1,0.1,0.1] # for position, velocity, acceleration and jerk
|
||||
smooth_weight: [0.0, 100.0, 0.0,0.0] # [vel, acc, jerk, alpha_vel, eta_position, eta_vel, eta_acc]
|
||||
run_weight_velocity: 0.0
|
||||
run_weight_acceleration: 1.0
|
||||
run_weight_jerk: 1.0
|
||||
primitive_collision_cfg:
|
||||
weight: 500.0
|
||||
use_sweep: True
|
||||
sweep_steps: 4
|
||||
classify: False
|
||||
use_sweep_kernel: True
|
||||
use_speed_metric: False
|
||||
speed_dt: 0.1 # used only for speed metric
|
||||
activation_distance: 0.025
|
||||
self_collision_cfg:
|
||||
weight: 500.0
|
||||
classify: False
|
||||
|
||||
null_space_cfg:
|
||||
weight: 0.1
|
||||
terminal: True
|
||||
run_weight: 1.0
|
||||
use_null_space: True
|
||||
|
||||
stop_cfg:
|
||||
weight: 10.0 #50.0
|
||||
max_nlimit: 0.5 #0.2
|
||||
|
||||
lbfgs:
|
||||
n_iters: 150 #125 #@200 #250 #250 # 150 #25
|
||||
inner_iters: 25
|
||||
cold_start_n_iters: 500 #125 #200 #250 #$150 #25
|
||||
min_iters: 50
|
||||
line_search_scale: [0.01,0.25, 0.75,1.0] #[0.01,0.25,0.7, 1.0] # [0.01, 0.8, 1.0] #
|
||||
fixed_iters: True
|
||||
cost_convergence: 0.01
|
||||
cost_delta_threshold: 0.0001
|
||||
epsilon: 0.01
|
||||
history: 6 #15
|
||||
use_cuda_graph: True
|
||||
n_envs: 1
|
||||
store_debug: False
|
||||
use_cuda_kernel: True
|
||||
stable_mode: True
|
||||
line_search_type: "approx_wolfe" #"strong_wolfe" #"strong_wolfe"
|
||||
use_cuda_line_search_kernel: True
|
||||
use_cuda_update_best_kernel: True
|
||||
sync_cuda_time: True
|
||||
use_temporal_smooth: False
|
||||
last_best: 26
|
||||
step_scale: 1.0
|
||||
use_coo_sparse: True
|
||||
debug_info:
|
||||
visual_traj : null #'ee_pos_seq'
|
||||
98
src/curobo/content/configs/task/gradient_trajopt.yml
Normal file
98
src/curobo/content/configs/task/gradient_trajopt.yml
Normal file
@@ -0,0 +1,98 @@
|
||||
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
|
||||
model:
|
||||
horizon: 32
|
||||
state_filter_cfg:
|
||||
filter_coeff:
|
||||
position: 1.0
|
||||
velocity: 1.0
|
||||
acceleration: 1.0
|
||||
enable: False
|
||||
dt_traj_params:
|
||||
base_dt: 0.2
|
||||
base_ratio: 1.0
|
||||
max_dt: 0.2
|
||||
vel_scale: 1.0
|
||||
control_space: 'POSITION'
|
||||
state_finite_difference_mode: "CENTRAL"
|
||||
teleport_mode: False
|
||||
return_full_act_buffer: True
|
||||
|
||||
cost:
|
||||
pose_cfg:
|
||||
vec_weight: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] # orientation, position for all timesteps
|
||||
run_vec_weight: [1.00,1.00,1.00,1.0,1.0,1.0] # running weight orientation, position
|
||||
weight: [2000,20000.0,30,30] #[150.0, 2000.0, 30, 40]
|
||||
vec_convergence: [0.0,0.0] # orientation, position, orientation metric activation, position metric activation
|
||||
terminal: True
|
||||
run_weight: 0.0 #0.05
|
||||
use_metric: True
|
||||
|
||||
cspace_cfg:
|
||||
weight: 10000.0
|
||||
terminal: True
|
||||
run_weight: 0.00 #1
|
||||
|
||||
bound_cfg:
|
||||
weight: [5000.0, 5000.0, 5000.0,5000.0] # needs to be 3 values
|
||||
smooth_weight: [0.0,3000.0,10.0, 0.0] # [vel, acc, jerk,]
|
||||
run_weight_velocity: 0.00
|
||||
run_weight_acceleration: 1.0
|
||||
run_weight_jerk: 1.0
|
||||
activation_distance: [0.1,0.1,0.1,10.0] # for position, velocity, acceleration and jerk
|
||||
null_space_weight: [1.0]
|
||||
|
||||
primitive_collision_cfg:
|
||||
weight: 10000.0
|
||||
use_sweep: True
|
||||
sweep_steps: 4
|
||||
classify: False
|
||||
use_sweep_kernel: True
|
||||
use_speed_metric: True
|
||||
speed_dt: 0.01 # used only for speed metric
|
||||
activation_distance: 0.025
|
||||
|
||||
|
||||
self_collision_cfg:
|
||||
weight: 5000.0
|
||||
classify: False
|
||||
|
||||
|
||||
|
||||
lbfgs:
|
||||
n_iters: 100 #125 #@200 #250 #250 # 150 #25
|
||||
inner_iters: 25 #25 # 25
|
||||
cold_start_n_iters: 100 #125 #200 #250 #$150 #25
|
||||
min_iters: 25
|
||||
line_search_scale: [0.01,0.3,0.7,1.0] #[0.01,0.2, 0.3,0.5,0.7,0.9, 1.0] #
|
||||
fixed_iters: False
|
||||
cost_convergence: 0.01
|
||||
cost_delta_threshold: 1.0
|
||||
cost_relative_threshold: 0.9999
|
||||
epsilon: 0.01
|
||||
history: 4 #15 #$14
|
||||
use_cuda_graph: True
|
||||
n_envs: 1
|
||||
store_debug: False
|
||||
use_cuda_kernel: True
|
||||
stable_mode: True
|
||||
line_search_type: "approx_wolfe"
|
||||
use_cuda_line_search_kernel: True
|
||||
use_cuda_update_best_kernel: True
|
||||
use_temporal_smooth: False
|
||||
sync_cuda_time: True
|
||||
step_scale: 1.0 #1.0
|
||||
last_best: 5
|
||||
use_coo_sparse: True
|
||||
debug_info:
|
||||
visual_traj : null #'ee_pos_seq'
|
||||
53
src/curobo/content/configs/task/graph.yml
Normal file
53
src/curobo/content/configs/task/graph.yml
Normal file
@@ -0,0 +1,53 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
model:
|
||||
horizon: 1
|
||||
state_filter_cfg:
|
||||
filter_coeff:
|
||||
position: 1.0
|
||||
velocity: 1.0
|
||||
acceleration: 0.0
|
||||
enable: True
|
||||
dt_traj_params:
|
||||
base_dt: 0.02
|
||||
base_ratio: 1.0
|
||||
max_dt: 0.02
|
||||
vel_scale: 1.0
|
||||
control_space: 'POSITION'
|
||||
teleport_mode: True
|
||||
state_finite_difference_mode: "CENTRAL"
|
||||
|
||||
|
||||
|
||||
graph:
|
||||
max_nodes: 5000 # node list
|
||||
steer_delta_buffer: 500 # for steering
|
||||
sample_pts: 1500
|
||||
node_similarity_distance: 0.1
|
||||
|
||||
rejection_ratio: 20 #$20
|
||||
k_nn: 15
|
||||
max_buffer: 10000
|
||||
max_cg_buffer: 1000
|
||||
vertex_n : 30
|
||||
|
||||
graph_max_attempts: 10
|
||||
graph_min_attempts: 1
|
||||
init_nodes: 30
|
||||
|
||||
c_max: 1.25
|
||||
use_bias_node: True
|
||||
compute_metrics: False
|
||||
interpolation_steps: 1000
|
||||
interpolation_type: "linear"
|
||||
seed: 0
|
||||
interpolation_deviation: 0.05
|
||||
interpolation_acceleration_scale: 0.25
|
||||
81
src/curobo/content/configs/task/particle_ik.yml
Normal file
81
src/curobo/content/configs/task/particle_ik.yml
Normal file
@@ -0,0 +1,81 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
|
||||
model:
|
||||
horizon: 1
|
||||
state_filter_cfg:
|
||||
filter_coeff:
|
||||
position: 1.0
|
||||
velocity: 1.0
|
||||
acceleration: 0.0
|
||||
enable: False
|
||||
dt_traj_params:
|
||||
base_dt: 0.02
|
||||
base_ratio: 1.0
|
||||
max_dt: 0.25
|
||||
vel_scale: 1.0
|
||||
control_space: 'POSITION'
|
||||
teleport_mode: True
|
||||
cost:
|
||||
pose_cfg:
|
||||
vec_weight: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
|
||||
weight: [30, 50, 10, 10] #[20.0, 100.0]
|
||||
vec_convergence: [0.00, 0.000] # orientation, position
|
||||
terminal: False
|
||||
use_metric: True
|
||||
cspace_cfg:
|
||||
weight: 0.00
|
||||
bound_cfg:
|
||||
weight: 5000.0
|
||||
activation_distance: [0.05]
|
||||
primitive_collision_cfg:
|
||||
weight: 500.0
|
||||
use_sweep: False
|
||||
classify: False
|
||||
activation_distance: 0.035
|
||||
self_collision_cfg:
|
||||
weight: 500.0
|
||||
classify: False
|
||||
|
||||
|
||||
mppi:
|
||||
init_cov : 1.0 #0.15 #.5 #.5
|
||||
gamma : 1.0
|
||||
n_iters : 4
|
||||
cold_start_n_iters: 4
|
||||
step_size_mean : 0.9
|
||||
step_size_cov : 0.1
|
||||
beta : 0.01
|
||||
alpha : 1
|
||||
num_particles : 25 #10000
|
||||
update_cov : True
|
||||
cov_type : "DIAG_A" #
|
||||
kappa : 0.01
|
||||
null_act_frac : 0.0
|
||||
sample_mode : 'BEST'
|
||||
base_action : 'REPEAT'
|
||||
squash_fn : 'CLAMP'
|
||||
n_envs : 1
|
||||
use_cuda_graph : True
|
||||
seed : 0
|
||||
store_debug : False
|
||||
random_mean : False
|
||||
sample_per_env : True
|
||||
sync_cuda_time : False
|
||||
use_coo_sparse : True
|
||||
sample_params:
|
||||
fixed_samples: True
|
||||
sample_ratio: {'halton':1.0, 'halton-knot':0.0, 'random':0.0, 'random-knot':0.0}
|
||||
seed: 23
|
||||
filter_coeffs: [0.0, 0.0, 1.0]
|
||||
n_knots: 5
|
||||
debug_info:
|
||||
visual_traj : null #'state_seq'
|
||||
107
src/curobo/content/configs/task/particle_mpc.yml
Normal file
107
src/curobo/content/configs/task/particle_mpc.yml
Normal file
@@ -0,0 +1,107 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
|
||||
model:
|
||||
horizon: 30
|
||||
state_filter_cfg:
|
||||
filter_coeff:
|
||||
position: 0.1
|
||||
velocity: 0.1
|
||||
acceleration: 0.0
|
||||
enable: True
|
||||
dt_traj_params:
|
||||
base_dt: 0.01
|
||||
base_ratio: 0.5
|
||||
max_dt: 0.04
|
||||
vel_scale: 1.0
|
||||
control_space: 'ACCELERATION'
|
||||
teleport_mode: False
|
||||
state_finite_difference_mode: "CENTRAL"
|
||||
|
||||
|
||||
cost:
|
||||
pose_cfg:
|
||||
vec_weight: [1.0,1.0, 1.0, 1.0, 1.0, 1.0]
|
||||
run_vec_weight: [1.0,1.0, 1.0, 1.0, 1.0, 1.0]
|
||||
weight: [60,300.0,20,20] #[150.0, 2000.0, 30, 40]
|
||||
vec_convergence: [0.0, 0.00] # orientation, position
|
||||
terminal: True
|
||||
run_weight: 1.0
|
||||
use_metric: True
|
||||
|
||||
cspace_cfg:
|
||||
weight: 500.0
|
||||
terminal: True
|
||||
run_weight: 1.0
|
||||
|
||||
bound_cfg:
|
||||
weight: [5000.0, 5000.0,5000.0,000.0]
|
||||
activation_distance: [0.1,0.1,0.1,0.1] # for position, velocity, acceleration and jerk
|
||||
smooth_weight: [0.0, 50.0, 0.0,0.0] # [vel, acc, jerk, alpha_vel, eta_position, eta_vel, eta_acc]
|
||||
run_weight_velocity: 0.0
|
||||
run_weight_acceleration: 1.0
|
||||
run_weight_jerk: 1.0
|
||||
null_space_weight: [10.0]
|
||||
|
||||
primitive_collision_cfg:
|
||||
weight: 100000.0
|
||||
use_sweep: True
|
||||
sweep_steps: 4
|
||||
classify: False
|
||||
use_sweep_kernel: True
|
||||
use_speed_metric: False
|
||||
speed_dt: 0.1 # used only for speed metric
|
||||
activation_distance: 0.025
|
||||
|
||||
self_collision_cfg:
|
||||
weight: 50000.0
|
||||
classify: False
|
||||
|
||||
|
||||
stop_cfg:
|
||||
weight: 100.0 #50.0
|
||||
max_nlimit: 0.25 #0.2
|
||||
|
||||
|
||||
|
||||
mppi:
|
||||
init_cov : 0.05 #.5 #.5
|
||||
gamma : 0.98
|
||||
n_iters : 5
|
||||
cold_start_n_iters: 5
|
||||
step_size_mean : 0.9
|
||||
step_size_cov : 0.01
|
||||
beta : 0.1
|
||||
alpha : 1
|
||||
num_particles : 400 #10000
|
||||
update_cov : True
|
||||
cov_type : "DIAG_A" #
|
||||
kappa : 0.0001
|
||||
null_act_frac : 0.05
|
||||
sample_mode : 'BEST'
|
||||
base_action : 'REPEAT'
|
||||
squash_fn : 'CLAMP'
|
||||
n_envs : 1
|
||||
use_cuda_graph : True
|
||||
seed : 0
|
||||
store_debug : False
|
||||
random_mean : True
|
||||
sample_per_env : False
|
||||
sync_cuda_time : True
|
||||
use_coo_sparse : True
|
||||
sample_params:
|
||||
fixed_samples: True
|
||||
sample_ratio: {'halton':0.3, 'halton-knot':0.7, 'random':0.0, 'random-knot':0.0}
|
||||
seed: 0
|
||||
filter_coeffs: [0.3, 0.3, 0.4]
|
||||
n_knots: 5
|
||||
debug_info:
|
||||
visual_traj : 'ee_pos_seq'
|
||||
104
src/curobo/content/configs/task/particle_trajopt.yml
Normal file
104
src/curobo/content/configs/task/particle_trajopt.yml
Normal file
@@ -0,0 +1,104 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
|
||||
model:
|
||||
horizon: 32
|
||||
state_filter_cfg:
|
||||
filter_coeff:
|
||||
position: 1.0
|
||||
velocity: 1.0
|
||||
acceleration: 1.0
|
||||
enable: False
|
||||
dt_traj_params:
|
||||
base_dt: 0.2
|
||||
base_ratio: 1.0
|
||||
max_dt: 0.2
|
||||
vel_scale: 1.0
|
||||
control_space: 'POSITION'
|
||||
teleport_mode: False
|
||||
return_full_act_buffer: True
|
||||
state_finite_difference_mode: "CENTRAL"
|
||||
|
||||
cost:
|
||||
pose_cfg:
|
||||
vec_weight: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
|
||||
run_vec_weight: [1.0,1.0,1.0,1.0,1.0,1.0] # running weight
|
||||
weight: [250.0, 5000.0, 40, 40]
|
||||
vec_convergence: [0.0,0.0,1000.0,1000.0]
|
||||
terminal: True
|
||||
run_weight: 0.00
|
||||
use_metric: True
|
||||
|
||||
cspace_cfg:
|
||||
weight: 500.0
|
||||
terminal: True
|
||||
run_weight: 0.001
|
||||
|
||||
bound_cfg:
|
||||
weight: [0.1, 0.1,0.0,0.0]
|
||||
activation_distance: [0.0,0.0,0.0,0.0] #-#0.01
|
||||
smooth_weight: [0.0,100.0,1.0,0.0]
|
||||
run_weight_velocity: 0.0
|
||||
run_weight_acceleration: 1.0
|
||||
run_weight_jerk: 1.0
|
||||
|
||||
primitive_collision_cfg:
|
||||
weight: 5000.0
|
||||
use_sweep: True
|
||||
classify: False
|
||||
sweep_steps: 4
|
||||
use_sweep_kernel: True
|
||||
use_speed_metric: True
|
||||
speed_dt: 0.01 # used only for speed metric
|
||||
activation_distance: 0.025
|
||||
|
||||
self_collision_cfg:
|
||||
weight: 500.0
|
||||
classify: False
|
||||
|
||||
|
||||
|
||||
|
||||
mppi:
|
||||
init_cov : 0.1 #0.5
|
||||
gamma : 1.0
|
||||
n_iters : 2
|
||||
cold_start_n_iters: 2
|
||||
step_size_mean : 0.9
|
||||
step_size_cov : 0.01
|
||||
beta : 0.01
|
||||
alpha : 1
|
||||
num_particles : 25 # 100
|
||||
update_cov : True
|
||||
cov_type : "DIAG_A" #
|
||||
kappa : 0.001
|
||||
null_act_frac : 0.0
|
||||
sample_mode : 'BEST'
|
||||
base_action : 'REPEAT'
|
||||
squash_fn : 'CLAMP'
|
||||
n_envs : 1
|
||||
use_cuda_graph : True
|
||||
seed : 0
|
||||
store_debug : False
|
||||
random_mean : True
|
||||
sample_per_env : True
|
||||
sync_cuda_time : False
|
||||
use_coo_sparse : True
|
||||
sample_params:
|
||||
fixed_samples: True
|
||||
sample_ratio: {'halton':0.0, 'halton-knot':0.0, 'random':0.0, 'random-knot':0.0, "stomp": 1.0}
|
||||
seed: 25
|
||||
filter_coeffs: [0.3, 0.3, 0.4]
|
||||
n_knots: 5
|
||||
debug_info:
|
||||
visual_traj : null #'ee_pos_seq'
|
||||
|
||||
|
||||
14
src/curobo/content/configs/world/collision_base.yml
Normal file
14
src/curobo/content/configs/world/collision_base.yml
Normal file
@@ -0,0 +1,14 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
cuboid:
|
||||
table:
|
||||
dims: [0.1, 0.1, 0.8] # x, y, z
|
||||
pose: [0.0, 0.0, -0.45, 1, 0, 0, 0.0] # x, y, z, qw, qx, qy, qz
|
||||
116
src/curobo/content/configs/world/collision_cage.yml
Normal file
116
src/curobo/content/configs/world/collision_cage.yml
Normal file
@@ -0,0 +1,116 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
|
||||
cuboid:
|
||||
cube0:
|
||||
dims:
|
||||
- 0.44759031573031055
|
||||
- 0.6041613589568638
|
||||
- 0.04
|
||||
pose:
|
||||
- 0.5368277748379187
|
||||
- 0.5609131160539919
|
||||
- -0.21207545174379683
|
||||
- 0.8967553371515242
|
||||
- 0.0
|
||||
- 0.0
|
||||
- -0.44252668313928384
|
||||
cube1:
|
||||
dims:
|
||||
- 0.04
|
||||
- 0.6441613589568638
|
||||
- 0.507432552596414
|
||||
pose:
|
||||
- 0.4043908894165481
|
||||
- 0.7665743675779062
|
||||
- 0.02164082455441016
|
||||
- 0.8967553371515242
|
||||
- 0.0
|
||||
- 0.0
|
||||
- -0.44252668313928384
|
||||
cube2:
|
||||
dims:
|
||||
- 0.04
|
||||
- 0.6441613589568638
|
||||
- 0.507432552596414
|
||||
pose:
|
||||
- 0.7010117134542583
|
||||
- 0.37958547530685705
|
||||
- 0.02164082455441016
|
||||
- 0.8967553371515242
|
||||
- 0.0
|
||||
- 0.0
|
||||
- -0.44252668313928384
|
||||
cube3:
|
||||
dims:
|
||||
- 0.5275903157303106
|
||||
- 0.04
|
||||
- 0.04
|
||||
pose:
|
||||
- 0.2811999632260791
|
||||
- 0.36497846872527095
|
||||
- -0.07378708334618755
|
||||
- 0.8967553371515242
|
||||
- 0.0
|
||||
- 0.0
|
||||
- -0.44252668313928384
|
||||
cube4:
|
||||
dims:
|
||||
- 0.5275903157303106
|
||||
- 0.04
|
||||
- 0.04
|
||||
pose:
|
||||
- 0.2811999632260791
|
||||
- 0.36497846872527095
|
||||
- 0.1293126754499218
|
||||
- 0.8967553371515242
|
||||
- 0.0
|
||||
- 0.0
|
||||
- -0.44252668313928384
|
||||
cube5:
|
||||
dims:
|
||||
- 0.44759031573031055
|
||||
- 0.42891295126980467
|
||||
- 0.04
|
||||
pose:
|
||||
- 0.6381200845475716
|
||||
- 0.6385520586046123
|
||||
- 0.25535710085261715
|
||||
- 0.8967553371515242
|
||||
- 0.0
|
||||
- 0.0
|
||||
- -0.44252668313928384
|
||||
cube6:
|
||||
dims:
|
||||
- 0.44759031573031055
|
||||
- 0.04
|
||||
- 0.467432552596414
|
||||
pose:
|
||||
- 0.7924555864497582
|
||||
- 0.7568477633827128
|
||||
- 0.0016408245544101419
|
||||
- 0.8967553371515242
|
||||
- 0.0
|
||||
- 0.0
|
||||
- -0.44252668313928384
|
||||
cube7:
|
||||
dims:
|
||||
- 0.25
|
||||
- 0.2
|
||||
- 0.5
|
||||
pose:
|
||||
- -0.05
|
||||
- 0.0
|
||||
- -0.25
|
||||
- 1.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
58
src/curobo/content/configs/world/collision_cubby.yml
Normal file
58
src/curobo/content/configs/world/collision_cubby.yml
Normal file
@@ -0,0 +1,58 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
cuboid:
|
||||
table:
|
||||
dims: [5.2, 5.2, 0.2] # x, y, z
|
||||
pose: [0.0, 0.0, -0.1, 1, 0, 0, 0.0] # x, y, z, qx, qy, qz, qw
|
||||
|
||||
|
||||
|
||||
|
||||
cube4:
|
||||
dims:
|
||||
- 1.5
|
||||
- 0.1
|
||||
- 0.9
|
||||
pose:
|
||||
- -0.65
|
||||
- -0.7248229665483708
|
||||
- 0.552010009365394
|
||||
- 0.9982952839725183
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.05836545209478731
|
||||
|
||||
cube51:
|
||||
dims:
|
||||
- 0.10037689095815354
|
||||
- 0.2
|
||||
- 0.9
|
||||
pose:
|
||||
- -0.33
|
||||
- 0.3
|
||||
- 0.53552010009365394
|
||||
- 0.9982952839725183
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.05836545209478731
|
||||
cube6:
|
||||
dims:
|
||||
- 0.1
|
||||
- 0.8
|
||||
- 1.6
|
||||
pose:
|
||||
- 0.4
|
||||
- -0.1
|
||||
- 0.3
|
||||
- 0.9982952839725183
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.05836545209478731
|
||||
71
src/curobo/content/configs/world/collision_floor_plan.yml
Normal file
71
src/curobo/content/configs/world/collision_floor_plan.yml
Normal file
@@ -0,0 +1,71 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
cuboid:
|
||||
table:
|
||||
dims: [5.2, 5.2, 0.2] # x, y, z
|
||||
pose: [0.0, 0.0, -0.1, 0, 0, 0, 1.0] # x, y, z, qx, qy, qz, qw
|
||||
|
||||
|
||||
|
||||
|
||||
cube4:
|
||||
dims:
|
||||
- 1.5
|
||||
- 0.1
|
||||
- 0.9
|
||||
pose:
|
||||
- -0.65
|
||||
- -0.7248229665483708
|
||||
- 0.552010009365394
|
||||
- 0.9982952839725183
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.05836545209478731
|
||||
|
||||
cube51:
|
||||
dims:
|
||||
- 0.10037689095815354
|
||||
- 0.6
|
||||
- 0.9
|
||||
pose:
|
||||
- -0.5
|
||||
- 0.3
|
||||
- 0.53552010009365394
|
||||
- 0.9982952839725183
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.05836545209478731
|
||||
cube6:
|
||||
dims:
|
||||
- 0.1
|
||||
- 1.8
|
||||
- 1.6
|
||||
pose:
|
||||
- 0.4
|
||||
- -0.1
|
||||
- 0.3
|
||||
- 0.9982952839725183
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.05836545209478731
|
||||
cube61:
|
||||
dims:
|
||||
- 1.5
|
||||
- 0.1
|
||||
- 1.6
|
||||
pose:
|
||||
- -0.4
|
||||
- 0.25
|
||||
- 0.3
|
||||
- 1.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
17
src/curobo/content/configs/world/collision_handover.yml
Normal file
17
src/curobo/content/configs/world/collision_handover.yml
Normal file
@@ -0,0 +1,17 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
cuboid:
|
||||
table:
|
||||
dims: [2.0, 2.0, 0.2] # x, y, z
|
||||
pose: [0.0, 0.0, -0.1, 1, 0, 0, 0.0] # x, y, z, qx, qy, qz, qw
|
||||
hand:
|
||||
dims: [0.05, 0.18, 0.15] # x, y, z
|
||||
pose: [0.0, 0.0, -0.3, 1, 0, 0, 0.0] # x, y, z, qx, qy, qz, qw
|
||||
14
src/curobo/content/configs/world/collision_mesh_scene.yml
Normal file
14
src/curobo/content/configs/world/collision_mesh_scene.yml
Normal file
@@ -0,0 +1,14 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
mesh:
|
||||
base_scene:
|
||||
pose: [1.5, 0.080, 1.6, 0.043, -0.471, 0.284, 0.834]
|
||||
file_path: "scene/nvblox/srl_ur10_bins.obj"
|
||||
19
src/curobo/content/configs/world/collision_nvblox.yml
Normal file
19
src/curobo/content/configs/world/collision_nvblox.yml
Normal file
@@ -0,0 +1,19 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
blox:
|
||||
world:
|
||||
pose: [1.5, 0.080, 1.55, 0.043, -0.471, 0.284, 0.834]
|
||||
map_path: "scene/nvblox/srl_ur10_bins.nvblx"
|
||||
mesh_file_path: "scene/nvblox/srl_ur10_bins.obj"
|
||||
integrator_type: "tsdf"
|
||||
voxel_size: 0.03
|
||||
|
||||
|
||||
17
src/curobo/content/configs/world/collision_nvblox_online.yml
Normal file
17
src/curobo/content/configs/world/collision_nvblox_online.yml
Normal file
@@ -0,0 +1,17 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
blox:
|
||||
world:
|
||||
pose: [0,0,0,1,0,0,0]
|
||||
integrator_type: "tsdf"
|
||||
voxel_size: 0.03
|
||||
|
||||
|
||||
36
src/curobo/content/configs/world/collision_nvblox_ur10.yml
Normal file
36
src/curobo/content/configs/world/collision_nvblox_ur10.yml
Normal file
@@ -0,0 +1,36 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
blox:
|
||||
world_map:
|
||||
pose: [-0.38, 0.280, 1.05,0.043, -0.471, 0.284, 0.834]
|
||||
map_path: "scene/nvblox/srl_ur10_bins.nvblx"
|
||||
mesh_file_path: "scene/nvblox/srl_ur10_bins.obj"
|
||||
|
||||
cuboid:
|
||||
case:
|
||||
dims: [0.9, 0.7, 0.4] # x, y, z
|
||||
pose: [0.0, 0.0, -0.6, 1, 0, 0, 0.0] # x, y, z, qx, qy, qz, qw
|
||||
|
||||
stand:
|
||||
dims: [0.3, 0.3, 0.8] # x, y, z
|
||||
pose: [0.0, 0.0, -0.4, 1, 0, 0, 0.0] # x, y, z, qx, qy, qz, qw
|
||||
table:
|
||||
dims: [4.0, 4.0, 0.2] # x, y, z
|
||||
pose: [0.0, 0.0, -0.85, 1, 0, 0, 0.0] # x, y, z, qx, qy, qz, qw
|
||||
|
||||
safety_wall:
|
||||
dims: [2.0, 0.2, 2.0] # x, y, z
|
||||
pose: [0.0, -1.2, 0.0, 1, 0, 0, 0.0] # x, y, z, qx, qy, qz, qw
|
||||
|
||||
|
||||
#safety_wall_ceiling:
|
||||
# dims: [2.0, 2.0, 0.1] # x, y, z
|
||||
# pose: [0.0, 0.0, 1.4, 1, 0, 0, 0.0] # x, y, z, qx, qy, qz, qw
|
||||
60
src/curobo/content/configs/world/collision_pillar_wall.yml
Normal file
60
src/curobo/content/configs/world/collision_pillar_wall.yml
Normal file
@@ -0,0 +1,60 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
|
||||
##
|
||||
cuboid:
|
||||
table:
|
||||
dims: [2.2, 2.2, 0.2] # x, y, z
|
||||
pose: [0.0, 0.0, -0.1, 1, 0, 0, 0.0] # x, y, z, qx, qy, qz, qw
|
||||
color:
|
||||
- 0.6
|
||||
- 0.6
|
||||
- 0.8
|
||||
- 1.0
|
||||
cube6:
|
||||
dims:
|
||||
- 0.05
|
||||
- 0.05
|
||||
- 0.6
|
||||
pose:
|
||||
- 0.4
|
||||
- 0.0
|
||||
- 0.3
|
||||
- 1.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
color:
|
||||
- 0.8
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 1.0
|
||||
|
||||
cube4:
|
||||
dims:
|
||||
- 0.05
|
||||
- 0.05
|
||||
- 0.6
|
||||
pose:
|
||||
- -0.6
|
||||
- 0.0
|
||||
- 0.3
|
||||
- 1.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
color:
|
||||
- 0.8
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 1.0
|
||||
|
||||
|
||||
39
src/curobo/content/configs/world/collision_primitives_3d.yml
Normal file
39
src/curobo/content/configs/world/collision_primitives_3d.yml
Normal file
@@ -0,0 +1,39 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
cuboid:
|
||||
#cube1:
|
||||
# dims: [0.7, 0.1, 0.4] # x, y, z
|
||||
# pose: [0.6, 0.2, 0.1, 0, 0, 0, 1.0] # x
|
||||
cube2:
|
||||
dims: [0.3, 0.1, 0.5] # x, y, z
|
||||
pose: [0.4, -0.3, 0.2, 1, 0, 0, 0.0] # x
|
||||
cube3:
|
||||
dims: [2.0, 2.0, 0.2] # x, y, z
|
||||
pose: [0.0, 0.0, -0.1, 1, 0, 0, 0.0] # x
|
||||
sphere:
|
||||
sphere1:
|
||||
position: [0.5,0.1,0.1]
|
||||
radius: 0.1
|
||||
sphere2:
|
||||
position: [-0.5,0.1,0.1]
|
||||
radius: 0.1
|
||||
|
||||
capsule:
|
||||
capsule1:
|
||||
radius: 0.1
|
||||
base: [0.0,0.0,0.1]
|
||||
tip: [0.0,0.0,0.5]
|
||||
pose: [0.5,0.0,0.0,1.0,0.0,0.0,0.0]
|
||||
capsule2:
|
||||
radius: 0.1
|
||||
base: [0.0,0.0,0.1]
|
||||
tip: [0.0,0.0,0.5]
|
||||
pose: [0.0,0.5,0.0,1.0,0.0,0.0,0.0]
|
||||
14
src/curobo/content/configs/world/collision_table.yml
Normal file
14
src/curobo/content/configs/world/collision_table.yml
Normal file
@@ -0,0 +1,14 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
cuboid:
|
||||
table:
|
||||
dims: [5.0, 5.0, 0.2] # x, y, z
|
||||
pose: [0.0, 0.0, -0.1, 1, 0, 0, 0.0] # x, y, z, qx, qy, qz, qw
|
||||
37
src/curobo/content/configs/world/collision_test.yml
Normal file
37
src/curobo/content/configs/world/collision_test.yml
Normal file
@@ -0,0 +1,37 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
|
||||
##
|
||||
cuboid:
|
||||
table:
|
||||
dims: [2.2, 2.2, 0.2] # x, y, z
|
||||
pose: [0.0, 0.0, -0.1, 1, 0, 0, 0.0]
|
||||
|
||||
cube6:
|
||||
dims:
|
||||
- 0.1
|
||||
- 0.1
|
||||
- 1.5
|
||||
pose:
|
||||
- 0.4
|
||||
- -0.1
|
||||
- 0.3
|
||||
- 1.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
color:
|
||||
- 1.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 1.0
|
||||
|
||||
|
||||
72
src/curobo/content/configs/world/collision_thin_walls.yml
Normal file
72
src/curobo/content/configs/world/collision_thin_walls.yml
Normal file
@@ -0,0 +1,72 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
|
||||
##
|
||||
cuboid:
|
||||
table:
|
||||
dims: [2.2, 2.2, 0.2] # x, y, z
|
||||
pose: [0.0, 0.0, -0.1, 1, 0, 0, 0.0] #
|
||||
|
||||
cube6:
|
||||
dims:
|
||||
- 0.05
|
||||
- 0.01
|
||||
- 1.5
|
||||
pose:
|
||||
- 0.4
|
||||
- -0.1
|
||||
- 0.3
|
||||
- 1.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
|
||||
cube62:
|
||||
dims:
|
||||
- 0.05
|
||||
- 0.01
|
||||
- 1.5
|
||||
pose:
|
||||
- 0.0
|
||||
- 0.4
|
||||
- 0.3
|
||||
- 1.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
cube63:
|
||||
dims:
|
||||
- 0.05
|
||||
- 0.01
|
||||
- 1.5
|
||||
pose:
|
||||
- 0.0
|
||||
- -0.4
|
||||
- 0.3
|
||||
- 1.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
|
||||
#cube8:
|
||||
# dims:
|
||||
# - 0.9
|
||||
# - 0.1
|
||||
# - 0.02
|
||||
# pose:
|
||||
# - 0.0
|
||||
# - 0.0
|
||||
# - 0.9
|
||||
# - 1.0
|
||||
# - 0.0
|
||||
# - 0.0
|
||||
# - 0.0
|
||||
|
||||
42
src/curobo/content/configs/world/collision_wall.yml
Normal file
42
src/curobo/content/configs/world/collision_wall.yml
Normal file
@@ -0,0 +1,42 @@
|
||||
##
|
||||
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
##
|
||||
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
## property and proprietary rights in and to this material, related
|
||||
## documentation and any modifications thereto. Any use, reproduction,
|
||||
## disclosure or distribution of this material and related documentation
|
||||
## without an express license agreement from NVIDIA CORPORATION or
|
||||
## its affiliates is strictly prohibited.
|
||||
##
|
||||
|
||||
##
|
||||
cuboid:
|
||||
table:
|
||||
dims: [2.2, 2.2, 0.2] # x, y, z
|
||||
pose: [0.0, 0.0, -0.1, 1, 0, 0, 0.0] # x, y, z, qx, qy, qz, qw
|
||||
color:
|
||||
- 0.6
|
||||
- 0.6
|
||||
- 0.8
|
||||
- 1.0
|
||||
|
||||
cube4:
|
||||
dims:
|
||||
- 0.05
|
||||
- 2.0
|
||||
- 2.0
|
||||
pose:
|
||||
- -0.5
|
||||
- 0.0
|
||||
- 0.3
|
||||
- 1.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
color:
|
||||
- 0.6
|
||||
- 0.6
|
||||
- 0.8
|
||||
- 1.0
|
||||
|
||||
|
||||
43
src/curobo/cuda_robot_model/__init__.py
Normal file
43
src/curobo/cuda_robot_model/__init__.py
Normal file
@@ -0,0 +1,43 @@
|
||||
#
|
||||
# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
#
|
||||
# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
# property and proprietary rights in and to this material, related
|
||||
# documentation and any modifications thereto. Any use, reproduction,
|
||||
# disclosure or distribution of this material and related documentation
|
||||
# without an express license agreement from NVIDIA CORPORATION or
|
||||
# its affiliates is strictly prohibited.
|
||||
#
|
||||
|
||||
"""
|
||||
This module contains code for cuda accelerated kinematics.
|
||||
|
||||
Kinematics in CuRobo currently supports single axis actuated joints, where the joint can be actuated
|
||||
as prismatic or revolute joints. Continuous joints are approximated to revolute joints with limits
|
||||
at [-6, +6] radians. Mimic joints are not supported, so convert mimic joints to independent joints.
|
||||
|
||||
CuRobo loads a robot's kinematic tree from :class:`types.KinematicsTensorConfig`. This config is
|
||||
generated using :class:`cuda_robot_generator.CudaRobotGenerator`. A parser base class
|
||||
:class:`kinematics_parser.KinematicsParer` is provided to help with parsing kinematics from
|
||||
standard formats. Kinematics parsing from URDF is implemented in
|
||||
:class:`urdf_kinematics_parser.UrdfKinematicsParser`. An experimental USD kinematics parser is
|
||||
provided in :class:`usd_kinematics_parser.UsdKinematicsParser`, which is missing an additional
|
||||
transform between the joint origin and link origin, so this might not work for all robots.
|
||||
|
||||
In addition to parsing data from a kinematics file (urdf, usd), CuRobo also needs a sphere
|
||||
representation of the robot that approximates the volume of the robot's links with spheres.
|
||||
Several other parameters are also needed to represent kinematics in CuRobo. A tutorial on setting up a
|
||||
robot is provided in :ref:`tut_robot_configuration`.
|
||||
|
||||
Once a robot configuration file is setup, you can pass this to
|
||||
:class:`cuda_robot_model.CudaRobotModelConfig` to generate an instance of kinematics configuraiton.
|
||||
:class:`cuda_robot_model.CudaRobotModel` takes this configuration and provides access to kinematics
|
||||
computations.
|
||||
|
||||
.. note::
|
||||
:class:`cuda_robot_model.CudaRobotModel` creates memory tensors that are used by CUDA kernels
|
||||
while :class:`cuda_robot_model.CudaRobotModelConfig` contains only the robot kinematics
|
||||
configuration. To reduce memory overhead, you can pass one instance of
|
||||
:class:`cuda_robot_model.CudaRobotModelConfig` to many instances of
|
||||
:class:`cuda_robot_model.CudaRobotModel`.
|
||||
"""
|
||||
888
src/curobo/cuda_robot_model/cuda_robot_generator.py
Normal file
888
src/curobo/cuda_robot_model/cuda_robot_generator.py
Normal file
@@ -0,0 +1,888 @@
|
||||
#
|
||||
# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
#
|
||||
# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
# property and proprietary rights in and to this material, related
|
||||
# documentation and any modifications thereto. Any use, reproduction,
|
||||
# disclosure or distribution of this material and related documentation
|
||||
# without an express license agreement from NVIDIA CORPORATION or
|
||||
# its affiliates is strictly prohibited.
|
||||
#
|
||||
from __future__ import annotations
|
||||
|
||||
# Standard Library
|
||||
import copy
|
||||
import os
|
||||
from dataclasses import dataclass
|
||||
from typing import Any, Dict, List, Optional, Union
|
||||
|
||||
# Third Party
|
||||
import importlib_resources
|
||||
import torch
|
||||
import torch.autograd.profiler as profiler
|
||||
|
||||
# CuRobo
|
||||
from curobo.cuda_robot_model.kinematics_parser import LinkParams
|
||||
from curobo.cuda_robot_model.types import (
|
||||
CSpaceConfig,
|
||||
JointLimits,
|
||||
JointType,
|
||||
KinematicsTensorConfig,
|
||||
SelfCollisionKinematicsConfig,
|
||||
)
|
||||
from curobo.cuda_robot_model.urdf_kinematics_parser import UrdfKinematicsParser
|
||||
from curobo.curobolib.kinematics import get_cuda_kinematics
|
||||
from curobo.geom.types import tensor_sphere
|
||||
from curobo.types.base import TensorDeviceType
|
||||
from curobo.types.math import Pose
|
||||
from curobo.types.state import JointState
|
||||
from curobo.util.logger import log_error, log_info, log_warn
|
||||
from curobo.util_file import get_assets_path, get_robot_configs_path, join_path, load_yaml
|
||||
|
||||
try:
|
||||
# CuRobo
|
||||
from curobo.cuda_robot_model.usd_kinematics_parser import UsdKinematicsParser
|
||||
except ImportError:
|
||||
log_warn(
|
||||
"USDParser failed to import, install curobo with pip install .[usd] "
|
||||
+ "or pip install usd-core"
|
||||
)
|
||||
|
||||
|
||||
@dataclass
|
||||
class CudaRobotGeneratorConfig:
|
||||
"""Create Cuda Robot Model Configuration."""
|
||||
|
||||
#: Name of base link for kinematic tree.
|
||||
base_link: str
|
||||
|
||||
#: Name of end-effector link to compute pose.
|
||||
ee_link: str
|
||||
|
||||
#: Device to load cuda robot model.
|
||||
tensor_args: TensorDeviceType = TensorDeviceType()
|
||||
|
||||
#: Name of link names to compute pose in addition to ee_link.
|
||||
link_names: Optional[List[str]] = None
|
||||
|
||||
#: Name of links to compute sphere positions for use in collision checking.
|
||||
collision_link_names: Optional[List[str]] = None
|
||||
|
||||
#: Collision spheres that fill the volume occupied by the links of the robot.
|
||||
#: Collision spheres can be generated for robot using https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/advanced_tutorials/tutorial_motion_generation_robot_description_editor.html#collision-spheres
|
||||
collision_spheres: Union[None, str, Dict[str, Any]] = None
|
||||
|
||||
#: Radius buffer to add to collision spheres as padding.
|
||||
collision_sphere_buffer: float = 0.0
|
||||
|
||||
#: Compute jacobian of link poses. Currently not supported.
|
||||
compute_jacobian: bool = False
|
||||
|
||||
#: Padding to add for self collision between links. Some robots use a large padding
|
||||
#: for self collision avoidance (https://github.com/ros-planning/panda_moveit_config/pull/35#issuecomment-671333863)
|
||||
self_collision_buffer: Optional[Dict[str, float]] = None
|
||||
|
||||
#: Self-collision ignore
|
||||
self_collision_ignore: Optional[Dict[str, List[str]]] = None
|
||||
|
||||
#: debug config
|
||||
debug: Optional[Dict[str, Any]] = None
|
||||
|
||||
#: Enabling this flag writes out the cumulative transformation matrix to global memory. This
|
||||
#: allows for reusing the cumulative matrix during backward of kinematics (15% speedup over
|
||||
#: recomputing cumul in backward).
|
||||
use_global_cumul: bool = True
|
||||
|
||||
#: Path of meshes of robot links. Currently not used as we represent robot link geometry with
|
||||
#: collision spheres.
|
||||
asset_root_path: str = ""
|
||||
|
||||
#: Names of links to load meshes for visualization. This is only used for exporting
|
||||
#: visualizations.
|
||||
mesh_link_names: Optional[List[str]] = None
|
||||
|
||||
#: Set this to true to add mesh_link_names to link_names when computing kinematics.
|
||||
load_link_names_with_mesh: bool = False
|
||||
|
||||
#: Path to load robot urdf.
|
||||
urdf_path: Optional[str] = None
|
||||
|
||||
#: Path to load robot usd.
|
||||
usd_path: Optional[str] = None
|
||||
|
||||
#: Root prim of robot in usd.
|
||||
usd_robot_root: Optional[str] = None
|
||||
|
||||
#: Path of robot in Isaac server.
|
||||
isaac_usd_path: Optional[str] = None
|
||||
|
||||
#: Load Kinematics chain from usd.
|
||||
use_usd_kinematics: bool = False
|
||||
|
||||
#: Joints to flip axis when loading from USD
|
||||
usd_flip_joints: Optional[List[str]] = None
|
||||
|
||||
#: Flip joint limits in USD.
|
||||
usd_flip_joint_limits: Optional[List[str]] = None
|
||||
|
||||
#: Lock active joints in the kinematic tree. This will convert the joint to a fixed joint with
|
||||
#: joint angle given from this dictionary.
|
||||
lock_joints: Optional[Dict[str, float]] = None
|
||||
|
||||
extra_links: Optional[Dict[str, LinkParams]] = None
|
||||
|
||||
#: this is deprecated
|
||||
add_object_link: bool = False
|
||||
|
||||
use_external_assets: bool = False
|
||||
|
||||
#: Create n collision spheres for links with name
|
||||
extra_collision_spheres: Optional[Dict[str, int]] = None
|
||||
|
||||
#: cspace config
|
||||
cspace: Union[None, CSpaceConfig, Dict[str, List[Any]]] = None
|
||||
|
||||
def __post_init__(self):
|
||||
# add root path:
|
||||
if self.urdf_path is not None:
|
||||
self.urdf_path = join_path(get_assets_path(), self.urdf_path)
|
||||
if self.usd_path is not None:
|
||||
self.usd_path = join_path(get_assets_path(), self.usd_path)
|
||||
if self.asset_root_path != "":
|
||||
if self.use_external_assets:
|
||||
with importlib_resources.files("content") as path:
|
||||
content_dir_posix = path
|
||||
self.asset_root_path = join_path(content_dir_posix, self.asset_root_path)
|
||||
|
||||
else:
|
||||
self.asset_root_path = join_path(get_assets_path(), self.asset_root_path)
|
||||
elif self.urdf_path is not None:
|
||||
self.asset_root_path = os.path.dirname(self.urdf_path)
|
||||
|
||||
if self.collision_spheres is None and (
|
||||
self.collision_link_names is not None and len(self.collision_link_names) > 0
|
||||
):
|
||||
log_error("collision link names are provided without robot collision spheres")
|
||||
if self.load_link_names_with_mesh:
|
||||
if self.link_names is None:
|
||||
self.link_names = copy.deepcopy(self.mesh_link_names)
|
||||
else:
|
||||
for i in self.mesh_link_names:
|
||||
if i not in self.link_names:
|
||||
self.link_names.append(i)
|
||||
if self.link_names is None:
|
||||
self.link_names = [self.ee_link]
|
||||
if self.collision_link_names is None:
|
||||
self.collision_link_names = []
|
||||
if self.ee_link not in self.link_names:
|
||||
self.link_names.append(self.ee_link)
|
||||
if self.collision_spheres is not None:
|
||||
if isinstance(self.collision_spheres, str):
|
||||
coll_yml = join_path(get_robot_configs_path(), self.collision_spheres)
|
||||
coll_params = load_yaml(coll_yml)
|
||||
|
||||
self.collision_spheres = coll_params["collision_spheres"]
|
||||
if self.extra_collision_spheres is not None:
|
||||
for k in self.extra_collision_spheres.keys():
|
||||
new_spheres = [
|
||||
{"center": [0.0, 0.0, 0.0], "radius": -10.0}
|
||||
for n in range(self.extra_collision_spheres[k])
|
||||
]
|
||||
self.collision_spheres[k] = new_spheres
|
||||
if self.use_usd_kinematics and self.usd_path is None:
|
||||
log_error("usd_path is required to load kinematics from usd")
|
||||
if self.usd_flip_joints is None:
|
||||
self.usd_flip_joints = {}
|
||||
if self.usd_flip_joint_limits is None:
|
||||
self.usd_flip_joint_limits = []
|
||||
if self.extra_links is None:
|
||||
self.extra_links = {}
|
||||
else:
|
||||
for k in self.extra_links.keys():
|
||||
if isinstance(self.extra_links[k], dict):
|
||||
self.extra_links[k] = LinkParams.from_dict(self.extra_links[k])
|
||||
if isinstance(self.cspace, Dict):
|
||||
self.cspace = CSpaceConfig(**self.cspace, tensor_args=self.tensor_args)
|
||||
|
||||
|
||||
class CudaRobotGenerator(CudaRobotGeneratorConfig):
|
||||
def __init__(self, config: CudaRobotGeneratorConfig) -> None:
|
||||
super().__init__(**vars(config))
|
||||
self.cpu_tensor_args = self.tensor_args.cpu()
|
||||
|
||||
self._self_collision_data = None
|
||||
self.non_fixed_joint_names = []
|
||||
self._n_dofs = 1
|
||||
|
||||
self.initialize_tensors()
|
||||
|
||||
@property
|
||||
def kinematics_config(self):
|
||||
return self._kinematics_config
|
||||
|
||||
@property
|
||||
def self_collision_config(self):
|
||||
return self._self_collision_data
|
||||
|
||||
@property
|
||||
def kinematics_parser(self):
|
||||
return self._kinematics_parser
|
||||
|
||||
@profiler.record_function("robot_generator/initialize_tensors")
|
||||
def initialize_tensors(self):
|
||||
self._joint_limits = None
|
||||
self._self_collision_data = None
|
||||
self.lock_jointstate = None
|
||||
self.lin_jac, self.ang_jac = None, None
|
||||
|
||||
self._link_spheres_tensor = torch.empty(
|
||||
(0, 4), device=self.tensor_args.device, dtype=self.tensor_args.dtype
|
||||
)
|
||||
self._link_sphere_idx_map = torch.empty(
|
||||
(0), dtype=torch.int16, device=self.tensor_args.device
|
||||
)
|
||||
self.total_spheres = 0
|
||||
self.self_collision_distance = (
|
||||
torch.zeros(
|
||||
(self.total_spheres, self.total_spheres),
|
||||
dtype=self.tensor_args.dtype,
|
||||
device=self.tensor_args.device,
|
||||
)
|
||||
- torch.inf
|
||||
)
|
||||
self.self_collision_offset = torch.zeros(
|
||||
(self.total_spheres), dtype=self.tensor_args.dtype, device=self.tensor_args.device
|
||||
)
|
||||
# create a mega list of all links that we need:
|
||||
other_links = copy.deepcopy(self.link_names)
|
||||
|
||||
for i in self.collision_link_names:
|
||||
if i not in self.link_names:
|
||||
other_links.append(i)
|
||||
for i in self.extra_links:
|
||||
p_name = self.extra_links[i].parent_link_name
|
||||
if p_name not in self.link_names and p_name not in other_links:
|
||||
other_links.append(p_name)
|
||||
|
||||
# other_links = list(set(self.link_names + self.collision_link_names))
|
||||
|
||||
# load kinematics parser based on file type:
|
||||
# NOTE: Also add option to load from data buffers.
|
||||
if self.use_usd_kinematics:
|
||||
self._kinematics_parser = UsdKinematicsParser(
|
||||
self.usd_path,
|
||||
flip_joints=self.usd_flip_joints,
|
||||
flip_joint_limits=self.usd_flip_joint_limits,
|
||||
extra_links=self.extra_links,
|
||||
usd_robot_root=self.usd_robot_root,
|
||||
)
|
||||
else:
|
||||
self._kinematics_parser = UrdfKinematicsParser(
|
||||
self.urdf_path, mesh_root=self.asset_root_path, extra_links=self.extra_links
|
||||
)
|
||||
|
||||
if self.lock_joints is None:
|
||||
self._build_kinematics(self.base_link, self.ee_link, other_links, self.link_names)
|
||||
else:
|
||||
self._build_kinematics_with_lock_joints(
|
||||
self.base_link, self.ee_link, other_links, self.link_names, self.lock_joints
|
||||
)
|
||||
if self.cspace is None:
|
||||
jpv = self._get_joint_position_velocity_limits()
|
||||
self.cspace = CSpaceConfig.load_from_joint_limits(
|
||||
jpv["position"][1, :], jpv["position"][0, :], self.joint_names, self.tensor_args
|
||||
)
|
||||
|
||||
self.cspace.inplace_reindex(self.joint_names)
|
||||
self._update_joint_limits()
|
||||
self._ee_idx = self.link_names.index(self.ee_link)
|
||||
|
||||
# create kinematics tensor:
|
||||
self._kinematics_config = KinematicsTensorConfig(
|
||||
fixed_transforms=self._fixed_transform,
|
||||
link_map=self._link_map,
|
||||
joint_map=self._joint_map,
|
||||
joint_map_type=self._joint_map_type,
|
||||
store_link_map=self._store_link_map,
|
||||
link_chain_map=self._link_chain_map,
|
||||
link_names=self.link_names,
|
||||
link_spheres=self._link_spheres_tensor,
|
||||
link_sphere_idx_map=self._link_sphere_idx_map,
|
||||
n_dof=self._n_dofs,
|
||||
joint_limits=self._joint_limits,
|
||||
non_fixed_joint_names=self.non_fixed_joint_names,
|
||||
total_spheres=self.total_spheres,
|
||||
link_name_to_idx_map=self._name_to_idx_map,
|
||||
joint_names=self.joint_names,
|
||||
debug=self.debug,
|
||||
ee_idx=self._ee_idx,
|
||||
mesh_link_names=self.mesh_link_names,
|
||||
cspace=self.cspace,
|
||||
base_link=self.base_link,
|
||||
ee_link=self.ee_link,
|
||||
lock_jointstate=self.lock_jointstate,
|
||||
)
|
||||
if self.asset_root_path != "":
|
||||
self._kinematics_parser.add_absolute_path_to_link_meshes(self.asset_root_path)
|
||||
|
||||
def add_link(self, link_params: LinkParams):
|
||||
self.extra_links[link_params.link_name] = link_params
|
||||
|
||||
def add_fixed_link(
|
||||
self,
|
||||
link_name: str,
|
||||
parent_link_name: str,
|
||||
joint_name: Optional[str] = None,
|
||||
transform: Optional[Pose] = None,
|
||||
):
|
||||
if transform is None:
|
||||
transform = (
|
||||
Pose.from_list([0, 0, 0, 1, 0, 0, 0], self.tensor_args)
|
||||
.get_matrix()
|
||||
.view(4, 4)
|
||||
.cpu()
|
||||
.numpy()
|
||||
)
|
||||
if joint_name is None:
|
||||
joint_name = link_name + "_j_" + parent_link_name
|
||||
link_params = LinkParams(
|
||||
link_name=link_name,
|
||||
parent_link_name=parent_link_name,
|
||||
joint_name=joint_name,
|
||||
fixed_transform=transform,
|
||||
joint_type=JointType.FIXED,
|
||||
)
|
||||
self.add_link(link_params)
|
||||
|
||||
@profiler.record_function("robot_generator/build_chain")
|
||||
def _build_chain(self, base_link, ee_link, other_links, link_names):
|
||||
self._n_dofs = 0
|
||||
self._controlled_joints = []
|
||||
self._bodies = []
|
||||
|
||||
self._name_to_idx_map = dict()
|
||||
self.base_link = base_link
|
||||
self.ee_link = ee_link
|
||||
self.joint_names = []
|
||||
self._fixed_transform = []
|
||||
chain_link_names = self._kinematics_parser.get_chain(base_link, ee_link)
|
||||
self._add_body_to_tree(chain_link_names[0], base=True)
|
||||
for i, l_name in enumerate(chain_link_names[1:]):
|
||||
self._add_body_to_tree(l_name)
|
||||
# check if all links are in the built tree:
|
||||
|
||||
for i in other_links:
|
||||
if i in self._name_to_idx_map:
|
||||
continue
|
||||
if i not in self.extra_links.keys():
|
||||
chain_l_names = self._kinematics_parser.get_chain(base_link, i)
|
||||
|
||||
for k in chain_l_names:
|
||||
if k in chain_link_names:
|
||||
continue
|
||||
# if link name is not in chain, add to chain
|
||||
chain_link_names.append(k)
|
||||
# add to tree:
|
||||
self._add_body_to_tree(k, base=False)
|
||||
for i in self.extra_links.keys():
|
||||
if i not in chain_link_names:
|
||||
self._add_body_to_tree(i, base=False)
|
||||
chain_link_names.append(i)
|
||||
|
||||
self.non_fixed_joint_names = self.joint_names.copy()
|
||||
return chain_link_names
|
||||
|
||||
@profiler.record_function("robot_generator/build_kinematics_tensors")
|
||||
def _build_kinematics_tensors(self, base_link, ee_link, link_names, chain_link_names):
|
||||
link_map = [0 for i in range(len(self._bodies))]
|
||||
store_link_map = [] # [-1 for i in range(len(self._bodies))]
|
||||
|
||||
joint_map = [
|
||||
-1 if i not in self._controlled_joints else i for i in range(len(self._bodies))
|
||||
]
|
||||
joint_map_type = [
|
||||
-1 if i not in self._controlled_joints else i for i in range(len(self._bodies))
|
||||
]
|
||||
all_joint_names = []
|
||||
j_count = 0
|
||||
ordered_link_names = []
|
||||
# add body 0 details:
|
||||
if self._bodies[0].link_name in link_names:
|
||||
store_link_map.append(chain_link_names.index(self._bodies[0].link_name))
|
||||
ordered_link_names.append(self._bodies[0].link_name)
|
||||
# get joint types:
|
||||
for i in range(1, len(self._bodies)):
|
||||
body = self._bodies[i]
|
||||
parent_name = body.parent_link_name
|
||||
link_map[i] = self._name_to_idx_map[parent_name]
|
||||
all_joint_names.append(body.joint_name)
|
||||
if body.link_name in link_names:
|
||||
store_link_map.append(chain_link_names.index(body.link_name))
|
||||
ordered_link_names.append(body.link_name)
|
||||
if i in self._controlled_joints:
|
||||
joint_map[i] = j_count
|
||||
joint_map_type[i] = body.joint_type.value
|
||||
j_count += 1
|
||||
self.link_names = ordered_link_names
|
||||
# do a for loop to get link matrix:
|
||||
link_chain_map = torch.eye(
|
||||
len(chain_link_names), dtype=torch.int16, device=self.cpu_tensor_args.device
|
||||
)
|
||||
|
||||
# iterate and set true:
|
||||
for i in range(len(chain_link_names)):
|
||||
chain_l_names = self._kinematics_parser.get_chain(base_link, chain_link_names[i])
|
||||
for k in chain_l_names:
|
||||
link_chain_map[i, chain_link_names.index(k)] = 1.0
|
||||
|
||||
self._link_map = torch.as_tensor(
|
||||
link_map, device=self.tensor_args.device, dtype=torch.int16
|
||||
)
|
||||
self._joint_map = torch.as_tensor(
|
||||
joint_map, device=self.tensor_args.device, dtype=torch.int16
|
||||
)
|
||||
self._joint_map_type = torch.as_tensor(
|
||||
joint_map_type, device=self.tensor_args.device, dtype=torch.int8
|
||||
)
|
||||
self._store_link_map = torch.as_tensor(
|
||||
store_link_map, device=self.tensor_args.device, dtype=torch.int16
|
||||
)
|
||||
self._link_chain_map = link_chain_map.to(device=self.tensor_args.device)
|
||||
self._fixed_transform = torch.cat((self._fixed_transform), dim=0).to(
|
||||
device=self.tensor_args.device
|
||||
)
|
||||
self._all_joint_names = all_joint_names
|
||||
|
||||
@profiler.record_function("robot_generator/build_kinematics")
|
||||
def _build_kinematics(self, base_link, ee_link, other_links, link_names):
|
||||
chain_link_names = self._build_chain(base_link, ee_link, other_links, link_names)
|
||||
self._build_kinematics_tensors(base_link, ee_link, link_names, chain_link_names)
|
||||
if self.collision_spheres is not None and len(self.collision_link_names) > 0:
|
||||
self._build_collision_model(
|
||||
self.collision_spheres, self.collision_link_names, self.collision_sphere_buffer
|
||||
)
|
||||
|
||||
@profiler.record_function("robot_generator/build_kinematics_with_lock_joints")
|
||||
def _build_kinematics_with_lock_joints(
|
||||
self,
|
||||
base_link,
|
||||
ee_link,
|
||||
other_links,
|
||||
link_names,
|
||||
lock_joints: Dict[str, float],
|
||||
):
|
||||
chain_link_names = self._build_chain(base_link, ee_link, other_links, link_names)
|
||||
# find links attached to lock joints:
|
||||
lock_joint_names = list(lock_joints.keys())
|
||||
|
||||
joint_data = self._get_joint_links(lock_joint_names)
|
||||
|
||||
lock_links = list(
|
||||
set(
|
||||
[joint_data[j]["parent"] for j in joint_data.keys()]
|
||||
+ [joint_data[j]["child"] for j in joint_data.keys()]
|
||||
)
|
||||
)
|
||||
new_link_names = link_names + lock_links
|
||||
|
||||
# rebuild kinematic tree with link names added to link pose computation:
|
||||
self._build_kinematics_tensors(base_link, ee_link, new_link_names, chain_link_names)
|
||||
if self.collision_spheres is not None and len(self.collision_link_names) > 0:
|
||||
self._build_collision_model(
|
||||
self.collision_spheres, self.collision_link_names, self.collision_sphere_buffer
|
||||
)
|
||||
# do forward kinematics and get transform for locked joints:
|
||||
q = torch.zeros(
|
||||
(1, self._n_dofs), device=self.tensor_args.device, dtype=self.tensor_args.dtype
|
||||
)
|
||||
# set lock joints in the joint angles:
|
||||
l_idx = torch.as_tensor(
|
||||
[self.joint_names.index(l) for l in lock_joints.keys()],
|
||||
dtype=torch.long,
|
||||
device=self.tensor_args.device,
|
||||
)
|
||||
l_val = self.tensor_args.to_device([lock_joints[l] for l in lock_joints.keys()])
|
||||
|
||||
q[0, l_idx] = l_val
|
||||
kinematics_config = KinematicsTensorConfig(
|
||||
fixed_transforms=self._fixed_transform,
|
||||
link_map=self._link_map,
|
||||
joint_map=self._joint_map,
|
||||
joint_map_type=self._joint_map_type,
|
||||
store_link_map=self._store_link_map,
|
||||
link_chain_map=self._link_chain_map,
|
||||
link_names=self.link_names,
|
||||
link_spheres=self._link_spheres_tensor,
|
||||
link_sphere_idx_map=self._link_sphere_idx_map,
|
||||
n_dof=self._n_dofs,
|
||||
joint_limits=self._joint_limits,
|
||||
non_fixed_joint_names=self.non_fixed_joint_names,
|
||||
total_spheres=self.total_spheres,
|
||||
)
|
||||
link_poses = self._get_link_poses(q, lock_links, kinematics_config)
|
||||
|
||||
# remove lock links from store map:
|
||||
store_link_map = [chain_link_names.index(l) for l in link_names]
|
||||
self._store_link_map = torch.as_tensor(
|
||||
store_link_map, device=self.tensor_args.device, dtype=torch.int16
|
||||
)
|
||||
self.link_names = link_names
|
||||
|
||||
# compute a fixed transform for fixing joints:
|
||||
with profiler.record_function("cuda_robot_generator/fix_locked_joints"):
|
||||
# convert tensors to cpu:
|
||||
self._joint_map_type = self._joint_map_type.to(device=self.cpu_tensor_args.device)
|
||||
self._joint_map = self._joint_map.to(device=self.cpu_tensor_args.device)
|
||||
|
||||
for j in lock_joint_names:
|
||||
w_parent = lock_links.index(joint_data[j]["parent"])
|
||||
w_child = lock_links.index(joint_data[j]["child"])
|
||||
parent_t_child = (
|
||||
link_poses.get_index(0, w_parent)
|
||||
.inverse()
|
||||
.multiply(link_poses.get_index(0, w_child))
|
||||
)
|
||||
# Make this joint as fixed
|
||||
i = self._all_joint_names.index(j) + 1
|
||||
self._joint_map_type[i] = -1
|
||||
self._joint_map[i:] -= 1
|
||||
self._joint_map[i] = -1
|
||||
self._n_dofs -= 1
|
||||
self._fixed_transform[i] = parent_t_child.get_matrix()
|
||||
self._controlled_joints.remove(i)
|
||||
self.joint_names.remove(j)
|
||||
self._joint_map[self._joint_map < -1] = -1
|
||||
self._joint_map = self._joint_map.to(device=self.tensor_args.device)
|
||||
self._joint_map_type = self._joint_map_type.to(device=self.tensor_args.device)
|
||||
if len(self.lock_joints.keys()) > 0:
|
||||
self.lock_jointstate = JointState(
|
||||
position=l_val, joint_names=list(self.lock_joints.keys())
|
||||
)
|
||||
|
||||
@profiler.record_function("robot_generator/build_collision_model")
|
||||
def _build_collision_model(
|
||||
self,
|
||||
collision_spheres: Dict,
|
||||
collision_link_names: List[str],
|
||||
collision_sphere_buffer: float = 0.0,
|
||||
):
|
||||
"""
|
||||
|
||||
Args:
|
||||
collision_spheres (_type_): _description_
|
||||
collision_link_names (_type_): _description_
|
||||
collision_sphere_buffer (float, optional): _description_. Defaults to 0.0.
|
||||
"""
|
||||
|
||||
# We create all tensors on cpu and then finally move them to gpu
|
||||
coll_link_spheres = []
|
||||
# we store as [n_link, 7]
|
||||
link_sphere_idx_map = []
|
||||
cpu_tensor_args = self.tensor_args.cpu()
|
||||
with profiler.record_function("robot_generator/build_collision_spheres"):
|
||||
for j_idx, j in enumerate(collision_link_names):
|
||||
# print(j_idx)
|
||||
n_spheres = len(collision_spheres[j])
|
||||
link_spheres = torch.zeros(
|
||||
(n_spheres, 4), dtype=cpu_tensor_args.dtype, device=cpu_tensor_args.device
|
||||
)
|
||||
# find link index in global map:
|
||||
l_idx = self._name_to_idx_map[j]
|
||||
|
||||
for i in range(n_spheres):
|
||||
link_spheres[i, :] = tensor_sphere(
|
||||
collision_spheres[j][i]["center"],
|
||||
collision_spheres[j][i]["radius"],
|
||||
tensor_args=cpu_tensor_args,
|
||||
tensor=link_spheres[i, :],
|
||||
)
|
||||
link_sphere_idx_map.append(l_idx)
|
||||
coll_link_spheres.append(link_spheres)
|
||||
self.total_spheres += n_spheres
|
||||
|
||||
self._link_spheres_tensor = torch.cat(coll_link_spheres, dim=0)
|
||||
new_radius = self._link_spheres_tensor[..., 3] + collision_sphere_buffer
|
||||
flag = torch.logical_and(new_radius > -1.0, new_radius <= 0.0)
|
||||
new_radius[flag] = 0.001
|
||||
self._link_spheres_tensor[:, 3] = new_radius
|
||||
self._link_sphere_idx_map = torch.as_tensor(
|
||||
link_sphere_idx_map, dtype=torch.int16, device=cpu_tensor_args.device
|
||||
)
|
||||
|
||||
# build self collision distance tensor:
|
||||
self.self_collision_distance = (
|
||||
torch.zeros(
|
||||
(self.total_spheres, self.total_spheres),
|
||||
dtype=cpu_tensor_args.dtype,
|
||||
device=cpu_tensor_args.device,
|
||||
)
|
||||
- torch.inf
|
||||
)
|
||||
self.self_collision_offset = torch.zeros(
|
||||
(self.total_spheres), dtype=cpu_tensor_args.dtype, device=cpu_tensor_args.device
|
||||
)
|
||||
|
||||
with profiler.record_function("robot_generator/self_collision_distance"):
|
||||
# iterate through each link:
|
||||
for j_idx, j in enumerate(collision_link_names):
|
||||
ignore_links = []
|
||||
if j in self.self_collision_ignore.keys():
|
||||
ignore_links = self.self_collision_ignore[j]
|
||||
link1_idx = self._name_to_idx_map[j]
|
||||
link1_spheres_idx = torch.nonzero(self._link_sphere_idx_map == link1_idx)
|
||||
|
||||
rad1 = self._link_spheres_tensor[link1_spheres_idx, 3]
|
||||
if j not in self.self_collision_buffer.keys():
|
||||
self.self_collision_buffer[j] = 0.0
|
||||
c1 = self.self_collision_buffer[j]
|
||||
self.self_collision_offset[link1_spheres_idx] = c1
|
||||
for _, i_name in enumerate(collision_link_names):
|
||||
if i_name == j or i_name in ignore_links:
|
||||
continue
|
||||
if i_name not in collision_link_names:
|
||||
log_error("Self Collision Link name not found in collision_link_names")
|
||||
# find index of this link name:
|
||||
if i_name not in self.self_collision_buffer.keys():
|
||||
self.self_collision_buffer[i_name] = 0.0
|
||||
c2 = self.self_collision_buffer[i_name]
|
||||
link2_idx = self._name_to_idx_map[i_name]
|
||||
# update collision distance between spheres from these two links:
|
||||
link2_spheres_idx = torch.nonzero(self._link_sphere_idx_map == link2_idx)
|
||||
rad2 = self._link_spheres_tensor[link2_spheres_idx, 3]
|
||||
|
||||
for k1 in range(len(rad1)):
|
||||
sp1 = link1_spheres_idx[k1]
|
||||
for k2 in range(len(rad2)):
|
||||
sp2 = link2_spheres_idx[k2]
|
||||
self.self_collision_distance[sp1, sp2] = rad1[k1] + rad2[k2] + c1 + c2
|
||||
|
||||
with profiler.record_function("robot_generator/self_collision_min"):
|
||||
d_mat = self.self_collision_distance
|
||||
self.self_collision_distance = torch.minimum(d_mat, d_mat.transpose(0, 1))
|
||||
|
||||
if self.debug is not None and "self_collision_experimental" in self.debug:
|
||||
use_experimental_kernel = self.debug["self_collision_experimental"]
|
||||
self.self_collision_distance = self.self_collision_distance.to(
|
||||
device=self.tensor_args.device
|
||||
)
|
||||
(
|
||||
self._self_coll_thread_locations,
|
||||
self._self_coll_idx,
|
||||
valid_data,
|
||||
checks_per_thread,
|
||||
) = self._create_self_collision_thread_data(self.self_collision_distance)
|
||||
self._self_coll_matrix = (self.self_collision_distance != -(torch.inf)).to(
|
||||
dtype=torch.uint8
|
||||
)
|
||||
|
||||
use_experimental_kernel = True
|
||||
# convert all tensors to gpu:
|
||||
self._link_sphere_idx_map = self._link_sphere_idx_map.to(device=self.tensor_args.device)
|
||||
self._link_spheres_tensor = self._link_spheres_tensor.to(device=self.tensor_args.device)
|
||||
self.self_collision_offset = self.self_collision_offset.to(device=self.tensor_args.device)
|
||||
self._self_collision_data = SelfCollisionKinematicsConfig(
|
||||
offset=self.self_collision_offset,
|
||||
distance_threshold=self.self_collision_distance,
|
||||
thread_location=self._self_coll_thread_locations,
|
||||
thread_max=self._self_coll_idx,
|
||||
collision_matrix=self._self_coll_matrix,
|
||||
experimental_kernel=valid_data and use_experimental_kernel,
|
||||
checks_per_thread=checks_per_thread,
|
||||
)
|
||||
|
||||
@profiler.record_function("robot_generator/create_self_collision_thread_data")
|
||||
def _create_self_collision_thread_data(self, collision_threshold):
|
||||
coll_cpu = collision_threshold.cpu()
|
||||
max_checks_per_thread = 512
|
||||
thread_loc = torch.zeros((2 * 32 * max_checks_per_thread), dtype=torch.int16) - 1
|
||||
n_spheres = coll_cpu.shape[0]
|
||||
sl_idx = 0
|
||||
skip_count = 0
|
||||
all_val = 0
|
||||
valid_data = True
|
||||
for i in range(n_spheres):
|
||||
if not valid_data:
|
||||
break
|
||||
if torch.max(coll_cpu[i]) == -torch.inf:
|
||||
print("skip", i)
|
||||
for j in range(i + 1, n_spheres):
|
||||
if sl_idx > thread_loc.shape[0] - 1:
|
||||
valid_data = False
|
||||
log_warn(
|
||||
"Self Collision checks are greater than "
|
||||
+ str(32 * max_checks_per_thread)
|
||||
+ ", using slower kernel"
|
||||
)
|
||||
break
|
||||
if coll_cpu[i, j] != -torch.inf:
|
||||
thread_loc[sl_idx] = i
|
||||
sl_idx += 1
|
||||
thread_loc[sl_idx] = j
|
||||
sl_idx += 1
|
||||
else:
|
||||
skip_count += 1
|
||||
all_val += 1
|
||||
log_info("Self Collision threads, skipped %: " + str(100 * float(skip_count) / all_val))
|
||||
log_info("Self Collision count: " + str(sl_idx / (2)))
|
||||
log_info("Self Collision per thread: " + str(sl_idx / (2 * 1024)))
|
||||
|
||||
max_checks_per_thread = 512
|
||||
val = sl_idx / (2 * 1024)
|
||||
if val < 1:
|
||||
max_checks_per_thread = 1
|
||||
elif val < 2:
|
||||
max_checks_per_thread = 2
|
||||
elif val < 4:
|
||||
max_checks_per_thread = 4
|
||||
elif val < 8:
|
||||
max_checks_per_thread = 8
|
||||
elif val < 32:
|
||||
max_checks_per_thread = 32
|
||||
elif val < 64:
|
||||
max_checks_per_thread = 64
|
||||
elif val < 128:
|
||||
max_checks_per_thread = 128
|
||||
elif val < 512:
|
||||
max_checks_per_thread = 512
|
||||
else:
|
||||
raise ValueError("Self Collision not supported")
|
||||
|
||||
if max_checks_per_thread < 2:
|
||||
max_checks_per_thread = 2
|
||||
log_info("Self Collision using: " + str(max_checks_per_thread))
|
||||
|
||||
return (
|
||||
thread_loc.to(device=collision_threshold.device),
|
||||
sl_idx,
|
||||
valid_data,
|
||||
max_checks_per_thread,
|
||||
)
|
||||
|
||||
@profiler.record_function("robot_generator/add_body_to_tree")
|
||||
def _add_body_to_tree(self, link_name, base=False):
|
||||
body_idx = len(self._bodies)
|
||||
|
||||
rigid_body_params = self._kinematics_parser.get_link_parameters(link_name, base=base)
|
||||
self._bodies.append(rigid_body_params)
|
||||
if rigid_body_params.joint_type != JointType.FIXED:
|
||||
self._controlled_joints.append(body_idx)
|
||||
self.joint_names.append(rigid_body_params.joint_name)
|
||||
self._n_dofs = self._n_dofs + 1
|
||||
self._fixed_transform.append(
|
||||
torch.as_tensor(
|
||||
rigid_body_params.fixed_transform,
|
||||
device=self.cpu_tensor_args.device,
|
||||
dtype=self.cpu_tensor_args.dtype,
|
||||
).unsqueeze(0)
|
||||
)
|
||||
self._name_to_idx_map[rigid_body_params.link_name] = body_idx
|
||||
|
||||
def _get_joint_links(self, joint_names: List[str]):
|
||||
j_data = {}
|
||||
for j in joint_names:
|
||||
for b in self._bodies:
|
||||
if b.joint_name == j:
|
||||
j_data[j] = {"parent": b.parent_link_name, "child": b.link_name}
|
||||
return j_data
|
||||
|
||||
@profiler.record_function("robot_generator/get_link_poses")
|
||||
def _get_link_poses(
|
||||
self, q: torch.Tensor, link_names: List[str], kinematics_config: KinematicsTensorConfig
|
||||
) -> Pose:
|
||||
if q.is_contiguous():
|
||||
q_in = q.view(-1)
|
||||
else:
|
||||
q_in = q.reshape(-1) # .reshape(-1)
|
||||
# q_in = q.reshape(-1)
|
||||
link_pos_seq = torch.zeros(
|
||||
(1, len(self.link_names), 3),
|
||||
device=self.tensor_args.device,
|
||||
dtype=self.tensor_args.dtype,
|
||||
)
|
||||
link_quat_seq = torch.zeros(
|
||||
(1, len(self.link_names), 4),
|
||||
device=self.tensor_args.device,
|
||||
dtype=self.tensor_args.dtype,
|
||||
)
|
||||
batch_robot_spheres = torch.zeros(
|
||||
(1, self.total_spheres, 4),
|
||||
device=self.tensor_args.device,
|
||||
dtype=self.tensor_args.dtype,
|
||||
)
|
||||
grad_out_q = torch.zeros(
|
||||
(1 * q.shape[-1]),
|
||||
device=self.tensor_args.device,
|
||||
dtype=self.tensor_args.dtype,
|
||||
)
|
||||
global_cumul_mat = torch.zeros(
|
||||
(1, self._link_map.shape[0], 4, 4),
|
||||
device=self.tensor_args.device,
|
||||
dtype=self.tensor_args.dtype,
|
||||
)
|
||||
link_pos_seq, link_quat_seq, _ = get_cuda_kinematics(
|
||||
# self._link_mat_seq, # data will be stored here
|
||||
link_pos_seq,
|
||||
link_quat_seq,
|
||||
batch_robot_spheres,
|
||||
global_cumul_mat,
|
||||
q_in,
|
||||
kinematics_config.fixed_transforms,
|
||||
kinematics_config.link_spheres,
|
||||
kinematics_config.link_map, # tells which link is attached to which link i
|
||||
kinematics_config.joint_map, # tells which joint is attached to a link i
|
||||
kinematics_config.joint_map_type, # joint type
|
||||
kinematics_config.store_link_map,
|
||||
kinematics_config.link_sphere_idx_map, # sphere idx map
|
||||
kinematics_config.link_chain_map,
|
||||
grad_out_q,
|
||||
self.use_global_cumul,
|
||||
)
|
||||
position = torch.zeros(
|
||||
(q.shape[0], len(link_names), 3),
|
||||
device=self.tensor_args.device,
|
||||
dtype=self.tensor_args.dtype,
|
||||
)
|
||||
quaternion = torch.zeros(
|
||||
(q.shape[0], len(link_names), 4),
|
||||
device=self.tensor_args.device,
|
||||
dtype=self.tensor_args.dtype,
|
||||
)
|
||||
|
||||
for li, l in enumerate(link_names):
|
||||
i = self.link_names.index(l)
|
||||
position[:, li, :] = link_pos_seq[:, i, :]
|
||||
quaternion[:, li, :] = link_quat_seq[:, i, :]
|
||||
return Pose(position=position, quaternion=quaternion)
|
||||
|
||||
@property
|
||||
def get_joint_limits(self):
|
||||
return self._joint_limits
|
||||
|
||||
@profiler.record_function("robot_generator/get_joint_limits")
|
||||
def _get_joint_position_velocity_limits(self):
|
||||
joint_limits = {"position": [[], []], "velocity": [[], []]}
|
||||
|
||||
for idx in self._controlled_joints:
|
||||
joint_limits["position"][0].append(self._bodies[idx].joint_limits[0])
|
||||
joint_limits["position"][1].append(self._bodies[idx].joint_limits[1])
|
||||
joint_limits["velocity"][0].append(self._bodies[idx].joint_velocity_limits[0])
|
||||
joint_limits["velocity"][1].append(self._bodies[idx].joint_velocity_limits[1])
|
||||
for k in joint_limits:
|
||||
joint_limits[k] = torch.as_tensor(
|
||||
joint_limits[k], device=self.tensor_args.device, dtype=self.tensor_args.dtype
|
||||
)
|
||||
return joint_limits
|
||||
|
||||
@profiler.record_function("robot_generator/update_joint_limits")
|
||||
def _update_joint_limits(self):
|
||||
joint_limits = self._get_joint_position_velocity_limits()
|
||||
joint_limits["jerk"] = torch.cat(
|
||||
[-1.0 * self.cspace.max_jerk.unsqueeze(0), self.cspace.max_jerk.unsqueeze(0)]
|
||||
)
|
||||
joint_limits["acceleration"] = torch.cat(
|
||||
[
|
||||
-1.0 * self.cspace.max_acceleration.unsqueeze(0),
|
||||
self.cspace.max_acceleration.unsqueeze(0),
|
||||
]
|
||||
)
|
||||
self._joint_limits = JointLimits(joint_names=self.joint_names, **joint_limits)
|
||||
386
src/curobo/cuda_robot_model/cuda_robot_model.py
Normal file
386
src/curobo/cuda_robot_model/cuda_robot_model.py
Normal file
@@ -0,0 +1,386 @@
|
||||
#
|
||||
# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
#
|
||||
# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
# property and proprietary rights in and to this material, related
|
||||
# documentation and any modifications thereto. Any use, reproduction,
|
||||
# disclosure or distribution of this material and related documentation
|
||||
# without an express license agreement from NVIDIA CORPORATION or
|
||||
# its affiliates is strictly prohibited.
|
||||
#
|
||||
from __future__ import annotations
|
||||
|
||||
# Standard Library
|
||||
from dataclasses import dataclass
|
||||
from typing import Any, Dict, List, Optional
|
||||
|
||||
# Third Party
|
||||
import torch
|
||||
import torch.autograd.profiler as profiler
|
||||
|
||||
# CuRobo
|
||||
from curobo.cuda_robot_model.cuda_robot_generator import (
|
||||
CudaRobotGenerator,
|
||||
CudaRobotGeneratorConfig,
|
||||
)
|
||||
from curobo.cuda_robot_model.kinematics_parser import KinematicsParser
|
||||
from curobo.cuda_robot_model.types import (
|
||||
CudaRobotModelState,
|
||||
KinematicsTensorConfig,
|
||||
SelfCollisionKinematicsConfig,
|
||||
)
|
||||
from curobo.curobolib.kinematics import get_cuda_kinematics
|
||||
from curobo.geom.types import Sphere
|
||||
from curobo.types.base import TensorDeviceType
|
||||
from curobo.types.math import Pose
|
||||
from curobo.util.logger import log_error
|
||||
from curobo.util_file import get_robot_path, join_path, load_yaml
|
||||
|
||||
|
||||
@dataclass
|
||||
class CudaRobotModelConfig:
|
||||
tensor_args: TensorDeviceType
|
||||
link_names: List[str]
|
||||
kinematics_config: KinematicsTensorConfig
|
||||
self_collision_config: Optional[SelfCollisionKinematicsConfig] = None
|
||||
kinematics_parser: Optional[KinematicsParser] = None
|
||||
compute_jacobian: bool = False
|
||||
use_global_cumul: bool = False
|
||||
generator_config: Optional[CudaRobotGeneratorConfig] = None
|
||||
|
||||
def get_joint_limits(self):
|
||||
return self.kinematics_config.joint_limits
|
||||
|
||||
@staticmethod
|
||||
def from_basic_urdf(
|
||||
urdf_path: str,
|
||||
base_link: str,
|
||||
ee_link: str,
|
||||
tensor_args: TensorDeviceType = TensorDeviceType(),
|
||||
) -> CudaRobotModelConfig:
|
||||
"""Load a cuda robot model from only urdf. This does not support collision queries.
|
||||
|
||||
Args:
|
||||
urdf_path : Path of urdf file.
|
||||
base_link : Name of base link.
|
||||
ee_link : Name of end-effector link.
|
||||
tensor_args : Device to load robot model. Defaults to TensorDeviceType().
|
||||
|
||||
Returns:
|
||||
CudaRobotModelConfig: cuda robot model configuration.
|
||||
"""
|
||||
config = CudaRobotGeneratorConfig(base_link, ee_link, tensor_args, urdf_path=urdf_path)
|
||||
return CudaRobotModelConfig.from_config(config)
|
||||
|
||||
@staticmethod
|
||||
def from_basic_usd(
|
||||
usd_path: str,
|
||||
usd_robot_root: str,
|
||||
base_link: str,
|
||||
ee_link: str,
|
||||
tensor_args: TensorDeviceType = TensorDeviceType(),
|
||||
) -> CudaRobotModelConfig:
|
||||
"""Load a cuda robot model from only urdf. This does not support collision queries.
|
||||
|
||||
Args:
|
||||
urdf_path : Path of urdf file.
|
||||
base_link : Name of base link.
|
||||
ee_link : Name of end-effector link.
|
||||
tensor_args : Device to load robot model. Defaults to TensorDeviceType().
|
||||
|
||||
Returns:
|
||||
CudaRobotModelConfig: cuda robot model configuration.
|
||||
"""
|
||||
config = CudaRobotGeneratorConfig(
|
||||
tensor_args,
|
||||
base_link,
|
||||
ee_link,
|
||||
usd_path=usd_path,
|
||||
usd_robot_root=usd_robot_root,
|
||||
use_usd_kinematics=True,
|
||||
)
|
||||
return CudaRobotModelConfig.from_config(config)
|
||||
|
||||
@staticmethod
|
||||
def from_robot_yaml_file(
|
||||
file_path: str,
|
||||
ee_link: Optional[str] = None,
|
||||
tensor_args: TensorDeviceType = TensorDeviceType(),
|
||||
):
|
||||
config_file = load_yaml(join_path(get_robot_path(), file_path))["robot_cfg"]["kinematics"]
|
||||
if ee_link is not None:
|
||||
config_file["ee_link"] = ee_link
|
||||
return CudaRobotModelConfig.from_config(
|
||||
CudaRobotGeneratorConfig(**config_file, tensor_args=tensor_args)
|
||||
)
|
||||
|
||||
@staticmethod
|
||||
def from_data_dict(
|
||||
data_dict: Dict[str, Any], tensor_args: TensorDeviceType = TensorDeviceType()
|
||||
):
|
||||
return CudaRobotModelConfig.from_config(
|
||||
CudaRobotGeneratorConfig(**data_dict, tensor_args=tensor_args)
|
||||
)
|
||||
|
||||
@staticmethod
|
||||
def from_config(config: CudaRobotGeneratorConfig):
|
||||
# create a config generator and load all values
|
||||
generator = CudaRobotGenerator(config)
|
||||
return CudaRobotModelConfig(
|
||||
tensor_args=generator.tensor_args,
|
||||
link_names=generator.link_names,
|
||||
kinematics_config=generator.kinematics_config,
|
||||
self_collision_config=generator.self_collision_config,
|
||||
kinematics_parser=generator.kinematics_parser,
|
||||
use_global_cumul=generator.use_global_cumul,
|
||||
compute_jacobian=generator.compute_jacobian,
|
||||
generator_config=config,
|
||||
)
|
||||
|
||||
@property
|
||||
def cspace(self):
|
||||
return self.kinematics_config.cspace
|
||||
|
||||
|
||||
class CudaRobotModel(CudaRobotModelConfig):
|
||||
"""
|
||||
CUDA Accelerated Robot Model
|
||||
|
||||
NOTE: Currently dof is created only for links that we need to compute kinematics.
|
||||
E.g., for robots with many serial chains, add all links of the robot to get the correct dof.
|
||||
This is not an issue if you are loading collision spheres as that will cover the full geometry
|
||||
of the robot.
|
||||
"""
|
||||
|
||||
def __init__(self, config: CudaRobotModelConfig):
|
||||
super().__init__(**vars(config))
|
||||
self._batch_size = 0
|
||||
self.update_batch_size(1, reset_buffers=True)
|
||||
|
||||
def update_batch_size(self, batch_size, force_update=False, reset_buffers=False):
|
||||
if batch_size == 0:
|
||||
log_error("batch size is zero")
|
||||
if force_update and self._batch_size == batch_size and self.compute_jacobian:
|
||||
self.lin_jac = self.lin_jac.detach() # .requires_grad_(True)
|
||||
self.ang_jac = self.ang_jac.detach() # .requires_grad_(True)
|
||||
elif self._batch_size != batch_size or reset_buffers:
|
||||
self._batch_size = batch_size
|
||||
self._link_pos_seq = torch.zeros(
|
||||
(self._batch_size, len(self.link_names), 3),
|
||||
device=self.tensor_args.device,
|
||||
dtype=self.tensor_args.dtype,
|
||||
)
|
||||
self._link_quat_seq = torch.zeros(
|
||||
(self._batch_size, len(self.link_names), 4),
|
||||
device=self.tensor_args.device,
|
||||
dtype=self.tensor_args.dtype,
|
||||
)
|
||||
|
||||
self._batch_robot_spheres = torch.zeros(
|
||||
(self._batch_size, self.kinematics_config.total_spheres, 4),
|
||||
device=self.tensor_args.device,
|
||||
dtype=self.tensor_args.dtype,
|
||||
)
|
||||
self._grad_out_q = torch.zeros(
|
||||
(self._batch_size, self.get_dof()),
|
||||
device=self.tensor_args.device,
|
||||
dtype=self.tensor_args.dtype,
|
||||
)
|
||||
self._global_cumul_mat = torch.zeros(
|
||||
(self._batch_size, self.kinematics_config.link_map.shape[0], 4, 4),
|
||||
device=self.tensor_args.device,
|
||||
dtype=self.tensor_args.dtype,
|
||||
)
|
||||
if self.compute_jacobian:
|
||||
self.lin_jac = torch.zeros(
|
||||
[batch_size, 3, self.kinematics_config.n_dofs],
|
||||
device=self.tensor_args.device,
|
||||
dtype=self.tensor_args.dtype,
|
||||
)
|
||||
self.ang_jac = torch.zeros(
|
||||
[batch_size, 3, self.kinematics_config.n_dofs],
|
||||
device=self.tensor_args.device,
|
||||
dtype=self.tensor_args.dtype,
|
||||
)
|
||||
|
||||
@profiler.record_function("cuda_robot_model/forward_kinematics")
|
||||
def forward(self, q, link_name=None, calculate_jacobian=False):
|
||||
# pos, rot = self.compute_forward_kinematics(q, qd, link_name)
|
||||
if len(q.shape) > 2:
|
||||
raise ValueError("q shape should be [batch_size, dof]")
|
||||
batch_size = q.shape[0]
|
||||
self.update_batch_size(batch_size, force_update=q.requires_grad)
|
||||
|
||||
# do fused forward:
|
||||
link_pos_seq, link_quat_seq, link_spheres_tensor = self._cuda_forward(q)
|
||||
|
||||
if len(self.link_names) == 1:
|
||||
ee_pos = link_pos_seq.squeeze(1)
|
||||
ee_quat = link_quat_seq.squeeze(1)
|
||||
else:
|
||||
link_idx = self.kinematics_config.ee_idx
|
||||
if link_name is not None:
|
||||
link_idx = self.link_names.index(link_name)
|
||||
ee_pos = link_pos_seq.contiguous()[..., link_idx, :]
|
||||
ee_quat = link_quat_seq.contiguous()[..., link_idx, :]
|
||||
lin_jac = ang_jac = None
|
||||
|
||||
# compute jacobians?
|
||||
if calculate_jacobian:
|
||||
raise NotImplementedError
|
||||
return (
|
||||
ee_pos,
|
||||
ee_quat,
|
||||
lin_jac,
|
||||
ang_jac,
|
||||
link_pos_seq,
|
||||
link_quat_seq,
|
||||
link_spheres_tensor,
|
||||
)
|
||||
|
||||
def get_state(self, q, link_name=None, calculate_jacobian=False) -> CudaRobotModelState:
|
||||
out = self.forward(q, link_name, calculate_jacobian)
|
||||
state = CudaRobotModelState(
|
||||
out[0],
|
||||
out[1],
|
||||
None,
|
||||
None,
|
||||
out[4],
|
||||
out[5],
|
||||
out[6],
|
||||
self.link_names,
|
||||
)
|
||||
return state
|
||||
|
||||
def get_robot_as_mesh(self, q: torch.Tensor):
|
||||
# get all link meshes:
|
||||
m_list = [self.get_link_mesh(l) for l in self.mesh_link_names]
|
||||
pose = self.get_link_poses(q, self.mesh_link_names)
|
||||
for li, l in enumerate(self.mesh_link_names):
|
||||
m_list[li].pose = pose.get_index(0, li).tolist()
|
||||
|
||||
return m_list
|
||||
|
||||
def get_robot_as_spheres(self, q: torch.Tensor, filter_valid: bool = True):
|
||||
state = self.get_state(q)
|
||||
|
||||
# state has sphere position and radius
|
||||
|
||||
sph_all = state.get_link_spheres().cpu().numpy()
|
||||
|
||||
sph_traj = []
|
||||
for j in range(sph_all.shape[0]):
|
||||
sph = sph_all[j, :, :]
|
||||
if filter_valid:
|
||||
sph_list = [
|
||||
Sphere(
|
||||
name="robot_curobo_sphere_" + str(i),
|
||||
pose=[sph[i, 0], sph[i, 1], sph[i, 2], 1, 0, 0, 0],
|
||||
radius=sph[i, 3],
|
||||
)
|
||||
for i in range(sph.shape[0])
|
||||
if (sph[i, 3] > 0.0)
|
||||
]
|
||||
else:
|
||||
sph_list = [
|
||||
Sphere(
|
||||
name="robot_curobo_sphere_" + str(i),
|
||||
pose=[sph[i, 0], sph[i, 1], sph[i, 2], 1, 0, 0, 0],
|
||||
radius=sph[i, 3],
|
||||
)
|
||||
for i in range(sph.shape[0])
|
||||
]
|
||||
sph_traj.append(sph_list)
|
||||
return sph_traj
|
||||
|
||||
def get_link_poses(self, q: torch.Tensor, link_names: List[str]) -> Pose:
|
||||
state = self.get_state(q)
|
||||
position = torch.zeros(
|
||||
(q.shape[0], len(link_names), 3),
|
||||
device=self.tensor_args.device,
|
||||
dtype=self.tensor_args.dtype,
|
||||
)
|
||||
quaternion = torch.zeros(
|
||||
(q.shape[0], len(link_names), 4),
|
||||
device=self.tensor_args.device,
|
||||
dtype=self.tensor_args.dtype,
|
||||
)
|
||||
|
||||
for li, l in enumerate(link_names):
|
||||
i = self.link_names.index(l)
|
||||
position[:, li, :] = state.links_position[:, i, :]
|
||||
quaternion[:, li, :] = state.links_quaternion[:, i, :]
|
||||
return Pose(position=position, quaternion=quaternion)
|
||||
|
||||
def _cuda_forward(self, q):
|
||||
link_pos, link_quat, robot_spheres = get_cuda_kinematics(
|
||||
# self._link_mat_seq, # data will be stored here
|
||||
self._link_pos_seq,
|
||||
self._link_quat_seq,
|
||||
self._batch_robot_spheres,
|
||||
self._global_cumul_mat,
|
||||
q,
|
||||
self.kinematics_config.fixed_transforms,
|
||||
self.kinematics_config.link_spheres,
|
||||
self.kinematics_config.link_map, # tells which link is attached to which link i
|
||||
self.kinematics_config.joint_map, # tells which joint is attached to a link i
|
||||
self.kinematics_config.joint_map_type, # joint type
|
||||
self.kinematics_config.store_link_map,
|
||||
self.kinematics_config.link_sphere_idx_map, # sphere idx map
|
||||
self.kinematics_config.link_chain_map,
|
||||
self._grad_out_q,
|
||||
self.use_global_cumul,
|
||||
)
|
||||
# if(robot_spheres.shape[0]<10):
|
||||
# print(robot_spheres)
|
||||
return link_pos, link_quat, robot_spheres
|
||||
|
||||
@property
|
||||
def all_articulated_joint_names(self):
|
||||
return self.kinematics_config.non_fixed_joint_names
|
||||
|
||||
def get_self_collision_config(self) -> SelfCollisionKinematicsConfig:
|
||||
return self.self_collision_config
|
||||
|
||||
def get_link_mesh(self, link_name: str) -> Mesh:
|
||||
mesh = self.kinematics_parser.get_link_mesh(link_name)
|
||||
mesh.pose = [0, 0, 0, 1, 0, 0, 0]
|
||||
return mesh
|
||||
|
||||
def get_link_transform(self, link_name: str) -> Pose:
|
||||
mat = self._kinematics_config.fixed_transforms[self._name_to_idx_map[link_name]]
|
||||
pose = Pose(position=mat[:3, 3], rotation=mat[:3, :3])
|
||||
return pose
|
||||
|
||||
def get_all_link_transforms(self) -> Pose:
|
||||
pose = Pose(
|
||||
self.kinematics_config.fixed_transforms[:, :3, 3],
|
||||
rotation=self.kinematics_config.fixed_transforms[:, :3, :3],
|
||||
)
|
||||
return pose
|
||||
|
||||
def get_dof(self) -> int:
|
||||
return self.kinematics_config.n_dof
|
||||
|
||||
@property
|
||||
def joint_names(self) -> List[str]:
|
||||
return self.kinematics_config.joint_names
|
||||
|
||||
@property
|
||||
def total_spheres(self) -> int:
|
||||
return self.kinematics_config.total_spheres
|
||||
|
||||
@property
|
||||
def lock_jointstate(self):
|
||||
return self.kinematics_config.lock_jointstate
|
||||
|
||||
@property
|
||||
def ee_link(self):
|
||||
return self.kinematics_config.ee_link
|
||||
|
||||
@property
|
||||
def base_link(self):
|
||||
return self.kinematics_config.base_link
|
||||
|
||||
def update_kinematics_config(self, new_kin_config: KinematicsTensorConfig):
|
||||
self.kinematics_config.copy(new_kin_config)
|
||||
101
src/curobo/cuda_robot_model/kinematics_parser.py
Normal file
101
src/curobo/cuda_robot_model/kinematics_parser.py
Normal file
@@ -0,0 +1,101 @@
|
||||
#
|
||||
# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
#
|
||||
# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
# property and proprietary rights in and to this material, related
|
||||
# documentation and any modifications thereto. Any use, reproduction,
|
||||
# disclosure or distribution of this material and related documentation
|
||||
# without an express license agreement from NVIDIA CORPORATION or
|
||||
# its affiliates is strictly prohibited.
|
||||
#
|
||||
|
||||
# Standard Library
|
||||
from dataclasses import dataclass, field
|
||||
from typing import Dict, List, Optional, Union
|
||||
|
||||
# Third Party
|
||||
import numpy as np
|
||||
|
||||
# CuRobo
|
||||
from curobo.cuda_robot_model.types import JointType
|
||||
from curobo.geom.types import Mesh
|
||||
from curobo.types.base import TensorDeviceType
|
||||
from curobo.types.math import Pose
|
||||
from curobo.util.logger import log_error
|
||||
|
||||
|
||||
@dataclass
|
||||
class LinkParams:
|
||||
link_name: str
|
||||
joint_name: str
|
||||
joint_type: JointType
|
||||
fixed_transform: np.ndarray
|
||||
parent_link_name: Optional[str] = None
|
||||
joint_limits: Optional[List[float]] = None
|
||||
joint_axis: Optional[np.ndarray] = None
|
||||
joint_id: Optional[int] = None
|
||||
joint_velocity_limits: List[float] = field(default_factory=lambda: [-2.0, 2.0])
|
||||
|
||||
@staticmethod
|
||||
def from_dict(dict_data):
|
||||
dict_data["joint_type"] = JointType[dict_data["joint_type"]]
|
||||
dict_data["fixed_transform"] = (
|
||||
Pose.from_list(dict_data["fixed_transform"], tensor_args=TensorDeviceType())
|
||||
.get_numpy_matrix()
|
||||
.reshape(4, 4)
|
||||
)
|
||||
|
||||
return LinkParams(**dict_data)
|
||||
|
||||
|
||||
class KinematicsParser:
|
||||
def __init__(self, extra_links: Optional[Dict[str, LinkParams]] = None) -> None:
|
||||
self._parent_map = {}
|
||||
self.extra_links = extra_links
|
||||
self.build_link_parent()
|
||||
# add extra links to parent:
|
||||
if self.extra_links is not None and len(list(self.extra_links.keys())) > 0:
|
||||
for i in self.extra_links:
|
||||
self._parent_map[i] = self.extra_links[i].parent_link_name
|
||||
|
||||
def build_link_parent(self):
|
||||
"""Build a map of parent links to each link in the kinematic tree.
|
||||
|
||||
NOTE: Use this function to fill self._parent_map.
|
||||
"""
|
||||
pass
|
||||
|
||||
def get_chain(self, base_link: str, ee_link: str) -> List[str]:
|
||||
"""Get list of links attaching ee_link to base_link.
|
||||
|
||||
Args:
|
||||
base_link (str): Name of base link.
|
||||
ee_link (str): Name of end-effector link.
|
||||
|
||||
Returns:
|
||||
List[str]: List of link names starting from base_link to ee_link.
|
||||
"""
|
||||
chain_links = [ee_link]
|
||||
link = ee_link
|
||||
while link != base_link:
|
||||
link = self._parent_map[link]
|
||||
# add link to chain:
|
||||
chain_links.append(link)
|
||||
chain_links.reverse()
|
||||
return chain_links
|
||||
|
||||
def _get_from_extra_links(self, link_name: str) -> LinkParams:
|
||||
if self.extra_links is None:
|
||||
return None
|
||||
if link_name in self.extra_links.keys():
|
||||
return self.extra_links[link_name]
|
||||
return None
|
||||
|
||||
def get_link_parameters(self, link_name: str, base: bool = False) -> LinkParams:
|
||||
pass
|
||||
|
||||
def add_absolute_path_to_link_meshes(self, mesh_dir: str = ""):
|
||||
pass
|
||||
|
||||
def get_link_mesh(self, link_name: str) -> Mesh:
|
||||
pass
|
||||
388
src/curobo/cuda_robot_model/types.py
Normal file
388
src/curobo/cuda_robot_model/types.py
Normal file
@@ -0,0 +1,388 @@
|
||||
#
|
||||
# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
#
|
||||
# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
# property and proprietary rights in and to this material, related
|
||||
# documentation and any modifications thereto. Any use, reproduction,
|
||||
# disclosure or distribution of this material and related documentation
|
||||
# without an express license agreement from NVIDIA CORPORATION or
|
||||
# its affiliates is strictly prohibited.
|
||||
#
|
||||
from __future__ import annotations
|
||||
|
||||
# Standard Library
|
||||
from dataclasses import dataclass
|
||||
from enum import Enum
|
||||
from typing import Any, Dict, List, Optional, Union
|
||||
|
||||
# Third Party
|
||||
import torch
|
||||
|
||||
# CuRobo
|
||||
from curobo.types.base import TensorDeviceType
|
||||
from curobo.types.math import Pose
|
||||
from curobo.types.state import JointState
|
||||
from curobo.types.tensor import T_DOF
|
||||
from curobo.util.tensor_util import copy_if_not_none
|
||||
|
||||
|
||||
class JointType(Enum):
|
||||
FIXED = -1
|
||||
X_PRISM = 0
|
||||
Y_PRISM = 1
|
||||
Z_PRISM = 2
|
||||
X_ROT = 3
|
||||
Y_ROT = 4
|
||||
Z_ROT = 5
|
||||
|
||||
|
||||
@dataclass
|
||||
class JointLimits:
|
||||
joint_names: List[str]
|
||||
position: torch.Tensor
|
||||
velocity: torch.Tensor
|
||||
acceleration: torch.Tensor
|
||||
jerk: torch.Tensor
|
||||
effort: Optional[torch.Tensor] = None
|
||||
tensor_args: TensorDeviceType = TensorDeviceType()
|
||||
|
||||
@staticmethod
|
||||
def from_data_dict(data: Dict, tensor_args: TensorDeviceType = TensorDeviceType()):
|
||||
p = tensor_args.to_device(data["position"])
|
||||
v = tensor_args.to_device(data["velocity"])
|
||||
a = tensor_args.to_device(data["acceleration"])
|
||||
j = tensor_args.to_device(data["jerk"])
|
||||
e = None
|
||||
if "effort" in data and data["effort"] is not None:
|
||||
e = tensor_args.to_device(data["effort"])
|
||||
|
||||
return JointLimits(data["joint_names"], p, v, a, j, e)
|
||||
|
||||
def clone(self) -> JointLimits:
|
||||
return JointLimits(
|
||||
self.joint_names.copy(),
|
||||
self.position.clone(),
|
||||
self.velocity.clone(),
|
||||
self.acceleration.clone(),
|
||||
self.jerk.clone(),
|
||||
self.effort.clone() if self.effort is not None else None,
|
||||
self.tensor_args,
|
||||
)
|
||||
|
||||
|
||||
@dataclass
|
||||
class CSpaceConfig:
|
||||
joint_names: List[str]
|
||||
retract_config: Optional[T_DOF] = None
|
||||
cspace_distance_weight: Optional[T_DOF] = None
|
||||
null_space_weight: Optional[T_DOF] = None # List[float]
|
||||
tensor_args: TensorDeviceType = TensorDeviceType()
|
||||
max_acceleration: Union[float, List[float]] = 10.0
|
||||
max_jerk: Union[float, List[float]] = 500.0
|
||||
velocity_scale: Union[float, List[float]] = 1.0
|
||||
acceleration_scale: Union[float, List[float]] = 1.0
|
||||
jerk_scale: Union[float, List[float]] = 1.0
|
||||
|
||||
def __post_init__(self):
|
||||
if self.retract_config is not None:
|
||||
self.retract_config = self.tensor_args.to_device(self.retract_config)
|
||||
if self.cspace_distance_weight is not None:
|
||||
self.cspace_distance_weight = self.tensor_args.to_device(self.cspace_distance_weight)
|
||||
if self.null_space_weight is not None:
|
||||
self.null_space_weight = self.tensor_args.to_device(self.null_space_weight)
|
||||
if isinstance(self.max_acceleration, float):
|
||||
self.max_acceleration = self.tensor_args.to_device(
|
||||
[self.max_acceleration for _ in self.joint_names]
|
||||
)
|
||||
|
||||
if isinstance(self.velocity_scale, float) or len(self.velocity_scale) == 1:
|
||||
self.velocity_scale = self.tensor_args.to_device(
|
||||
[self.velocity_scale for _ in self.joint_names]
|
||||
).view(-1)
|
||||
|
||||
if isinstance(self.acceleration_scale, float) or len(self.acceleration_scale) == 1:
|
||||
self.acceleration_scale = self.tensor_args.to_device(
|
||||
[self.acceleration_scale for _ in self.joint_names]
|
||||
).view(-1)
|
||||
|
||||
if isinstance(self.jerk_scale, float) or len(self.jerk_scale) == 1:
|
||||
self.jerk_scale = self.tensor_args.to_device(
|
||||
[self.jerk_scale for _ in self.joint_names]
|
||||
).view(-1)
|
||||
|
||||
if isinstance(self.max_acceleration, List):
|
||||
self.max_acceleration = self.tensor_args.to_device(self.max_acceleration)
|
||||
if isinstance(self.max_jerk, float):
|
||||
self.max_jerk = [self.max_jerk for _ in self.joint_names]
|
||||
if isinstance(self.max_jerk, List):
|
||||
self.max_jerk = self.tensor_args.to_device(self.max_jerk)
|
||||
if isinstance(self.velocity_scale, List):
|
||||
self.velocity_scale = self.tensor_args.to_device(self.velocity_scale)
|
||||
|
||||
if isinstance(self.acceleration_scale, List):
|
||||
self.acceleration_scale = self.tensor_args.to_device(self.acceleration_scale)
|
||||
if isinstance(self.jerk_scale, List):
|
||||
self.jerk_scale = self.tensor_args.to_device(self.jerk_scale)
|
||||
|
||||
def inplace_reindex(self, joint_names: List[str]):
|
||||
new_index = [self.joint_names.index(j) for j in joint_names]
|
||||
if self.retract_config is not None:
|
||||
self.retract_config = self.retract_config[new_index].clone()
|
||||
if self.cspace_distance_weight is not None:
|
||||
self.cspace_distance_weight = self.cspace_distance_weight[new_index].clone()
|
||||
if self.null_space_weight is not None:
|
||||
self.null_space_weight = self.null_space_weight[new_index].clone()
|
||||
self.max_acceleration = self.max_acceleration[new_index].clone()
|
||||
self.max_jerk = self.max_jerk[new_index].clone()
|
||||
self.velocity_scale = self.velocity_scale[new_index].clone()
|
||||
self.acceleration_scale = self.acceleration_scale[new_index].clone()
|
||||
self.jerk_scale = self.jerk_scale[new_index].clone()
|
||||
joint_names = [self.joint_names[n] for n in new_index]
|
||||
self.joint_names = joint_names
|
||||
|
||||
def clone(self) -> CSpaceConfig:
|
||||
return CSpaceConfig(
|
||||
joint_names=self.joint_names.copy(),
|
||||
retract_config=copy_if_not_none(self.retract_config),
|
||||
null_space_weight=copy_if_not_none(self.null_space_weight),
|
||||
cspace_distance_weight=copy_if_not_none(self.cspace_distance_weight),
|
||||
tensor_args=self.tensor_args,
|
||||
max_jerk=self.max_jerk.clone(),
|
||||
max_acceleration=self.max_acceleration.clone(),
|
||||
velocity_scale=self.velocity_scale.clone(),
|
||||
acceleration_scale=self.acceleration_scale.clone(),
|
||||
jerk_scale=self.jerk_scale.clone(),
|
||||
)
|
||||
|
||||
def scale_joint_limits(self, joint_limits: JointLimits):
|
||||
if self.velocity_scale is not None:
|
||||
joint_limits.velocity = joint_limits.velocity * self.velocity_scale
|
||||
if self.acceleration_scale is not None:
|
||||
joint_limits.acceleration = joint_limits.acceleration * self.acceleration_scale
|
||||
if self.jerk_scale is not None:
|
||||
joint_limits.jerk = joint_limits.jerk * self.jerk_scale
|
||||
|
||||
return joint_limits
|
||||
|
||||
@staticmethod
|
||||
def load_from_joint_limits(
|
||||
joint_position_upper: torch.Tensor,
|
||||
joint_position_lower: torch.Tensor,
|
||||
joint_names: List[str],
|
||||
tensor_args: TensorDeviceType = TensorDeviceType(),
|
||||
):
|
||||
retract_config = ((joint_position_upper + joint_position_lower) / 2).flatten()
|
||||
n_dof = retract_config.shape[-1]
|
||||
null_space_weight = torch.ones(n_dof, **vars(tensor_args))
|
||||
cspace_distance_weight = torch.ones(n_dof, **vars(tensor_args))
|
||||
return CSpaceConfig(
|
||||
joint_names,
|
||||
retract_config,
|
||||
cspace_distance_weight,
|
||||
null_space_weight,
|
||||
tensor_args=tensor_args,
|
||||
)
|
||||
|
||||
|
||||
@dataclass
|
||||
class KinematicsTensorConfig:
|
||||
fixed_transforms: torch.Tensor
|
||||
link_map: torch.Tensor
|
||||
joint_map: torch.Tensor
|
||||
joint_map_type: torch.Tensor
|
||||
store_link_map: torch.Tensor
|
||||
link_chain_map: torch.Tensor
|
||||
link_names: List[str]
|
||||
joint_limits: JointLimits
|
||||
non_fixed_joint_names: List[str]
|
||||
n_dof: int
|
||||
mesh_link_names: Optional[List[str]] = None
|
||||
joint_names: Optional[List[str]] = None
|
||||
lock_jointstate: Optional[JointState] = None
|
||||
link_spheres: Optional[torch.Tensor] = None
|
||||
link_sphere_idx_map: Optional[torch.Tensor] = None
|
||||
link_name_to_idx_map: Optional[Dict[str, int]] = None
|
||||
total_spheres: int = 0
|
||||
debug: Optional[Any] = None
|
||||
ee_idx: int = 0
|
||||
cspace: Optional[CSpaceConfig] = None
|
||||
base_link: str = "base_link"
|
||||
ee_link: str = "ee_link"
|
||||
|
||||
def __post_init__(self):
|
||||
if self.cspace is None and self.joint_limits is not None:
|
||||
self.load_cspace_cfg_from_kinematics()
|
||||
if self.joint_limits is not None and self.cspace is not None:
|
||||
self.joint_limits = self.cspace.scale_joint_limits(self.joint_limits)
|
||||
|
||||
def load_cspace_cfg_from_kinematics(self):
|
||||
retract_config = (
|
||||
(self.joint_limits.position[1] + self.joint_limits.position[0]) / 2
|
||||
).flatten()
|
||||
null_space_weight = torch.ones(self.n_dof, **vars(self.tensor_args))
|
||||
cspace_distance_weight = torch.ones(self.n_dof, **vars(self.tensor_args))
|
||||
joint_names = self.joint_names
|
||||
self.cspace = CSpaceConfig(
|
||||
joint_names,
|
||||
retract_config,
|
||||
cspace_distance_weight,
|
||||
null_space_weight,
|
||||
tensor_args=self.tensor_args,
|
||||
max_acceleration=self.joint_limits.acceleration[1],
|
||||
max_jerk=self.joint_limits.max_jerk[1],
|
||||
)
|
||||
|
||||
def get_sphere_index_from_link_name(self, link_name: str) -> torch.Tensor:
|
||||
link_idx = self.link_name_to_idx_map[link_name]
|
||||
# get sphere index for this link:
|
||||
link_spheres_idx = torch.nonzero(self.link_sphere_idx_map == link_idx).view(-1)
|
||||
return link_spheres_idx
|
||||
|
||||
def update_link_spheres(
|
||||
self, link_name: str, sphere_position_radius: torch.Tensor, start_sph_idx: int = 0
|
||||
):
|
||||
"""Update sphere parameters
|
||||
|
||||
#NOTE: This currently does not update self collision distances.
|
||||
Args:
|
||||
link_name: _description_
|
||||
sphere_position_radius: _description_
|
||||
start_sph_idx: _description_. Defaults to 0.
|
||||
"""
|
||||
# get sphere indices from link name:
|
||||
link_sphere_index = self.get_sphere_index_from_link_name(link_name)[
|
||||
start_sph_idx : start_sph_idx + sphere_position_radius.shape[0]
|
||||
]
|
||||
# update sphere data:
|
||||
self.link_spheres[link_sphere_index, :] = sphere_position_radius
|
||||
|
||||
def get_link_spheres(
|
||||
self,
|
||||
link_name: str,
|
||||
):
|
||||
link_sphere_index = self.get_sphere_index_from_link_name(link_name)
|
||||
return self.link_spheres[link_sphere_index, :]
|
||||
|
||||
def attach_object(
|
||||
self,
|
||||
sphere_radius: Optional[float] = None,
|
||||
sphere_tensor: Optional[torch.Tensor] = None,
|
||||
link_name: str = "attached_object",
|
||||
) -> bool:
|
||||
"""_summary_
|
||||
|
||||
Args:
|
||||
sphere_radius: _description_. Defaults to None.
|
||||
sphere_tensor: _description_. Defaults to None.
|
||||
link_name: _description_. Defaults to "attached_object".
|
||||
|
||||
Raises:
|
||||
ValueError: _description_
|
||||
ValueError: _description_
|
||||
|
||||
Returns:
|
||||
_description_
|
||||
"""
|
||||
if link_name not in self.link_name_to_idx_map.keys():
|
||||
raise ValueError(link_name + " not found in spheres")
|
||||
curr_spheres = self.get_link_spheres(link_name)
|
||||
|
||||
if sphere_radius is not None:
|
||||
curr_spheres[:, 3] = sphere_radius
|
||||
if sphere_tensor is not None:
|
||||
if sphere_tensor.shape != curr_spheres.shape and sphere_tensor.shape[0] != 1:
|
||||
raise ValueError("sphere_tensor shape does not match current spheres")
|
||||
curr_spheres[:, :] = sphere_tensor
|
||||
self.update_link_spheres(link_name, curr_spheres)
|
||||
return True
|
||||
|
||||
def detach_object(self, link_name: str = "attached_object") -> bool:
|
||||
"""Detach object from robot end-effector
|
||||
|
||||
Args:
|
||||
link_name: _description_. Defaults to "attached_object".
|
||||
|
||||
Raises:
|
||||
ValueError: _description_
|
||||
|
||||
Returns:
|
||||
_description_
|
||||
"""
|
||||
if link_name not in self.link_name_to_idx_map.keys():
|
||||
raise ValueError(link_name + " not found in spheres")
|
||||
curr_spheres = self.get_link_spheres(link_name)
|
||||
curr_spheres[:, 3] = -100.0
|
||||
curr_spheres[:, :3] = 0.0
|
||||
self.update_link_spheres(link_name, curr_spheres)
|
||||
|
||||
return True
|
||||
|
||||
def get_number_of_spheres(self, link_name: str) -> int:
|
||||
"""Get number of spheres for a link
|
||||
|
||||
Args:
|
||||
link_name: name of link
|
||||
"""
|
||||
return self.get_link_spheres(link_name).shape[0]
|
||||
|
||||
|
||||
@dataclass
|
||||
class SelfCollisionKinematicsConfig:
|
||||
"""Dataclass that stores self collision attributes to pass to cuda kernel."""
|
||||
|
||||
offset: Optional[torch.Tensor] = None
|
||||
distance_threshold: Optional[torch.Tensor] = None
|
||||
thread_location: Optional[torch.Tensor] = None
|
||||
thread_max: Optional[int] = None
|
||||
collision_matrix: Optional[torch.Tensor] = None
|
||||
experimental_kernel: bool = True
|
||||
checks_per_thread: int = 32
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CudaRobotModelState:
|
||||
"""Dataclass that stores kinematics information."""
|
||||
|
||||
#: End-effector position stored as x,y,z in meters [b, 3]. End-effector is defined by
|
||||
#: :py:attr:`curobo.cuda_robot_model.cuda_robot_generator.CudaRobotGeneratorConfig.ee_link`.
|
||||
ee_position: torch.Tensor
|
||||
|
||||
#: End-effector orientaiton stored as quaternion qw, qx, qy, qz [b,4]. End-effector is defined
|
||||
# by :py:attr:`CudaRobotModelConfig.ee_link`.
|
||||
ee_quaternion: torch.Tensor
|
||||
|
||||
#: Linear Jacobian. Currently not supported.
|
||||
lin_jacobian: Optional[torch.Tensor] = None
|
||||
|
||||
#: Angular Jacobian. Currently not supported.
|
||||
ang_jacobian: Optional[torch.Tensor] = None
|
||||
|
||||
#: Position of links specified by link_names (:py:attr:`CudaRobotModelConfig.link_names`).
|
||||
links_position: Optional[torch.Tensor] = None
|
||||
|
||||
#: Quaternions of links specified by link names (:py:attr:`CudaRobotModelConfig.link_names`).
|
||||
links_quaternion: Optional[torch.Tensor] = None
|
||||
|
||||
#: Position of spheres specified by collision spheres (:py:attr:`CudaRobotModelConfig.collision_spheres`)
|
||||
#: in x, y, z, r format [b,n,4].
|
||||
link_spheres_tensor: Optional[torch.Tensor] = None
|
||||
|
||||
link_names: Optional[str] = None
|
||||
|
||||
@property
|
||||
def ee_pose(self):
|
||||
return Pose(self.ee_position, self.ee_quaternion)
|
||||
|
||||
def get_link_spheres(self):
|
||||
return self.link_spheres_tensor
|
||||
|
||||
@property
|
||||
def link_pose(self):
|
||||
link_poses = None
|
||||
if self.link_names is not None:
|
||||
link_poses = {}
|
||||
link_pos = self.links_position.contiguous()
|
||||
link_quat = self.links_quaternion.contiguous()
|
||||
for i, v in enumerate(self.link_names):
|
||||
link_poses[v] = Pose(link_pos[..., i, :], link_quat[..., i, :])
|
||||
return link_poses
|
||||
167
src/curobo/cuda_robot_model/urdf_kinematics_parser.py
Normal file
167
src/curobo/cuda_robot_model/urdf_kinematics_parser.py
Normal file
@@ -0,0 +1,167 @@
|
||||
#
|
||||
# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
#
|
||||
# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
# property and proprietary rights in and to this material, related
|
||||
# documentation and any modifications thereto. Any use, reproduction,
|
||||
# disclosure or distribution of this material and related documentation
|
||||
# without an express license agreement from NVIDIA CORPORATION or
|
||||
# its affiliates is strictly prohibited.
|
||||
#
|
||||
# Standard Library
|
||||
from typing import Dict, Optional
|
||||
|
||||
# Third Party
|
||||
import numpy as np
|
||||
import yourdfpy
|
||||
from lxml import etree
|
||||
|
||||
# CuRobo
|
||||
from curobo.cuda_robot_model.kinematics_parser import KinematicsParser, LinkParams
|
||||
from curobo.cuda_robot_model.types import JointType
|
||||
from curobo.geom.types import Mesh as CuroboMesh
|
||||
from curobo.types.base import TensorDeviceType
|
||||
from curobo.util.logger import log_error, log_warn
|
||||
from curobo.util_file import join_path
|
||||
|
||||
|
||||
class UrdfKinematicsParser(KinematicsParser):
|
||||
def __init__(
|
||||
self,
|
||||
urdf_path,
|
||||
load_meshes: bool = False,
|
||||
mesh_root: str = "",
|
||||
extra_links: Optional[Dict[str, LinkParams]] = None,
|
||||
) -> None:
|
||||
# load robot from urdf:
|
||||
self._robot = yourdfpy.URDF.load(
|
||||
urdf_path,
|
||||
load_meshes=load_meshes,
|
||||
build_scene_graph=False,
|
||||
mesh_dir=mesh_root,
|
||||
filename_handler=yourdfpy.filename_handler_null,
|
||||
)
|
||||
super().__init__(extra_links)
|
||||
|
||||
def build_link_parent(self):
|
||||
self._parent_map = {}
|
||||
for j in self._robot.joint_map:
|
||||
self._parent_map[self._robot.joint_map[j].child] = self._robot.joint_map[j].parent
|
||||
|
||||
def _find_parent_joint_of_link(self, link_name):
|
||||
for j_idx, j in enumerate(self._robot.joint_map):
|
||||
if self._robot.joint_map[j].child == link_name:
|
||||
return j_idx, j
|
||||
log_error("Link is not attached to any joint")
|
||||
|
||||
def _get_joint_name(self, idx):
|
||||
joint = self._robot.joint_names[idx]
|
||||
return joint
|
||||
|
||||
def get_link_parameters(self, link_name: str, base=False) -> LinkParams:
|
||||
link_params = self._get_from_extra_links(link_name)
|
||||
if link_params is not None:
|
||||
return link_params
|
||||
body_params = {}
|
||||
body_params["link_name"] = link_name
|
||||
|
||||
if base:
|
||||
body_params["parent_link_name"] = None
|
||||
joint_transform = np.eye(4)
|
||||
joint_name = "base_joint"
|
||||
joint_type = "fixed"
|
||||
joint_limits = None
|
||||
joint_axis = None
|
||||
body_params["joint_id"] = 0
|
||||
else:
|
||||
body_params["parent_link_name"] = self._parent_map[link_name]
|
||||
|
||||
jid, joint_name = self._find_parent_joint_of_link(link_name)
|
||||
body_params["joint_id"] = jid
|
||||
joint = self._robot.joint_map[joint_name]
|
||||
joint_transform = joint.origin
|
||||
if joint_transform is None:
|
||||
joint_transform = np.eye(4)
|
||||
joint_type = joint.type
|
||||
joint_limits = None
|
||||
joint_axis = None
|
||||
if joint_type != "fixed":
|
||||
if joint_type != "continuous":
|
||||
joint_limits = {
|
||||
"effort": joint.limit.effort,
|
||||
"lower": joint.limit.lower,
|
||||
"upper": joint.limit.upper,
|
||||
"velocity": joint.limit.velocity,
|
||||
}
|
||||
else:
|
||||
# log_warn("Converting continuous joint to revolute")
|
||||
joint_type = "revolute"
|
||||
joint_limits = {
|
||||
"effort": joint.limit.effort,
|
||||
"lower": -3.14 * 2,
|
||||
"upper": 3.14 * 2,
|
||||
"velocity": joint.limit.velocity,
|
||||
}
|
||||
|
||||
joint_axis = joint.axis
|
||||
if (joint_axis < 0).any():
|
||||
log_warn("Joint axis has negative value (-1). This is not supported.")
|
||||
joint_axis = np.abs(joint_axis)
|
||||
body_params["joint_limits"] = [joint_limits["lower"], joint_limits["upper"]]
|
||||
body_params["joint_velocity_limits"] = [
|
||||
-1.0 * joint_limits["velocity"],
|
||||
joint_limits["velocity"],
|
||||
]
|
||||
|
||||
body_params["fixed_transform"] = joint_transform
|
||||
body_params["joint_name"] = joint_name
|
||||
|
||||
body_params["joint_axis"] = joint_axis
|
||||
|
||||
if joint_type == "fixed":
|
||||
joint_type = JointType.FIXED
|
||||
elif joint_type == "prismatic":
|
||||
if joint_axis[0] == 1:
|
||||
joint_type = JointType.X_PRISM
|
||||
if joint_axis[1] == 1:
|
||||
joint_type = JointType.Y_PRISM
|
||||
if joint_axis[2] == 1:
|
||||
joint_type = JointType.Z_PRISM
|
||||
elif joint_type == "revolute":
|
||||
if joint_axis[0] == 1:
|
||||
joint_type = JointType.X_ROT
|
||||
if joint_axis[1] == 1:
|
||||
joint_type = JointType.Y_ROT
|
||||
if joint_axis[2] == 1:
|
||||
joint_type = JointType.Z_ROT
|
||||
else:
|
||||
log_error("Joint type not supported")
|
||||
|
||||
body_params["joint_type"] = joint_type
|
||||
link_params = LinkParams(**body_params)
|
||||
|
||||
return link_params
|
||||
|
||||
def add_absolute_path_to_link_meshes(self, mesh_dir: str = ""):
|
||||
# read all link meshes and update their mesh paths by prepending mesh_dir
|
||||
links = self._robot.link_map
|
||||
for k in links.keys():
|
||||
# read visual and collision
|
||||
vis = links[k].visuals
|
||||
for i in range(len(vis)):
|
||||
m = vis[i].geometry.mesh
|
||||
if m is not None:
|
||||
m.filename = join_path(mesh_dir, m.filename)
|
||||
col = links[k].collisions
|
||||
for i in range(len(col)):
|
||||
m = col[i].geometry.mesh
|
||||
if m is not None:
|
||||
m.filename = join_path(mesh_dir, m.filename)
|
||||
|
||||
def get_urdf_string(self):
|
||||
txt = etree.tostring(self._robot.write_xml(), method="xml", encoding="unicode")
|
||||
return txt
|
||||
|
||||
def get_link_mesh(self, link_name):
|
||||
m = self._robot.link_map[link_name].visuals[0].geometry.mesh
|
||||
return CuroboMesh(name=link_name, pose=None, scale=m.scale, file_path=m.filename)
|
||||
213
src/curobo/cuda_robot_model/usd_kinematics_parser.py
Normal file
213
src/curobo/cuda_robot_model/usd_kinematics_parser.py
Normal file
@@ -0,0 +1,213 @@
|
||||
#
|
||||
# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
#
|
||||
# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
# property and proprietary rights in and to this material, related
|
||||
# documentation and any modifications thereto. Any use, reproduction,
|
||||
# disclosure or distribution of this material and related documentation
|
||||
# without an express license agreement from NVIDIA CORPORATION or
|
||||
# its affiliates is strictly prohibited.
|
||||
#
|
||||
# Standard Library
|
||||
from typing import Dict, List, Optional, Tuple
|
||||
|
||||
# Third Party
|
||||
import numpy as np
|
||||
from pxr import Usd, UsdPhysics
|
||||
|
||||
# CuRobo
|
||||
from curobo.cuda_robot_model.kinematics_parser import KinematicsParser, LinkParams
|
||||
from curobo.cuda_robot_model.types import JointType
|
||||
from curobo.types.base import TensorDeviceType
|
||||
from curobo.types.math import Pose
|
||||
from curobo.util.logger import log_error
|
||||
|
||||
|
||||
class UsdKinematicsParser(KinematicsParser):
|
||||
"""An experimental kinematics parser from USD.
|
||||
NOTE: A more complete solution will be available in a future release. Current implementation
|
||||
does not account for link geometry transformations after a joints.
|
||||
"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
usd_path: str,
|
||||
flip_joints: List[str] = [],
|
||||
flip_joint_limits: List[str] = [],
|
||||
usd_robot_root: str = "robot",
|
||||
tensor_args: TensorDeviceType = TensorDeviceType(),
|
||||
extra_links: Optional[Dict[str, LinkParams]] = None,
|
||||
) -> None:
|
||||
# load usd file:
|
||||
|
||||
# create a usd stage
|
||||
self._flip_joints = flip_joints
|
||||
self._flip_joint_limits = flip_joint_limits
|
||||
self._stage = Usd.Stage.Open(usd_path)
|
||||
self._usd_robot_root = usd_robot_root
|
||||
self._parent_joint_map = {}
|
||||
self.tensor_args = tensor_args
|
||||
super().__init__(extra_links)
|
||||
|
||||
@property
|
||||
def robot_prim_root(self):
|
||||
return self._usd_robot_root
|
||||
|
||||
def build_link_parent(self):
|
||||
self._parent_map = {}
|
||||
all_joints = [
|
||||
x
|
||||
for x in self._stage.Traverse()
|
||||
if (x.IsA(UsdPhysics.Joint) and str(x.GetPath()).startswith(self._usd_robot_root))
|
||||
]
|
||||
for l in all_joints:
|
||||
parent, child = get_links_for_joint(l)
|
||||
if child is not None and parent is not None:
|
||||
self._parent_map[child.GetName()] = parent.GetName()
|
||||
self._parent_joint_map[child.GetName()] = l # store joint prim
|
||||
|
||||
def get_link_parameters(self, link_name: str, base: bool = False) -> LinkParams:
|
||||
"""Get Link parameters from usd stage.
|
||||
|
||||
NOTE: USD kinematics "X" axis joints map to "Z" in URDF. Specifically,
|
||||
uniform token physics:axis = "X" value only matches "Z" in URDF. This is because of usd
|
||||
files assuming Y axis as up while urdf files assume Z axis as up.
|
||||
|
||||
Args:
|
||||
link_name (str): Name of link.
|
||||
base (bool, optional): flag to specify base link. Defaults to False.
|
||||
|
||||
Returns:
|
||||
LinkParams: obtained link parameters.
|
||||
"""
|
||||
link_params = self._get_from_extra_links(link_name)
|
||||
if link_params is not None:
|
||||
return link_params
|
||||
joint_limits = None
|
||||
joint_axis = None
|
||||
if base:
|
||||
parent_link_name = None
|
||||
joint_transform = np.eye(4)
|
||||
joint_name = "base_joint"
|
||||
joint_type = JointType.FIXED
|
||||
|
||||
else:
|
||||
parent_link_name = self._parent_map[link_name]
|
||||
joint_prim = self._parent_joint_map[link_name] # joint prim connects link
|
||||
joint_transform = self._get_joint_transform(joint_prim)
|
||||
joint_axis = None
|
||||
joint_name = joint_prim.GetName()
|
||||
if joint_prim.IsA(UsdPhysics.FixedJoint):
|
||||
joint_type = JointType.FIXED
|
||||
elif joint_prim.IsA(UsdPhysics.RevoluteJoint):
|
||||
j_prim = UsdPhysics.RevoluteJoint(joint_prim)
|
||||
joint_axis = j_prim.GetAxisAttr().Get()
|
||||
joint_limits = np.radians(
|
||||
np.ravel([j_prim.GetLowerLimitAttr().Get(), j_prim.GetUpperLimitAttr().Get()])
|
||||
)
|
||||
if joint_name in self._flip_joints.keys():
|
||||
joint_axis = self._flip_joints[joint_name]
|
||||
if joint_axis == "X":
|
||||
joint_type = JointType.X_ROT
|
||||
elif joint_axis == "Y":
|
||||
joint_type = JointType.Y_ROT
|
||||
elif joint_axis == "Z":
|
||||
joint_type = JointType.Z_ROT
|
||||
else:
|
||||
log_error("Joint axis not supported" + str(joint_axis))
|
||||
|
||||
elif joint_prim.IsA(UsdPhysics.PrismaticJoint):
|
||||
j_prim = UsdPhysics.PrismaticJoint(joint_prim)
|
||||
|
||||
joint_axis = j_prim.GetAxisAttr().Get()
|
||||
joint_limits = np.ravel(
|
||||
[j_prim.GetLowerLimitAttr().Get(), j_prim.GetUpperLimitAttr().Get()]
|
||||
)
|
||||
if joint_name in self._flip_joints.keys():
|
||||
joint_axis = self._flip_joints[joint_name]
|
||||
if joint_name in self._flip_joint_limits:
|
||||
joint_limits = np.ravel(
|
||||
[-1.0 * j_prim.GetUpperLimitAttr().Get(), j_prim.GetLowerLimitAttr().Get()]
|
||||
)
|
||||
if joint_axis == "X":
|
||||
joint_type = JointType.X_PRISM
|
||||
elif joint_axis == "Y":
|
||||
joint_type = JointType.Y_PRISM
|
||||
elif joint_axis == "Z":
|
||||
joint_type = JointType.Z_PRISM
|
||||
else:
|
||||
log_error("Joint axis not supported" + str(joint_axis))
|
||||
else:
|
||||
log_error("Joint type not supported")
|
||||
link_params = LinkParams(
|
||||
link_name=link_name,
|
||||
joint_name=joint_name,
|
||||
joint_type=joint_type,
|
||||
fixed_transform=joint_transform,
|
||||
parent_link_name=parent_link_name,
|
||||
joint_limits=joint_limits,
|
||||
)
|
||||
return link_params
|
||||
|
||||
def _get_joint_transform(self, prim: Usd.Prim):
|
||||
j_prim = UsdPhysics.Joint(prim)
|
||||
position = np.ravel(j_prim.GetLocalPos0Attr().Get())
|
||||
quatf = j_prim.GetLocalRot0Attr().Get()
|
||||
quat = np.zeros(4)
|
||||
quat[0] = quatf.GetReal()
|
||||
quat[1:] = quatf.GetImaginary()
|
||||
|
||||
# create a homogenous transformation matrix:
|
||||
transform_0 = Pose(self.tensor_args.to_device(position), self.tensor_args.to_device(quat))
|
||||
|
||||
position = np.ravel(j_prim.GetLocalPos1Attr().Get())
|
||||
quatf = j_prim.GetLocalRot1Attr().Get()
|
||||
quat = np.zeros(4)
|
||||
quat[0] = quatf.GetReal()
|
||||
quat[1:] = quatf.GetImaginary()
|
||||
|
||||
# create a homogenous transformation matrix:
|
||||
transform_1 = Pose(self.tensor_args.to_device(position), self.tensor_args.to_device(quat))
|
||||
transform = (
|
||||
transform_0.multiply(transform_1.inverse()).get_matrix().cpu().view(4, 4).numpy()
|
||||
)
|
||||
|
||||
# get attached link transform:
|
||||
|
||||
return transform
|
||||
|
||||
|
||||
def get_links_for_joint(prim: Usd.Prim) -> Tuple[Optional[Usd.Prim], Optional[Usd.Prim]]:
|
||||
"""Get all link prims from the given joint prim.
|
||||
|
||||
Note:
|
||||
This assumes that the `body0_rel_targets` and `body1_rel_targets` are configured such
|
||||
that the parent link is specified in `body0_rel_targets` and the child links is specified
|
||||
in `body1_rel_targets`.
|
||||
"""
|
||||
stage = prim.GetStage()
|
||||
joint_api = UsdPhysics.Joint(prim)
|
||||
|
||||
rel0_targets = joint_api.GetBody0Rel().GetTargets()
|
||||
if len(rel0_targets) > 1:
|
||||
raise NotImplementedError(
|
||||
"`get_links_for_joint` does not currently handle more than one relative"
|
||||
f" body target in the joint. joint_prim: {prim}, body0_rel_targets:"
|
||||
f" {rel0_targets}"
|
||||
)
|
||||
link0_prim = None
|
||||
if len(rel0_targets) != 0:
|
||||
link0_prim = stage.GetPrimAtPath(rel0_targets[0])
|
||||
|
||||
rel1_targets = joint_api.GetBody1Rel().GetTargets()
|
||||
if len(rel1_targets) > 1:
|
||||
raise NotImplementedError(
|
||||
"`get_links_for_joint` does not currently handle more than one relative"
|
||||
f" body target in the joint. joint_prim: {prim}, body1_rel_targets:"
|
||||
f" {rel0_targets}"
|
||||
)
|
||||
link1_prim = None
|
||||
if len(rel1_targets) != 0:
|
||||
link1_prim = stage.GetPrimAtPath(rel1_targets[0])
|
||||
|
||||
return (link0_prim, link1_prim)
|
||||
11
src/curobo/curobolib/__init__.py
Normal file
11
src/curobo/curobolib/__init__.py
Normal file
@@ -0,0 +1,11 @@
|
||||
#
|
||||
# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
#
|
||||
# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
# property and proprietary rights in and to this material, related
|
||||
# documentation and any modifications thereto. Any use, reproduction,
|
||||
# disclosure or distribution of this material and related documentation
|
||||
# without an express license agreement from NVIDIA CORPORATION or
|
||||
# its affiliates is strictly prohibited.
|
||||
#
|
||||
"""CuRoboLib Module."""
|
||||
260
src/curobo/curobolib/cpp/geom_cuda.cpp
Normal file
260
src/curobo/curobolib/cpp/geom_cuda.cpp
Normal file
@@ -0,0 +1,260 @@
|
||||
/*
|
||||
* Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
*
|
||||
* NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
* property and proprietary rights in and to this material, related
|
||||
* documentation and any modifications thereto. Any use, reproduction,
|
||||
* disclosure or distribution of this material and related documentation
|
||||
* without an express license agreement from NVIDIA CORPORATION or
|
||||
* its affiliates is strictly prohibited.
|
||||
*/
|
||||
#include <map>
|
||||
#include <torch/extension.h>
|
||||
|
||||
#include <c10/cuda/CUDAGuard.h>
|
||||
#include <vector>
|
||||
|
||||
// CUDA forward declarations
|
||||
|
||||
std::vector<torch::Tensor> self_collision_distance(
|
||||
torch::Tensor out_distance, torch::Tensor out_vec,
|
||||
torch::Tensor sparse_index,
|
||||
const torch::Tensor robot_spheres, // batch_size x n_spheres x 4
|
||||
const torch::Tensor collision_offset, // n_spheres x n_spheres
|
||||
const torch::Tensor weight,
|
||||
const torch::Tensor collision_matrix, // n_spheres x n_spheres
|
||||
const torch::Tensor thread_locations, const int locations_size,
|
||||
const int batch_size, const int nspheres, const bool compute_grad = false,
|
||||
const int ndpt = 8, // Does this need to match template?
|
||||
const bool debug = false);
|
||||
|
||||
// CUDA forward declarations
|
||||
|
||||
std::vector<torch::Tensor> swept_sphere_obb_clpt(
|
||||
const torch::Tensor sphere_position, // batch_size, 3
|
||||
torch::Tensor distance, // batch_size, 1
|
||||
torch::Tensor
|
||||
closest_point, // batch size, 4 -> written out as x,y,z,0 for gradient
|
||||
torch::Tensor sparsity_idx, const torch::Tensor weight,
|
||||
const torch::Tensor activation_distance, const torch::Tensor speed_dt,
|
||||
const torch::Tensor obb_accel, // n_boxes, 4, 4
|
||||
const torch::Tensor obb_bounds, // n_boxes, 3
|
||||
const torch::Tensor obb_pose, // n_boxes, 4, 4
|
||||
const torch::Tensor obb_enable, // n_boxes, 4,
|
||||
const torch::Tensor n_env_obb, // n_boxes, 4, 4
|
||||
const torch::Tensor env_query_idx, // n_boxes, 4, 4
|
||||
const int max_nobs, const int batch_size, const int horizon,
|
||||
const int n_spheres, const int sweep_steps, const bool enable_speed_metric,
|
||||
const bool transform_back, const bool compute_distance,
|
||||
const bool use_batch_env);
|
||||
|
||||
std::vector<torch::Tensor>
|
||||
sphere_obb_clpt(const torch::Tensor sphere_position, // batch_size, 4
|
||||
torch::Tensor distance,
|
||||
torch::Tensor closest_point, // batch size, 3
|
||||
torch::Tensor sparsity_idx, const torch::Tensor weight,
|
||||
const torch::Tensor activation_distance,
|
||||
const torch::Tensor obb_accel, // n_boxes, 4, 4
|
||||
const torch::Tensor obb_bounds, // n_boxes, 3
|
||||
const torch::Tensor obb_pose, // n_boxes, 4, 4
|
||||
const torch::Tensor obb_enable, // n_boxes, 4, 4
|
||||
const torch::Tensor n_env_obb, // n_boxes, 4, 4
|
||||
const torch::Tensor env_query_idx, // n_boxes, 4, 4
|
||||
const int max_nobs, const int batch_size, const int horizon,
|
||||
const int n_spheres, const bool transform_back,
|
||||
const bool compute_distance, const bool use_batch_env);
|
||||
|
||||
std::vector<torch::Tensor> pose_distance(
|
||||
torch::Tensor out_distance, torch::Tensor out_position_distance,
|
||||
torch::Tensor out_rotation_distance,
|
||||
torch::Tensor distance_p_vector, // batch size, 3
|
||||
torch::Tensor distance_q_vector, // batch size, 4
|
||||
torch::Tensor out_gidx,
|
||||
const torch::Tensor current_position, // batch_size, 3
|
||||
const torch::Tensor goal_position, // n_boxes, 3
|
||||
const torch::Tensor current_quat, const torch::Tensor goal_quat,
|
||||
const torch::Tensor vec_weight, // n_boxes, 4, 4
|
||||
const torch::Tensor weight, // n_boxes, 4, 4
|
||||
const torch::Tensor vec_convergence, const torch::Tensor run_weight,
|
||||
const torch::Tensor run_vec_weight, const torch::Tensor batch_pose_idx,
|
||||
const int batch_size, const int horizon, const int mode,
|
||||
const int num_goals = 1, const bool compute_grad = false,
|
||||
const bool write_distance = true, const bool use_metric = false);
|
||||
|
||||
std::vector<torch::Tensor>
|
||||
backward_pose_distance(torch::Tensor out_grad_p, torch::Tensor out_grad_q,
|
||||
const torch::Tensor grad_distance, // batch_size, 3
|
||||
const torch::Tensor grad_p_distance, // n_boxes, 3
|
||||
const torch::Tensor grad_q_distance,
|
||||
const torch::Tensor pose_weight,
|
||||
const torch::Tensor grad_p_vec, // n_boxes, 4, 4
|
||||
const torch::Tensor grad_q_vec, const int batch_size,
|
||||
const bool use_distance = false);
|
||||
|
||||
// C++ interface
|
||||
|
||||
// NOTE: AT_ASSERT has become AT_CHECK on master after 0.4.
|
||||
#define CHECK_CUDA(x) AT_ASSERTM(x.is_cuda(), #x " must be a CUDA tensor")
|
||||
#define CHECK_CONTIGUOUS(x) \
|
||||
AT_ASSERTM(x.is_contiguous(), #x " must be contiguous")
|
||||
#define CHECK_INPUT(x) \
|
||||
CHECK_CUDA(x); \
|
||||
CHECK_CONTIGUOUS(x)
|
||||
|
||||
std::vector<torch::Tensor> self_collision_distance_wrapper(
|
||||
torch::Tensor out_distance, torch::Tensor out_vec,
|
||||
torch::Tensor sparse_index,
|
||||
const torch::Tensor robot_spheres, // batch_size x n_spheres x 4
|
||||
const torch::Tensor collision_offset, // n_spheres
|
||||
const torch::Tensor weight,
|
||||
const torch::Tensor collision_matrix, // n_spheres
|
||||
const torch::Tensor thread_locations, const int thread_locations_size,
|
||||
const int batch_size, const int nspheres, const bool compute_grad = false,
|
||||
const int ndpt = 8, const bool debug = false) {
|
||||
|
||||
CHECK_INPUT(out_distance);
|
||||
CHECK_INPUT(out_vec);
|
||||
CHECK_INPUT(robot_spheres);
|
||||
CHECK_INPUT(collision_offset);
|
||||
CHECK_INPUT(sparse_index);
|
||||
CHECK_INPUT(weight);
|
||||
CHECK_INPUT(thread_locations);
|
||||
CHECK_INPUT(collision_matrix);
|
||||
const at::cuda::OptionalCUDAGuard guard(robot_spheres.device());
|
||||
|
||||
return self_collision_distance(
|
||||
out_distance, out_vec, sparse_index, robot_spheres,
|
||||
collision_offset, weight, collision_matrix, thread_locations,
|
||||
thread_locations_size, batch_size, nspheres, compute_grad, ndpt, debug);
|
||||
}
|
||||
|
||||
std::vector<torch::Tensor> sphere_obb_clpt_wrapper(
|
||||
const torch::Tensor sphere_position, // batch_size, 4
|
||||
|
||||
torch::Tensor distance,
|
||||
torch::Tensor closest_point, // batch size, 3
|
||||
torch::Tensor sparsity_idx, const torch::Tensor weight,
|
||||
const torch::Tensor activation_distance,
|
||||
const torch::Tensor obb_accel, // n_boxes, 4, 4
|
||||
const torch::Tensor obb_bounds, // n_boxes, 3
|
||||
const torch::Tensor obb_pose, // n_boxes, 4, 4
|
||||
const torch::Tensor obb_enable, // n_boxes, 4, 4
|
||||
const torch::Tensor n_env_obb, // n_boxes, 4, 4
|
||||
const torch::Tensor env_query_idx, // n_boxes, 4, 4
|
||||
const int max_nobs, const int batch_size, const int horizon,
|
||||
const int n_spheres, const bool transform_back, const bool compute_distance,
|
||||
const bool use_batch_env) {
|
||||
|
||||
const at::cuda::OptionalCUDAGuard guard(sphere_position.device());
|
||||
CHECK_INPUT(distance);
|
||||
CHECK_INPUT(closest_point);
|
||||
CHECK_INPUT(sphere_position);
|
||||
CHECK_INPUT(sparsity_idx);
|
||||
CHECK_INPUT(weight);
|
||||
CHECK_INPUT(activation_distance);
|
||||
CHECK_INPUT(obb_accel);
|
||||
return sphere_obb_clpt(
|
||||
sphere_position, distance, closest_point, sparsity_idx, weight,
|
||||
activation_distance, obb_accel, obb_bounds, obb_pose, obb_enable,
|
||||
n_env_obb, env_query_idx, max_nobs, batch_size, horizon, n_spheres,
|
||||
transform_back, compute_distance, use_batch_env);
|
||||
}
|
||||
std::vector<torch::Tensor> swept_sphere_obb_clpt_wrapper(
|
||||
const torch::Tensor sphere_position, // batch_size, 4
|
||||
torch::Tensor distance,
|
||||
torch::Tensor closest_point, // batch size, 3
|
||||
torch::Tensor sparsity_idx, const torch::Tensor weight,
|
||||
const torch::Tensor activation_distance, const torch::Tensor speed_dt,
|
||||
const torch::Tensor obb_accel, // n_boxes, 4, 4
|
||||
const torch::Tensor obb_bounds, // n_boxes, 3
|
||||
const torch::Tensor obb_pose, // n_boxes, 4, 4
|
||||
const torch::Tensor obb_enable, // n_boxes, 4, 4
|
||||
const torch::Tensor n_env_obb, // n_boxes, 4, 4
|
||||
const torch::Tensor env_query_idx, // n_boxes, 4, 4
|
||||
const int max_nobs, const int batch_size, const int horizon,
|
||||
const int n_spheres, const int sweep_steps, const bool enable_speed_metric,
|
||||
const bool transform_back, const bool compute_distance,
|
||||
const bool use_batch_env) {
|
||||
const at::cuda::OptionalCUDAGuard guard(sphere_position.device());
|
||||
CHECK_INPUT(distance);
|
||||
CHECK_INPUT(closest_point);
|
||||
CHECK_INPUT(sphere_position);
|
||||
|
||||
return swept_sphere_obb_clpt(
|
||||
sphere_position,
|
||||
distance, closest_point, sparsity_idx, weight, activation_distance,
|
||||
speed_dt, obb_accel, obb_bounds, obb_pose, obb_enable, n_env_obb,
|
||||
env_query_idx, max_nobs, batch_size, horizon, n_spheres, sweep_steps,
|
||||
enable_speed_metric, transform_back, compute_distance, use_batch_env);
|
||||
}
|
||||
std::vector<torch::Tensor> pose_distance_wrapper(
|
||||
torch::Tensor out_distance, torch::Tensor out_position_distance,
|
||||
torch::Tensor out_rotation_distance,
|
||||
torch::Tensor distance_p_vector, // batch size, 3
|
||||
torch::Tensor distance_q_vector, // batch size, 4
|
||||
torch::Tensor out_gidx,
|
||||
const torch::Tensor current_position, // batch_size, 3
|
||||
const torch::Tensor goal_position, // n_boxes, 3
|
||||
const torch::Tensor current_quat, const torch::Tensor goal_quat,
|
||||
const torch::Tensor vec_weight, // n_boxes, 4, 4
|
||||
const torch::Tensor weight, const torch::Tensor vec_convergence,
|
||||
const torch::Tensor run_weight, const torch::Tensor run_vec_weight,
|
||||
const torch::Tensor batch_pose_idx, const int batch_size, const int horizon,
|
||||
const int mode, const int num_goals = 1, const bool compute_grad = false,
|
||||
const bool write_distance = false, const bool use_metric = false) {
|
||||
|
||||
// at::cuda::DeviceGuard guard(angle.device());
|
||||
CHECK_INPUT(out_distance);
|
||||
CHECK_INPUT(out_position_distance);
|
||||
CHECK_INPUT(out_rotation_distance);
|
||||
CHECK_INPUT(distance_p_vector);
|
||||
CHECK_INPUT(distance_q_vector);
|
||||
CHECK_INPUT(current_position);
|
||||
CHECK_INPUT(goal_position);
|
||||
CHECK_INPUT(current_quat);
|
||||
CHECK_INPUT(goal_quat);
|
||||
CHECK_INPUT(batch_pose_idx);
|
||||
const at::cuda::OptionalCUDAGuard guard(current_position.device());
|
||||
|
||||
return pose_distance(
|
||||
out_distance, out_position_distance, out_rotation_distance,
|
||||
distance_p_vector, distance_q_vector, out_gidx, current_position,
|
||||
goal_position, current_quat, goal_quat, vec_weight, weight,
|
||||
vec_convergence, run_weight, run_vec_weight, batch_pose_idx, batch_size,
|
||||
horizon, mode, num_goals, compute_grad, write_distance, use_metric);
|
||||
}
|
||||
|
||||
std::vector<torch::Tensor> backward_pose_distance_wrapper(
|
||||
torch::Tensor out_grad_p, torch::Tensor out_grad_q,
|
||||
const torch::Tensor grad_distance, // batch_size, 3
|
||||
const torch::Tensor grad_p_distance, // n_boxes, 3
|
||||
const torch::Tensor grad_q_distance, const torch::Tensor pose_weight,
|
||||
const torch::Tensor grad_p_vec, // n_boxes, 4, 4
|
||||
const torch::Tensor grad_q_vec, const int batch_size,
|
||||
const bool use_distance) {
|
||||
CHECK_INPUT(out_grad_p);
|
||||
CHECK_INPUT(out_grad_q);
|
||||
CHECK_INPUT(grad_distance);
|
||||
CHECK_INPUT(grad_p_distance);
|
||||
CHECK_INPUT(grad_q_distance);
|
||||
|
||||
const at::cuda::OptionalCUDAGuard guard(grad_distance.device());
|
||||
|
||||
return backward_pose_distance(
|
||||
out_grad_p, out_grad_q, grad_distance, grad_p_distance, grad_q_distance,
|
||||
pose_weight, grad_p_vec, grad_q_vec, batch_size, use_distance);
|
||||
}
|
||||
|
||||
PYBIND11_MODULE(TORCH_EXTENSION_NAME, m) {
|
||||
m.def("pose_distance", &pose_distance_wrapper, "Pose Distance (curobolib)");
|
||||
m.def("pose_distance_backward", &backward_pose_distance_wrapper,
|
||||
"Pose Distance Backward (curobolib)");
|
||||
|
||||
m.def("closest_point", &sphere_obb_clpt_wrapper,
|
||||
"Closest Point OBB(curobolib)");
|
||||
m.def("swept_closest_point", &swept_sphere_obb_clpt_wrapper,
|
||||
"Swept Closest Point OBB(curobolib)");
|
||||
|
||||
m.def("self_collision_distance", &self_collision_distance_wrapper,
|
||||
"Self Collision Distance (curobolib)");
|
||||
}
|
||||
1453
src/curobo/curobolib/cpp/helper_math.h
Normal file
1453
src/curobo/curobolib/cpp/helper_math.h
Normal file
File diff suppressed because it is too large
Load Diff
106
src/curobo/curobolib/cpp/kinematics_fused_cuda.cpp
Normal file
106
src/curobo/curobolib/cpp/kinematics_fused_cuda.cpp
Normal file
@@ -0,0 +1,106 @@
|
||||
/*
|
||||
* Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
*
|
||||
* NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
* property and proprietary rights in and to this material, related
|
||||
* documentation and any modifications thereto. Any use, reproduction,
|
||||
* disclosure or distribution of this material and related documentation
|
||||
* without an express license agreement from NVIDIA CORPORATION or
|
||||
* its affiliates is strictly prohibited.
|
||||
*/
|
||||
#include <torch/extension.h>
|
||||
|
||||
#include <c10/cuda/CUDAGuard.h>
|
||||
#include <vector>
|
||||
|
||||
// CUDA forward declarations
|
||||
|
||||
std::vector<torch::Tensor>
|
||||
matrix_to_quaternion(torch::Tensor out_quat,
|
||||
const torch::Tensor in_rot // batch_size, 3
|
||||
);
|
||||
|
||||
std::vector<torch::Tensor> kin_fused_forward(
|
||||
torch::Tensor link_pos, torch::Tensor link_quat,
|
||||
torch::Tensor batch_robot_spheres, torch::Tensor global_cumul_mat,
|
||||
const torch::Tensor joint_vec, const torch::Tensor fixed_transform,
|
||||
const torch::Tensor robot_spheres, const torch::Tensor link_map,
|
||||
const torch::Tensor joint_map, const torch::Tensor joint_map_type,
|
||||
const torch::Tensor store_link_map, const torch::Tensor link_sphere_map,
|
||||
const int batch_size, const int n_spheres,
|
||||
const bool use_global_cumul = false);
|
||||
|
||||
std::vector<torch::Tensor> kin_fused_backward_16t(
|
||||
torch::Tensor grad_out, const torch::Tensor grad_nlinks_pos,
|
||||
const torch::Tensor grad_nlinks_quat, const torch::Tensor grad_spheres,
|
||||
const torch::Tensor global_cumul_mat, const torch::Tensor joint_vec,
|
||||
const torch::Tensor fixed_transform, const torch::Tensor robot_spheres,
|
||||
const torch::Tensor link_map, const torch::Tensor joint_map,
|
||||
const torch::Tensor joint_map_type, const torch::Tensor store_link_map,
|
||||
const torch::Tensor link_sphere_map, const torch::Tensor link_chain_map,
|
||||
const int batch_size, const int n_spheres, const bool sparsity_opt = true,
|
||||
const bool use_global_cumul = false);
|
||||
// C++ interface
|
||||
|
||||
// NOTE: AT_ASSERT has become AT_CHECK on master after 0.4.
|
||||
#define CHECK_CUDA(x) AT_ASSERTM(x.is_cuda(), #x " must be a CUDA tensor")
|
||||
#define CHECK_CONTIGUOUS(x) \
|
||||
AT_ASSERTM(x.is_contiguous(), #x " must be contiguous")
|
||||
#define CHECK_INPUT(x) \
|
||||
CHECK_CUDA(x); \
|
||||
CHECK_CONTIGUOUS(x)
|
||||
|
||||
std::vector<torch::Tensor> kin_forward_wrapper(
|
||||
torch::Tensor link_pos, torch::Tensor link_quat,
|
||||
torch::Tensor batch_robot_spheres, torch::Tensor global_cumul_mat,
|
||||
const torch::Tensor joint_vec, const torch::Tensor fixed_transform,
|
||||
const torch::Tensor robot_spheres, const torch::Tensor link_map,
|
||||
const torch::Tensor joint_map, const torch::Tensor joint_map_type,
|
||||
const torch::Tensor store_link_map, const torch::Tensor link_sphere_map,
|
||||
const int batch_size, const int n_spheres,
|
||||
const bool use_global_cumul = false) {
|
||||
|
||||
const at::cuda::OptionalCUDAGuard guard(joint_vec.device());
|
||||
|
||||
// TODO: add check input
|
||||
return kin_fused_forward(
|
||||
link_pos, link_quat, batch_robot_spheres, global_cumul_mat, joint_vec,
|
||||
fixed_transform, robot_spheres, link_map, joint_map, joint_map_type,
|
||||
store_link_map, link_sphere_map, batch_size, n_spheres, use_global_cumul);
|
||||
}
|
||||
|
||||
std::vector<torch::Tensor> kin_backward_wrapper(
|
||||
torch::Tensor grad_out, const torch::Tensor grad_nlinks_pos,
|
||||
const torch::Tensor grad_nlinks_quat, const torch::Tensor grad_spheres,
|
||||
const torch::Tensor global_cumul_mat, const torch::Tensor joint_vec,
|
||||
const torch::Tensor fixed_transform, const torch::Tensor robot_spheres,
|
||||
const torch::Tensor link_map, const torch::Tensor joint_map,
|
||||
const torch::Tensor joint_map_type, const torch::Tensor store_link_map,
|
||||
const torch::Tensor link_sphere_map, const torch::Tensor link_chain_map,
|
||||
const int batch_size, const int n_spheres, const bool sparsity_opt = true,
|
||||
const bool use_global_cumul = false) {
|
||||
const at::cuda::OptionalCUDAGuard guard(joint_vec.device());
|
||||
|
||||
return kin_fused_backward_16t(
|
||||
grad_out, grad_nlinks_pos, grad_nlinks_quat, grad_spheres,
|
||||
global_cumul_mat, joint_vec, fixed_transform, robot_spheres, link_map,
|
||||
joint_map, joint_map_type, store_link_map, link_sphere_map,
|
||||
link_chain_map, batch_size, n_spheres, sparsity_opt, use_global_cumul);
|
||||
}
|
||||
|
||||
std::vector<torch::Tensor>
|
||||
matrix_to_quaternion_wrapper(torch::Tensor out_quat,
|
||||
const torch::Tensor in_rot // batch_size, 3
|
||||
) {
|
||||
const at::cuda::OptionalCUDAGuard guard(in_rot.device());
|
||||
CHECK_INPUT(in_rot);
|
||||
CHECK_INPUT(out_quat);
|
||||
return matrix_to_quaternion(out_quat, in_rot);
|
||||
}
|
||||
|
||||
PYBIND11_MODULE(TORCH_EXTENSION_NAME, m) {
|
||||
m.def("forward", &kin_forward_wrapper, "Kinematics fused forward (CUDA)");
|
||||
m.def("backward", &kin_backward_wrapper, "Kinematics fused backward (CUDA)");
|
||||
m.def("matrix_to_quaternion", &matrix_to_quaternion_wrapper,
|
||||
"Rotation Matrix to Quaternion (CUDA)");
|
||||
}
|
||||
1255
src/curobo/curobolib/cpp/kinematics_fused_kernel.cu
Normal file
1255
src/curobo/curobolib/cpp/kinematics_fused_kernel.cu
Normal file
File diff suppressed because it is too large
Load Diff
121
src/curobo/curobolib/cpp/lbfgs_step_cuda.cpp
Normal file
121
src/curobo/curobolib/cpp/lbfgs_step_cuda.cpp
Normal file
@@ -0,0 +1,121 @@
|
||||
/*
|
||||
* Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
*
|
||||
* NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
* property and proprietary rights in and to this material, related
|
||||
* documentation and any modifications thereto. Any use, reproduction,
|
||||
* disclosure or distribution of this material and related documentation
|
||||
* without an express license agreement from NVIDIA CORPORATION or
|
||||
* its affiliates is strictly prohibited.
|
||||
*/
|
||||
#include <torch/extension.h>
|
||||
|
||||
#include <vector>
|
||||
|
||||
#include <c10/cuda/CUDAGuard.h>
|
||||
|
||||
// CUDA forward declarations
|
||||
std::vector<torch::Tensor> reduce_cuda(torch::Tensor vec, torch::Tensor vec2,
|
||||
torch::Tensor rho_buffer,
|
||||
torch::Tensor sum, const int batch_size,
|
||||
const int v_dim, const int m);
|
||||
|
||||
std::vector<torch::Tensor>
|
||||
lbfgs_step_cuda(torch::Tensor step_vec, torch::Tensor rho_buffer,
|
||||
torch::Tensor y_buffer, torch::Tensor s_buffer,
|
||||
torch::Tensor grad_q, const float epsilon, const int batch_size,
|
||||
const int m, const int v_dim);
|
||||
|
||||
std::vector<torch::Tensor>
|
||||
lbfgs_update_cuda(torch::Tensor rho_buffer, torch::Tensor y_buffer,
|
||||
torch::Tensor s_buffer, torch::Tensor q, torch::Tensor grad_q,
|
||||
torch::Tensor x_0, torch::Tensor grad_0, const int batch_size,
|
||||
const int m, const int v_dim);
|
||||
|
||||
std::vector<torch::Tensor>
|
||||
lbfgs_cuda_fuse(torch::Tensor step_vec, torch::Tensor rho_buffer,
|
||||
torch::Tensor y_buffer, torch::Tensor s_buffer, torch::Tensor q,
|
||||
torch::Tensor grad_q, torch::Tensor x_0, torch::Tensor grad_0,
|
||||
const float epsilon, const int batch_size, const int m,
|
||||
const int v_dim, const bool stable_mode);
|
||||
// C++ interface
|
||||
|
||||
// NOTE: AT_ASSERT has become AT_CHECK on master after 0.4.
|
||||
#define CHECK_CUDA(x) AT_ASSERTM(x.is_cuda(), #x " must be a CUDA tensor")
|
||||
#define CHECK_CONTIGUOUS(x) \
|
||||
AT_ASSERTM(x.is_contiguous(), #x " must be contiguous")
|
||||
#define CHECK_INPUT(x) \
|
||||
CHECK_CUDA(x); \
|
||||
CHECK_CONTIGUOUS(x)
|
||||
|
||||
std::vector<torch::Tensor>
|
||||
lbfgs_step_call(torch::Tensor step_vec, torch::Tensor rho_buffer,
|
||||
torch::Tensor y_buffer, torch::Tensor s_buffer,
|
||||
torch::Tensor grad_q, const float epsilon, const int batch_size,
|
||||
const int m, const int v_dim) {
|
||||
|
||||
CHECK_INPUT(step_vec);
|
||||
CHECK_INPUT(rho_buffer);
|
||||
CHECK_INPUT(y_buffer);
|
||||
CHECK_INPUT(s_buffer);
|
||||
CHECK_INPUT(grad_q);
|
||||
const at::cuda::OptionalCUDAGuard guard(grad_q.device());
|
||||
|
||||
return lbfgs_step_cuda(step_vec, rho_buffer, y_buffer, s_buffer, grad_q,
|
||||
epsilon, batch_size, m, v_dim);
|
||||
}
|
||||
|
||||
std::vector<torch::Tensor>
|
||||
lbfgs_update_call(torch::Tensor rho_buffer, torch::Tensor y_buffer,
|
||||
torch::Tensor s_buffer, torch::Tensor q, torch::Tensor grad_q,
|
||||
torch::Tensor x_0, torch::Tensor grad_0, const int batch_size,
|
||||
const int m, const int v_dim) {
|
||||
CHECK_INPUT(rho_buffer);
|
||||
CHECK_INPUT(y_buffer);
|
||||
CHECK_INPUT(s_buffer);
|
||||
CHECK_INPUT(grad_q);
|
||||
CHECK_INPUT(x_0);
|
||||
CHECK_INPUT(grad_0);
|
||||
CHECK_INPUT(q);
|
||||
const at::cuda::OptionalCUDAGuard guard(grad_q.device());
|
||||
|
||||
return lbfgs_update_cuda(rho_buffer, y_buffer, s_buffer, q, grad_q, x_0,
|
||||
grad_0, batch_size, m, v_dim);
|
||||
}
|
||||
|
||||
std::vector<torch::Tensor>
|
||||
reduce_cuda_call(torch::Tensor vec, torch::Tensor vec2,
|
||||
torch::Tensor rho_buffer, torch::Tensor sum,
|
||||
const int batch_size, const int v_dim, const int m) {
|
||||
const at::cuda::OptionalCUDAGuard guard(sum.device());
|
||||
|
||||
return reduce_cuda(vec, vec2, rho_buffer, sum, batch_size, v_dim, m);
|
||||
}
|
||||
|
||||
std::vector<torch::Tensor>
|
||||
lbfgs_call(torch::Tensor step_vec, torch::Tensor rho_buffer,
|
||||
torch::Tensor y_buffer, torch::Tensor s_buffer, torch::Tensor q,
|
||||
torch::Tensor grad_q, torch::Tensor x_0, torch::Tensor grad_0,
|
||||
const float epsilon, const int batch_size, const int m,
|
||||
const int v_dim, const bool stable_mode) {
|
||||
CHECK_INPUT(step_vec);
|
||||
CHECK_INPUT(rho_buffer);
|
||||
CHECK_INPUT(y_buffer);
|
||||
CHECK_INPUT(s_buffer);
|
||||
CHECK_INPUT(grad_q);
|
||||
CHECK_INPUT(x_0);
|
||||
CHECK_INPUT(grad_0);
|
||||
CHECK_INPUT(q);
|
||||
const at::cuda::OptionalCUDAGuard guard(grad_q.device());
|
||||
|
||||
return lbfgs_cuda_fuse(step_vec, rho_buffer, y_buffer, s_buffer, q, grad_q,
|
||||
x_0, grad_0, epsilon, batch_size, m, v_dim,
|
||||
stable_mode);
|
||||
}
|
||||
|
||||
PYBIND11_MODULE(TORCH_EXTENSION_NAME, m) {
|
||||
m.def("step", &lbfgs_step_call, "L-BFGS step (CUDA)");
|
||||
m.def("update", &lbfgs_update_call, "L-BFGS Update (CUDA)");
|
||||
m.def("forward", &lbfgs_call, "L-BFGS Update + Step (CUDA)");
|
||||
m.def("debug_reduce", &reduce_cuda_call, "L-BFGS Debug");
|
||||
}
|
||||
869
src/curobo/curobolib/cpp/lbfgs_step_kernel.cu
Normal file
869
src/curobo/curobolib/cpp/lbfgs_step_kernel.cu
Normal file
@@ -0,0 +1,869 @@
|
||||
/*
|
||||
* Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
*
|
||||
* NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
* property and proprietary rights in and to this material, related
|
||||
* documentation and any modifications thereto. Any use, reproduction,
|
||||
* disclosure or distribution of this material and related documentation
|
||||
* without an express license agreement from NVIDIA CORPORATION or
|
||||
* its affiliates is strictly prohibited.
|
||||
*/
|
||||
|
||||
#include <cuda.h>
|
||||
#include <torch/extension.h>
|
||||
#include <vector>
|
||||
|
||||
#include <c10/cuda/CUDAStream.h>
|
||||
#include <cuda_fp16.h>
|
||||
|
||||
// #include "helper_cuda.h"
|
||||
#include "helper_math.h"
|
||||
|
||||
#include <assert.h>
|
||||
#include <cstdio>
|
||||
#include <cstdlib>
|
||||
#include <ctime>
|
||||
#include <math.h>
|
||||
|
||||
// #include <stdio.h>
|
||||
//
|
||||
// For the CUDA runtime routines (prefixed with "cuda_")
|
||||
// #include <cuda_runtime.h>
|
||||
|
||||
// #include <cuda_fp16.h>
|
||||
// #include <helper_cuda.h>
|
||||
#define M_MAX 512
|
||||
#define HALF_MAX 65504.0
|
||||
#define M 15
|
||||
#define VDIM 175 // 25 * 7,
|
||||
|
||||
#define FULL_MASK 0xffffffff
|
||||
|
||||
namespace Curobo {
|
||||
|
||||
namespace Optimization {
|
||||
|
||||
template <typename scalar_t>
|
||||
__device__ __forceinline__ void
|
||||
scalar_vec_product(const scalar_t a, const scalar_t *b, scalar_t *out,
|
||||
const int v_dim) {
|
||||
|
||||
for (int i = 0; i < v_dim; i++) {
|
||||
out[i] = a * b[i];
|
||||
}
|
||||
}
|
||||
|
||||
template <typename scalar_t>
|
||||
__device__ __forceinline__ void
|
||||
m_scalar_vec_product(const scalar_t *a, const scalar_t *b, scalar_t *out,
|
||||
const int v_dim, const int m) {
|
||||
for (int j = 0; j < m; j++) {
|
||||
for (int i = 0; i < v_dim; i++) {
|
||||
out[j * v_dim + i] = a[j] * b[j * v_dim + i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
template <typename scalar_t>
|
||||
__device__ __forceinline__ void vec_vec_dot(const scalar_t *a,
|
||||
const scalar_t *b, scalar_t &out,
|
||||
const int v_dim) {
|
||||
|
||||
for (int i = 0; i < v_dim; i++) {
|
||||
out += a[i] * b[i];
|
||||
}
|
||||
}
|
||||
|
||||
template <typename scalar_t>
|
||||
__device__ __forceinline__ void update_r(const scalar_t *rho_y,
|
||||
const scalar_t *s_buffer, scalar_t *r,
|
||||
scalar_t &alpha, const int v_dim) {
|
||||
// dot product: and subtract with alpha
|
||||
for (int i = 0; i < v_dim; i++) {
|
||||
alpha -= rho_y[i] * r[i];
|
||||
}
|
||||
// scalar vector product:
|
||||
for (int i = 0; i < v_dim; i++) {
|
||||
r[i] = r[i] + alpha * s_buffer[i];
|
||||
}
|
||||
}
|
||||
template <typename scalar_t>
|
||||
__device__ __forceinline__ void update_q(const scalar_t *y_buffer, scalar_t *gq,
|
||||
const scalar_t alpha,
|
||||
const int v_dim) {
|
||||
//
|
||||
for (int i = 0; i < v_dim; i++) {
|
||||
gq[i] = gq[i] - (alpha * y_buffer[i]);
|
||||
}
|
||||
}
|
||||
template <typename scalar_t>
|
||||
__global__ void
|
||||
lbfgs_step_kernel_old(scalar_t *step_vec, scalar_t *rho_buffer,
|
||||
const scalar_t *y_buffer, const scalar_t *s_buffer,
|
||||
const scalar_t *grad_q, const float epsilon,
|
||||
const int batchSize, const int m, const int v_dim) {
|
||||
// each thread writes one sphere of some link
|
||||
const int t = blockDim.x * blockIdx.x + threadIdx.x; // batch
|
||||
const int b_idx = t;
|
||||
if (t >= (batchSize)) {
|
||||
return;
|
||||
}
|
||||
|
||||
// get thread start address:
|
||||
const int b_start_scalar_adrs = b_idx * m;
|
||||
const int b_start_vec_adrs = b_idx * m * v_dim;
|
||||
const int b_step_start_adrs = b_idx * v_dim;
|
||||
|
||||
scalar_t rho_s[M * VDIM];
|
||||
|
||||
// copy floats to local buffer?
|
||||
// y_buffer, s_buffer, rho_buffer
|
||||
// compute rho_s
|
||||
scalar_t loc_ybuf[M * VDIM];
|
||||
scalar_t loc_sbuf[M * VDIM];
|
||||
scalar_t loc_rho[M];
|
||||
scalar_t gq[VDIM]; //, l_q[VDIM];
|
||||
scalar_t alpha_buffer[M];
|
||||
scalar_t t_1, t_2;
|
||||
|
||||
for (int i = 0; i < m * v_dim; i++) {
|
||||
loc_ybuf[i] = y_buffer[b_start_vec_adrs + i];
|
||||
loc_sbuf[i] = s_buffer[b_start_vec_adrs + i];
|
||||
}
|
||||
for (int i = 0; i < v_dim; i++) {
|
||||
gq[i] = grad_q[b_step_start_adrs + i];
|
||||
}
|
||||
for (int i = 0; i < m; i++) {
|
||||
loc_rho[i] = rho_buffer[b_start_scalar_adrs + i];
|
||||
}
|
||||
|
||||
m_scalar_vec_product(&loc_rho[0], &loc_sbuf[0], &rho_s[0], v_dim, m);
|
||||
// for loop over m
|
||||
for (int i = m - 1; i > m - 2; i--) {
|
||||
// l_start_vec_adrs = i * v_dim;
|
||||
// scalar_vec_product(loc_rho[i], &loc_sbuf[i*v_dim], &rho_s[i*v_dim],
|
||||
// v_dim);
|
||||
vec_vec_dot(&rho_s[i * v_dim], &gq[0], alpha_buffer[i], v_dim);
|
||||
update_q(&loc_ybuf[(i * v_dim)], &gq[0], alpha_buffer[i], v_dim);
|
||||
}
|
||||
// compute initial hessian:
|
||||
vec_vec_dot(&loc_sbuf[(m - 1) * v_dim], &loc_ybuf[(m - 1) * v_dim], t_1,
|
||||
v_dim);
|
||||
vec_vec_dot(&loc_ybuf[(m - 1) * v_dim], &loc_ybuf[(m - 1) * v_dim], t_2,
|
||||
v_dim);
|
||||
t_1 = t_1 / t_2;
|
||||
if (t_1 < 0) {
|
||||
t_1 = 0;
|
||||
}
|
||||
|
||||
t_1 += epsilon;
|
||||
scalar_vec_product(t_1, &gq[0], &gq[0], v_dim);
|
||||
|
||||
m_scalar_vec_product(&loc_rho[0], &loc_ybuf[0], &rho_s[0], v_dim, m);
|
||||
|
||||
for (int i = 0; i < m; i++) {
|
||||
// scalar_vec_product(loc_rho[i], &loc_ybuf[i*v_dim], &rho_s[i*v_dim],
|
||||
// v_dim);
|
||||
update_r(&rho_s[i * v_dim], &loc_sbuf[i * v_dim], &gq[0], alpha_buffer[i],
|
||||
v_dim);
|
||||
}
|
||||
|
||||
// write gq to out grad:
|
||||
|
||||
for (int i = 0; i < v_dim; i++) {
|
||||
step_vec[b_step_start_adrs + i] = -1.0 * gq[i];
|
||||
}
|
||||
}
|
||||
|
||||
template <typename psum_t>
|
||||
__forceinline__ __device__ psum_t warpReduce(psum_t v, int elems,
|
||||
unsigned mask) {
|
||||
psum_t val = v;
|
||||
int shift = 1;
|
||||
for (int i = elems; i > 1; i /= 2) {
|
||||
val += __shfl_down_sync(mask, val, shift);
|
||||
shift *= 2;
|
||||
}
|
||||
// val += __shfl_down_sync(mask, val, 1); // i=32
|
||||
// val += __shfl_down_sync(mask, val, 2); // i=16
|
||||
// val += __shfl_down_sync(mask, val, 4); // i=8
|
||||
// val += __shfl_down_sync(mask, val, 8); // i=4
|
||||
// val += __shfl_down_sync(mask, val, 16); // i=2
|
||||
return val;
|
||||
}
|
||||
|
||||
template <typename scalar_t, typename psum_t>
|
||||
__forceinline__ __device__ void reduce(scalar_t v, int m, psum_t *data,
|
||||
scalar_t *result) {
|
||||
psum_t val = v;
|
||||
unsigned mask = __ballot_sync(FULL_MASK, threadIdx.x < m);
|
||||
val += __shfl_down_sync(mask, val, 1);
|
||||
val += __shfl_down_sync(mask, val, 2);
|
||||
val += __shfl_down_sync(mask, val, 4);
|
||||
val += __shfl_down_sync(mask, val, 8);
|
||||
val += __shfl_down_sync(mask, val, 16);
|
||||
// int leader = __ffs(mask) – 1; // select a leader lane
|
||||
int leader = 0;
|
||||
if (threadIdx.x % 32 == leader) {
|
||||
if (m < 32) {
|
||||
result[0] = (scalar_t)val;
|
||||
} else {
|
||||
data[(threadIdx.x + 1) / 32] = val;
|
||||
}
|
||||
}
|
||||
if (m >= 32) {
|
||||
|
||||
__syncthreads();
|
||||
|
||||
int elems = (m + 31) / 32;
|
||||
unsigned mask2 = __ballot_sync(FULL_MASK, threadIdx.x < elems);
|
||||
if (threadIdx.x / 32 == 0) { // only the first warp will do this work
|
||||
psum_t val2 = data[threadIdx.x % 32];
|
||||
int shift = 1;
|
||||
for (int i = elems - 1; i > 0; i /= 2) {
|
||||
val2 += __shfl_down_sync(mask2, val2, shift);
|
||||
shift *= 2;
|
||||
}
|
||||
// int leader = __ffs(mask2) – 1; // select a leader lane
|
||||
int leader = 0;
|
||||
if (threadIdx.x % 32 == leader) {
|
||||
result[0] = (scalar_t)val2;
|
||||
}
|
||||
}
|
||||
}
|
||||
__syncthreads();
|
||||
}
|
||||
|
||||
// blockReduce
|
||||
template <typename scalar_t, typename psum_t>
|
||||
__forceinline__ __device__ void reduce_v1(scalar_t v, int m, psum_t *data,
|
||||
scalar_t *result) {
|
||||
unsigned mask = __ballot_sync(FULL_MASK, threadIdx.x < m);
|
||||
psum_t val = warpReduce(v, 32, mask);
|
||||
|
||||
// int leader = __ffs(mask) – 1; // select a leader lane
|
||||
int leader = 0;
|
||||
if (threadIdx.x % 32 == leader) {
|
||||
data[(threadIdx.x + 1) / 32] = val;
|
||||
}
|
||||
|
||||
if (m >= 32) {
|
||||
|
||||
__syncthreads();
|
||||
|
||||
int elems = (m + 31) / 32;
|
||||
unsigned mask2 = __ballot_sync(FULL_MASK, threadIdx.x < elems);
|
||||
if (threadIdx.x / 32 == 0) { // only the first warp will do this work
|
||||
psum_t val2 = warpReduce(data[threadIdx.x % 32], elems, mask2);
|
||||
|
||||
// // int leader = __ffs(mask2) – 1; // select a leader lane
|
||||
if (threadIdx.x == leader) {
|
||||
result[0] = (scalar_t)val2;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
if (threadIdx.x == leader) {
|
||||
result[0] = (scalar_t)val;
|
||||
}
|
||||
}
|
||||
__syncthreads();
|
||||
}
|
||||
|
||||
template <typename scalar_t, typename psum_t>
|
||||
__inline__ __device__ void dot(const scalar_t *mat1, const scalar_t *mat2,
|
||||
const int m, psum_t *data, scalar_t *result) {
|
||||
|
||||
scalar_t val = mat1[threadIdx.x] * mat2[threadIdx.x];
|
||||
reduce(val, m, data, result);
|
||||
}
|
||||
|
||||
template <typename scalar_t> __inline__ __device__ scalar_t relu(scalar_t var) {
|
||||
if (var < 0)
|
||||
return 0;
|
||||
else
|
||||
return var;
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////
|
||||
// one block per batch
|
||||
// v_dim threads per block
|
||||
//////////////////////////////////////////////////////////
|
||||
template <typename scalar_t, typename psum_t>
|
||||
__global__ void lbfgs_step_kernel(scalar_t *step_vec, // b x 175
|
||||
scalar_t *rho_buffer, // m x b x 1
|
||||
const scalar_t *y_buffer, // m x b x 175
|
||||
const scalar_t *s_buffer, // m x b x 175
|
||||
const scalar_t *grad_q, // b x 175
|
||||
const float epsilon, const int batchsize,
|
||||
const int m, const int v_dim) {
|
||||
__shared__ psum_t
|
||||
data[32]; // temporary buffer needed for block-wide reduction
|
||||
__shared__ scalar_t
|
||||
result; // result of the reduction or vector-vector dot product
|
||||
__shared__ scalar_t gq[175]; /// gq = batch * v_dim
|
||||
assert(v_dim < 176);
|
||||
int batch = blockIdx.x; // one block per batch
|
||||
if (threadIdx.x >= v_dim)
|
||||
return;
|
||||
|
||||
gq[threadIdx.x] = grad_q[batch * v_dim + threadIdx.x]; // copy grad_q to gq
|
||||
|
||||
scalar_t alpha_buffer[16];
|
||||
// assert(m<16); // allocating a buffer assuming m < 16
|
||||
|
||||
for (int i = m - 1; i > -1; i--) {
|
||||
dot(&gq[0], &s_buffer[i * batchsize * v_dim + batch * v_dim], v_dim,
|
||||
&data[0], &result);
|
||||
alpha_buffer[i] = result * rho_buffer[i * batchsize + batch];
|
||||
gq[threadIdx.x] =
|
||||
gq[threadIdx.x] -
|
||||
alpha_buffer[i] *
|
||||
y_buffer[i * batchsize * v_dim + batch * v_dim + threadIdx.x];
|
||||
}
|
||||
|
||||
// compute var1
|
||||
scalar_t val1 =
|
||||
y_buffer[(m - 1) * batchsize * v_dim + batch * v_dim + threadIdx.x];
|
||||
scalar_t val2 =
|
||||
s_buffer[(m - 1) * batchsize * v_dim + batch * v_dim + threadIdx.x];
|
||||
reduce(val1 * val1, v_dim, data, &result);
|
||||
scalar_t denominator = result;
|
||||
reduce(val1 * val2, v_dim, data, &result);
|
||||
scalar_t numerator = result;
|
||||
scalar_t var1 = numerator / denominator;
|
||||
|
||||
scalar_t gamma = relu(var1) + epsilon; // epsilon
|
||||
|
||||
gq[threadIdx.x] = gamma * gq[threadIdx.x];
|
||||
|
||||
for (int i = 0; i < m; i++) {
|
||||
dot(&gq[0], &y_buffer[i * batchsize * v_dim + batch * v_dim], v_dim,
|
||||
&data[0], &result);
|
||||
gq[threadIdx.x] =
|
||||
gq[threadIdx.x] +
|
||||
(alpha_buffer[i] - result * rho_buffer[i * batchsize + batch]) *
|
||||
s_buffer[i * batchsize * v_dim + batch * v_dim + threadIdx.x];
|
||||
}
|
||||
|
||||
step_vec[batch * v_dim + threadIdx.x] =
|
||||
-1 * gq[threadIdx.x]; // copy from shared memory to global memory
|
||||
}
|
||||
|
||||
template <typename scalar_t, typename psum_t>
|
||||
__global__ void lbfgs_update_buffer_kernel(scalar_t *rho_buffer, // m x b x 1
|
||||
scalar_t *y_buffer, // m x b x 175
|
||||
scalar_t *s_buffer, // m x b x 175
|
||||
scalar_t *q, // b x 175
|
||||
scalar_t *x_0, // b x 175
|
||||
scalar_t *grad_0, // b x 175
|
||||
const scalar_t *grad_q, // b x 175
|
||||
const int batchsize, const int m,
|
||||
const int v_dim) {
|
||||
__shared__ psum_t
|
||||
data[32]; // temporary buffer needed for block-wide reduction
|
||||
__shared__ scalar_t
|
||||
result; // result of the reduction or vector-vector dot product
|
||||
// __shared__ scalar_t y[175]; // temporary shared memory storage
|
||||
// __shared__ scalar_t s[175]; // temporary shared memory storage
|
||||
assert(v_dim <= VDIM);
|
||||
int batch = blockIdx.x; // one block per batch
|
||||
if (threadIdx.x >= v_dim)
|
||||
return;
|
||||
|
||||
scalar_t y =
|
||||
grad_q[batch * v_dim + threadIdx.x] - grad_0[batch * v_dim + threadIdx.x];
|
||||
scalar_t s =
|
||||
q[batch * v_dim + threadIdx.x] - x_0[batch * v_dim + threadIdx.x];
|
||||
reduce(y * s, v_dim, &data[0], &result);
|
||||
|
||||
for (int i = 1; i < m; i++) {
|
||||
s_buffer[(i - 1) * batchsize * v_dim + batch * v_dim + threadIdx.x] =
|
||||
s_buffer[i * batchsize * v_dim + batch * v_dim + threadIdx.x];
|
||||
y_buffer[(i - 1) * batchsize * v_dim + batch * v_dim + threadIdx.x] =
|
||||
y_buffer[i * batchsize * v_dim + batch * v_dim + threadIdx.x];
|
||||
}
|
||||
|
||||
// __syncthreads();
|
||||
|
||||
s_buffer[(m - 1) * batchsize * v_dim + batch * v_dim + threadIdx.x] = s;
|
||||
y_buffer[(m - 1) * batchsize * v_dim + batch * v_dim + threadIdx.x] = y;
|
||||
grad_0[batch * v_dim + threadIdx.x] = grad_q[batch * v_dim + threadIdx.x];
|
||||
x_0[batch * v_dim + threadIdx.x] = q[batch * v_dim + threadIdx.x];
|
||||
|
||||
if (threadIdx.x == 0) {
|
||||
scalar_t rho = 1 / result;
|
||||
for (int i = 1; i < m; i++) {
|
||||
rho_buffer[(i - 1) * batchsize + batch] =
|
||||
rho_buffer[i * batchsize + batch];
|
||||
}
|
||||
rho_buffer[(m - 1) * batchsize + batch] = rho;
|
||||
}
|
||||
}
|
||||
|
||||
template <typename scalar_t, typename psum_t>
|
||||
__global__ void reduce_kernel(
|
||||
scalar_t *vec1, // b x 175
|
||||
scalar_t *vec2, // b x 175
|
||||
scalar_t *rho_buffer, // m x b x 1
|
||||
|
||||
scalar_t *sum_out, // m x b x 1
|
||||
|
||||
const int batchsize, const int m,
|
||||
const int v_dim) // s_buffer and y_buffer are not rolled by default
|
||||
{
|
||||
__shared__ psum_t
|
||||
data[32]; // temporary buffer needed for block-wide reduction
|
||||
__shared__ scalar_t
|
||||
result; // result of the reduction or vector-vector dot product
|
||||
int batch = blockIdx.x; // one block per batch
|
||||
if (threadIdx.x >= v_dim)
|
||||
return;
|
||||
|
||||
////////////////////
|
||||
// update_buffer
|
||||
////////////////////
|
||||
scalar_t y = vec1[batch * v_dim + threadIdx.x];
|
||||
scalar_t s = vec2[batch * v_dim + threadIdx.x];
|
||||
|
||||
reduce(y * s, v_dim, &data[0], &result);
|
||||
scalar_t numerator = result;
|
||||
if (threadIdx.x == 0) {
|
||||
sum_out[batch] = 1 / numerator;
|
||||
}
|
||||
// return;
|
||||
if (threadIdx.x < m - 1) {
|
||||
// m thread participate to shif the values
|
||||
// this is safe as m<32 and this happens in lockstep
|
||||
rho_buffer[threadIdx.x * batchsize + batch] =
|
||||
rho_buffer[(threadIdx.x + 1) * batchsize + batch];
|
||||
} else if (threadIdx.x == m - 1) {
|
||||
scalar_t rho = (1 / numerator);
|
||||
rho_buffer[threadIdx.x * batchsize + batch] = rho;
|
||||
}
|
||||
}
|
||||
template <typename scalar_t, typename psum_t>
|
||||
__global__ void lbfgs_update_buffer_and_step_v1(
|
||||
scalar_t *step_vec, // b x 175
|
||||
scalar_t *rho_buffer, // m x b x 1
|
||||
scalar_t *y_buffer, // m x b x 175
|
||||
scalar_t *s_buffer, // m x b x 175
|
||||
scalar_t *q, // b x 175
|
||||
scalar_t *x_0, // b x 175
|
||||
scalar_t *grad_0, // b x 175
|
||||
const scalar_t *grad_q, // b x 175
|
||||
const float epsilon, const int batchsize, const int m, const int v_dim,
|
||||
const bool rolled_ys = false,
|
||||
const bool stable_mode =
|
||||
false) // s_buffer and y_buffer are not rolled by default
|
||||
{
|
||||
// extern __shared__ scalar_t alpha_buffer_sh[];
|
||||
extern __shared__ __align__(sizeof(scalar_t)) unsigned char my_smem[];
|
||||
scalar_t *my_smem_rc = reinterpret_cast<scalar_t *>(my_smem);
|
||||
scalar_t *alpha_buffer_sh = &my_smem_rc[0]; // m*blockDim.x
|
||||
scalar_t *rho_buffer_sh = &my_smem_rc[m * blockDim.x]; // batchsize*m
|
||||
scalar_t *s_buffer_sh =
|
||||
&my_smem_rc[m * blockDim.x + m * batchsize]; // m*blockDim.x
|
||||
scalar_t *y_buffer_sh =
|
||||
&my_smem_rc[2 * m * blockDim.x + m * batchsize]; // m*blockDim.x
|
||||
|
||||
__shared__ psum_t
|
||||
data[32]; // temporary buffer needed for block-wide reduction
|
||||
__shared__ scalar_t
|
||||
result; // result of the reduction or vector-vector dot product
|
||||
int batch = blockIdx.x; // one block per batch
|
||||
if (threadIdx.x >= v_dim)
|
||||
return;
|
||||
|
||||
scalar_t gq;
|
||||
gq = grad_q[batch * v_dim + threadIdx.x]; // copy grad_q to gq
|
||||
|
||||
////////////////////
|
||||
// update_buffer
|
||||
////////////////////
|
||||
scalar_t y = gq - grad_0[batch * v_dim + threadIdx.x];
|
||||
// if y is close to zero
|
||||
scalar_t s =
|
||||
q[batch * v_dim + threadIdx.x] - x_0[batch * v_dim + threadIdx.x];
|
||||
reduce_v1(y * s, v_dim, &data[0], &result);
|
||||
scalar_t numerator = result;
|
||||
// scalar_t rho = 1.0/numerator;
|
||||
|
||||
if (!rolled_ys) {
|
||||
#pragma unroll
|
||||
for (int i = 1; i < m; i++) {
|
||||
scalar_t st =
|
||||
s_buffer[i * batchsize * v_dim + batch * v_dim + threadIdx.x];
|
||||
scalar_t yt =
|
||||
y_buffer[i * batchsize * v_dim + batch * v_dim + threadIdx.x];
|
||||
s_buffer[(i - 1) * batchsize * v_dim + batch * v_dim + threadIdx.x] = st;
|
||||
y_buffer[(i - 1) * batchsize * v_dim + batch * v_dim + threadIdx.x] = yt;
|
||||
s_buffer_sh[m * threadIdx.x + i - 1] = st;
|
||||
y_buffer_sh[m * threadIdx.x + i - 1] = yt;
|
||||
}
|
||||
}
|
||||
|
||||
s_buffer[(m - 1) * batchsize * v_dim + batch * v_dim + threadIdx.x] = s;
|
||||
y_buffer[(m - 1) * batchsize * v_dim + batch * v_dim + threadIdx.x] = y;
|
||||
s_buffer_sh[m * threadIdx.x + m - 1] = s;
|
||||
y_buffer_sh[m * threadIdx.x + m - 1] = y;
|
||||
grad_0[batch * v_dim + threadIdx.x] = gq;
|
||||
x_0[batch * v_dim + threadIdx.x] = q[batch * v_dim + threadIdx.x];
|
||||
if (threadIdx.x < m - 1) {
|
||||
// m thread participate to shif the values
|
||||
// this is safe as m<32 and this happens in lockstep
|
||||
scalar_t rho = rho_buffer[(threadIdx.x + 1) * batchsize + batch];
|
||||
rho_buffer[threadIdx.x * batchsize + batch] = rho;
|
||||
rho_buffer_sh[threadIdx.x * batchsize + batch] = rho;
|
||||
}
|
||||
if (threadIdx.x == m - 1) {
|
||||
scalar_t rho = 1.0 / numerator;
|
||||
// if this is nan, make it zero:
|
||||
if (stable_mode && numerator == 0.0) {
|
||||
rho = 0.0;
|
||||
}
|
||||
rho_buffer[threadIdx.x * batchsize + batch] = rho;
|
||||
rho_buffer_sh[threadIdx.x * batchsize + batch] = rho;
|
||||
}
|
||||
// return;
|
||||
__syncthreads();
|
||||
////////////////////
|
||||
// step
|
||||
////////////////////
|
||||
// scalar_t alpha_buffer[16];
|
||||
// assert(m<16); // allocating a buffer assuming m < 16
|
||||
|
||||
#pragma unroll
|
||||
for (int i = m - 1; i > -1; i--) {
|
||||
// reduce(gq * s_buffer[i*batchsize*v_dim + batch*v_dim + threadIdx.x],
|
||||
// v_dim, &data[0], &result);
|
||||
reduce_v1(gq * s_buffer_sh[m * threadIdx.x + i], v_dim, &data[0], &result);
|
||||
alpha_buffer_sh[threadIdx.x * m + i] =
|
||||
result * rho_buffer_sh[i * batchsize + batch];
|
||||
// gq = gq - alpha_buffer_sh[threadIdx.x*m+i]*y_buffer[i*batchsize*v_dim +
|
||||
// batch*v_dim + threadIdx.x];
|
||||
gq = gq - alpha_buffer_sh[threadIdx.x * m + i] *
|
||||
y_buffer_sh[m * threadIdx.x + i];
|
||||
}
|
||||
|
||||
// compute var1
|
||||
reduce_v1(y * y, v_dim, data, &result);
|
||||
scalar_t denominator = result;
|
||||
// reduce(s*y, v_dim, data, &result); // redundant - already computed it above
|
||||
// scalar_t numerator = result;
|
||||
scalar_t var1 = numerator / denominator;
|
||||
|
||||
// To improve stability, uncomment below line: [this however leads to poor
|
||||
// convergence]
|
||||
|
||||
if (stable_mode && denominator == 0.0) {
|
||||
var1 = epsilon;
|
||||
}
|
||||
|
||||
scalar_t gamma = relu(var1);
|
||||
gq = gamma * gq;
|
||||
|
||||
#pragma unroll
|
||||
for (int i = 0; i < m; i++) {
|
||||
// reduce(gq * y_buffer[i*batchsize*v_dim + batch*v_dim + threadIdx.x],
|
||||
// v_dim, &data[0], &result); gq = gq + (alpha_buffer_sh[threadIdx.x*m+i] -
|
||||
// result * rho_buffer_sh[i*batchsize+batch]) * s_buffer[i*batchsize*v_dim +
|
||||
// batch*v_dim + threadIdx.x];
|
||||
reduce_v1(gq * y_buffer_sh[m * threadIdx.x + i], v_dim, &data[0], &result);
|
||||
gq = gq + (alpha_buffer_sh[threadIdx.x * m + i] -
|
||||
result * rho_buffer_sh[i * batchsize + batch]) *
|
||||
s_buffer_sh[m * threadIdx.x + i];
|
||||
}
|
||||
|
||||
step_vec[batch * v_dim + threadIdx.x] =
|
||||
-1.0 * gq; // copy from shared memory to global memory
|
||||
}
|
||||
|
||||
// (32/M) rolls per warp
|
||||
// Threads in a warp in a GPU execute in a lock-step. We leverage that to do
|
||||
// the roll without using temporary storage or explicit synchronization.
|
||||
template <typename scalar_t>
|
||||
__global__ void lbfgs_roll(scalar_t *a, // m x b x 175
|
||||
scalar_t *b, // m x b x 175
|
||||
const int m_t, const int batchsize, const int m,
|
||||
const int v_dim) {
|
||||
assert(m_t <= 32);
|
||||
int t = blockDim.x * blockIdx.x + threadIdx.x;
|
||||
if (t >= m_t * v_dim * batchsize)
|
||||
return;
|
||||
int _m = t % m_t;
|
||||
int _v_dim = (t / m_t) % v_dim;
|
||||
int batch = t / (m * v_dim); // this line could be wrong?
|
||||
|
||||
if (_m < m - 1) {
|
||||
a[_m * batchsize * v_dim + batch * v_dim + _v_dim] =
|
||||
a[(_m + 1) * batchsize * v_dim + batch * v_dim + _v_dim];
|
||||
b[_m * batchsize * v_dim + batch * v_dim + _v_dim] =
|
||||
b[(_m + 1) * batchsize * v_dim + batch * v_dim + _v_dim];
|
||||
}
|
||||
}
|
||||
|
||||
template <typename scalar_t, typename psum_t, bool rolled_ys>
|
||||
__global__ void lbfgs_update_buffer_and_step(
|
||||
scalar_t *step_vec, // b x 175
|
||||
scalar_t *rho_buffer, // m x b x 1
|
||||
scalar_t *y_buffer, // m x b x 175
|
||||
scalar_t *s_buffer, // m x b x 175
|
||||
scalar_t *q, // b x 175
|
||||
scalar_t *x_0, // b x 175
|
||||
scalar_t *grad_0, // b x 175
|
||||
const scalar_t *grad_q, // b x 175
|
||||
const float epsilon, const int batchsize, const int m, const int v_dim,
|
||||
const bool stable_mode =
|
||||
false) // s_buffer and y_buffer are not rolled by default
|
||||
{
|
||||
// extern __shared__ scalar_t alpha_buffer_sh[];
|
||||
extern __shared__ __align__(sizeof(scalar_t)) unsigned char my_smem[];
|
||||
scalar_t *alpha_buffer_sh = reinterpret_cast<scalar_t *>(my_smem);
|
||||
|
||||
__shared__ psum_t
|
||||
data[32]; // temporary buffer needed for block-wide reduction
|
||||
__shared__ scalar_t
|
||||
result; // result of the reduction or vector-vector dot product
|
||||
int batch = blockIdx.x; // one block per batch
|
||||
if (threadIdx.x >= v_dim)
|
||||
return;
|
||||
|
||||
scalar_t gq;
|
||||
gq = grad_q[batch * v_dim + threadIdx.x]; // copy grad_q to gq
|
||||
|
||||
////////////////////
|
||||
// update_buffer
|
||||
////////////////////
|
||||
scalar_t y = gq - grad_0[batch * v_dim + threadIdx.x];
|
||||
// if y is close to zero
|
||||
scalar_t s =
|
||||
q[batch * v_dim + threadIdx.x] - x_0[batch * v_dim + threadIdx.x];
|
||||
reduce(y * s, v_dim, &data[0], &result);
|
||||
scalar_t numerator = result;
|
||||
// scalar_t rho = 1.0/numerator;
|
||||
|
||||
if (!rolled_ys) {
|
||||
for (int i = 1; i < m; i++) {
|
||||
s_buffer[(i - 1) * batchsize * v_dim + batch * v_dim + threadIdx.x] =
|
||||
s_buffer[i * batchsize * v_dim + batch * v_dim + threadIdx.x];
|
||||
y_buffer[(i - 1) * batchsize * v_dim + batch * v_dim + threadIdx.x] =
|
||||
y_buffer[i * batchsize * v_dim + batch * v_dim + threadIdx.x];
|
||||
}
|
||||
}
|
||||
|
||||
s_buffer[(m - 1) * batchsize * v_dim + batch * v_dim + threadIdx.x] = s;
|
||||
y_buffer[(m - 1) * batchsize * v_dim + batch * v_dim + threadIdx.x] = y;
|
||||
grad_0[batch * v_dim + threadIdx.x] = gq;
|
||||
x_0[batch * v_dim + threadIdx.x] = q[batch * v_dim + threadIdx.x];
|
||||
if (threadIdx.x < m - 1) {
|
||||
// m thread participate to shif the values
|
||||
// this is safe as m<32 and this happens in lockstep
|
||||
rho_buffer[threadIdx.x * batchsize + batch] =
|
||||
rho_buffer[(threadIdx.x + 1) * batchsize + batch];
|
||||
}
|
||||
if (threadIdx.x == m - 1) {
|
||||
scalar_t rho = 1.0 / numerator;
|
||||
// if this is nan, make it zero:
|
||||
if (stable_mode && numerator == 0.0) {
|
||||
rho = 0.0;
|
||||
}
|
||||
rho_buffer[threadIdx.x * batchsize + batch] = rho;
|
||||
}
|
||||
// return;
|
||||
//__syncthreads();
|
||||
////////////////////
|
||||
// step
|
||||
////////////////////
|
||||
// scalar_t alpha_buffer[16];
|
||||
// assert(m<16); // allocating a buffer assuming m < 16
|
||||
|
||||
#pragma unroll
|
||||
for (int i = m - 1; i > -1; i--) {
|
||||
reduce(gq * s_buffer[i * batchsize * v_dim + batch * v_dim + threadIdx.x],
|
||||
v_dim, &data[0], &result);
|
||||
alpha_buffer_sh[threadIdx.x * m + i] =
|
||||
result * rho_buffer[i * batchsize + batch];
|
||||
gq = gq - alpha_buffer_sh[threadIdx.x * m + i] *
|
||||
y_buffer[i * batchsize * v_dim + batch * v_dim + threadIdx.x];
|
||||
}
|
||||
|
||||
// compute var1
|
||||
reduce(y * y, v_dim, data, &result);
|
||||
scalar_t denominator = result;
|
||||
// reduce(s*y, v_dim, data, &result); // redundant - already computed it above
|
||||
// scalar_t numerator = result;
|
||||
scalar_t var1 = numerator / denominator;
|
||||
|
||||
// To improve stability, uncomment below line: [this however leads to poor
|
||||
// convergence]
|
||||
|
||||
if (stable_mode && denominator == 0.0) {
|
||||
var1 = epsilon;
|
||||
}
|
||||
|
||||
scalar_t gamma = relu(var1);
|
||||
|
||||
gq = gamma * gq;
|
||||
|
||||
#pragma unroll
|
||||
for (int i = 0; i < m; i++) {
|
||||
reduce(gq * y_buffer[i * batchsize * v_dim + batch * v_dim + threadIdx.x],
|
||||
v_dim, &data[0], &result);
|
||||
gq = gq + (alpha_buffer_sh[threadIdx.x * m + i] -
|
||||
result * rho_buffer[i * batchsize + batch]) *
|
||||
s_buffer[i * batchsize * v_dim + batch * v_dim + threadIdx.x];
|
||||
}
|
||||
|
||||
step_vec[batch * v_dim + threadIdx.x] =
|
||||
-1.0 * gq; // copy from shared memory to global memory
|
||||
}
|
||||
|
||||
} // namespace Optimization
|
||||
} // namespace Curobo
|
||||
std::vector<torch::Tensor>
|
||||
lbfgs_step_cuda(torch::Tensor step_vec, torch::Tensor rho_buffer,
|
||||
torch::Tensor y_buffer, torch::Tensor s_buffer,
|
||||
torch::Tensor grad_q, const float epsilon, const int batch_size,
|
||||
const int m, const int v_dim) {
|
||||
using namespace Curobo::Optimization;
|
||||
const int threadsPerBlock = 128;
|
||||
const int blocksPerGrid =
|
||||
((batch_size) + threadsPerBlock - 1) / threadsPerBlock;
|
||||
|
||||
// launch threads per batch:
|
||||
// int threadsPerBlock = pow(2,((int)log2(v_dim))+1);
|
||||
|
||||
// const int blocksPerGrid = batch_size; //((batch_size) + threadsPerBlock -
|
||||
// 1) / threadsPerBlock;
|
||||
|
||||
cudaStream_t stream = at::cuda::getCurrentCUDAStream();
|
||||
|
||||
AT_DISPATCH_FLOATING_TYPES(
|
||||
step_vec.scalar_type(), "lbfgs_step_cu", ([&] {
|
||||
lbfgs_step_kernel_old<scalar_t>
|
||||
<<<blocksPerGrid, threadsPerBlock,
|
||||
v_dim * sizeof(step_vec.scalar_type()), stream>>>(
|
||||
step_vec.data_ptr<scalar_t>(), rho_buffer.data_ptr<scalar_t>(),
|
||||
y_buffer.data_ptr<scalar_t>(), s_buffer.data_ptr<scalar_t>(),
|
||||
grad_q.data_ptr<scalar_t>(), epsilon, batch_size, m, v_dim);
|
||||
}));
|
||||
|
||||
C10_CUDA_KERNEL_LAUNCH_CHECK();
|
||||
|
||||
return {step_vec};
|
||||
}
|
||||
|
||||
std::vector<torch::Tensor>
|
||||
lbfgs_update_cuda(torch::Tensor rho_buffer, torch::Tensor y_buffer,
|
||||
torch::Tensor s_buffer, torch::Tensor q, torch::Tensor grad_q,
|
||||
torch::Tensor x_0, torch::Tensor grad_0, const int batch_size,
|
||||
const int m, const int v_dim) {
|
||||
|
||||
using namespace Curobo::Optimization;
|
||||
|
||||
// const int threadsPerBlock = 128;
|
||||
// launch threads per batch:
|
||||
// int threadsPerBlock = pow(2,((int)log2(v_dim))+1);
|
||||
int threadsPerBlock = v_dim;
|
||||
|
||||
const int blocksPerGrid =
|
||||
batch_size; //((batch_size) + threadsPerBlock - 1) / threadsPerBlock;
|
||||
|
||||
cudaStream_t stream = at::cuda::getCurrentCUDAStream();
|
||||
|
||||
AT_DISPATCH_FLOATING_TYPES(
|
||||
y_buffer.scalar_type(), "lbfgs_update_cu", ([&] {
|
||||
lbfgs_update_buffer_kernel<scalar_t, scalar_t>
|
||||
<<<blocksPerGrid, threadsPerBlock,
|
||||
v_dim * sizeof(y_buffer.scalar_type()), stream>>>(
|
||||
rho_buffer.data_ptr<scalar_t>(), y_buffer.data_ptr<scalar_t>(),
|
||||
s_buffer.data_ptr<scalar_t>(), q.data_ptr<scalar_t>(),
|
||||
x_0.data_ptr<scalar_t>(), grad_0.data_ptr<scalar_t>(),
|
||||
grad_q.data_ptr<scalar_t>(), batch_size, m, v_dim);
|
||||
}));
|
||||
C10_CUDA_KERNEL_LAUNCH_CHECK();
|
||||
|
||||
return {rho_buffer, y_buffer, s_buffer, x_0, grad_0};
|
||||
}
|
||||
|
||||
std::vector<torch::Tensor>
|
||||
lbfgs_cuda_fuse(torch::Tensor step_vec, torch::Tensor rho_buffer,
|
||||
torch::Tensor y_buffer, torch::Tensor s_buffer, torch::Tensor q,
|
||||
torch::Tensor grad_q, torch::Tensor x_0, torch::Tensor grad_0,
|
||||
const float epsilon, const int batch_size, const int m,
|
||||
const int v_dim, const bool stable_mode) {
|
||||
using namespace Curobo::Optimization;
|
||||
|
||||
// call first kernel:
|
||||
|
||||
int threadsPerBlock = v_dim;
|
||||
assert(threadsPerBlock < 1024);
|
||||
assert(m < M_MAX);
|
||||
int blocksPerGrid =
|
||||
batch_size; //((batch_size) + threadsPerBlock - 1) / threadsPerBlock;
|
||||
|
||||
cudaStream_t stream = at::cuda::getCurrentCUDAStream();
|
||||
int smemsize = 0;
|
||||
if (true) {
|
||||
AT_DISPATCH_FLOATING_TYPES(
|
||||
y_buffer.scalar_type(), "lbfgs_cuda_fuse_kernel", [&] {
|
||||
smemsize = m * threadsPerBlock * sizeof(scalar_t);
|
||||
lbfgs_update_buffer_and_step<scalar_t, scalar_t, false>
|
||||
<<<blocksPerGrid, threadsPerBlock, smemsize, stream>>>(
|
||||
step_vec.data_ptr<scalar_t>(),
|
||||
rho_buffer.data_ptr<scalar_t>(),
|
||||
y_buffer.data_ptr<scalar_t>(), s_buffer.data_ptr<scalar_t>(),
|
||||
q.data_ptr<scalar_t>(), x_0.data_ptr<scalar_t>(),
|
||||
grad_0.data_ptr<scalar_t>(), grad_q.data_ptr<scalar_t>(),
|
||||
epsilon, batch_size, m, v_dim, stable_mode);
|
||||
});
|
||||
} else {
|
||||
|
||||
// v1 does not work
|
||||
AT_DISPATCH_FLOATING_TYPES(
|
||||
y_buffer.scalar_type(), "lbfgs_cuda_fuse_kernel_v1", [&] {
|
||||
smemsize = 3 * m * threadsPerBlock * sizeof(scalar_t) +
|
||||
m * batch_size * sizeof(scalar_t);
|
||||
lbfgs_update_buffer_and_step_v1<scalar_t, scalar_t>
|
||||
<<<blocksPerGrid, threadsPerBlock, smemsize, stream>>>(
|
||||
step_vec.data_ptr<scalar_t>(),
|
||||
rho_buffer.data_ptr<scalar_t>(),
|
||||
y_buffer.data_ptr<scalar_t>(), s_buffer.data_ptr<scalar_t>(),
|
||||
q.data_ptr<scalar_t>(), x_0.data_ptr<scalar_t>(),
|
||||
grad_0.data_ptr<scalar_t>(), grad_q.data_ptr<scalar_t>(),
|
||||
epsilon, batch_size, m, v_dim, false, stable_mode);
|
||||
});
|
||||
}
|
||||
|
||||
C10_CUDA_KERNEL_LAUNCH_CHECK();
|
||||
|
||||
return {step_vec, rho_buffer, y_buffer, s_buffer, x_0, grad_0};
|
||||
}
|
||||
|
||||
std::vector<torch::Tensor> reduce_cuda(torch::Tensor vec, torch::Tensor vec2,
|
||||
torch::Tensor rho_buffer,
|
||||
torch::Tensor sum, const int batch_size,
|
||||
const int m, const int v_dim) {
|
||||
|
||||
using namespace Curobo::Optimization;
|
||||
|
||||
int threadsPerBlock = pow(2, ((int)log2(v_dim)) + 1);
|
||||
int blocksPerGrid =
|
||||
batch_size; //((batch_size) + threadsPerBlock - 1) / threadsPerBlock;
|
||||
// printf("threadsPerBlock:%d, blocksPerGrid: %d\n",
|
||||
// threadsPerBlock, blocksPerGrid);
|
||||
|
||||
cudaStream_t stream = at::cuda::getCurrentCUDAStream();
|
||||
|
||||
AT_DISPATCH_FLOATING_TYPES(
|
||||
vec.scalar_type(), "reduce_cu", ([&] {
|
||||
reduce_kernel<scalar_t, scalar_t>
|
||||
<<<blocksPerGrid, threadsPerBlock, 0, stream>>>(
|
||||
vec.data_ptr<scalar_t>(), vec2.data_ptr<scalar_t>(),
|
||||
rho_buffer.data_ptr<scalar_t>(), sum.data_ptr<scalar_t>(),
|
||||
batch_size, m, v_dim);
|
||||
}));
|
||||
C10_CUDA_KERNEL_LAUNCH_CHECK();
|
||||
|
||||
return {sum, rho_buffer};
|
||||
}
|
||||
103
src/curobo/curobolib/cpp/line_search_cuda.cpp
Normal file
103
src/curobo/curobolib/cpp/line_search_cuda.cpp
Normal file
@@ -0,0 +1,103 @@
|
||||
/*
|
||||
* Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
*
|
||||
* NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
* property and proprietary rights in and to this material, related
|
||||
* documentation and any modifications thereto. Any use, reproduction,
|
||||
* disclosure or distribution of this material and related documentation
|
||||
* without an express license agreement from NVIDIA CORPORATION or
|
||||
* its affiliates is strictly prohibited.
|
||||
*/
|
||||
|
||||
#include <torch/extension.h>
|
||||
|
||||
#include <vector>
|
||||
|
||||
#include <c10/cuda/CUDAGuard.h>
|
||||
|
||||
// CUDA forward declarations
|
||||
|
||||
std::vector<torch::Tensor>
|
||||
update_best_cuda(torch::Tensor best_cost, torch::Tensor best_q,
|
||||
torch::Tensor best_iteration,
|
||||
torch::Tensor current_iteration,
|
||||
const torch::Tensor cost,
|
||||
const torch::Tensor q, const int d_opt, const int cost_s1,
|
||||
const int cost_s2, const int iteration,
|
||||
const float delta_threshold,
|
||||
const float relative_threshold = 0.999);
|
||||
|
||||
std::vector<torch::Tensor> line_search_cuda(
|
||||
// torch::Tensor m,
|
||||
torch::Tensor best_x, torch::Tensor best_c, torch::Tensor best_grad,
|
||||
const torch::Tensor g_x, const torch::Tensor x_set,
|
||||
const torch::Tensor step_vec, const torch::Tensor c_0,
|
||||
const torch::Tensor alpha_list, const torch::Tensor c_idx, const float c_1,
|
||||
const float c_2, const bool strong_wolfe, const bool approx_wolfe,
|
||||
const int l1, const int l2, const int batchsize);
|
||||
// C++ interface
|
||||
|
||||
// NOTE: AT_ASSERT has become AT_CHECK on master after 0.4.
|
||||
#define CHECK_CUDA(x) AT_ASSERTM(x.is_cuda(), #x " must be a CUDA tensor")
|
||||
#define CHECK_CONTIGUOUS(x) \
|
||||
AT_ASSERTM(x.is_contiguous(), #x " must be contiguous")
|
||||
#define CHECK_INPUT(x) \
|
||||
CHECK_CUDA(x); \
|
||||
CHECK_CONTIGUOUS(x)
|
||||
|
||||
|
||||
std::vector<torch::Tensor> line_search_call(
|
||||
// torch::Tensor m,
|
||||
torch::Tensor best_x, torch::Tensor best_c, torch::Tensor best_grad,
|
||||
const torch::Tensor g_x, const torch::Tensor x_set,
|
||||
const torch::Tensor step_vec, const torch::Tensor c_0,
|
||||
const torch::Tensor alpha_list, const torch::Tensor c_idx, const float c_1,
|
||||
const float c_2, const bool strong_wolfe, const bool approx_wolfe,
|
||||
const int l1, const int l2, const int batchsize) {
|
||||
|
||||
CHECK_INPUT(g_x);
|
||||
CHECK_INPUT(x_set);
|
||||
CHECK_INPUT(step_vec);
|
||||
CHECK_INPUT(c_0);
|
||||
CHECK_INPUT(alpha_list);
|
||||
CHECK_INPUT(c_idx);
|
||||
// CHECK_INPUT(m);
|
||||
CHECK_INPUT(best_x);
|
||||
CHECK_INPUT(best_c);
|
||||
CHECK_INPUT(best_grad);
|
||||
const at::cuda::OptionalCUDAGuard guard(best_x.device());
|
||||
|
||||
// return line_search_cuda(m, g_x, step_vec, c_0, alpha_list, c_1, c_2,
|
||||
// strong_wolfe, l1, l2, batchsize);
|
||||
return line_search_cuda(best_x, best_c, best_grad, g_x, x_set, step_vec, c_0,
|
||||
alpha_list, c_idx, c_1, c_2, strong_wolfe,
|
||||
approx_wolfe, l1, l2, batchsize);
|
||||
}
|
||||
|
||||
std::vector<torch::Tensor>
|
||||
update_best_call(torch::Tensor best_cost, torch::Tensor best_q,
|
||||
torch::Tensor best_iteration,
|
||||
torch::Tensor current_iteration,
|
||||
const torch::Tensor cost,
|
||||
const torch::Tensor q, const int d_opt, const int cost_s1,
|
||||
const int cost_s2, const int iteration,
|
||||
const float delta_threshold,
|
||||
const float relative_threshold=0.999) {
|
||||
|
||||
CHECK_INPUT(best_cost);
|
||||
CHECK_INPUT(best_q);
|
||||
CHECK_INPUT(cost);
|
||||
CHECK_INPUT(q);
|
||||
CHECK_INPUT(current_iteration);
|
||||
CHECK_INPUT(best_iteration);
|
||||
const at::cuda::OptionalCUDAGuard guard(cost.device());
|
||||
|
||||
return update_best_cuda(best_cost, best_q, best_iteration, current_iteration, cost, q, d_opt,
|
||||
cost_s1, cost_s2, iteration, delta_threshold, relative_threshold);
|
||||
}
|
||||
|
||||
PYBIND11_MODULE(TORCH_EXTENSION_NAME, m) {
|
||||
m.def("update_best", &update_best_call, "Update Best (CUDA)");
|
||||
m.def("line_search", &line_search_call, "Line search (CUDA)");
|
||||
|
||||
}
|
||||
398
src/curobo/curobolib/cpp/line_search_kernel.cu
Normal file
398
src/curobo/curobolib/cpp/line_search_kernel.cu
Normal file
@@ -0,0 +1,398 @@
|
||||
/*
|
||||
* Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
*
|
||||
* NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
* property and proprietary rights in and to this material, related
|
||||
* documentation and any modifications thereto. Any use, reproduction,
|
||||
* disclosure or distribution of this material and related documentation
|
||||
* without an express license agreement from NVIDIA CORPORATION or
|
||||
* its affiliates is strictly prohibited.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include <cuda.h>
|
||||
#include <torch/extension.h>
|
||||
#include <vector>
|
||||
|
||||
#include <c10/cuda/CUDAStream.h>
|
||||
#include <cuda_fp16.h>
|
||||
|
||||
// #include "helper_cuda.h"
|
||||
#include "helper_math.h"
|
||||
|
||||
#include <assert.h>
|
||||
#include <cstdio>
|
||||
#include <cstdlib>
|
||||
#include <ctime>
|
||||
#include <cub/cub.cuh>
|
||||
#include <math.h>
|
||||
|
||||
// #include <stdio.h>
|
||||
//
|
||||
// For the CUDA runtime routines (prefixed with "cuda_")
|
||||
// #include <cuda_runtime.h>
|
||||
|
||||
// #include <cuda_fp16.h>
|
||||
// #include <helper_cuda.h>
|
||||
|
||||
#define FULL_MASK 0xffffffff
|
||||
|
||||
namespace Curobo {
|
||||
|
||||
namespace Optimization {
|
||||
|
||||
template <typename scalar_t, typename psum_t>
|
||||
__inline__ __device__ void reduce(scalar_t v, int m, unsigned mask,
|
||||
psum_t *data, scalar_t *result) {
|
||||
psum_t val = v;
|
||||
val += __shfl_down_sync(mask, val, 1);
|
||||
val += __shfl_down_sync(mask, val, 2);
|
||||
val += __shfl_down_sync(mask, val, 4);
|
||||
val += __shfl_down_sync(mask, val, 8);
|
||||
val += __shfl_down_sync(mask, val, 16);
|
||||
// int leader = __ffs(mask) – 1; // select a leader lane
|
||||
int leader = 0;
|
||||
if (threadIdx.x % 32 == leader) {
|
||||
if (m <= 32) {
|
||||
result[0] = (scalar_t)val;
|
||||
} else {
|
||||
data[(threadIdx.x + 1) / 32] = val;
|
||||
}
|
||||
}
|
||||
if (m > 32) {
|
||||
|
||||
__syncthreads();
|
||||
|
||||
int elems = (m + 31) / 32;
|
||||
assert(elems <= 32);
|
||||
unsigned mask2 = __ballot_sync(FULL_MASK, threadIdx.x < elems);
|
||||
if (threadIdx.x < elems) { // only the first warp will do this work
|
||||
psum_t val2 = data[threadIdx.x % 32];
|
||||
int shift = 1;
|
||||
for (int i = elems - 1; i > 0; i /= 2) {
|
||||
val2 += __shfl_down_sync(mask2, val2, shift);
|
||||
shift *= 2;
|
||||
}
|
||||
// int leader = __ffs(mask2) – 1; // select a leader lane
|
||||
int leader = 0;
|
||||
if (threadIdx.x % 32 == leader) {
|
||||
result[0] = (scalar_t)val2;
|
||||
}
|
||||
}
|
||||
}
|
||||
__syncthreads();
|
||||
}
|
||||
|
||||
// Launched with l2 threads/block and batchsize blocks
|
||||
template <typename scalar_t, typename psum_t>
|
||||
__global__ void line_search_kernel(
|
||||
// int64_t *m_idx, // 4x1x1
|
||||
scalar_t *best_x, // 4x280
|
||||
scalar_t *best_c, // 4x1
|
||||
scalar_t *best_grad, // 4x280
|
||||
const scalar_t *g_x, // 4x6x280
|
||||
const scalar_t *x_set, // 4x6x280
|
||||
const scalar_t *step_vec, // 4x280x1
|
||||
const scalar_t *c, // 4x6x1
|
||||
const scalar_t *alpha_list, // 4x6x1
|
||||
const int64_t *c_idx, // 4x1x1
|
||||
const float c_1, const float c_2, const bool strong_wolfe,
|
||||
const bool approx_wolfe,
|
||||
const int l1, // 6
|
||||
const int l2, // 280
|
||||
const int batchsize) // 4
|
||||
{
|
||||
|
||||
int batch = blockIdx.x;
|
||||
__shared__ psum_t data[32];
|
||||
__shared__ scalar_t result[32];
|
||||
assert(l1 <= 32);
|
||||
unsigned mask = __ballot_sync(FULL_MASK, threadIdx.x < l2);
|
||||
|
||||
if (threadIdx.x >= l2) {
|
||||
return;
|
||||
}
|
||||
|
||||
scalar_t sv_elem = step_vec[batch * l2 + threadIdx.x];
|
||||
|
||||
// g_step = g0 @ step_vec_T
|
||||
// g_x @ step_vec_T
|
||||
for (int i = 0; i < l1; i++) {
|
||||
reduce(g_x[batch * l1 * l2 + l2 * i + threadIdx.x] * sv_elem, l2, mask,
|
||||
&data[0], &result[i]);
|
||||
}
|
||||
|
||||
__shared__ scalar_t step_success[32];
|
||||
__shared__ scalar_t step_success_w1[32];
|
||||
assert(blockDim.x >= l1);
|
||||
bool wolfe_1 = false;
|
||||
bool wolfe = false;
|
||||
bool condition = threadIdx.x < l1;
|
||||
if (condition) {
|
||||
// scalar_t alpha_list_elem = alpha_list[batch*l1 + threadIdx.x];
|
||||
scalar_t alpha_list_elem = alpha_list[threadIdx.x];
|
||||
|
||||
// condition 1:
|
||||
wolfe_1 = c[batch * l1 + threadIdx.x] <=
|
||||
(c[batch * l1] + c_1 * alpha_list_elem * result[0]);
|
||||
|
||||
// condition 2:
|
||||
bool wolfe_2;
|
||||
if (strong_wolfe) {
|
||||
wolfe_2 = abs(result[threadIdx.x]) <= c_2 * abs(result[0]);
|
||||
} else {
|
||||
wolfe_2 = result[threadIdx.x] >= c_2 * result[0];
|
||||
}
|
||||
|
||||
wolfe = wolfe_1 & wolfe_2;
|
||||
|
||||
step_success[threadIdx.x] = wolfe * (alpha_list_elem + 0.1);
|
||||
step_success_w1[threadIdx.x] = wolfe_1 * (alpha_list_elem + 0.1);
|
||||
}
|
||||
|
||||
__syncthreads();
|
||||
|
||||
__shared__ int idx_shared;
|
||||
if (threadIdx.x == 0) {
|
||||
int m_id = 0;
|
||||
int m1_id = 0;
|
||||
scalar_t max1 = step_success[0];
|
||||
scalar_t max2 = step_success_w1[0];
|
||||
for (int i = 1; i < l1; i++) {
|
||||
if (max1 < step_success[i]) {
|
||||
max1 = step_success[i];
|
||||
m_id = i;
|
||||
}
|
||||
if (max2 < step_success_w1[i]) {
|
||||
max2 = step_success_w1[i];
|
||||
m1_id = i;
|
||||
}
|
||||
}
|
||||
|
||||
if (!approx_wolfe) {
|
||||
|
||||
// m_idx = torch.where(m_idx == 0, m1_idx, m_idx)
|
||||
if (m_id == 0) {
|
||||
m_id = m1_id;
|
||||
}
|
||||
// m_idx[m_idx == 0] = 1
|
||||
if (m_id == 0) {
|
||||
m_id = 1;
|
||||
}
|
||||
}
|
||||
idx_shared = m_id + c_idx[batch];
|
||||
}
|
||||
|
||||
////////////////////////////////////
|
||||
// write outputs using the computed index.
|
||||
// one index per batch is computed
|
||||
////////////////////////////////////
|
||||
// l2 is d_opt, l1 is line_search n.
|
||||
// idx_shared contains index in l1
|
||||
//
|
||||
__syncthreads();
|
||||
if (threadIdx.x < l2) {
|
||||
if (threadIdx.x == 0) {
|
||||
// printf("block: %d, idx_shared: %d\n", batch, idx_shared);
|
||||
}
|
||||
best_x[batch * l2 + threadIdx.x] = x_set[idx_shared * l2 + threadIdx.x];
|
||||
best_grad[batch * l2 + threadIdx.x] = g_x[idx_shared * l2 + threadIdx.x];
|
||||
}
|
||||
if (threadIdx.x == 0) {
|
||||
best_c[batch] = c[idx_shared];
|
||||
}
|
||||
}
|
||||
|
||||
// Launched with l2 threads/block and #blocks = batchsize
|
||||
template <typename scalar_t, typename psum_t>
|
||||
__global__ void line_search_kernel_mask(
|
||||
// int64_t *m_idx, // 4x1x1
|
||||
scalar_t *best_x, // 4x280
|
||||
scalar_t *best_c, // 4x1
|
||||
scalar_t *best_grad, // 4x280
|
||||
const scalar_t *g_x, // 4x6x280
|
||||
const scalar_t *x_set, // 4x6x280
|
||||
const scalar_t *step_vec, // 4x280x1
|
||||
const scalar_t *c, // 4x6x1
|
||||
const scalar_t *alpha_list, // 4x6x1
|
||||
const int64_t *c_idx, // 4x1x1
|
||||
const float c_1, const float c_2, const bool strong_wolfe,
|
||||
const bool approx_wolfe,
|
||||
const int l1, // 6
|
||||
const int l2, // 280
|
||||
const int batchsize) // 4
|
||||
{
|
||||
|
||||
int batch = blockIdx.x;
|
||||
__shared__ psum_t data[32];
|
||||
__shared__ scalar_t result[32];
|
||||
assert(l1 <= 32);
|
||||
unsigned mask = __ballot_sync(FULL_MASK, threadIdx.x < l2);
|
||||
|
||||
if (threadIdx.x >= l2) {
|
||||
return;
|
||||
}
|
||||
|
||||
scalar_t sv_elem = step_vec[batch * l2 + threadIdx.x];
|
||||
|
||||
// g_step = g0 @ step_vec_T
|
||||
// g_x @ step_vec_T
|
||||
for (int i = 0; i < l1; i++) {
|
||||
reduce(g_x[batch * l1 * l2 + l2 * i + threadIdx.x] * sv_elem, l2, mask,
|
||||
&data[0], &result[i]);
|
||||
}
|
||||
|
||||
// __shared__ scalar_t step_success[32];
|
||||
// __shared__ scalar_t step_success_w1[32];
|
||||
assert(blockDim.x >= l1);
|
||||
bool wolfe_1 = false;
|
||||
bool wolfe = false;
|
||||
bool condition = threadIdx.x < l1;
|
||||
if (condition) {
|
||||
scalar_t alpha_list_elem = alpha_list[threadIdx.x];
|
||||
|
||||
// scalar_t alpha_list_elem = alpha_list[batch*l1 + threadIdx.x];
|
||||
|
||||
// condition 1:
|
||||
wolfe_1 = c[batch * l1 + threadIdx.x] <=
|
||||
(c[batch * l1] + c_1 * alpha_list_elem * result[0]);
|
||||
|
||||
// condition 2:
|
||||
bool wolfe_2;
|
||||
if (strong_wolfe) {
|
||||
wolfe_2 = abs(result[threadIdx.x]) <= c_2 * abs(result[0]);
|
||||
} else {
|
||||
wolfe_2 = result[threadIdx.x] >= c_2 * result[0];
|
||||
}
|
||||
|
||||
// wolfe = torch.logical_and(wolfe_1, wolfe_2)
|
||||
wolfe = wolfe_1 & wolfe_2;
|
||||
|
||||
// // step_success = wolfe * (self.alpha_list[:, :, 0:1] + 0.1)
|
||||
// // step_success_w1 = wolfe_1 * (self.alpha_list[:, :, 0:1] + 0.1)
|
||||
// step_success[threadIdx.x] = wolfe * (alpha_list_elem + 0.1);
|
||||
// step_success_w1[threadIdx.x] = wolfe_1 * (alpha_list_elem + 0.1);
|
||||
}
|
||||
unsigned msk1 = __ballot_sync(FULL_MASK, wolfe_1 & condition);
|
||||
unsigned msk = __ballot_sync(FULL_MASK, wolfe & condition);
|
||||
|
||||
// get the index of the last occurance of true
|
||||
unsigned msk1_brev = __brev(msk1);
|
||||
unsigned msk_brev = __brev(msk);
|
||||
|
||||
int id1 = 32 - __ffs(msk1_brev); // position of least signficant bit set to 1
|
||||
int id = 32 - __ffs(msk_brev); // position of least signficant bit set to 1
|
||||
|
||||
__syncthreads();
|
||||
|
||||
__shared__ int idx_shared;
|
||||
if (threadIdx.x == 0) {
|
||||
if (!approx_wolfe) {
|
||||
if (id == 32) { // msk is zero
|
||||
id = id1;
|
||||
}
|
||||
if (id == 0) { // bit 0 is set
|
||||
id = id1;
|
||||
}
|
||||
if (id == 32) { // msk is zero
|
||||
id = 1;
|
||||
}
|
||||
if (id == 0) {
|
||||
id = 1;
|
||||
}
|
||||
} else {
|
||||
if (id == 32) { // msk is zero
|
||||
id = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// // _, m_idx = torch.max(step_success, dim=-2)
|
||||
// // _, m1_idx = torch.max(step_success_w1, dim=-2)
|
||||
// int m_id = 0;
|
||||
// int m1_id = 0;
|
||||
// scalar_t max1 = step_success[0];
|
||||
// scalar_t max2 = step_success_w1[0];
|
||||
// for (int i=1; i<l1; i++) {
|
||||
// if (max1<step_success[i]) {
|
||||
// max1 = step_success[i];
|
||||
// m_id = i;
|
||||
// }
|
||||
// if (max2<step_success_w1[i]) {
|
||||
// max2 = step_success_w1[i];
|
||||
// m1_id = i;
|
||||
// }
|
||||
// }
|
||||
|
||||
// // m_idx = torch.where(m_idx == 0, m1_idx, m_idx)
|
||||
// if (m_id == 0) {
|
||||
// m_id = m1_id;
|
||||
// }
|
||||
|
||||
// // m_idx[m_idx == 0] = 1
|
||||
// if (m_id == 0) {
|
||||
// m_id = 1;
|
||||
// }
|
||||
|
||||
// if (id != m_id) {
|
||||
// printf("id=%d, m_id=%d\n", id, m_id);
|
||||
// printf("msk1=%x, msk=%x, raw id1=%d, raw id=%d\n", msk1, msk,
|
||||
// 32-__ffs(msk1_brev), 32-__ffs(msk_brev));
|
||||
// }
|
||||
|
||||
// m_idx[batch] = m_id;
|
||||
// m_idx[batch] = id;
|
||||
idx_shared = id + c_idx[batch];
|
||||
}
|
||||
|
||||
////////////////////////////////////
|
||||
// write outputs using the computed index.
|
||||
// one index per batch is computed
|
||||
////////////////////////////////////
|
||||
__syncthreads();
|
||||
if (threadIdx.x < l2) {
|
||||
if (threadIdx.x == 0) {
|
||||
// printf("block: %d, idx_shared: %d\n", batch, idx_shared);
|
||||
}
|
||||
best_x[batch * l2 + threadIdx.x] = x_set[idx_shared * l2 + threadIdx.x];
|
||||
best_grad[batch * l2 + threadIdx.x] = g_x[idx_shared * l2 + threadIdx.x];
|
||||
}
|
||||
if (threadIdx.x == 0) {
|
||||
best_c[batch] = c[idx_shared];
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace Optimization
|
||||
} // namespace Curobo
|
||||
std::vector<torch::Tensor> line_search_cuda(
|
||||
// torch::Tensor m_idx,
|
||||
torch::Tensor best_x, torch::Tensor best_c, torch::Tensor best_grad,
|
||||
const torch::Tensor g_x, const torch::Tensor x_set,
|
||||
const torch::Tensor step_vec, const torch::Tensor c_0,
|
||||
const torch::Tensor alpha_list, const torch::Tensor c_idx, const float c_1,
|
||||
const float c_2, const bool strong_wolfe, const bool approx_wolfe,
|
||||
const int l1, const int l2, const int batchsize) {
|
||||
using namespace Curobo::Optimization;
|
||||
assert(l2 <= 1024);
|
||||
|
||||
// multiple of 32
|
||||
const int threadsPerBlock = 32 * ((l2 + 31) / 32); // l2;
|
||||
const int blocksPerGrid = batchsize;
|
||||
|
||||
cudaStream_t stream = at::cuda::getCurrentCUDAStream();
|
||||
|
||||
AT_DISPATCH_FLOATING_TYPES(
|
||||
g_x.scalar_type(), "line_search_cu", ([&] {
|
||||
line_search_kernel_mask<scalar_t, scalar_t>
|
||||
<<<blocksPerGrid, threadsPerBlock, 0, stream>>>(
|
||||
// m_idx.data_ptr<int>(),
|
||||
best_x.data_ptr<scalar_t>(), best_c.data_ptr<scalar_t>(),
|
||||
best_grad.data_ptr<scalar_t>(), g_x.data_ptr<scalar_t>(),
|
||||
x_set.data_ptr<scalar_t>(), step_vec.data_ptr<scalar_t>(),
|
||||
c_0.data_ptr<scalar_t>(), alpha_list.data_ptr<scalar_t>(),
|
||||
c_idx.data_ptr<int64_t>(), c_1, c_2, strong_wolfe, approx_wolfe,
|
||||
l1, l2, batchsize);
|
||||
}));
|
||||
C10_CUDA_KERNEL_LAUNCH_CHECK();
|
||||
|
||||
return {best_x, best_c, best_grad};
|
||||
}
|
||||
790
src/curobo/curobolib/cpp/pose_distance_kernel.cu
Normal file
790
src/curobo/curobolib/cpp/pose_distance_kernel.cu
Normal file
@@ -0,0 +1,790 @@
|
||||
/*
|
||||
* Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
|
||||
*
|
||||
* NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
|
||||
* property and proprietary rights in and to this material, related
|
||||
* documentation and any modifications thereto. Any use, reproduction,
|
||||
* disclosure or distribution of this material and related documentation
|
||||
* without an express license agreement from NVIDIA CORPORATION or
|
||||
* its affiliates is strictly prohibited.
|
||||
*/
|
||||
#include <c10/cuda/CUDAStream.h>
|
||||
#include <cuda.h>
|
||||
#include <cuda_runtime.h>
|
||||
#include <torch/extension.h>
|
||||
|
||||
#include "helper_math.h"
|
||||
#include <cmath>
|
||||
#include <cuda_fp16.h>
|
||||
#include <vector>
|
||||
|
||||
#define SINGLE_GOAL 0
|
||||
#define BATCH_GOAL 1
|
||||
#define GOALSET 2
|
||||
#define BATCH_GOALSET 3
|
||||
|
||||
namespace Curobo
|
||||
{
|
||||
namespace Pose
|
||||
{
|
||||
__device__ __forceinline__ void
|
||||
transform_vec_quat(const float4 q, // x,y,z, qw, qx,qy,qz
|
||||
const float *d_vec_weight,
|
||||
float *C) {
|
||||
// do dot product:
|
||||
// new_p = q * p * q_inv + obs_p
|
||||
|
||||
if (false || q.x != 0 || q.y != 0 || q.z != 0) {
|
||||
|
||||
C[0] = q.w * q.w * d_vec_weight[0] + 2 * q.y * q.w * d_vec_weight[2] -
|
||||
2 * q.z * q.w * d_vec_weight[1] + q.x * q.x * d_vec_weight[0] +
|
||||
2 * q.y * q.x * d_vec_weight[1] + 2 * q.z * q.x * d_vec_weight[2] -
|
||||
q.z * q.z * d_vec_weight[0] - q.y * q.y * d_vec_weight[0];
|
||||
C[1] = 2 * q.x * q.y * d_vec_weight[0] + q.y * q.y * d_vec_weight[1] +
|
||||
2 * q.z * q.y * d_vec_weight[2] + 2 * q.w * q.z * d_vec_weight[0] -
|
||||
q.z * q.z * d_vec_weight[1] + q.w * q.w * d_vec_weight[1] -
|
||||
2 * q.x * q.w * d_vec_weight[2] - q.x * q.x * d_vec_weight[1];
|
||||
C[2] = 2 * q.x * q.z * d_vec_weight[0] + 2 * q.y * q.z * d_vec_weight[1] +
|
||||
q.z * q.z * d_vec_weight[2] - 2 * q.w * q.y * d_vec_weight[0] -
|
||||
q.y * q.y * d_vec_weight[2] + 2 * q.w * q.x * d_vec_weight[1] -
|
||||
q.x * q.x * d_vec_weight[2] + q.w * q.w * d_vec_weight[2];
|
||||
|
||||
C[3] = q.w * q.w * d_vec_weight[3] + 2 * q.y * q.w * d_vec_weight[5] -
|
||||
2 * q.z * q.w * d_vec_weight[4] + q.x * q.x * d_vec_weight[3] +
|
||||
2 * q.y * q.x * d_vec_weight[4] + 2 * q.z * q.x * d_vec_weight[5] -
|
||||
q.z * q.z * d_vec_weight[3] - q.y * q.y * d_vec_weight[3];
|
||||
C[4] = 2 * q.x * q.y * d_vec_weight[3] + q.y * q.y * d_vec_weight[4] +
|
||||
2 * q.z * q.y * d_vec_weight[5] + 2 * q.w * q.z * d_vec_weight[3] -
|
||||
q.z * q.z * d_vec_weight[4] + q.w * q.w * d_vec_weight[4] -
|
||||
2 * q.x * q.w * d_vec_weight[5] - q.x * q.x * d_vec_weight[4];
|
||||
C[5] = 2 * q.x * q.z * d_vec_weight[3] + 2 * q.y * q.z * d_vec_weight[4] +
|
||||
q.z * q.z * d_vec_weight[5] - 2 * q.w * q.y * d_vec_weight[3] -
|
||||
q.y * q.y * d_vec_weight[5] + 2 * q.w * q.x * d_vec_weight[4] -
|
||||
q.x * q.x * d_vec_weight[5] + q.w * q.w * d_vec_weight[5];
|
||||
}
|
||||
{
|
||||
C[0] = d_vec_weight[0];
|
||||
C[1] = d_vec_weight[1];
|
||||
C[2] = d_vec_weight[2];
|
||||
C[3] = d_vec_weight[3];
|
||||
C[4] = d_vec_weight[4];
|
||||
C[5] = d_vec_weight[5];
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
__device__ __forceinline__ void
|
||||
inv_transform_vec_quat(const float4 q_in, // x,y,z, qw, qx,qy,qz
|
||||
const float *d_vec_weight,
|
||||
float *C) {
|
||||
// do dot product:
|
||||
// new_p = q * p * q_inv + obs_p
|
||||
|
||||
if (q_in.x != 0 || q_in.y != 0 || q_in.z != 0) {
|
||||
float4 q = make_float4(-q_in.x, -q_in.y, -q_in.z, q_in.w);
|
||||
|
||||
C[0] = q.w * q.w * d_vec_weight[0] + 2 * q.y * q.w * d_vec_weight[2] -
|
||||
2 * q.z * q.w * d_vec_weight[1] + q.x * q.x * d_vec_weight[0] +
|
||||
2 * q.y * q.x * d_vec_weight[1] + 2 * q.z * q.x * d_vec_weight[2] -
|
||||
q.z * q.z * d_vec_weight[0] - q.y * q.y * d_vec_weight[0];
|
||||
C[1] = 2 * q.x * q.y * d_vec_weight[0] + q.y * q.y * d_vec_weight[1] +
|
||||
2 * q.z * q.y * d_vec_weight[2] + 2 * q.w * q.z * d_vec_weight[0] -
|
||||
q.z * q.z * d_vec_weight[1] + q.w * q.w * d_vec_weight[1] -
|
||||
2 * q.x * q.w * d_vec_weight[2] - q.x * q.x * d_vec_weight[1];
|
||||
C[2] = 2 * q.x * q.z * d_vec_weight[0] + 2 * q.y * q.z * d_vec_weight[1] +
|
||||
q.z * q.z * d_vec_weight[2] - 2 * q.w * q.y * d_vec_weight[0] -
|
||||
q.y * q.y * d_vec_weight[2] + 2 * q.w * q.x * d_vec_weight[1] -
|
||||
q.x * q.x * d_vec_weight[2] + q.w * q.w * d_vec_weight[2];
|
||||
|
||||
C[3] = q.w * q.w * d_vec_weight[3] + 2 * q.y * q.w * d_vec_weight[5] -
|
||||
2 * q.z * q.w * d_vec_weight[4] + q.x * q.x * d_vec_weight[3] +
|
||||
2 * q.y * q.x * d_vec_weight[4] + 2 * q.z * q.x * d_vec_weight[5] -
|
||||
q.z * q.z * d_vec_weight[3] - q.y * q.y * d_vec_weight[3];
|
||||
C[4] = 2 * q.x * q.y * d_vec_weight[3] + q.y * q.y * d_vec_weight[4] +
|
||||
2 * q.z * q.y * d_vec_weight[5] + 2 * q.w * q.z * d_vec_weight[3] -
|
||||
q.z * q.z * d_vec_weight[4] + q.w * q.w * d_vec_weight[4] -
|
||||
2 * q.x * q.w * d_vec_weight[5] - q.x * q.x * d_vec_weight[4];
|
||||
C[5] = 2 * q.x * q.z * d_vec_weight[3] + 2 * q.y * q.z * d_vec_weight[4] +
|
||||
q.z * q.z * d_vec_weight[5] - 2 * q.w * q.y * d_vec_weight[3] -
|
||||
q.y * q.y * d_vec_weight[5] + 2 * q.w * q.x * d_vec_weight[4] -
|
||||
q.x * q.x * d_vec_weight[5] + q.w * q.w * d_vec_weight[5];
|
||||
}
|
||||
{
|
||||
C[0] = d_vec_weight[0];
|
||||
C[1] = d_vec_weight[1];
|
||||
C[2] = d_vec_weight[2];
|
||||
C[3] = d_vec_weight[3];
|
||||
C[4] = d_vec_weight[4];
|
||||
C[5] = d_vec_weight[5];
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
__device__ __forceinline__ void
|
||||
compute_quat_distance(float *result, const float4 a, const float4 b) {
|
||||
// We compute distance with the conjugate of b
|
||||
float r_w = (a.w * b.w + a.x * b.x + a.y * b.y + a.z * b.z);
|
||||
if (r_w < 0.0) {
|
||||
r_w = 1.0;
|
||||
} else {
|
||||
r_w = -1.0;
|
||||
}
|
||||
|
||||
result[0] = r_w * (-1 * a.w * b.x + b.w * a.x - a.y * b.z + b.y * a.z);
|
||||
result[1] = r_w * (-1 * a.w * b.y + b.w * a.y - a.z * b.x + b.z * a.x);
|
||||
result[2] = r_w * (-1 * a.w * b.z + b.w * a.z - a.x * b.y + b.x * a.y);
|
||||
}
|
||||
|
||||
__device__ __forceinline__ void
|
||||
compute_distance(float *distance_vec, float &distance, float &position_distance,
|
||||
float &rotation_distance, const float3 current_position,
|
||||
const float3 goal_position, const float4 current_quat,
|
||||
const float4 goal_quat, const float *vec_weight,
|
||||
const float *vec_convergence, const float position_weight,
|
||||
const float rotation_weight) {
|
||||
compute_quat_distance(&distance_vec[3], goal_quat, current_quat);
|
||||
// compute position distance
|
||||
*(float3 *)&distance_vec[0] = current_position - goal_position;
|
||||
// distance_vec[0] = goal_position
|
||||
position_distance = 0;
|
||||
rotation_distance = 0;
|
||||
// scale by vec weight and position weight:
|
||||
#pragma unroll 3
|
||||
for (int i = 0; i < 3; i++) {
|
||||
distance_vec[i] = vec_weight[i + 3] * distance_vec[i];
|
||||
position_distance += distance_vec[i] * distance_vec[i];
|
||||
}
|
||||
#pragma unroll 3
|
||||
for (int i = 3; i < 6; i++) {
|
||||
distance_vec[i] = vec_weight[i - 3] * distance_vec[i];
|
||||
rotation_distance += distance_vec[i] * distance_vec[i];
|
||||
}
|
||||
|
||||
distance = 0;
|
||||
if (rotation_distance > vec_convergence[0] * vec_convergence[0]) {
|
||||
rotation_distance = sqrtf(rotation_distance);
|
||||
distance += rotation_weight * rotation_distance;
|
||||
}
|
||||
if (position_distance > vec_convergence[1] * vec_convergence[1]) {
|
||||
position_distance = sqrtf(position_distance);
|
||||
distance += position_weight * position_distance;
|
||||
}
|
||||
}
|
||||
|
||||
__device__ __forceinline__ void compute_metric_distance(
|
||||
float *distance_vec, float &distance, float &position_distance,
|
||||
float &rotation_distance, const float3 current_position,
|
||||
const float3 goal_position, const float4 current_quat,
|
||||
const float4 goal_quat, const float *vec_weight,
|
||||
const float *vec_convergence, const float position_weight,
|
||||
const float p_alpha, const float rotation_weight, const float r_alpha) {
|
||||
compute_quat_distance(&distance_vec[3], goal_quat, current_quat);
|
||||
// compute position distance
|
||||
*(float3 *)&distance_vec[0] = current_position - goal_position;
|
||||
// distance_vec[0] = goal_position
|
||||
position_distance = 0;
|
||||
rotation_distance = 0;
|
||||
// scale by vec weight and position weight:
|
||||
#pragma unroll 3
|
||||
for (int i = 0; i < 3; i++) {
|
||||
distance_vec[i] = vec_weight[i + 3] * distance_vec[i];
|
||||
// position_distance += powf(distance_vec[i],2);
|
||||
position_distance += distance_vec[i] * distance_vec[i];
|
||||
}
|
||||
#pragma unroll 3
|
||||
for (int i = 3; i < 6; i++) {
|
||||
distance_vec[i] = vec_weight[i - 3] * distance_vec[i];
|
||||
// rotation_distance += powf(distance_vec[i],2);
|
||||
rotation_distance += distance_vec[i] * distance_vec[i];
|
||||
}
|
||||
|
||||
distance = 0;
|
||||
if (rotation_distance > vec_convergence[0] * vec_convergence[0]) {
|
||||
rotation_distance = sqrtf(rotation_distance);
|
||||
distance += rotation_weight * log2f(coshf(r_alpha * rotation_distance));
|
||||
}
|
||||
if (position_distance > vec_convergence[1] * vec_convergence[1]) {
|
||||
position_distance = sqrtf(position_distance);
|
||||
distance += position_weight * log2f(coshf(p_alpha * position_distance));
|
||||
}
|
||||
}
|
||||
|
||||
template <typename scalar_t>
|
||||
__global__ void
|
||||
backward_pose_distance_kernel(scalar_t *out_grad_p, // [b,3]
|
||||
scalar_t *out_grad_q, // [b,4]
|
||||
const scalar_t *grad_distance, // [b,1]
|
||||
const scalar_t *grad_p_distance, // [b,1]
|
||||
const scalar_t *grad_q_distance, // [b,1]
|
||||
const scalar_t *pose_weight, // [2]
|
||||
const scalar_t *grad_p_vec, // [b,3]
|
||||
const scalar_t *grad_q_vec, // [b,4]
|
||||
const int batch_size) {
|
||||
const int batch_idx = blockDim.x * blockIdx.x + threadIdx.x;
|
||||
if (batch_idx >= batch_size) {
|
||||
return;
|
||||
}
|
||||
// read data
|
||||
const float g_distance = grad_distance[batch_idx];
|
||||
const float2 p_weight = *(float2 *)&pose_weight[0];
|
||||
const float3 g_p_v = *(float3 *)&grad_p_vec[batch_idx * 3];
|
||||
const float3 g_q_v = *(float3 *)&grad_q_vec[batch_idx * 4 + 1];
|
||||
const float g_p_distance = grad_p_distance[batch_idx];
|
||||
const float g_q_distance = grad_q_distance[batch_idx];
|
||||
|
||||
// compute position gradient
|
||||
float3 g_p =
|
||||
(g_p_v) * ((g_p_distance + g_distance * p_weight.y)); // scalar * float3
|
||||
float3 g_q =
|
||||
(g_q_v) * ((g_q_distance + g_distance * p_weight.x)); // scalar * float3
|
||||
|
||||
// write out
|
||||
*(float3 *)&out_grad_p[batch_idx * 3] = g_p;
|
||||
*(float3 *)&out_grad_q[batch_idx * 4 + 1] = g_q;
|
||||
}
|
||||
|
||||
template <typename scalar_t>
|
||||
__global__ void backward_pose_kernel(scalar_t *out_grad_p, // [b,3]
|
||||
scalar_t *out_grad_q, // [b,4]
|
||||
const scalar_t *grad_distance, // [b,1]
|
||||
const scalar_t *pose_weight, // [2]
|
||||
const scalar_t *grad_p_vec, // [b,3]
|
||||
const scalar_t *grad_q_vec, // [b,4]
|
||||
const int batch_size) {
|
||||
const int batch_idx = blockDim.x * blockIdx.x + threadIdx.x;
|
||||
if (batch_idx >= batch_size) {
|
||||
return;
|
||||
}
|
||||
// read data
|
||||
const float g_distance = grad_distance[batch_idx];
|
||||
const float2 p_weight = *(float2 *)&pose_weight[0];
|
||||
const float3 g_p_v = *(float3 *)&grad_p_vec[batch_idx * 3];
|
||||
const float3 g_q_v = *(float3 *)&grad_q_vec[batch_idx * 4 + 1];
|
||||
|
||||
// compute position gradient
|
||||
float3 g_p = (g_p_v) * ((g_distance * p_weight.y)); // scalar * float3
|
||||
float3 g_q = (g_q_v) * ((g_distance * p_weight.x)); // scalar * float3
|
||||
|
||||
// write out
|
||||
*(float3 *)&out_grad_p[batch_idx * 3] = g_p;
|
||||
*(float3 *)&out_grad_q[batch_idx * 4 + 1] = g_q;
|
||||
}
|
||||
|
||||
template <typename scalar_t, bool write_distance>
|
||||
__global__ void goalset_pose_distance_kernel(
|
||||
scalar_t *out_distance, scalar_t *out_position_distance,
|
||||
scalar_t *out_rotation_distance, scalar_t *out_p_vec, scalar_t *out_q_vec,
|
||||
int32_t *out_gidx, const scalar_t *current_position,
|
||||
const scalar_t *goal_position, const scalar_t *current_quat,
|
||||
const scalar_t *goal_quat, const scalar_t *vec_weight,
|
||||
const scalar_t *weight, const scalar_t *vec_convergence,
|
||||
const scalar_t *run_weight, const scalar_t *run_vec_weight,
|
||||
const int32_t *batch_pose_idx, const int mode, const int num_goals,
|
||||
const int batch_size, const int horizon, const bool write_grad = false) {
|
||||
const int t_idx = (blockDim.x * blockIdx.x + threadIdx.x);
|
||||
const int batch_idx = t_idx / horizon;
|
||||
const int h_idx = t_idx - (batch_idx * horizon);
|
||||
if (batch_idx >= batch_size || h_idx >= horizon) {
|
||||
return;
|
||||
}
|
||||
|
||||
// read current pose:
|
||||
float3 position =
|
||||
*(float3 *)¤t_position[batch_idx * horizon * 3 + h_idx * 3];
|
||||
float4 quat_4 = *(float4 *)¤t_quat[batch_idx * horizon * 4 + h_idx * 4];
|
||||
float4 quat = make_float4(quat_4.y, quat_4.z, quat_4.w, quat_4.x);
|
||||
|
||||
// read weights:
|
||||
|
||||
float position_weight = weight[1];
|
||||
float rotation_weight = weight[0];
|
||||
if (!write_distance) {
|
||||
position_weight *= run_weight[h_idx];
|
||||
rotation_weight *= run_weight[h_idx];
|
||||
if (position_weight == 0.0 && rotation_weight == 0.0) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
float3 l_goal_position;
|
||||
float4 l_goal_quat;
|
||||
float distance_vec[6]; // = {0.0};
|
||||
float pose_distance = 0.0;
|
||||
float position_distance = 0.0;
|
||||
float rotation_distance = 0.0;
|
||||
float best_distance = INFINITY;
|
||||
float best_position_distance = 0.0;
|
||||
float best_rotation_distance = 0.0;
|
||||
float best_distance_vec[6] = {0.0};
|
||||
float best_des_vec_weight[6] = {0.0};
|
||||
float d_vec_convergence[2];
|
||||
|
||||
*(float2 *)&d_vec_convergence[0] = *(float2 *)&vec_convergence[0];
|
||||
|
||||
int best_idx = -1;
|
||||
float d_vec_weight[6];
|
||||
float des_vec_weight[6] = {0.0};
|
||||
*(float3 *)&d_vec_weight[0] = *(float3 *)&vec_weight[0];
|
||||
*(float3 *)&d_vec_weight[3] = *(float3 *)&vec_weight[3];
|
||||
if (h_idx < horizon - 1) {
|
||||
*(float3 *)&d_vec_weight[0] *= *(float3 *)&run_vec_weight[0];
|
||||
*(float3 *)&d_vec_weight[3] *= *(float3 *)&run_vec_weight[3];
|
||||
}
|
||||
|
||||
|
||||
// read offset
|
||||
int offset = batch_pose_idx[batch_idx];
|
||||
if (mode == BATCH_GOALSET || mode == BATCH_GOAL) {
|
||||
offset = (offset)*num_goals;
|
||||
}
|
||||
|
||||
for (int k = 0; k < num_goals; k++) {
|
||||
|
||||
l_goal_position = *(float3 *)&goal_position[(offset + k) * 3];
|
||||
float4 gq4 = *(float4 *)&goal_quat[(offset + k) * 4];
|
||||
l_goal_quat = make_float4(gq4.y, gq4.z, gq4.w, gq4.x);
|
||||
transform_vec_quat(l_goal_quat, &d_vec_weight[0], &des_vec_weight[0]);
|
||||
|
||||
compute_distance(&distance_vec[0], pose_distance, position_distance,
|
||||
rotation_distance, position, l_goal_position, quat,
|
||||
l_goal_quat,
|
||||
&des_vec_weight[0], //&l_vec_weight[0],
|
||||
&d_vec_convergence[0], //&l_vec_convergence[0],
|
||||
position_weight, rotation_weight);
|
||||
if (pose_distance <= best_distance) {
|
||||
best_idx = k;
|
||||
best_distance = pose_distance;
|
||||
best_position_distance = position_distance;
|
||||
best_rotation_distance = rotation_distance;
|
||||
if (write_grad) {
|
||||
//inv_transform_vec_quat(l_goal_quat, &d_vec_weight[0], &best_des_vec_weight[0]);
|
||||
#pragma unroll 6
|
||||
for (int i = 0; i < 6; i++) {
|
||||
best_distance_vec[i] = distance_vec[i];
|
||||
best_des_vec_weight[i] = des_vec_weight[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
// write out:
|
||||
|
||||
// write out pose distance:
|
||||
out_distance[batch_idx * horizon + h_idx] = best_distance;
|
||||
if (write_distance) {
|
||||
out_position_distance[batch_idx * horizon + h_idx] = best_position_distance;
|
||||
out_rotation_distance[batch_idx * horizon + h_idx] = best_rotation_distance;
|
||||
}
|
||||
out_gidx[batch_idx * horizon + h_idx] = best_idx;
|
||||
|
||||
if (write_grad) {
|
||||
if (write_distance) {
|
||||
position_weight = 1;
|
||||
rotation_weight = 1;
|
||||
}
|
||||
if (best_position_distance > 0) {
|
||||
best_position_distance = (position_weight / best_position_distance);
|
||||
|
||||
out_p_vec[batch_idx * horizon * 3 + h_idx * 3] =
|
||||
best_des_vec_weight[3] * best_distance_vec[0] * best_position_distance;
|
||||
out_p_vec[batch_idx * horizon * 3 + h_idx * 3 + 1] =
|
||||
best_des_vec_weight[4] * best_distance_vec[1] * best_position_distance;
|
||||
out_p_vec[batch_idx * horizon * 3 + h_idx * 3 + 2] =
|
||||
best_des_vec_weight[5] * best_distance_vec[2] * best_position_distance;
|
||||
|
||||
} else {
|
||||
out_p_vec[batch_idx * horizon * 3 + h_idx * 3] = 0.0;
|
||||
out_p_vec[batch_idx * horizon * 3 + h_idx * 3 + 1] = 0.0;
|
||||
out_p_vec[batch_idx * horizon * 3 + h_idx * 3 + 2] = 0.0;
|
||||
}
|
||||
|
||||
if (best_rotation_distance > 0) {
|
||||
best_rotation_distance = rotation_weight / best_rotation_distance;
|
||||
|
||||
out_q_vec[batch_idx * horizon * 4 + h_idx * 4 + 1] =
|
||||
best_des_vec_weight[0] * best_distance_vec[3] * best_rotation_distance;
|
||||
out_q_vec[batch_idx * horizon * 4 + h_idx * 4 + 2] =
|
||||
best_des_vec_weight[1] * best_distance_vec[4] * best_rotation_distance;
|
||||
out_q_vec[batch_idx * horizon * 4 + h_idx * 4 + 3] =
|
||||
best_des_vec_weight[2] * best_distance_vec[5] * best_rotation_distance;
|
||||
} else {
|
||||
out_q_vec[batch_idx * horizon * 4 + h_idx * 4 + 1] = 0.0;
|
||||
out_q_vec[batch_idx * horizon * 4 + h_idx * 4 + 2] = 0.0;
|
||||
out_q_vec[batch_idx * horizon * 4 + h_idx * 4 + 3] = 0.0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
template <typename scalar_t, bool write_distance>
|
||||
__global__ void goalset_pose_distance_metric_kernel(
|
||||
scalar_t *out_distance, scalar_t *out_position_distance,
|
||||
scalar_t *out_rotation_distance, scalar_t *out_p_vec, scalar_t *out_q_vec,
|
||||
int32_t *out_gidx, const scalar_t *current_position,
|
||||
const scalar_t *goal_position, const scalar_t *current_quat,
|
||||
const scalar_t *goal_quat, const scalar_t *vec_weight,
|
||||
const scalar_t *weight, const scalar_t *vec_convergence,
|
||||
const scalar_t *run_weight, const scalar_t *run_vec_weight,
|
||||
const int32_t *batch_pose_idx, const int mode, const int num_goals,
|
||||
const int batch_size, const int horizon, const bool write_grad = false) {
|
||||
const int t_idx = (blockDim.x * blockIdx.x + threadIdx.x);
|
||||
const int batch_idx = t_idx / horizon;
|
||||
const int h_idx = t_idx - (batch_idx * horizon);
|
||||
if (batch_idx >= batch_size || h_idx >= horizon) {
|
||||
return;
|
||||
}
|
||||
|
||||
// read current pose:
|
||||
float3 position =
|
||||
*(float3 *)¤t_position[batch_idx * horizon * 3 + h_idx * 3];
|
||||
float4 quat_4 = *(float4 *)¤t_quat[batch_idx * horizon * 4 + h_idx * 4];
|
||||
float4 quat = make_float4(quat_4.y, quat_4.z, quat_4.w, quat_4.x);
|
||||
|
||||
// read weights:
|
||||
|
||||
float position_weight = weight[1];
|
||||
float p_w_alpha = weight[3];
|
||||
float r_w_alpha = weight[2];
|
||||
float rotation_weight = weight[0];
|
||||
if (!write_distance) {
|
||||
position_weight *= run_weight[h_idx];
|
||||
rotation_weight *= run_weight[h_idx];
|
||||
p_w_alpha *= run_weight[h_idx];
|
||||
r_w_alpha *= run_weight[h_idx];
|
||||
if (position_weight == 0.0 && rotation_weight == 0.0) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
float d_vec_convergence[2];
|
||||
*(float2 *)&d_vec_convergence[0] = *(float2 *)&vec_convergence[0];
|
||||
|
||||
float d_vec_weight[6];
|
||||
*(float3 *)&d_vec_weight[0] = *(float3 *)&vec_weight[0];
|
||||
*(float3 *)&d_vec_weight[3] = *(float3 *)&vec_weight[3];
|
||||
if (h_idx < horizon - 1) {
|
||||
*(float3 *)&d_vec_weight[0] *= *(float3 *)&run_vec_weight[0];
|
||||
*(float3 *)&d_vec_weight[3] *= *(float3 *)&run_vec_weight[3];
|
||||
}
|
||||
|
||||
float des_vec_weight[6] = {0.0};
|
||||
float3 l_goal_position;
|
||||
float4 l_goal_quat;
|
||||
float distance_vec[6]; // = {0.0};
|
||||
float pose_distance = 0.0;
|
||||
float position_distance = 0.0;
|
||||
float rotation_distance = 0.0;
|
||||
float best_distance = INFINITY;
|
||||
float best_position_distance = 0.0;
|
||||
float best_rotation_distance = 0.0;
|
||||
float best_distance_vec[6] = {0.0};
|
||||
float best_des_vec_weight[6] = {0.0};
|
||||
int best_idx = -1.0;
|
||||
int offset = batch_pose_idx[batch_idx];
|
||||
if (mode == BATCH_GOALSET || mode == BATCH_GOAL) {
|
||||
offset = (offset)*num_goals;
|
||||
}
|
||||
|
||||
for (int k = 0; k < num_goals; k++) {
|
||||
|
||||
l_goal_position = *(float3 *)&goal_position[(offset + k) * 3];
|
||||
float4 gq4 =
|
||||
*(float4 *)&goal_quat[(offset + k) * 4]; // reading qw, qx, qy, qz
|
||||
l_goal_quat = make_float4(gq4.y, gq4.z, gq4.w, gq4.x);
|
||||
|
||||
transform_vec_quat(l_goal_quat, &d_vec_weight[0], &des_vec_weight[0]);
|
||||
compute_metric_distance(
|
||||
&distance_vec[0], pose_distance, position_distance, rotation_distance,
|
||||
position, l_goal_position, quat, l_goal_quat,
|
||||
&des_vec_weight[0], //&l_vec_weight[0],
|
||||
&d_vec_convergence[0], //&l_vec_convergence[0],
|
||||
position_weight, p_w_alpha, rotation_weight, r_w_alpha);
|
||||
if (pose_distance <= best_distance) {
|
||||
best_idx = k;
|
||||
best_distance = pose_distance;
|
||||
best_position_distance = position_distance;
|
||||
best_rotation_distance = rotation_distance;
|
||||
if (write_grad) {
|
||||
// inv_transform_vec_quat(l_goal_quat, &d_vec_weight[0], &best_des_vec_weight[0]);
|
||||
|
||||
#pragma unroll 6
|
||||
for (int i = 0; i < 6; i++) {
|
||||
best_distance_vec[i] = distance_vec[i];
|
||||
best_des_vec_weight[i] = des_vec_weight[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// add scaling metric:
|
||||
// best_distance = log2f(acoshf(best_distance));
|
||||
// write out:
|
||||
|
||||
// write out pose distance:
|
||||
out_distance[batch_idx * horizon + h_idx] = best_distance;
|
||||
if (write_distance) {
|
||||
out_position_distance[batch_idx * horizon + h_idx] = best_position_distance;
|
||||
out_rotation_distance[batch_idx * horizon + h_idx] = best_rotation_distance;
|
||||
}
|
||||
out_gidx[batch_idx * horizon + h_idx] = best_idx;
|
||||
|
||||
if (write_grad) {
|
||||
// write gradient
|
||||
// compute scalar term:
|
||||
// -w * (d_vec)/ (length * length(negative +1) * acos(length)
|
||||
// -w * (d_vec) * sinh(length) / (length * cosh(length))
|
||||
// compute extra term:
|
||||
|
||||
if (write_distance) {
|
||||
position_weight = 1.0;
|
||||
rotation_weight = 1.0;
|
||||
}
|
||||
|
||||
if (best_position_distance > 0) {
|
||||
best_position_distance =
|
||||
(p_w_alpha * position_weight *
|
||||
sinhf(p_w_alpha * best_position_distance)) /
|
||||
(best_position_distance * coshf(p_w_alpha * best_position_distance));
|
||||
|
||||
// best_position_distance = (position_weight/best_position_distance);
|
||||
// comput scalar gradient
|
||||
|
||||
out_p_vec[batch_idx * horizon * 3 + h_idx * 3] =
|
||||
best_des_vec_weight[3] * best_distance_vec[0] * best_position_distance;
|
||||
out_p_vec[batch_idx * horizon * 3 + h_idx * 3 + 1] =
|
||||
best_des_vec_weight[4] * best_distance_vec[1] * best_position_distance;
|
||||
out_p_vec[batch_idx * horizon * 3 + h_idx * 3 + 2] =
|
||||
best_des_vec_weight[5] * best_distance_vec[2] * best_position_distance;
|
||||
|
||||
} else {
|
||||
out_p_vec[batch_idx * horizon * 3 + h_idx * 3] = 0.0;
|
||||
out_p_vec[batch_idx * horizon * 3 + h_idx * 3 + 1] = 0.0;
|
||||
out_p_vec[batch_idx * horizon * 3 + h_idx * 3 + 2] = 0.0;
|
||||
}
|
||||
|
||||
if (best_rotation_distance > 0) {
|
||||
best_rotation_distance =
|
||||
(r_w_alpha * rotation_weight *
|
||||
sinhf(r_w_alpha * best_rotation_distance)) /
|
||||
(best_rotation_distance * coshf(r_w_alpha * best_rotation_distance));
|
||||
|
||||
out_q_vec[batch_idx * horizon * 4 + h_idx * 4 + 1] =
|
||||
best_des_vec_weight[0] * best_distance_vec[3] * best_rotation_distance;
|
||||
out_q_vec[batch_idx * horizon * 4 + h_idx * 4 + 2] =
|
||||
best_des_vec_weight[1] * best_distance_vec[4] * best_rotation_distance;
|
||||
out_q_vec[batch_idx * horizon * 4 + h_idx * 4 + 3] =
|
||||
best_des_vec_weight[2] * best_distance_vec[5] * best_rotation_distance;
|
||||
} else {
|
||||
out_q_vec[batch_idx * horizon * 4 + h_idx * 4 + 1] = 0.0;
|
||||
out_q_vec[batch_idx * horizon * 4 + h_idx * 4 + 2] = 0.0;
|
||||
out_q_vec[batch_idx * horizon * 4 + h_idx * 4 + 3] = 0.0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
}
|
||||
|
||||
std::vector<torch::Tensor>
|
||||
pose_distance(torch::Tensor out_distance, torch::Tensor out_position_distance,
|
||||
torch::Tensor out_rotation_distance,
|
||||
torch::Tensor distance_p_vector, // batch size, 3
|
||||
torch::Tensor distance_q_vector, // batch size, 4
|
||||
torch::Tensor out_gidx,
|
||||
const torch::Tensor current_position, // batch_size, 3
|
||||
const torch::Tensor goal_position, // n_boxes, 3
|
||||
const torch::Tensor current_quat, const torch::Tensor goal_quat,
|
||||
const torch::Tensor vec_weight, // n_boxes, 4, 4
|
||||
const torch::Tensor weight, const torch::Tensor vec_convergence,
|
||||
const torch::Tensor run_weight,
|
||||
const torch::Tensor run_vec_weight,
|
||||
const torch::Tensor batch_pose_idx, // batch_size, 1
|
||||
const int batch_size, const int horizon, const int mode,
|
||||
const int num_goals = 1, const bool compute_grad = false,
|
||||
const bool write_distance = true, const bool use_metric = false) {
|
||||
using namespace Curobo::Pose;
|
||||
// we compute the warp threads based on number of boxes:
|
||||
assert(batch_pose_idx.size(0) == batch_size);
|
||||
// TODO: verify this math
|
||||
// const int batch_size = out_distance.size(0);
|
||||
assert(run_weight.size(-1) == horizon);
|
||||
const int bh = batch_size * horizon;
|
||||
int threadsPerBlock = bh;
|
||||
if (bh > 128) {
|
||||
threadsPerBlock = 128;
|
||||
}
|
||||
|
||||
// we fit warp thread spheres in a threadsPerBlock
|
||||
|
||||
int blocksPerGrid = (bh + threadsPerBlock - 1) / threadsPerBlock;
|
||||
|
||||
cudaStream_t stream = at::cuda::getCurrentCUDAStream();
|
||||
if (use_metric) {
|
||||
if (write_distance)
|
||||
{
|
||||
AT_DISPATCH_FLOATING_TYPES(
|
||||
current_position.scalar_type(), "batch_pose_distance", ([&] {
|
||||
goalset_pose_distance_metric_kernel
|
||||
<scalar_t, true><<<blocksPerGrid, threadsPerBlock, 0, stream>>>(
|
||||
out_distance.data_ptr<scalar_t>(),
|
||||
out_position_distance.data_ptr<scalar_t>(),
|
||||
out_rotation_distance.data_ptr<scalar_t>(),
|
||||
distance_p_vector.data_ptr<scalar_t>(),
|
||||
distance_q_vector.data_ptr<scalar_t>(),
|
||||
out_gidx.data_ptr<int32_t>(),
|
||||
current_position.data_ptr<scalar_t>(),
|
||||
goal_position.data_ptr<scalar_t>(),
|
||||
current_quat.data_ptr<scalar_t>(),
|
||||
goal_quat.data_ptr<scalar_t>(),
|
||||
vec_weight.data_ptr<scalar_t>(), weight.data_ptr<scalar_t>(),
|
||||
vec_convergence.data_ptr<scalar_t>(),
|
||||
run_weight.data_ptr<scalar_t>(),
|
||||
run_vec_weight.data_ptr<scalar_t>(),
|
||||
batch_pose_idx.data_ptr<int32_t>(), mode, num_goals,
|
||||
batch_size, horizon, compute_grad);
|
||||
}));
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
|
||||
|
||||
AT_DISPATCH_FLOATING_TYPES(
|
||||
current_position.scalar_type(), "batch_pose_distance", ([&] {
|
||||
goalset_pose_distance_metric_kernel
|
||||
// goalset_pose_distance_kernel
|
||||
<scalar_t, false><<<blocksPerGrid, threadsPerBlock, 0, stream>>>(
|
||||
out_distance.data_ptr<scalar_t>(),
|
||||
out_position_distance.data_ptr<scalar_t>(),
|
||||
out_rotation_distance.data_ptr<scalar_t>(),
|
||||
distance_p_vector.data_ptr<scalar_t>(),
|
||||
distance_q_vector.data_ptr<scalar_t>(),
|
||||
out_gidx.data_ptr<int32_t>(),
|
||||
current_position.data_ptr<scalar_t>(),
|
||||
goal_position.data_ptr<scalar_t>(),
|
||||
current_quat.data_ptr<scalar_t>(),
|
||||
goal_quat.data_ptr<scalar_t>(),
|
||||
vec_weight.data_ptr<scalar_t>(), weight.data_ptr<scalar_t>(),
|
||||
vec_convergence.data_ptr<scalar_t>(),
|
||||
run_weight.data_ptr<scalar_t>(),
|
||||
run_vec_weight.data_ptr<scalar_t>(),
|
||||
batch_pose_idx.data_ptr<int32_t>(), mode, num_goals,
|
||||
batch_size, horizon, compute_grad);
|
||||
}));
|
||||
}
|
||||
} else {
|
||||
if(write_distance)
|
||||
{
|
||||
AT_DISPATCH_FLOATING_TYPES(
|
||||
current_position.scalar_type(), "batch_pose_distance", ([&] {
|
||||
// goalset_pose_distance_metric_kernel
|
||||
goalset_pose_distance_kernel<scalar_t, true>
|
||||
<<<blocksPerGrid, threadsPerBlock, 0, stream>>>(
|
||||
out_distance.data_ptr<scalar_t>(),
|
||||
out_position_distance.data_ptr<scalar_t>(),
|
||||
out_rotation_distance.data_ptr<scalar_t>(),
|
||||
distance_p_vector.data_ptr<scalar_t>(),
|
||||
distance_q_vector.data_ptr<scalar_t>(),
|
||||
out_gidx.data_ptr<int32_t>(),
|
||||
current_position.data_ptr<scalar_t>(),
|
||||
goal_position.data_ptr<scalar_t>(),
|
||||
current_quat.data_ptr<scalar_t>(),
|
||||
goal_quat.data_ptr<scalar_t>(),
|
||||
vec_weight.data_ptr<scalar_t>(), weight.data_ptr<scalar_t>(),
|
||||
vec_convergence.data_ptr<scalar_t>(),
|
||||
run_weight.data_ptr<scalar_t>(),
|
||||
run_vec_weight.data_ptr<scalar_t>(),
|
||||
batch_pose_idx.data_ptr<int32_t>(), mode, num_goals,
|
||||
batch_size, horizon, compute_grad);
|
||||
}));
|
||||
}
|
||||
else
|
||||
{
|
||||
AT_DISPATCH_FLOATING_TYPES(
|
||||
current_position.scalar_type(), "batch_pose_distance", ([&] {
|
||||
// goalset_pose_distance_metric_kernel
|
||||
goalset_pose_distance_kernel<scalar_t, false>
|
||||
<<<blocksPerGrid, threadsPerBlock, 0, stream>>>(
|
||||
out_distance.data_ptr<scalar_t>(),
|
||||
out_position_distance.data_ptr<scalar_t>(),
|
||||
out_rotation_distance.data_ptr<scalar_t>(),
|
||||
distance_p_vector.data_ptr<scalar_t>(),
|
||||
distance_q_vector.data_ptr<scalar_t>(),
|
||||
out_gidx.data_ptr<int32_t>(),
|
||||
current_position.data_ptr<scalar_t>(),
|
||||
goal_position.data_ptr<scalar_t>(),
|
||||
current_quat.data_ptr<scalar_t>(),
|
||||
goal_quat.data_ptr<scalar_t>(),
|
||||
vec_weight.data_ptr<scalar_t>(), weight.data_ptr<scalar_t>(),
|
||||
vec_convergence.data_ptr<scalar_t>(),
|
||||
run_weight.data_ptr<scalar_t>(),
|
||||
run_vec_weight.data_ptr<scalar_t>(),
|
||||
batch_pose_idx.data_ptr<int32_t>(), mode, num_goals,
|
||||
batch_size, horizon, compute_grad);
|
||||
}));
|
||||
}
|
||||
}
|
||||
C10_CUDA_KERNEL_LAUNCH_CHECK();
|
||||
|
||||
return {out_distance, out_position_distance, out_rotation_distance,
|
||||
distance_p_vector, distance_q_vector, out_gidx};
|
||||
}
|
||||
|
||||
std::vector<torch::Tensor>
|
||||
backward_pose_distance(torch::Tensor out_grad_p, torch::Tensor out_grad_q,
|
||||
const torch::Tensor grad_distance, // batch_size, 3
|
||||
const torch::Tensor grad_p_distance, // n_boxes, 3
|
||||
const torch::Tensor grad_q_distance,
|
||||
const torch::Tensor pose_weight,
|
||||
const torch::Tensor grad_p_vec, // n_boxes, 4, 4
|
||||
const torch::Tensor grad_q_vec, const int batch_size,
|
||||
const bool use_distance = false) {
|
||||
|
||||
// we compute the warp threads based on number of boxes:
|
||||
|
||||
// TODO: verify this math
|
||||
// const int batch_size = grad_distance.size(0);
|
||||
using namespace Curobo::Pose;
|
||||
|
||||
int threadsPerBlock = batch_size;
|
||||
if (batch_size > 128) {
|
||||
threadsPerBlock = 128;
|
||||
}
|
||||
|
||||
// we fit warp thread spheres in a threadsPerBlock
|
||||
|
||||
int blocksPerGrid = (batch_size + threadsPerBlock - 1) / threadsPerBlock;
|
||||
|
||||
cudaStream_t stream = at::cuda::getCurrentCUDAStream();
|
||||
|
||||
if (use_distance) {
|
||||
|
||||
AT_DISPATCH_FLOATING_TYPES(
|
||||
grad_distance.scalar_type(), "backward_pose_distance", ([&] {
|
||||
backward_pose_distance_kernel<scalar_t>
|
||||
<<<blocksPerGrid, threadsPerBlock, 0, stream>>>(
|
||||
out_grad_p.data_ptr<scalar_t>(),
|
||||
out_grad_q.data_ptr<scalar_t>(),
|
||||
grad_distance.data_ptr<scalar_t>(),
|
||||
grad_p_distance.data_ptr<scalar_t>(),
|
||||
grad_q_distance.data_ptr<scalar_t>(),
|
||||
pose_weight.data_ptr<scalar_t>(),
|
||||
grad_p_vec.data_ptr<scalar_t>(),
|
||||
grad_q_vec.data_ptr<scalar_t>(), batch_size);
|
||||
}));
|
||||
} else {
|
||||
AT_DISPATCH_FLOATING_TYPES(
|
||||
grad_distance.scalar_type(), "backward_pose", ([&] {
|
||||
backward_pose_kernel<scalar_t>
|
||||
<<<blocksPerGrid, threadsPerBlock, 0, stream>>>(
|
||||
out_grad_p.data_ptr<scalar_t>(),
|
||||
out_grad_q.data_ptr<scalar_t>(),
|
||||
grad_distance.data_ptr<scalar_t>(),
|
||||
pose_weight.data_ptr<scalar_t>(),
|
||||
grad_p_vec.data_ptr<scalar_t>(),
|
||||
grad_q_vec.data_ptr<scalar_t>(), batch_size);
|
||||
}));
|
||||
}
|
||||
C10_CUDA_KERNEL_LAUNCH_CHECK();
|
||||
|
||||
return {out_grad_p, out_grad_q};
|
||||
}
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user