kinematics refactor with mimic joint support
This commit is contained in:
Balakumar Sundaralingam
@@ -324,7 +324,6 @@
|
||||
<axis xyz="0 -1 0"/>
|
||||
<dynamics damping="10.0"/>
|
||||
<limit effort="20" lower="0.0" upper="0.04" velocity="0.2"/>
|
||||
<mimic joint="panda_finger_joint1"/>
|
||||
</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,12 @@
|
||||
This is the README file from Robotiq's package for the 2F-140 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 140mm 2-Finger-Adaptive-Gripper
|
||||
|
||||
This package contains the URDF files describing the 140mm stroke gripper from robotiq, also known as series **C3**.
|
||||
|
||||
## Robot Visual
|
||||
|
||||
|
||||
## Robot Collision
|
||||
|
||||
@@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:043901d9c22b38d09b30d11326108ab3ef1445b4bd655ef313f94199f25f57ed
|
||||
size 7284
|
||||
@@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:d341d986aa8aca7262565c039c04fb6b4c0f2f5c93443519eb7ca9bfc67ba17c
|
||||
size 5484
|
||||
@@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:bd7da1b31b73f1aa1d61b26a582cea10f16f17396d54b8937890fa51547d26b0
|
||||
size 11684
|
||||
@@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:37c1779f71fb5504a5898048a36f9f4bfce0f5e7039ff1dce53808b95c229777
|
||||
size 9784
|
||||
@@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:111e37f13a664989dd54226f80f521b32ea0b71c975282a16696b14be7cc9249
|
||||
size 86384
|
||||
@@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:5ca9ffc28ed04193854b005358599dd9c3dc6fa92c8403e661fda94732d9ac25
|
||||
size 21184
|
||||
@@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:be21c5e18c901d4747cbc8fa1a2af15dad7173ff701482a8517e33278432bb21
|
||||
size 33984
|
||||
@@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:d61e9c5304d8015333856e4a26d99e32b695b6ada993253986b1afe8e396ab11
|
||||
size 43484
|
||||
@@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:666a92ee075f6f320ffb13b39995a5b374657cee90ded4c52c23ede20a812f34
|
||||
size 76084
|
||||
@@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:f042edc44ede9773e7cad00f7d7354d0b7c1c6a6353fe897b2ca2e6ac71107fc
|
||||
size 78384
|
||||
@@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:74a62de75ae10cf77c60f2c49749b5d11f4c265f8624bbe7697a941fa86f6b3b
|
||||
size 1054984
|
||||
@@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:4281e83002a25c20dc07c68b8d77da30a13e9a8401f157f6848ed8287d7cce44
|
||||
size 160684
|
||||
108
src/curobo/content/assets/robot/simple/simple_mimic_robot.urdf
Normal file
108
src/curobo/content/assets/robot/simple/simple_mimic_robot.urdf
Normal file
@@ -0,0 +1,108 @@
|
||||
<?xml version="1.0" ?>
|
||||
|
||||
<robot name="simple_closed_chain_robot">
|
||||
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.1 0.1 0.1"/>
|
||||
</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.01 0.01 0.01"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
|
||||
<joint name="chain_1_active_joint_1" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0 0 0.0"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="chain_1_link_1"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit effort="1000" lower="-6.0" upper="6.0" velocity="2.0"/>
|
||||
</joint>
|
||||
|
||||
|
||||
<link name="chain_1_link_1">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.35"/>
|
||||
<geometry>
|
||||
<box size="0.1 0.1 0.5"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.0 0.0 0.9 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.01 0.01 0.01"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
|
||||
<joint name="chain_1_mimic_joint_2" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0 0 0.6"/>
|
||||
<parent link="chain_1_link_1"/>
|
||||
<child link="chain_1_link_2"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit effort="1000" lower="-10.0" upper="10.0" velocity="5.0"/>
|
||||
<mimic joint="chain_1_active_joint_1" multiplier="-1.5" offset="0.5"/>
|
||||
</joint>
|
||||
|
||||
<link name="chain_1_link_2">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.35"/>
|
||||
<geometry>
|
||||
<box size="0.1 0.1 0.5"/>
|
||||
</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.01 0.01 0.01"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<link name="ee_link">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.2 0.2 0.1"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.0 0.9 0.0 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.01 0.01 0.01"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
|
||||
|
||||
<joint name="active_joint_2" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0 0 0.0"/>
|
||||
<parent link="chain_1_link_2"/>
|
||||
<child link="ee_link"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit effort="1000" lower="-6.0" upper="6.0" velocity="2.0"/>
|
||||
</joint>
|
||||
|
||||
|
||||
</robot>
|
||||
@@ -0,0 +1,691 @@
|
||||
<?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>
|
||||
<!-- This joint was added by Kinova -->
|
||||
<joint name="gripper_base_joint" type="fixed">
|
||||
<parent link="tool0"/>
|
||||
<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>
|
||||
<mesh filename="../kinova/kortex_description/grippers/robotiq_2f_140/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="../kinova/kortex_description/grippers/robotiq_2f_140/meshes/collision/robotiq_arg2f_base_link.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<link name="left_outer_knuckle">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="-0.000200000000003065 0.0199435877845359 0.0292245259211331"/>
|
||||
<mass value="0.00853198276973456"/>
|
||||
<inertia ixx="2.89328108496468E-06" ixy="-1.57935047237397E-19" ixz="-1.93980378593255E-19" iyy="1.86719750325683E-06" iyz="-1.21858577871576E-06" izz="1.21905238907251E-06"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../kinova/kortex_description/grippers/robotiq_2f_140/meshes/visual/robotiq_arg2f_140_outer_knuckle.stl" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../kinova/kortex_description/grippers/robotiq_2f_140/meshes/collision/robotiq_arg2f_140_outer_knuckle.stl" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="left_outer_finger">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.00030115855001899 0.0373907951953854 -0.0208027427000385"/>
|
||||
<mass value="0.022614240507152"/>
|
||||
<inertia ixx="1.52518312458174E-05" ixy="9.76583423954399E-10" ixz="-5.43838577022588E-10" iyy="6.17694243867776E-06" iyz="6.78636130740228E-06" izz="1.16494917907219E-05"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../kinova/kortex_description/grippers/robotiq_2f_140/meshes/visual/robotiq_arg2f_140_outer_finger.stl" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.1 0.1 0.1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../kinova/kortex_description/grippers/robotiq_2f_140/meshes/collision/robotiq_arg2f_140_outer_finger.stl" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="left_inner_finger">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.000299999999999317 0.0160078233491243 -0.0136945669206257"/>
|
||||
<mass value="0.0104003125914103"/>
|
||||
<inertia ixx="2.71909453810972E-06" ixy="1.35402465472579E-21" ixz="-7.1817349065269E-22" iyy="7.69100314106116E-07" iyz="6.74715432769696E-07" izz="2.30315190420171E-06"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../kinova/kortex_description/grippers/robotiq_2f_140/meshes/visual/robotiq_arg2f_140_inner_finger.stl" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.1 0.1 0.1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../kinova/kortex_description/grippers/robotiq_2f_140/meshes/collision/robotiq_arg2f_140_inner_finger.stl" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="left_inner_finger_pad">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.027 0.065 0.0075"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.9 0.9 0.9 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.027 0.065 0.0075"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.9 0.0 0.0 1"/>
|
||||
</material>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="left_inner_knuckle">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.000123011831763771 0.0507850843201817 0.00103968640075166"/>
|
||||
<mass value="0.0271177346495152"/>
|
||||
<inertia ixx="2.61910379223783E-05" ixy="-2.43616858946494E-07" ixz="-6.37789906117123E-09" iyy="2.8270243746167E-06" iyz="-5.37200748039765E-07" izz="2.83695868220296E-05"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../kinova/kortex_description/grippers/robotiq_2f_140/meshes/visual/robotiq_arg2f_140_inner_knuckle.stl" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.1 0.1 0.1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../kinova/kortex_description/grippers/robotiq_2f_140/meshes/collision/robotiq_arg2f_140_inner_knuckle.stl" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="right_outer_knuckle">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="-0.000200000000003065 0.0199435877845359 0.0292245259211331"/>
|
||||
<mass value="0.00853198276973456"/>
|
||||
<inertia ixx="2.89328108496468E-06" ixy="-1.57935047237397E-19" ixz="-1.93980378593255E-19" iyy="1.86719750325683E-06" iyz="-1.21858577871576E-06" izz="1.21905238907251E-06"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../kinova/kortex_description/grippers/robotiq_2f_140/meshes/visual/robotiq_arg2f_140_outer_knuckle.stl" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../kinova/kortex_description/grippers/robotiq_2f_140/meshes/collision/robotiq_arg2f_140_outer_knuckle.stl" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="right_outer_finger">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.00030115855001899 0.0373907951953854 -0.0208027427000385"/>
|
||||
<mass value="0.022614240507152"/>
|
||||
<inertia ixx="1.52518312458174E-05" ixy="9.76583423954399E-10" ixz="-5.43838577022588E-10" iyy="6.17694243867776E-06" iyz="6.78636130740228E-06" izz="1.16494917907219E-05"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../kinova/kortex_description/grippers/robotiq_2f_140/meshes/visual/robotiq_arg2f_140_outer_finger.stl" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.1 0.1 0.1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../kinova/kortex_description/grippers/robotiq_2f_140/meshes/collision/robotiq_arg2f_140_outer_finger.stl" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="right_inner_finger">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.000299999999999317 0.0160078233491243 -0.0136945669206257"/>
|
||||
<mass value="0.0104003125914103"/>
|
||||
<inertia ixx="2.71909453810972E-06" ixy="1.35402465472579E-21" ixz="-7.1817349065269E-22" iyy="7.69100314106116E-07" iyz="6.74715432769696E-07" izz="2.30315190420171E-06"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../kinova/kortex_description/grippers/robotiq_2f_140/meshes/visual/robotiq_arg2f_140_inner_finger.stl" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.1 0.1 0.1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../kinova/kortex_description/grippers/robotiq_2f_140/meshes/collision/robotiq_arg2f_140_inner_finger.stl" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="right_inner_finger_pad">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.027 0.065 0.0075"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.9 0.9 0.9 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.027 0.065 0.0075"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.9 0.0 0.0 1"/>
|
||||
</material>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="right_inner_knuckle">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.000123011831763771 0.0507850843201817 0.00103968640075166"/>
|
||||
<mass value="0.0271177346495152"/>
|
||||
<inertia ixx="2.61910379223783E-05" ixy="-2.43616858946494E-07" ixz="-6.37789906117123E-09" iyy="2.8270243746167E-06" iyz="-5.37200748039765E-07" izz="2.83695868220296E-05"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../kinova/kortex_description/grippers/robotiq_2f_140/meshes/visual/robotiq_arg2f_140_inner_knuckle.stl" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.1 0.1 0.1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../kinova/kortex_description/grippers/robotiq_2f_140/meshes/collision/robotiq_arg2f_140_inner_knuckle.stl" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="finger_joint" type="revolute">
|
||||
<origin rpy="2.29579632679 0 0" xyz="0 -0.030601 0.054905"/>
|
||||
<parent link="robotiq_arg2f_base_link"/>
|
||||
<child link="left_outer_knuckle"/>
|
||||
<axis xyz="-1 0 0"/>
|
||||
<limit effort="1000" lower="0.0" upper="0.7" velocity="2.0"/>
|
||||
</joint>
|
||||
<joint name="left_outer_finger_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0.01821998610742 0.0260018192872234"/>
|
||||
<parent link="left_outer_knuckle"/>
|
||||
<child link="left_outer_finger"/>
|
||||
|
||||
</joint>
|
||||
<joint name="left_inner_knuckle_joint" type="revolute">
|
||||
<origin rpy="2.29579632679 0 0.0" xyz="0 -0.0127 0.06142"/>
|
||||
<parent link="robotiq_arg2f_base_link"/>
|
||||
<child link="left_inner_knuckle"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit lower="-0.8757" upper="0.8757" velocity="2.0" effort="1000" />
|
||||
<mimic joint="finger_joint" multiplier="-1" offset="0" />
|
||||
|
||||
</joint>
|
||||
<joint name="left_inner_finger_joint" type="revolute">
|
||||
<origin rpy="-0.725 0 0" xyz="0 0.0817554015893473 -0.0282203446692936"/>
|
||||
<parent link="left_outer_finger"/>
|
||||
<child link="left_inner_finger"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit effort="1000" lower="-0.8757" upper="0.8757" velocity="2.0"/>
|
||||
<mimic joint="finger_joint" multiplier="1" offset="0" />
|
||||
|
||||
</joint>
|
||||
<joint name="left_inner_finger_pad_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0.0457554015893473 -0.0272203446692936"/>
|
||||
<parent link="left_inner_finger"/>
|
||||
<child link="left_inner_finger_pad"/>
|
||||
</joint>
|
||||
<joint name="right_outer_knuckle_joint" type="revolute">
|
||||
<origin rpy="2.29579632679 0 3.14159265359" xyz="0 0.030601 0.054905"/>
|
||||
<parent link="robotiq_arg2f_base_link"/>
|
||||
<child link="right_outer_knuckle"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit effort="1000" lower="-0.8757" upper="0.8757" velocity="2.0"/>
|
||||
<mimic joint="finger_joint" multiplier="-1" offset="0" />
|
||||
|
||||
</joint>
|
||||
<joint name="right_outer_finger_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0.01821998610742 0.0260018192872234"/>
|
||||
<parent link="right_outer_knuckle"/>
|
||||
<child link="right_outer_finger"/>
|
||||
</joint>
|
||||
<joint name="right_inner_knuckle_joint" type="revolute">
|
||||
<origin rpy="2.29579632679 0 -3.14159265359" xyz="0 0.0127 0.06142"/>
|
||||
<parent link="robotiq_arg2f_base_link"/>
|
||||
<child link="right_inner_knuckle"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit effort="1000" lower="-0.8757" upper="0.8757" velocity="2.0"/>
|
||||
<mimic joint="finger_joint" multiplier="-1" offset="0" />
|
||||
|
||||
</joint>
|
||||
<joint name="right_inner_finger_joint" type="revolute">
|
||||
<origin rpy="-0.725 0 0" xyz="0 0.0817554015893473 -0.0282203446692936"/>
|
||||
<parent link="right_outer_finger"/>
|
||||
<child link="right_inner_finger"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit effort="1000" lower="-0.8757" upper="0.8757" velocity="2.0"/>
|
||||
<mimic joint="finger_joint" multiplier="1" offset="0" />
|
||||
</joint>
|
||||
<joint name="right_inner_finger_pad_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0.0457554015893473 -0.0272203446692936"/>
|
||||
<parent link="right_inner_finger"/>
|
||||
<child link="right_inner_finger_pad"/>
|
||||
</joint>
|
||||
<link name="grasp_frame">
|
||||
<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="grasp_joint" type="fixed">
|
||||
<origin rpy="0 0 0.0" xyz="0 0 0.2"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<parent link="robotiq_arg2f_base_link"/>
|
||||
<child link="grasp_frame"/>
|
||||
</joint>
|
||||
</robot>
|
||||
47
src/curobo/content/configs/robot/simple_mimic_robot.yml
Normal file
47
src/curobo/content/configs/robot/simple_mimic_robot.yml
Normal file
@@ -0,0 +1,47 @@
|
||||
##
|
||||
## 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: null
|
||||
usd_path: "robot/simple/three_link_mimic.usda"
|
||||
usd_robot_root: "/base_link"
|
||||
urdf_path: "robot/simple/simple_mimic_robot.urdf"
|
||||
asset_root_path: "robot/simple"
|
||||
base_link: "base_link"
|
||||
ee_link: "ee_link"
|
||||
collision_link_names: null
|
||||
collision_spheres: null
|
||||
collision_sphere_buffer: 0.004 # 0.0025
|
||||
extra_collision_spheres: null
|
||||
use_global_cumul: True
|
||||
self_collision_ignore: null
|
||||
|
||||
|
||||
self_collision_buffer: null
|
||||
mesh_link_names:
|
||||
[
|
||||
"base_link",
|
||||
"chain_1_link_1",
|
||||
"chain_1_link_2",
|
||||
"ee_link",
|
||||
|
||||
]
|
||||
lock_joints: {"chain_1_active_joint_1": 0.2}
|
||||
extra_links: null
|
||||
cspace:
|
||||
joint_names: ["chain_1_active_joint_1", "active_joint_2"]
|
||||
retract_config: [0.3, 0.0]
|
||||
null_space_weight: [1,1]
|
||||
cspace_distance_weight: [1,1]
|
||||
max_acceleration: 15.0
|
||||
max_jerk: 500.0
|
||||
265
src/curobo/content/configs/robot/ur5e_robotiq_2f_140.yml
Normal file
265
src/curobo/content/configs/robot/ur5e_robotiq_2f_140.yml
Normal file
@@ -0,0 +1,265 @@
|
||||
##
|
||||
## 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_robotiq_2f_140.usd"
|
||||
usd_robot_root: "/robot"
|
||||
isaac_usd_path: ""
|
||||
usd_flip_joints: {}
|
||||
usd_flip_joint_limits: []
|
||||
|
||||
urdf_path: "robot/ur_description/ur5e_robotiq_2f_140.urdf"
|
||||
asset_root_path: "robot/ur_description"
|
||||
|
||||
base_link: "base_link"
|
||||
ee_link: "grasp_frame"
|
||||
link_names: null
|
||||
lock_joints: {'finger_joint': 0.7}
|
||||
|
||||
extra_links: null #{"attached_object":{"parent_link_name": "grasp_frame" ,
|
||||
#"link_name": "attached_object", "fixed_transform": [0,0,0,1,0,0,0], "joint_type":"FIXED",
|
||||
#"joint_name": "attach_joint" }}
|
||||
extra_collision_spheres: null #{"attached_object": 4}
|
||||
|
||||
collision_link_names: ['shoulder_link','upper_arm_link', 'forearm_link', 'wrist_1_link',
|
||||
'wrist_2_link' ,'wrist_3_link', 'tool0', 'robotiq_arg2f_base_link',
|
||||
'left_outer_knuckle',
|
||||
'left_inner_knuckle',
|
||||
'left_outer_finger',
|
||||
'left_inner_finger',
|
||||
'left_inner_finger_pad',
|
||||
'right_outer_knuckle',
|
||||
'right_inner_knuckle',
|
||||
'right_outer_finger' ,
|
||||
'right_inner_finger',
|
||||
'right_inner_finger_pad',
|
||||
] # 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.08
|
||||
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
|
||||
tool0:
|
||||
- "center": [0.001, 0.001, 0.05]
|
||||
"radius": -0.01 #0.05
|
||||
robotiq_arg2f_base_link:
|
||||
- "center": [0.0, -0.0, 0.04]
|
||||
"radius": 0.035
|
||||
- "center": [0.0, -0.0, 0.02]
|
||||
"radius": 0.035
|
||||
left_outer_finger:
|
||||
- "center": [0.0, -0.01, 0.0]
|
||||
"radius": 0.01
|
||||
- "center": [0.0, 0.015, -0.01]
|
||||
"radius": 0.01
|
||||
|
||||
left_inner_finger_pad:
|
||||
- "center": [0.0, -0.0, 0.005]
|
||||
"radius": 0.01
|
||||
- "center": [0.0, 0.02, 0.005]
|
||||
"radius": 0.01
|
||||
- "center": [0.0, -0.02, 0.005]
|
||||
"radius": 0.01
|
||||
left_inner_knuckle:
|
||||
- "center": [0.0, 0.04, -0.0]
|
||||
"radius": 0.01
|
||||
- "center": [0.0, 0.06, -0.0]
|
||||
"radius": 0.01
|
||||
- "center": [0.0, 0.08, -0.0]
|
||||
"radius": 0.01
|
||||
left_inner_finger:
|
||||
- "center": [0.0, -0.0, -0.0]
|
||||
"radius": 0.01
|
||||
|
||||
left_outer_knuckle:
|
||||
- "center": [0.0, 0.055, 0.01]
|
||||
"radius": 0.01
|
||||
- "center": [0.0, 0.08, 0.005]
|
||||
"radius": 0.01
|
||||
|
||||
|
||||
right_outer_finger:
|
||||
- "center": [0.0, -0.01, 0.0]
|
||||
"radius": 0.01
|
||||
- "center": [0.0, 0.015, -0.01]
|
||||
"radius": 0.01
|
||||
|
||||
right_inner_finger_pad:
|
||||
- "center": [0.0, -0.0, 0.005]
|
||||
"radius": 0.01
|
||||
- "center": [0.0, 0.02, 0.005]
|
||||
"radius": 0.01
|
||||
- "center": [0.0, -0.02, 0.005]
|
||||
"radius": 0.01
|
||||
right_inner_knuckle:
|
||||
- "center": [0.0, 0.04, -0.0]
|
||||
"radius": 0.01
|
||||
- "center": [0.0, 0.06, -0.0]
|
||||
"radius": 0.01
|
||||
- "center": [0.0, 0.08, -0.0]
|
||||
"radius": 0.01
|
||||
right_inner_finger:
|
||||
- "center": [0.0, -0.0, -0.0]
|
||||
"radius": 0.01
|
||||
|
||||
right_outer_knuckle:
|
||||
- "center": [0.0, 0.055, 0.01]
|
||||
"radius": 0.01
|
||||
- "center": [0.0, 0.08, 0.005]
|
||||
"radius": 0.01
|
||||
|
||||
|
||||
collision_sphere_buffer: 0.005
|
||||
|
||||
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","tool0", "robotiq_arg2f_base_link"],
|
||||
"wrist_2_link": ["wrist_3_link", "tool0", "robotiq_arg2f_base_link"],
|
||||
"wrist_3_link": ["tool0", "robotiq_arg2f_base_link"],
|
||||
|
||||
"tool0": ['robotiq_arg2f_base_link', 'left_outer_finger', 'left_inner_finger_pad',
|
||||
'left_inner_knuckle', 'left_inner_finger', 'left_outer_knuckle',
|
||||
'right_outer_finger', 'right_inner_finger_pad',
|
||||
'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle'],
|
||||
|
||||
"robotiq_arg2f_base_link": ['left_outer_finger', 'left_inner_finger_pad',
|
||||
'left_inner_knuckle', 'left_inner_finger', 'left_outer_knuckle',
|
||||
'right_outer_finger', 'right_inner_finger_pad',
|
||||
'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle'],
|
||||
|
||||
"left_outer_finger": ['left_inner_finger_pad',
|
||||
'left_inner_knuckle', 'left_inner_finger', 'left_outer_knuckle',
|
||||
'right_outer_finger', 'right_inner_finger_pad',
|
||||
'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle'],
|
||||
|
||||
"left_inner_finger_pad": [
|
||||
'left_inner_knuckle', 'left_inner_finger', 'left_outer_knuckle',
|
||||
'right_outer_finger', 'right_inner_finger_pad',
|
||||
'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle'],
|
||||
|
||||
"left_inner_knuckle": ['left_inner_finger', 'left_outer_knuckle',
|
||||
'right_outer_finger', 'right_inner_finger_pad',
|
||||
'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle'],
|
||||
|
||||
"left_inner_finger": ['left_outer_knuckle',
|
||||
'right_outer_finger', 'right_inner_finger_pad',
|
||||
'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle'],
|
||||
|
||||
"left_outer_knuckle": [
|
||||
'right_outer_finger', 'right_inner_finger_pad',
|
||||
'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle'],
|
||||
|
||||
|
||||
|
||||
"right_outer_finger": ['right_inner_finger_pad',
|
||||
'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle'],
|
||||
|
||||
"right_inner_finger_pad": [
|
||||
'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle'],
|
||||
|
||||
"right_inner_knuckle": [ 'right_inner_finger', 'right_outer_knuckle'],
|
||||
|
||||
"right_inner_finger": [ 'right_outer_knuckle'],
|
||||
|
||||
|
||||
|
||||
}
|
||||
self_collision_buffer: {
|
||||
'shoulder_link': 0.01,
|
||||
'upper_arm_link': 0,
|
||||
'forearm_link': 0,
|
||||
'wrist_1_link': 0,
|
||||
'wrist_2_link': 0,
|
||||
'wrist_3_link' : 0,
|
||||
'tool0': 0.02,
|
||||
}
|
||||
|
||||
use_global_cumul: True
|
||||
mesh_link_names: ['shoulder_link','upper_arm_link', 'forearm_link', 'wrist_1_link',
|
||||
'wrist_2_link' ,'wrist_3_link', 'robotiq_arg2f_base_link',
|
||||
'left_outer_knuckle',
|
||||
'left_inner_knuckle',
|
||||
'left_outer_finger',
|
||||
'left_inner_finger',
|
||||
'right_outer_knuckle',
|
||||
'right_inner_knuckle',
|
||||
'right_outer_finger' ,
|
||||
'right_inner_finger',
|
||||
] # List[str]
|
||||
|
||||
cspace:
|
||||
joint_names: ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint',
|
||||
'wrist_2_joint', 'wrist_3_joint',
|
||||
'finger_joint']
|
||||
retract_config: [0.0, -2.2, 1.0, -1.383, -1.57, 0.00, 0.6]
|
||||
null_space_weight: [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]
|
||||
max_jerk: 500.0
|
||||
max_acceleration: 12.0
|
||||
position_limit_clip: 0.0
|
||||
@@ -28,14 +28,16 @@ model:
|
||||
cost:
|
||||
pose_cfg:
|
||||
vec_weight: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
|
||||
weight: [1000, 20000, 30, 50]
|
||||
weight: [200,4000,10,20]
|
||||
#weight: [5000,500000,5,20]
|
||||
|
||||
vec_convergence: [0.0, 0.00]
|
||||
terminal: False
|
||||
use_metric: True
|
||||
run_weight: 1.0
|
||||
link_pose_cfg:
|
||||
vec_weight: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
|
||||
weight: [1000, 20000, 30, 50]
|
||||
weight: [200,4000,10,20]
|
||||
vec_convergence: [0.00, 0.000]
|
||||
terminal: False
|
||||
use_metric: True
|
||||
@@ -57,15 +59,15 @@ cost:
|
||||
|
||||
|
||||
lbfgs:
|
||||
n_iters: 80 #60
|
||||
inner_iters: 20
|
||||
n_iters: 100 #60
|
||||
inner_iters: 25
|
||||
cold_start_n_iters: null
|
||||
min_iters: 20
|
||||
min_iters: 25
|
||||
line_search_scale: [0.1, 0.3, 0.7, 1.0]
|
||||
fixed_iters: True
|
||||
cost_convergence: 1e-7
|
||||
cost_delta_threshold: 1e-6 #0.0001
|
||||
cost_relative_threshold: 1.0
|
||||
cost_convergence: 0.001
|
||||
cost_delta_threshold: 1.0 #0.0001
|
||||
cost_relative_threshold: 0.999
|
||||
epsilon: 0.01 #0.01 # used only in stable_mode
|
||||
history: 6
|
||||
horizon: 1
|
||||
@@ -73,6 +75,7 @@ lbfgs:
|
||||
n_problems: 1
|
||||
store_debug: False
|
||||
use_cuda_kernel: True
|
||||
use_shared_buffers_kernel: True
|
||||
stable_mode: True
|
||||
line_search_type: "approx_wolfe" #"wolfe"
|
||||
use_cuda_line_search_kernel: True
|
||||
|
||||
@@ -32,7 +32,8 @@ 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: [0.00,0.00,0.00,0.0,0.0,0.0] # running weight orientation, position
|
||||
weight: [2000,50000.0,30,60] #[150.0, 2000.0, 30, 40]
|
||||
weight: [2000,50000.0,30,50] #[150.0, 2000.0, 30, 40]
|
||||
|
||||
vec_convergence: [0.0,0.0] # orientation, position, orientation metric activation, position metric activation
|
||||
terminal: True
|
||||
run_weight: 1.0
|
||||
@@ -62,7 +63,7 @@ cost:
|
||||
null_space_weight: [0.0]
|
||||
|
||||
primitive_collision_cfg:
|
||||
weight: 100000.0
|
||||
weight: 100000
|
||||
use_sweep: True
|
||||
sweep_steps: 4
|
||||
classify: False
|
||||
|
||||
@@ -34,7 +34,7 @@ cost:
|
||||
run_weight: 1.0
|
||||
link_pose_cfg:
|
||||
vec_weight: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
|
||||
weight: [0, 50, 10, 10] #[20.0, 100.0]
|
||||
weight: [30, 50, 10, 10] #[20.0, 100.0]
|
||||
vec_convergence: [0.00, 0.000] # orientation, position
|
||||
terminal: False
|
||||
use_metric: True
|
||||
|
||||
@@ -314,6 +314,7 @@ class CudaRobotGenerator(CudaRobotGeneratorConfig):
|
||||
link_map=self._link_map,
|
||||
joint_map=self._joint_map,
|
||||
joint_map_type=self._joint_map_type,
|
||||
joint_offset_map=self._joint_offset_map,
|
||||
store_link_map=self._store_link_map,
|
||||
link_chain_map=self._link_chain_map,
|
||||
link_names=self.link_names,
|
||||
@@ -332,6 +333,7 @@ class CudaRobotGenerator(CudaRobotGeneratorConfig):
|
||||
base_link=self.base_link,
|
||||
ee_link=self.ee_link,
|
||||
lock_jointstate=self.lock_jointstate,
|
||||
mimic_joints=self._mimic_joint_data,
|
||||
)
|
||||
if self.asset_root_path != "":
|
||||
self._kinematics_parser.add_absolute_path_to_link_meshes(self.asset_root_path)
|
||||
@@ -368,9 +370,8 @@ class CudaRobotGenerator(CudaRobotGeneratorConfig):
|
||||
@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._controlled_links = []
|
||||
self._bodies = []
|
||||
|
||||
self._name_to_idx_map = dict()
|
||||
self.base_link = base_link
|
||||
self.ee_link = ee_link
|
||||
@@ -403,37 +404,61 @@ class CudaRobotGenerator(CudaRobotGeneratorConfig):
|
||||
self.non_fixed_joint_names = self.joint_names.copy()
|
||||
return chain_link_names
|
||||
|
||||
def _get_mimic_joint_data(self):
|
||||
# get joint types:
|
||||
mimic_joint_data = {}
|
||||
for i in range(1, len(self._bodies)):
|
||||
body = self._bodies[i]
|
||||
if i in self._controlled_links:
|
||||
if body.mimic_joint_name is not None:
|
||||
if body.joint_name not in mimic_joint_data:
|
||||
mimic_joint_data[body.joint_name] = []
|
||||
mimic_joint_data[body.joint_name].append(i)
|
||||
return mimic_joint_data
|
||||
|
||||
@profiler.record_function("robot_generator/build_kinematics_tensors")
|
||||
def _build_kinematics_tensors(self, base_link, ee_link, link_names, chain_link_names):
|
||||
self._active_joints = []
|
||||
self._mimic_joint_data = {}
|
||||
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))
|
||||
-1 if i not in self._controlled_links 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))
|
||||
-1 if i not in self._controlled_links else i for i in range(len(self._bodies))
|
||||
]
|
||||
all_joint_names = []
|
||||
j_count = 0
|
||||
ordered_link_names = []
|
||||
joint_offset_map = [[1.0, 0.0]]
|
||||
# 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)
|
||||
joint_offset_map.append(self._bodies[0].joint_offset)
|
||||
# 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)
|
||||
joint_offset_map.append(body.joint_offset)
|
||||
joint_map_type[i] = body.joint_type.value
|
||||
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
|
||||
if body.joint_name not in all_joint_names:
|
||||
all_joint_names.append(body.joint_name)
|
||||
if i in self._controlled_links:
|
||||
joint_map[i] = self.joint_names.index(body.joint_name)
|
||||
if body.mimic_joint_name is not None:
|
||||
if body.joint_name not in self._mimic_joint_data:
|
||||
self._mimic_joint_data[body.joint_name] = []
|
||||
self._mimic_joint_data[body.joint_name].append(
|
||||
{"joint_offset": body.joint_offset, "joint_name": body.mimic_joint_name}
|
||||
)
|
||||
else:
|
||||
self._active_joints.append(i)
|
||||
self.link_names = ordered_link_names
|
||||
# do a for loop to get link matrix:
|
||||
link_chain_map = torch.eye(
|
||||
@@ -458,6 +483,10 @@ class CudaRobotGenerator(CudaRobotGeneratorConfig):
|
||||
self._store_link_map = torch.as_tensor(
|
||||
store_link_map, device=self.tensor_args.device, dtype=torch.int16
|
||||
)
|
||||
self._joint_offset_map = torch.as_tensor(
|
||||
joint_offset_map, device=self.tensor_args.device, dtype=torch.float32
|
||||
)
|
||||
self._joint_offset_map = self._joint_offset_map.view(-1).contiguous()
|
||||
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
|
||||
@@ -489,12 +518,18 @@ class CudaRobotGenerator(CudaRobotGeneratorConfig):
|
||||
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()]
|
||||
)
|
||||
[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
|
||||
|
||||
for k in lock_joint_names:
|
||||
if "mimic" in joint_data[k]:
|
||||
mimic_link_names = [[x["parent"], x["child"]] for x in joint_data[k]["mimic"]]
|
||||
mimic_link_names = [x for xs in mimic_link_names for x in xs]
|
||||
lock_links += mimic_link_names
|
||||
lock_links = list(set(lock_links))
|
||||
|
||||
new_link_names = list(set(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)
|
||||
@@ -520,6 +555,7 @@ class CudaRobotGenerator(CudaRobotGeneratorConfig):
|
||||
link_map=self._link_map,
|
||||
joint_map=self._joint_map,
|
||||
joint_map_type=self._joint_map_type,
|
||||
joint_offset_map=self._joint_offset_map,
|
||||
store_link_map=self._store_link_map,
|
||||
link_chain_map=self._link_chain_map,
|
||||
link_names=self.link_names,
|
||||
@@ -531,14 +567,12 @@ class CudaRobotGenerator(CudaRobotGeneratorConfig):
|
||||
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:
|
||||
@@ -554,14 +588,33 @@ class CudaRobotGenerator(CudaRobotGeneratorConfig):
|
||||
.multiply(link_poses.get_index(0, w_child))
|
||||
)
|
||||
# Make this joint as fixed
|
||||
i = self._all_joint_names.index(j) + 1
|
||||
i = joint_data[j]["link_index"]
|
||||
self._fixed_transform[i] = parent_t_child.get_matrix()
|
||||
|
||||
if "mimic" in joint_data[j]:
|
||||
for mimic_joint in joint_data[j]["mimic"]:
|
||||
w_parent = lock_links.index(mimic_joint["parent"])
|
||||
w_child = lock_links.index(mimic_joint["child"])
|
||||
parent_t_child = (
|
||||
link_poses.get_index(0, w_parent)
|
||||
.inverse()
|
||||
.multiply(link_poses.get_index(0, w_child))
|
||||
)
|
||||
i_q = mimic_joint["link_index"]
|
||||
self._fixed_transform[i_q] = parent_t_child.get_matrix()
|
||||
self._controlled_links.remove(i_q)
|
||||
self._joint_map_type[i_q] = -1
|
||||
self._joint_map[i_q] = -1
|
||||
|
||||
i = joint_data[j]["link_index"]
|
||||
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._controlled_links.remove(i)
|
||||
self.joint_names.remove(j)
|
||||
self._n_dofs -= 1
|
||||
self._active_joints.remove(i)
|
||||
|
||||
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)
|
||||
@@ -715,7 +768,7 @@ class CudaRobotGenerator(CudaRobotGeneratorConfig):
|
||||
if not valid_data:
|
||||
break
|
||||
if torch.max(coll_cpu[i]) == -torch.inf:
|
||||
print("skip", i)
|
||||
log_info("skip" + str(i))
|
||||
for j in range(i + 1, n_spheres):
|
||||
if sl_idx > thread_loc.shape[0] - 1:
|
||||
valid_data = False
|
||||
@@ -776,9 +829,10 @@ class CudaRobotGenerator(CudaRobotGeneratorConfig):
|
||||
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._controlled_links.append(body_idx)
|
||||
if rigid_body_params.joint_name not in self.joint_names:
|
||||
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,
|
||||
@@ -790,21 +844,34 @@ class CudaRobotGenerator(CudaRobotGeneratorConfig):
|
||||
|
||||
def _get_joint_links(self, joint_names: List[str]):
|
||||
j_data = {}
|
||||
|
||||
for j in joint_names:
|
||||
for b in self._bodies:
|
||||
for bi, b in enumerate(self._bodies):
|
||||
if b.joint_name == j:
|
||||
j_data[j] = {"parent": b.parent_link_name, "child": b.link_name}
|
||||
if j not in j_data:
|
||||
j_data[j] = {}
|
||||
if b.mimic_joint_name is None:
|
||||
j_data[j]["parent"] = b.parent_link_name
|
||||
j_data[j]["child"] = b.link_name
|
||||
j_data[j]["link_index"] = bi
|
||||
else:
|
||||
if "mimic" not in j_data[j]:
|
||||
j_data[j]["mimic"] = []
|
||||
j_data[j]["mimic"].append(
|
||||
{
|
||||
"parent": b.parent_link_name,
|
||||
"child": b.link_name,
|
||||
"link_index": bi,
|
||||
"joint_offset": b.joint_offset,
|
||||
}
|
||||
)
|
||||
|
||||
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,
|
||||
@@ -831,12 +898,11 @@ class CudaRobotGenerator(CudaRobotGeneratorConfig):
|
||||
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,
|
||||
q,
|
||||
kinematics_config.fixed_transforms,
|
||||
kinematics_config.link_spheres,
|
||||
kinematics_config.link_map, # tells which link is attached to which link i
|
||||
@@ -845,6 +911,7 @@ class CudaRobotGenerator(CudaRobotGeneratorConfig):
|
||||
kinematics_config.store_link_map,
|
||||
kinematics_config.link_sphere_idx_map, # sphere idx map
|
||||
kinematics_config.link_chain_map,
|
||||
kinematics_config.joint_offset_map,
|
||||
grad_out_q,
|
||||
self.use_global_cumul,
|
||||
)
|
||||
@@ -873,7 +940,7 @@ class CudaRobotGenerator(CudaRobotGeneratorConfig):
|
||||
def _get_joint_position_velocity_limits(self):
|
||||
joint_limits = {"position": [[], []], "velocity": [[], []]}
|
||||
|
||||
for idx in self._controlled_joints:
|
||||
for idx in self._active_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])
|
||||
@@ -897,7 +964,7 @@ class CudaRobotGenerator(CudaRobotGeneratorConfig):
|
||||
]
|
||||
)
|
||||
# clip joint position:
|
||||
# TODO: change this to be per joint
|
||||
# NOTE: change this to be per joint
|
||||
joint_limits["position"][0] += self.cspace.position_limit_clip
|
||||
joint_limits["position"][1] -= self.cspace.position_limit_clip
|
||||
joint_limits["velocity"][0] *= self.cspace.velocity_scale
|
||||
|
||||
@@ -33,6 +33,7 @@ from curobo.curobolib.kinematics import get_cuda_kinematics
|
||||
from curobo.geom.types import Mesh, 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
|
||||
from curobo.util_file import get_robot_path, join_path, load_yaml
|
||||
|
||||
@@ -322,7 +323,6 @@ class CudaRobotModel(CudaRobotModelConfig):
|
||||
|
||||
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,
|
||||
@@ -336,11 +336,10 @@ class CudaRobotModel(CudaRobotModelConfig):
|
||||
self.kinematics_config.store_link_map,
|
||||
self.kinematics_config.link_sphere_idx_map, # sphere idx map
|
||||
self.kinematics_config.link_chain_map,
|
||||
self.kinematics_config.joint_offset_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
|
||||
@@ -381,6 +380,31 @@ class CudaRobotModel(CudaRobotModelConfig):
|
||||
def lock_jointstate(self):
|
||||
return self.kinematics_config.lock_jointstate
|
||||
|
||||
def get_full_js(self, js: JointState):
|
||||
all_joint_names = self.all_articulated_joint_names
|
||||
lock_joint_state = self.lock_jointstate
|
||||
|
||||
new_js = js.get_augmented_joint_state(all_joint_names, lock_joint_state)
|
||||
return new_js
|
||||
|
||||
def get_mimic_js(self, js: JointState):
|
||||
if self.kinematics_config.mimic_joints is None:
|
||||
return None
|
||||
extra_joints = {"position": [], "joint_names": []}
|
||||
# for every joint in mimic_joints, get active joint name
|
||||
for j in self.kinematics_config.mimic_joints:
|
||||
active_q = js.position[..., js.joint_names.index(j)]
|
||||
for k in self.kinematics_config.mimic_joints[j]:
|
||||
extra_joints["joint_names"].append(k["joint_name"])
|
||||
extra_joints["position"].append(
|
||||
k["joint_offset"][0] * active_q + k["joint_offset"][1]
|
||||
)
|
||||
extra_js = JointState.from_position(
|
||||
position=torch.stack(extra_joints["position"]), joint_names=extra_joints["joint_names"]
|
||||
)
|
||||
new_js = js.get_augmented_joint_state(js.joint_names + extra_js.joint_names, extra_js)
|
||||
return new_js
|
||||
|
||||
@property
|
||||
def ee_link(self):
|
||||
return self.kinematics_config.ee_link
|
||||
|
||||
@@ -11,7 +11,7 @@
|
||||
|
||||
# Standard Library
|
||||
from dataclasses import dataclass, field
|
||||
from typing import Dict, List, Optional, Union
|
||||
from typing import Dict, List, Optional
|
||||
|
||||
# Third Party
|
||||
import numpy as np
|
||||
@@ -21,7 +21,6 @@ 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
|
||||
@@ -35,6 +34,8 @@ class LinkParams:
|
||||
joint_axis: Optional[np.ndarray] = None
|
||||
joint_id: Optional[int] = None
|
||||
joint_velocity_limits: List[float] = field(default_factory=lambda: [-2.0, 2.0])
|
||||
joint_offset: List[float] = field(default_factory=lambda: [1.0, 0.0])
|
||||
mimic_joint_name: Optional[str] = None
|
||||
|
||||
@staticmethod
|
||||
def from_dict(dict_data):
|
||||
@@ -56,7 +57,7 @@ class KinematicsParser:
|
||||
# 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
|
||||
self._parent_map[i] = {"parent": 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.
|
||||
@@ -78,7 +79,7 @@ class KinematicsParser:
|
||||
chain_links = [ee_link]
|
||||
link = ee_link
|
||||
while link != base_link:
|
||||
link = self._parent_map[link]
|
||||
link = self._parent_map[link]["parent"]
|
||||
# add link to chain:
|
||||
chain_links.append(link)
|
||||
chain_links.reverse()
|
||||
|
||||
@@ -228,28 +228,87 @@ class CSpaceConfig:
|
||||
|
||||
@dataclass
|
||||
class KinematicsTensorConfig:
|
||||
"""Stores robot's kinematics parameters as Tensors to use in Kinematics computations.
|
||||
|
||||
Use :meth:`curobo.cuda_robot_model.cuda_robot_generator.CudaRobotGenerator` to generate this
|
||||
configuration from a urdf or usd.
|
||||
|
||||
"""
|
||||
|
||||
#: Static Homogenous Transform from parent link to child link for all links [n_links,4,4].
|
||||
fixed_transforms: torch.Tensor
|
||||
|
||||
#: index of fixed_transform given link index [n_links].
|
||||
link_map: torch.Tensor
|
||||
|
||||
#: joint index given link index [n_links].
|
||||
joint_map: torch.Tensor
|
||||
|
||||
#: type of joint given link index [n_links].
|
||||
joint_map_type: torch.Tensor
|
||||
|
||||
joint_offset_map: torch.Tensor
|
||||
|
||||
#: index of link to write out pose [n_store_links].
|
||||
store_link_map: torch.Tensor
|
||||
|
||||
#: Mapping between each link to every other link, this is used to check
|
||||
#: if a link is part of a serial chain formed by another link [n_links, n_links].
|
||||
link_chain_map: torch.Tensor
|
||||
|
||||
#: Name of links whose pose will be stored [n_store_links].
|
||||
link_names: List[str]
|
||||
|
||||
#: Joint limits
|
||||
joint_limits: JointLimits
|
||||
|
||||
#: Name of joints which are not fixed.
|
||||
non_fixed_joint_names: List[str]
|
||||
|
||||
#: Number of joints that are active. Each joint is only actuated along 1 dimension.
|
||||
n_dof: int
|
||||
|
||||
#: Name of links which have a mesh. Currently only used for debugging and rendering.
|
||||
mesh_link_names: Optional[List[str]] = None
|
||||
|
||||
#: Name of all actuated joints.
|
||||
joint_names: Optional[List[str]] = None
|
||||
|
||||
#:
|
||||
lock_jointstate: Optional[JointState] = None
|
||||
|
||||
#:
|
||||
mimic_joints: Optional[dict] = 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 number of spheres that represent the robot's geometry.
|
||||
total_spheres: int = 0
|
||||
|
||||
#: Additional debug parameters.
|
||||
debug: Optional[Any] = None
|
||||
|
||||
#: index of end-effector in stored link poses.
|
||||
ee_idx: int = 0
|
||||
|
||||
#: Cspace configuration
|
||||
cspace: Optional[CSpaceConfig] = None
|
||||
|
||||
#: Name of base link. This is the root link from which all kinematic parameters were computed.
|
||||
base_link: str = "base_link"
|
||||
|
||||
#: Name of end-effector link for which the Cartesian pose will be computed.
|
||||
ee_link: str = "ee_link"
|
||||
|
||||
#: A copy of link spheres that is used as reference, in case the link_spheres get modified at
|
||||
#: runtime.
|
||||
reference_link_spheres: Optional[torch.Tensor] = None
|
||||
|
||||
def __post_init__(self):
|
||||
@@ -260,7 +319,7 @@ class KinematicsTensorConfig:
|
||||
if self.link_spheres is not None and self.reference_link_spheres is None:
|
||||
self.reference_link_spheres = self.link_spheres.clone()
|
||||
|
||||
def copy_(self, new_config: KinematicsTensorConfig):
|
||||
def copy_(self, new_config: KinematicsTensorConfig) -> KinematicsTensorConfig:
|
||||
self.fixed_transforms.copy_(new_config.fixed_transforms)
|
||||
self.link_map.copy_(new_config.link_map)
|
||||
self.joint_map.copy_(new_config.joint_map)
|
||||
@@ -268,6 +327,7 @@ class KinematicsTensorConfig:
|
||||
self.store_link_map.copy_(new_config.store_link_map)
|
||||
self.link_chain_map.copy_(new_config.link_chain_map)
|
||||
self.joint_limits.copy_(new_config.joint_limits)
|
||||
self.joint_offset_map.copy_(new_config.joint_offset_map)
|
||||
if new_config.link_spheres is not None and self.link_spheres is not None:
|
||||
self.link_spheres.copy_(new_config.link_spheres)
|
||||
if new_config.link_sphere_idx_map is not None and self.link_sphere_idx_map is not None:
|
||||
@@ -321,7 +381,8 @@ class KinematicsTensorConfig:
|
||||
):
|
||||
"""Update sphere parameters
|
||||
|
||||
#NOTE: This currently does not update self collision distances.
|
||||
NOTE: This currently does not update self collision distances.
|
||||
|
||||
Args:
|
||||
link_name: _description_
|
||||
sphere_position_radius: _description_
|
||||
|
||||
@@ -9,7 +9,7 @@
|
||||
# its affiliates is strictly prohibited.
|
||||
#
|
||||
# Standard Library
|
||||
from typing import Dict, Optional
|
||||
from typing import Dict, List, Optional
|
||||
|
||||
# Third Party
|
||||
import numpy as np
|
||||
@@ -33,12 +33,13 @@ class UrdfKinematicsParser(KinematicsParser):
|
||||
load_meshes: bool = False,
|
||||
mesh_root: str = "",
|
||||
extra_links: Optional[Dict[str, LinkParams]] = None,
|
||||
build_scene_graph: bool = False,
|
||||
) -> None:
|
||||
# load robot from urdf:
|
||||
self._robot = yourdfpy.URDF.load(
|
||||
urdf_path,
|
||||
load_meshes=load_meshes,
|
||||
build_scene_graph=False,
|
||||
build_scene_graph=build_scene_graph,
|
||||
mesh_dir=mesh_root,
|
||||
filename_handler=yourdfpy.filename_handler_null,
|
||||
)
|
||||
@@ -46,63 +47,114 @@ class UrdfKinematicsParser(KinematicsParser):
|
||||
|
||||
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")
|
||||
for jid, j in enumerate(self._robot.joint_map):
|
||||
self._parent_map[self._robot.joint_map[j].child] = {
|
||||
"parent": self._robot.joint_map[j].parent,
|
||||
"jid": jid,
|
||||
"joint_name": j,
|
||||
}
|
||||
|
||||
def _get_joint_name(self, idx):
|
||||
joint = self._robot.joint_names[idx]
|
||||
return joint
|
||||
|
||||
def _get_joint_limits(self, joint):
|
||||
joint_type = joint.type
|
||||
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 with limits[-10,10]")
|
||||
joint_type = "revolute"
|
||||
joint_limits = {
|
||||
"effort": joint.limit.effort,
|
||||
"lower": -10,
|
||||
"upper": 10,
|
||||
"velocity": joint.limit.velocity,
|
||||
}
|
||||
return joint_limits, joint_type
|
||||
|
||||
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
|
||||
|
||||
mimic_joint_name = None
|
||||
if base:
|
||||
body_params["parent_link_name"] = None
|
||||
joint_transform = np.eye(4)
|
||||
joint_name = "base_joint"
|
||||
active_joint_name = joint_name
|
||||
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]
|
||||
body_params["joint_type"] = JointType.FIXED
|
||||
|
||||
jid, joint_name = self._find_parent_joint_of_link(link_name)
|
||||
else:
|
||||
parent_data = self._parent_map[link_name]
|
||||
body_params["parent_link_name"] = parent_data["parent"]
|
||||
|
||||
jid, joint_name = parent_data["jid"], parent_data["joint_name"]
|
||||
body_params["joint_id"] = jid
|
||||
joint = self._robot.joint_map[joint_name]
|
||||
active_joint_name = 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
|
||||
body_params["joint_type"] = JointType.FIXED
|
||||
|
||||
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_offset = [1.0, 0.0]
|
||||
joint_limits, joint_type = self._get_joint_limits(joint)
|
||||
|
||||
if joint.mimic is not None:
|
||||
joint_offset = [joint.mimic.multiplier, joint.mimic.offset]
|
||||
# read joint limits of active joint:
|
||||
mimic_joint_name = joint_name
|
||||
active_joint_name = joint.mimic.joint
|
||||
active_joint = self._robot.joint_map[active_joint_name]
|
||||
active_joint_limits, active_joint_type = self._get_joint_limits(active_joint)
|
||||
# check to make sure mimic joint limits are not larger than active joint:
|
||||
if (
|
||||
active_joint_limits["lower"] * joint_offset[0] + joint_offset[1]
|
||||
< joint_limits["lower"]
|
||||
):
|
||||
log_error(
|
||||
"mimic joint can go out of it's lower limit as active joint has larger range "
|
||||
+ "FIX: make mimic joint's lower limit even lower "
|
||||
+ active_joint_name
|
||||
+ " "
|
||||
+ mimic_joint_name
|
||||
)
|
||||
if (
|
||||
active_joint_limits["upper"] * joint_offset[0] + joint_offset[1]
|
||||
> joint_limits["upper"]
|
||||
):
|
||||
log_error(
|
||||
"mimic joint can go out of it's upper limit as active joint has larger range "
|
||||
+ "FIX: make mimic joint's upper limit higher"
|
||||
+ active_joint_name
|
||||
+ " "
|
||||
+ mimic_joint_name
|
||||
)
|
||||
if active_joint_limits["velocity"] * joint_offset[0] > joint_limits["velocity"]:
|
||||
log_error(
|
||||
"mimic joint can move at higher velocity than active joint,"
|
||||
+ "increase velocity limit for mimic joint"
|
||||
+ active_joint_name
|
||||
+ " "
|
||||
+ mimic_joint_name
|
||||
)
|
||||
joint_limits = active_joint_limits
|
||||
|
||||
joint_axis = joint.axis
|
||||
|
||||
@@ -111,45 +163,33 @@ class UrdfKinematicsParser(KinematicsParser):
|
||||
-1.0 * joint_limits["velocity"],
|
||||
joint_limits["velocity"],
|
||||
]
|
||||
if joint_type == "prismatic":
|
||||
if abs(joint_axis[0]) == 1:
|
||||
joint_type = JointType.X_PRISM
|
||||
if abs(joint_axis[1]) == 1:
|
||||
joint_type = JointType.Y_PRISM
|
||||
if abs(joint_axis[2]) == 1:
|
||||
joint_type = JointType.Z_PRISM
|
||||
elif joint_type == "revolute":
|
||||
if abs(joint_axis[0]) == 1:
|
||||
joint_type = JointType.X_ROT
|
||||
if abs(joint_axis[1]) == 1:
|
||||
joint_type = JointType.Y_ROT
|
||||
if abs(joint_axis[2]) == 1:
|
||||
joint_type = JointType.Z_ROT
|
||||
else:
|
||||
log_error("Joint type not supported")
|
||||
if joint_axis[0] == -1 or joint_axis[1] == -1 or joint_axis[2] == -1:
|
||||
joint_offset[0] = -1.0 * joint_offset[0]
|
||||
body_params["joint_type"] = joint_type
|
||||
body_params["joint_offset"] = joint_offset
|
||||
|
||||
body_params["fixed_transform"] = joint_transform
|
||||
body_params["joint_name"] = joint_name
|
||||
body_params["joint_name"] = active_joint_name
|
||||
|
||||
body_params["joint_axis"] = joint_axis
|
||||
body_params["mimic_joint_name"] = mimic_joint_name
|
||||
|
||||
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
|
||||
if joint_axis[0] == -1:
|
||||
joint_type = JointType.X_PRISM_NEG
|
||||
if joint_axis[1] == -1:
|
||||
joint_type = JointType.Y_PRISM_NEG
|
||||
if joint_axis[2] == -1:
|
||||
joint_type = JointType.Z_PRISM_NEG
|
||||
|
||||
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
|
||||
if joint_axis[0] == -1:
|
||||
joint_type = JointType.X_ROT_NEG
|
||||
if joint_axis[1] == -1:
|
||||
joint_type = JointType.Y_ROT_NEG
|
||||
if joint_axis[2] == -1:
|
||||
joint_type = JointType.Z_ROT_NEG
|
||||
else:
|
||||
log_error("Joint type not supported")
|
||||
|
||||
body_params["joint_type"] = joint_type
|
||||
link_params = LinkParams(**body_params)
|
||||
|
||||
return link_params
|
||||
@@ -194,3 +234,16 @@ class UrdfKinematicsParser(KinematicsParser):
|
||||
scale=m.scale,
|
||||
file_path=m.filename,
|
||||
)
|
||||
|
||||
@property
|
||||
def root_link(self):
|
||||
return self._robot.base_link
|
||||
|
||||
def get_controlled_joint_names(self) -> List[str]:
|
||||
j_list = []
|
||||
for k in self._parent_map.keys():
|
||||
joint_name = self._parent_map[k]["joint_name"]
|
||||
joint = self._robot.joint_map[joint_name]
|
||||
if joint.type != "fixed" and joint.mimic is None:
|
||||
j_list.append(joint_name)
|
||||
return j_list
|
||||
|
||||
@@ -71,7 +71,7 @@ class UsdKinematicsParser(KinematicsParser):
|
||||
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_map[child.GetName()] = {"parent": parent.GetName()}
|
||||
self._parent_joint_map[child.GetName()] = l # store joint prim
|
||||
|
||||
def get_link_parameters(self, link_name: str, base: bool = False) -> LinkParams:
|
||||
@@ -100,7 +100,7 @@ class UsdKinematicsParser(KinematicsParser):
|
||||
joint_type = JointType.FIXED
|
||||
|
||||
else:
|
||||
parent_link_name = self._parent_map[link_name]
|
||||
parent_link_name = self._parent_map[link_name]["parent"]
|
||||
joint_prim = self._parent_joint_map[link_name] # joint prim connects link
|
||||
joint_transform = self._get_joint_transform(joint_prim)
|
||||
joint_axis = None
|
||||
|
||||
21
src/curobo/curobolib/cpp/check_cuda.h
Normal file
21
src/curobo/curobolib/cpp/check_cuda.h
Normal file
@@ -0,0 +1,21 @@
|
||||
/*
|
||||
* 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>
|
||||
|
||||
|
||||
// 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)
|
||||
|
||||
#define CHECK_FP8 defined(CUDA_VERSION) && CUDA_VERSION >= 11080 && TORCH_VERSION_MAJOR >= 2 && TORCH_VERSION_MINOR >= 2
|
||||
#define CHECK_INPUT_GUARD(x) CHECK_INPUT(x); const at::cuda::OptionalCUDAGuard guard(x.device())
|
||||
17
src/curobo/curobolib/cpp/cuda_precisions.h
Normal file
17
src/curobo/curobolib/cpp/cuda_precisions.h
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.
|
||||
*/
|
||||
#include "check_cuda.h"
|
||||
|
||||
#include <cuda_fp16.h>
|
||||
#include <cuda_bf16.h>
|
||||
#if CHECK_FP8
|
||||
#include <cuda_fp8.h>
|
||||
#endif
|
||||
@@ -13,7 +13,7 @@
|
||||
|
||||
#include <c10/cuda/CUDAGuard.h>
|
||||
#include <vector>
|
||||
|
||||
#include "check_cuda.h"
|
||||
// CUDA forward declarations
|
||||
|
||||
std::vector<torch::Tensor>self_collision_distance(
|
||||
@@ -168,13 +168,6 @@ backward_pose_distance(torch::Tensor out_grad_p,
|
||||
|
||||
// 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,
|
||||
|
||||
@@ -13,6 +13,8 @@
|
||||
#include <c10/cuda/CUDAGuard.h>
|
||||
#include <vector>
|
||||
|
||||
#include "check_cuda.h"
|
||||
|
||||
// CUDA forward declarations
|
||||
|
||||
std::vector<torch::Tensor>
|
||||
@@ -33,7 +35,9 @@ std::vector<torch::Tensor>kin_fused_forward(
|
||||
const torch::Tensor joint_map_type,
|
||||
const torch::Tensor store_link_map,
|
||||
const torch::Tensor link_sphere_map,
|
||||
const torch::Tensor joint_offset_map,
|
||||
const int batch_size,
|
||||
const int n_joints,
|
||||
const int n_spheres,
|
||||
const bool use_global_cumul = false);
|
||||
|
||||
@@ -52,76 +56,20 @@ std::vector<torch::Tensor>kin_fused_backward_16t(
|
||||
const torch::Tensor store_link_map,
|
||||
const torch::Tensor link_sphere_map,
|
||||
const torch::Tensor link_chain_map,
|
||||
const torch::Tensor joint_offset_map,
|
||||
const int batch_size,
|
||||
const int n_joints,
|
||||
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,
|
||||
m.def("forward", &kin_fused_forward, "Kinematics fused forward (CUDA)");
|
||||
m.def("backward", &kin_fused_backward_16t, "Kinematics fused backward (CUDA)");
|
||||
m.def("matrix_to_quaternion", &matrix_to_quaternion,
|
||||
"Rotation Matrix to Quaternion (CUDA)");
|
||||
}
|
||||
|
||||
@@ -10,12 +10,13 @@
|
||||
*/
|
||||
|
||||
#include <c10/cuda/CUDAStream.h>
|
||||
|
||||
#include <cuda.h>
|
||||
#include <cuda_runtime.h>
|
||||
#include <torch/extension.h>
|
||||
|
||||
#include "helper_math.h"
|
||||
#include <cuda_fp16.h>
|
||||
#include "check_cuda.h"
|
||||
#include <vector>
|
||||
|
||||
#define M 4
|
||||
@@ -28,13 +29,6 @@
|
||||
#define Y_ROT 4
|
||||
#define Z_ROT 5
|
||||
|
||||
#define X_PRISM_NEG 6
|
||||
#define Y_PRISM_NEG 7
|
||||
#define Z_PRISM_NEG 8
|
||||
#define X_ROT_NEG 9
|
||||
#define Y_ROT_NEG 10
|
||||
#define Z_ROT_NEG 11
|
||||
|
||||
#define MAX_BATCH_PER_BLOCK 32 // tunable parameter for improving occupancy
|
||||
#define MAX_BW_BATCH_PER_BLOCK 16 // tunable parameter for improving occupancy
|
||||
|
||||
@@ -46,6 +40,8 @@ namespace Curobo
|
||||
{
|
||||
namespace Kinematics
|
||||
{
|
||||
|
||||
|
||||
template<typename psum_t>
|
||||
__device__ __forceinline__ void scale_cross_sum(float3 a, float3 b,
|
||||
float3 scale, psum_t& sum_out)
|
||||
@@ -293,36 +289,11 @@ namespace Curobo
|
||||
}
|
||||
}
|
||||
|
||||
__device__ __forceinline__ void update_joint_type_direction(int& j_type, int8_t& axis_sign)
|
||||
{
|
||||
// Assume that input j_type >= 0 . Check fixed joint outside of this function.
|
||||
|
||||
// Don't do anything if j_type < 6. j_type range is [0, 11]
|
||||
// divergence here.
|
||||
axis_sign = 1;
|
||||
|
||||
if (j_type >= 6)
|
||||
{
|
||||
j_type -= 6;
|
||||
axis_sign = -1;
|
||||
}
|
||||
}
|
||||
|
||||
__device__ __forceinline__ void update_joint_type_direction(int& j_type)
|
||||
{
|
||||
// Assume that input j_type >= 0 . Check fixed joint outside of this function.
|
||||
|
||||
// Don't do anything if j_type < 6. j_type range is [0, 11]
|
||||
// divergence here.
|
||||
if (j_type >= 6)
|
||||
{
|
||||
j_type -= 6;
|
||||
}
|
||||
}
|
||||
|
||||
__device__ __forceinline__ void update_axis_direction(
|
||||
float& angle,
|
||||
int & j_type)
|
||||
int & j_type,
|
||||
const float2 &j_offset)
|
||||
{
|
||||
// Assume that input j_type >= 0 . Check fixed joint outside of this function.
|
||||
// sign should be +ve <= 5 and -ve >5
|
||||
@@ -330,9 +301,7 @@ namespace Curobo
|
||||
// cuda code treats -1.0 * 0.0 as negative. Hence we subtract 6. If in future, -1.0 * 0.0 =
|
||||
// +ve,
|
||||
// then this code should be j_type - 5.
|
||||
angle = -1 * copysignf(1.0, j_type - 6) * angle;
|
||||
|
||||
update_joint_type_direction(j_type);
|
||||
angle = j_offset.x * angle + j_offset.y;
|
||||
}
|
||||
|
||||
// In the following versions of rot_fn, some non-nan values may become nan as we
|
||||
@@ -462,7 +431,7 @@ namespace Curobo
|
||||
template<typename psum_t>
|
||||
__device__ __forceinline__ void
|
||||
rot_backward_translation(const float3& vec, float *cumul_mat, float *l_pos,
|
||||
const float3& loc_grad, psum_t& grad_q, const int8_t axis_sign = 1)
|
||||
const float3& loc_grad, psum_t& grad_q, const float axis_sign = 1)
|
||||
{
|
||||
float3 e_pos, j_pos;
|
||||
|
||||
@@ -481,7 +450,7 @@ namespace Curobo
|
||||
rot_backward_rotation(const float3 vec,
|
||||
const float3 grad_vec,
|
||||
psum_t & grad_q,
|
||||
const int8_t axis_sign = 1)
|
||||
const float axis_sign = 1)
|
||||
{
|
||||
grad_q += axis_sign * dot(vec, grad_vec);
|
||||
}
|
||||
@@ -489,7 +458,7 @@ namespace Curobo
|
||||
template<typename psum_t>
|
||||
__device__ __forceinline__ void
|
||||
prism_backward_translation(const float3 vec, const float3 grad_vec,
|
||||
psum_t& grad_q, const int8_t axis_sign = 1)
|
||||
psum_t& grad_q, const float axis_sign = 1)
|
||||
{
|
||||
grad_q += axis_sign * dot(vec, grad_vec);
|
||||
}
|
||||
@@ -497,7 +466,7 @@ namespace Curobo
|
||||
template<typename psum_t>
|
||||
__device__ __forceinline__ void
|
||||
z_rot_backward(float *link_cumul_mat, float *l_pos, float3& loc_grad_position,
|
||||
float3& loc_grad_orientation, psum_t& grad_q, const int8_t axis_sign = 1)
|
||||
float3& loc_grad_orientation, psum_t& grad_q, const float axis_sign = 1)
|
||||
{
|
||||
float3 vec =
|
||||
make_float3(link_cumul_mat[2], link_cumul_mat[6], link_cumul_mat[10]);
|
||||
@@ -512,7 +481,7 @@ namespace Curobo
|
||||
template<typename psum_t>
|
||||
__device__ __forceinline__ void
|
||||
x_rot_backward(float *link_cumul_mat, float *l_pos, float3& loc_grad_position,
|
||||
float3& loc_grad_orientation, psum_t& grad_q, const int8_t axis_sign = 1)
|
||||
float3& loc_grad_orientation, psum_t& grad_q, const float axis_sign = 1)
|
||||
{
|
||||
float3 vec =
|
||||
make_float3(link_cumul_mat[0], link_cumul_mat[4], link_cumul_mat[8]);
|
||||
@@ -527,7 +496,7 @@ namespace Curobo
|
||||
template<typename psum_t>
|
||||
__device__ __forceinline__ void
|
||||
y_rot_backward(float *link_cumul_mat, float *l_pos, float3& loc_grad_position,
|
||||
float3& loc_grad_orientation, psum_t& grad_q, const int8_t axis_sign = 1)
|
||||
float3& loc_grad_orientation, psum_t& grad_q, const float axis_sign = 1)
|
||||
{
|
||||
float3 vec =
|
||||
make_float3(link_cumul_mat[1], link_cumul_mat[5], link_cumul_mat[9]);
|
||||
@@ -542,7 +511,7 @@ namespace Curobo
|
||||
template<typename psum_t>
|
||||
__device__ __forceinline__ void
|
||||
xyz_prism_backward_translation(float *cumul_mat, float3& loc_grad,
|
||||
psum_t& grad_q, int xyz, const int8_t axis_sign = 1)
|
||||
psum_t& grad_q, int xyz, const float axis_sign = 1)
|
||||
{
|
||||
prism_backward_translation(
|
||||
make_float3(cumul_mat[0 + xyz], cumul_mat[4 + xyz], cumul_mat[8 + xyz]),
|
||||
@@ -553,7 +522,7 @@ namespace Curobo
|
||||
__device__ __forceinline__ void x_prism_backward_translation(float *cumul_mat,
|
||||
float3 & loc_grad,
|
||||
psum_t & grad_q,
|
||||
const int8_t axis_sign = 1)
|
||||
const float axis_sign = 1)
|
||||
{
|
||||
// get rotation vector:
|
||||
prism_backward_translation(
|
||||
@@ -564,7 +533,7 @@ namespace Curobo
|
||||
__device__ __forceinline__ void y_prism_backward_translation(float *cumul_mat,
|
||||
float3 & loc_grad,
|
||||
psum_t & grad_q,
|
||||
const int8_t axis_sign = 1)
|
||||
const float axis_sign = 1)
|
||||
{
|
||||
// get rotation vector:
|
||||
prism_backward_translation(
|
||||
@@ -575,16 +544,15 @@ namespace Curobo
|
||||
__device__ __forceinline__ void z_prism_backward_translation(float *cumul_mat,
|
||||
float3 & loc_grad,
|
||||
psum_t & grad_q,
|
||||
const int8_t axis_sign = 1)
|
||||
const float axis_sign = 1)
|
||||
{
|
||||
// get rotation vector:
|
||||
prism_backward_translation(
|
||||
make_float3(cumul_mat[2], cumul_mat[6], cumul_mat[10]), loc_grad, grad_q, axis_sign);
|
||||
}
|
||||
|
||||
__device__ __forceinline__ void
|
||||
xyz_rot_backward_translation(float *cumul_mat, float *l_pos, float3& loc_grad,
|
||||
float& grad_q, int xyz, const int8_t axis_sign = 1)
|
||||
float& grad_q, int xyz, const float axis_sign = 1)
|
||||
{
|
||||
// get rotation vector:
|
||||
rot_backward_translation(
|
||||
@@ -592,10 +560,11 @@ namespace Curobo
|
||||
&cumul_mat[0], &l_pos[0], loc_grad, grad_q, axis_sign);
|
||||
}
|
||||
|
||||
|
||||
template<typename psum_t>
|
||||
__device__ __forceinline__ void
|
||||
x_rot_backward_translation(float *cumul_mat, float *l_pos, float3& loc_grad,
|
||||
psum_t& grad_q, const int8_t axis_sign = 1)
|
||||
psum_t& grad_q, const float axis_sign = 1)
|
||||
{
|
||||
// get rotation vector:
|
||||
rot_backward_translation(
|
||||
@@ -606,7 +575,7 @@ namespace Curobo
|
||||
template<typename psum_t>
|
||||
__device__ __forceinline__ void
|
||||
y_rot_backward_translation(float *cumul_mat, float *l_pos, float3& loc_grad,
|
||||
psum_t& grad_q, const int8_t axis_sign = 1)
|
||||
psum_t& grad_q, const float axis_sign = 1)
|
||||
{
|
||||
// get rotation vector:
|
||||
rot_backward_translation(
|
||||
@@ -617,7 +586,7 @@ namespace Curobo
|
||||
template<typename psum_t>
|
||||
__device__ __forceinline__ void
|
||||
z_rot_backward_translation(float *cumul_mat, float *l_pos, float3& loc_grad,
|
||||
psum_t& grad_q, const int8_t axis_sign = 1)
|
||||
psum_t& grad_q, const float axis_sign = 1)
|
||||
{
|
||||
// get rotation vector:
|
||||
rot_backward_translation(
|
||||
@@ -629,18 +598,19 @@ namespace Curobo
|
||||
// This one should be about 10% faster.
|
||||
template<typename scalar_t, bool use_global_cumul>
|
||||
__global__ void
|
||||
kin_fused_warp_kernel2(scalar_t *link_pos, // batchSize xz store_n_links x M x M
|
||||
scalar_t *link_quat, // batchSize x store_n_links x M x M
|
||||
kin_fused_warp_kernel2(float *link_pos, // batchSize xz store_n_links x M x M
|
||||
float *link_quat, // batchSize x store_n_links x M x M
|
||||
scalar_t *b_robot_spheres, // batchSize x nspheres x M
|
||||
scalar_t *global_cumul_mat,
|
||||
const scalar_t *q, // batchSize x njoints
|
||||
const scalar_t *fixedTransform, // nlinks x M x M
|
||||
const scalar_t *robot_spheres, // nspheres x M
|
||||
float *global_cumul_mat, // batchSize x nlinks x M x M
|
||||
const float *q, // batchSize x njoints
|
||||
const float *fixedTransform, // nlinks x M x M
|
||||
const float *robot_spheres, // nspheres x M
|
||||
const int8_t *jointMapType, // nlinks
|
||||
const int16_t *jointMap, // nlinks
|
||||
const int16_t *linkMap, // nlinks
|
||||
const int16_t *storeLinkMap, // store_n_links
|
||||
const int16_t *linkSphereMap, // nspheres
|
||||
const float *jointOffset, // nlinks
|
||||
const int batchSize, const int nspheres,
|
||||
const int nlinks, const int njoints,
|
||||
const int store_n_links)
|
||||
@@ -688,7 +658,8 @@ namespace Curobo
|
||||
else
|
||||
{
|
||||
float angle = q[batch * njoints + jointMap[l]];
|
||||
update_axis_direction(angle, j_type);
|
||||
float2 angle_offset = *(float2 *)&jointOffset[l*2];
|
||||
update_axis_direction(angle, j_type, angle_offset);
|
||||
|
||||
if (j_type <= Z_PRISM)
|
||||
{
|
||||
@@ -794,20 +765,21 @@ namespace Curobo
|
||||
template<typename scalar_t, typename psum_t, bool use_global_cumul,
|
||||
bool enable_sparsity_opt, int16_t MAX_JOINTS, bool PARALLEL_WRITE>
|
||||
__global__ void kin_fused_backward_kernel3(
|
||||
scalar_t *grad_out_link_q, // batchSize * njoints
|
||||
const scalar_t *grad_nlinks_pos, // batchSize * store_n_links * 16
|
||||
const scalar_t *grad_nlinks_quat,
|
||||
float *grad_out_link_q, // batchSize * njoints
|
||||
const float *grad_nlinks_pos, // batchSize * store_n_links * 16
|
||||
const float *grad_nlinks_quat,
|
||||
const scalar_t *grad_spheres, // batchSize * nspheres * 4
|
||||
const scalar_t *global_cumul_mat,
|
||||
const scalar_t *q, // batchSize * njoints
|
||||
const scalar_t *fixedTransform, // nlinks * 16
|
||||
const scalar_t *robotSpheres, // batchSize * nspheres * 4
|
||||
const float *global_cumul_mat,
|
||||
const float *q, // batchSize * njoints
|
||||
const float *fixedTransform, // nlinks * 16
|
||||
const float *robotSpheres, // batchSize * nspheres * 4
|
||||
const int8_t *jointMapType, // nlinks
|
||||
const int16_t *jointMap, // nlinks
|
||||
const int16_t *linkMap, // nlinks
|
||||
const int16_t *storeLinkMap, // store_n_links
|
||||
const int16_t *linkSphereMap, // nspheres
|
||||
const int16_t *linkChainMap, // nlinks*nlinks
|
||||
const float *jointOffset, // nlinks*2
|
||||
const int batchSize, const int nspheres, const int nlinks,
|
||||
const int njoints, const int store_n_links)
|
||||
{
|
||||
@@ -819,8 +791,7 @@ namespace Curobo
|
||||
|
||||
if (batch >= batchSize)
|
||||
return;
|
||||
|
||||
// Each thread computes one element of the cumul_mat.
|
||||
// Each thread computes one element of the cumul_mat.
|
||||
// first 4 threads compute a row of the output;
|
||||
const int elem_idx = threadIdx.x % 16;
|
||||
const int col_idx = elem_idx % 4;
|
||||
@@ -832,6 +803,7 @@ namespace Curobo
|
||||
for (int l = 0; l < nlinks; l++)
|
||||
{
|
||||
int outAddrStart = matAddrBase + l * M * M; // + (t % M) * M;
|
||||
|
||||
cumul_mat[outAddrStart + elem_idx] =
|
||||
global_cumul_mat[batch * nlinks * M * M + l * M * M + elem_idx];
|
||||
}
|
||||
@@ -857,8 +829,8 @@ namespace Curobo
|
||||
else
|
||||
{
|
||||
float angle = q[batch * njoints + jointMap[l]];
|
||||
|
||||
update_axis_direction(angle, j_type);
|
||||
float2 angle_offset = *(float2 *)&jointOffset[l*2];
|
||||
update_axis_direction(angle, j_type, angle_offset);
|
||||
|
||||
if (j_type <= Z_PRISM)
|
||||
{
|
||||
@@ -949,15 +921,11 @@ namespace Curobo
|
||||
{
|
||||
continue;
|
||||
}
|
||||
int8_t axis_sign = 1;
|
||||
float axis_sign = jointOffset[j*2];
|
||||
|
||||
int j_type = jointMapType[j];
|
||||
|
||||
if (j_type != FIXED)
|
||||
{
|
||||
update_joint_type_direction(j_type, axis_sign);
|
||||
}
|
||||
|
||||
|
||||
if (j_type == Z_ROT)
|
||||
{
|
||||
float result = 0.0;
|
||||
@@ -1046,12 +1014,13 @@ namespace Curobo
|
||||
// for (int16_t k = joints_per_thread; k >= 0; k--)
|
||||
for (int16_t k = 0; k < joints_per_thread; k++)
|
||||
{
|
||||
// int16_t j = elem_idx * joints_per_thread + k;
|
||||
int16_t j = elem_idx + k * 16;
|
||||
int16_t j = elem_idx * joints_per_thread + k;
|
||||
//int16_t j = elem_idx + k * 16;
|
||||
// int16_t j = elem_idx + k * 16; // (threadidx.x % 16) + k * 16 (0 to 16)
|
||||
|
||||
// int16_t j = k * M + elem_idx;
|
||||
if ((j > max_lmap) || (j < 0))
|
||||
continue;
|
||||
if ((j > max_lmap))
|
||||
break;
|
||||
|
||||
// This can be spread across threads as they are not sequential?
|
||||
if (linkChainMap[l_map * nlinks + j] == 0.0)
|
||||
@@ -1061,12 +1030,8 @@ namespace Curobo
|
||||
int16_t j_idx = jointMap[j];
|
||||
int j_type = jointMapType[j];
|
||||
|
||||
int8_t axis_sign = 1;
|
||||
float axis_sign = jointOffset[j*2];
|
||||
|
||||
if (j_type != FIXED)
|
||||
{
|
||||
update_joint_type_direction(j_type, axis_sign);
|
||||
}
|
||||
|
||||
// get rotation vector:
|
||||
if (j_type == Z_ROT)
|
||||
@@ -1090,8 +1055,9 @@ namespace Curobo
|
||||
g_position, g_orientation, psum_grad[j_idx], axis_sign);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
__syncthreads();
|
||||
if (PARALLEL_WRITE)
|
||||
{
|
||||
// accumulate the partial sums across the 16 threads
|
||||
@@ -1114,8 +1080,8 @@ namespace Curobo
|
||||
|
||||
for (int16_t j = 0; j < joints_per_thread; j++)
|
||||
{
|
||||
// const int16_t j_idx = elem_idx * joints_per_thread + j;
|
||||
const int16_t j_idx = elem_idx + j * 16;
|
||||
const int16_t j_idx = elem_idx * joints_per_thread + j;
|
||||
//const int16_t j_idx = elem_idx + j * 16;
|
||||
|
||||
if (j_idx >= njoints)
|
||||
{
|
||||
@@ -1151,7 +1117,7 @@ namespace Curobo
|
||||
{
|
||||
{
|
||||
grad_out_link_q[batch * njoints + j] =
|
||||
psum_grad[j]; // write the sum to memory
|
||||
(float) psum_grad[j]; // write the sum to memory
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1195,11 +1161,27 @@ std::vector<torch::Tensor>kin_fused_forward(
|
||||
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 torch::Tensor joint_offset_map,
|
||||
const int batch_size,
|
||||
const int n_joints,const int n_spheres,
|
||||
const bool use_global_cumul = false)
|
||||
{
|
||||
using namespace Curobo::Kinematics;
|
||||
const int n_joints = joint_vec.size(0) / batch_size;
|
||||
CHECK_INPUT_GUARD(joint_vec);
|
||||
CHECK_INPUT(link_pos);
|
||||
CHECK_INPUT(link_quat);
|
||||
CHECK_INPUT(global_cumul_mat);
|
||||
CHECK_INPUT(batch_robot_spheres);
|
||||
CHECK_INPUT(fixed_transform);
|
||||
CHECK_INPUT(robot_spheres);
|
||||
CHECK_INPUT(link_map);
|
||||
CHECK_INPUT(joint_map);
|
||||
CHECK_INPUT(joint_map_type);
|
||||
CHECK_INPUT(store_link_map);
|
||||
CHECK_INPUT(link_sphere_map);
|
||||
//CHECK_INPUT(link_chain_map);
|
||||
CHECK_INPUT(joint_offset_map);
|
||||
|
||||
const int n_links = link_map.size(0);
|
||||
const int store_n_links = link_pos.size(1);
|
||||
assert(joint_map.dtype() == torch::kInt16);
|
||||
@@ -1238,38 +1220,42 @@ std::vector<torch::Tensor>kin_fused_forward(
|
||||
if (use_global_cumul)
|
||||
{
|
||||
AT_DISPATCH_FLOATING_TYPES(
|
||||
link_pos.scalar_type(), "kin_fused_forward", ([&] {
|
||||
batch_robot_spheres.scalar_type(), "kin_fused_forward", ([&] {
|
||||
kin_fused_warp_kernel2<scalar_t, true>
|
||||
<< < blocksPerGrid, threadsPerBlock, sharedMemSize, stream >> > (
|
||||
link_pos.data_ptr<scalar_t>(), link_quat.data_ptr<scalar_t>(),
|
||||
link_pos.data_ptr<float>(), link_quat.data_ptr<float>(),
|
||||
batch_robot_spheres.data_ptr<scalar_t>(),
|
||||
global_cumul_mat.data_ptr<scalar_t>(),
|
||||
joint_vec.data_ptr<scalar_t>(),
|
||||
fixed_transform.data_ptr<scalar_t>(),
|
||||
robot_spheres.data_ptr<scalar_t>(),
|
||||
global_cumul_mat.data_ptr<float>(),
|
||||
joint_vec.data_ptr<float>(),
|
||||
fixed_transform.data_ptr<float>(),
|
||||
robot_spheres.data_ptr<float>(),
|
||||
joint_map_type.data_ptr<int8_t>(),
|
||||
joint_map.data_ptr<int16_t>(), link_map.data_ptr<int16_t>(),
|
||||
store_link_map.data_ptr<int16_t>(),
|
||||
link_sphere_map.data_ptr<int16_t>(), batch_size, n_spheres,
|
||||
link_sphere_map.data_ptr<int16_t>(),
|
||||
joint_offset_map.data_ptr<float>(),
|
||||
batch_size, n_spheres,
|
||||
n_links, n_joints, store_n_links);
|
||||
}));
|
||||
}
|
||||
else
|
||||
{
|
||||
AT_DISPATCH_FLOATING_TYPES(
|
||||
link_pos.scalar_type(), "kin_fused_forward", ([&] {
|
||||
batch_robot_spheres.scalar_type(), "kin_fused_forward", ([&] {
|
||||
kin_fused_warp_kernel2<scalar_t, false>
|
||||
<< < blocksPerGrid, threadsPerBlock, sharedMemSize, stream >> > (
|
||||
link_pos.data_ptr<scalar_t>(), link_quat.data_ptr<scalar_t>(),
|
||||
link_pos.data_ptr<float>(), link_quat.data_ptr<float>(),
|
||||
batch_robot_spheres.data_ptr<scalar_t>(),
|
||||
global_cumul_mat.data_ptr<scalar_t>(),
|
||||
joint_vec.data_ptr<scalar_t>(),
|
||||
fixed_transform.data_ptr<scalar_t>(),
|
||||
robot_spheres.data_ptr<scalar_t>(),
|
||||
global_cumul_mat.data_ptr<float>(),
|
||||
joint_vec.data_ptr<float>(),
|
||||
fixed_transform.data_ptr<float>(),
|
||||
robot_spheres.data_ptr<float>(),
|
||||
joint_map_type.data_ptr<int8_t>(),
|
||||
joint_map.data_ptr<int16_t>(), link_map.data_ptr<int16_t>(),
|
||||
store_link_map.data_ptr<int16_t>(),
|
||||
link_sphere_map.data_ptr<int16_t>(), batch_size, n_spheres,
|
||||
link_sphere_map.data_ptr<int16_t>(),
|
||||
joint_offset_map.data_ptr<float>(),
|
||||
batch_size, n_spheres,
|
||||
n_links, n_joints, store_n_links);
|
||||
}));
|
||||
}
|
||||
@@ -1292,15 +1278,29 @@ std::vector<torch::Tensor>kin_fused_backward_16t(
|
||||
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 torch::Tensor joint_offset_map,
|
||||
const int batch_size, const int n_joints, const int n_spheres, const bool sparsity_opt = true,
|
||||
const bool use_global_cumul = false)
|
||||
{
|
||||
using namespace Curobo::Kinematics;
|
||||
|
||||
const int n_joints = joint_vec.size(0) / batch_size;
|
||||
CHECK_INPUT_GUARD(joint_vec);
|
||||
CHECK_INPUT(grad_out);
|
||||
CHECK_INPUT(grad_nlinks_pos);
|
||||
CHECK_INPUT(grad_nlinks_quat);
|
||||
CHECK_INPUT(global_cumul_mat);
|
||||
CHECK_INPUT(fixed_transform);
|
||||
CHECK_INPUT(robot_spheres);
|
||||
CHECK_INPUT(link_map);
|
||||
CHECK_INPUT(joint_map);
|
||||
CHECK_INPUT(joint_map_type);
|
||||
CHECK_INPUT(store_link_map);
|
||||
CHECK_INPUT(link_sphere_map);
|
||||
CHECK_INPUT(link_chain_map);
|
||||
CHECK_INPUT(joint_offset_map);
|
||||
|
||||
const int n_links = link_map.size(0);
|
||||
const int store_n_links = store_link_map.size(0);
|
||||
|
||||
|
||||
// assert(n_links < 128);
|
||||
assert(n_joints < 128); // for larger num. of joints, change kernel3's
|
||||
// MAX_JOINTS template value.
|
||||
@@ -1341,72 +1341,77 @@ std::vector<torch::Tensor>kin_fused_backward_16t(
|
||||
|
||||
cudaStream_t stream = at::cuda::getCurrentCUDAStream();
|
||||
assert(sparsity_opt);
|
||||
|
||||
if (use_global_cumul)
|
||||
{
|
||||
if (n_joints < 16)
|
||||
{
|
||||
AT_DISPATCH_FLOATING_TYPES(
|
||||
grad_nlinks_pos.scalar_type(), "kin_fused_backward_16t", ([&] {
|
||||
kin_fused_backward_kernel3<scalar_t, float, true, true, 16, true>
|
||||
grad_spheres.scalar_type(), "kin_fused_backward_16t", ([&] {
|
||||
kin_fused_backward_kernel3<scalar_t, double, true, true, 16, true>
|
||||
<< < blocksPerGrid, threadsPerBlock, sharedMemSize, stream >> > (
|
||||
grad_out.data_ptr<scalar_t>(),
|
||||
grad_nlinks_pos.data_ptr<scalar_t>(),
|
||||
grad_nlinks_quat.data_ptr<scalar_t>(),
|
||||
grad_out.data_ptr<float>(),
|
||||
grad_nlinks_pos.data_ptr<float>(),
|
||||
grad_nlinks_quat.data_ptr<float>(),
|
||||
grad_spheres.data_ptr<scalar_t>(),
|
||||
global_cumul_mat.data_ptr<scalar_t>(),
|
||||
joint_vec.data_ptr<scalar_t>(),
|
||||
fixed_transform.data_ptr<scalar_t>(),
|
||||
robot_spheres.data_ptr<scalar_t>(),
|
||||
global_cumul_mat.data_ptr<float>(),
|
||||
joint_vec.data_ptr<float>(),
|
||||
fixed_transform.data_ptr<float>(),
|
||||
robot_spheres.data_ptr<float>(),
|
||||
joint_map_type.data_ptr<int8_t>(),
|
||||
joint_map.data_ptr<int16_t>(), link_map.data_ptr<int16_t>(),
|
||||
store_link_map.data_ptr<int16_t>(),
|
||||
link_sphere_map.data_ptr<int16_t>(),
|
||||
link_chain_map.data_ptr<int16_t>(), batch_size, n_spheres,
|
||||
link_chain_map.data_ptr<int16_t>(),
|
||||
joint_offset_map.data_ptr<float>(),
|
||||
batch_size, n_spheres,
|
||||
n_links, n_joints, store_n_links);
|
||||
}));
|
||||
}
|
||||
else if (n_joints < 64)
|
||||
{
|
||||
AT_DISPATCH_FLOATING_TYPES(
|
||||
grad_nlinks_pos.scalar_type(), "kin_fused_backward_16t", ([&] {
|
||||
kin_fused_backward_kernel3<scalar_t, float, true, true, 64, true>
|
||||
grad_spheres.scalar_type(), "kin_fused_backward_16t", ([&] {
|
||||
kin_fused_backward_kernel3<scalar_t, double, true, true, 64, true>
|
||||
<< < blocksPerGrid, threadsPerBlock, sharedMemSize, stream >> > (
|
||||
grad_out.data_ptr<scalar_t>(),
|
||||
grad_nlinks_pos.data_ptr<scalar_t>(),
|
||||
grad_nlinks_quat.data_ptr<scalar_t>(),
|
||||
grad_out.data_ptr<float>(),
|
||||
grad_nlinks_pos.data_ptr<float>(),
|
||||
grad_nlinks_quat.data_ptr<float>(),
|
||||
grad_spheres.data_ptr<scalar_t>(),
|
||||
global_cumul_mat.data_ptr<scalar_t>(),
|
||||
joint_vec.data_ptr<scalar_t>(),
|
||||
fixed_transform.data_ptr<scalar_t>(),
|
||||
robot_spheres.data_ptr<scalar_t>(),
|
||||
global_cumul_mat.data_ptr<float>(),
|
||||
joint_vec.data_ptr<float>(),
|
||||
fixed_transform.data_ptr<float>(),
|
||||
robot_spheres.data_ptr<float>(),
|
||||
joint_map_type.data_ptr<int8_t>(),
|
||||
joint_map.data_ptr<int16_t>(), link_map.data_ptr<int16_t>(),
|
||||
store_link_map.data_ptr<int16_t>(),
|
||||
link_sphere_map.data_ptr<int16_t>(),
|
||||
link_chain_map.data_ptr<int16_t>(), batch_size, n_spheres,
|
||||
link_chain_map.data_ptr<int16_t>(),
|
||||
joint_offset_map.data_ptr<float>(),
|
||||
batch_size, n_spheres,
|
||||
n_links, n_joints, store_n_links);
|
||||
}));
|
||||
}
|
||||
else
|
||||
{
|
||||
AT_DISPATCH_FLOATING_TYPES(
|
||||
grad_nlinks_pos.scalar_type(), "kin_fused_backward_16t", ([&] {
|
||||
kin_fused_backward_kernel3<scalar_t, float, true, true, 128, true>
|
||||
grad_spheres.scalar_type(), "kin_fused_backward_16t", ([&] {
|
||||
kin_fused_backward_kernel3<scalar_t, double, true, true, 128, true>
|
||||
<< < blocksPerGrid, threadsPerBlock, sharedMemSize, stream >> > (
|
||||
grad_out.data_ptr<scalar_t>(),
|
||||
grad_nlinks_pos.data_ptr<scalar_t>(),
|
||||
grad_nlinks_quat.data_ptr<scalar_t>(),
|
||||
grad_out.data_ptr<float>(),
|
||||
grad_nlinks_pos.data_ptr<float>(),
|
||||
grad_nlinks_quat.data_ptr<float>(),
|
||||
grad_spheres.data_ptr<scalar_t>(),
|
||||
global_cumul_mat.data_ptr<scalar_t>(),
|
||||
joint_vec.data_ptr<scalar_t>(),
|
||||
fixed_transform.data_ptr<scalar_t>(),
|
||||
robot_spheres.data_ptr<scalar_t>(),
|
||||
global_cumul_mat.data_ptr<float>(),
|
||||
joint_vec.data_ptr<float>(),
|
||||
fixed_transform.data_ptr<float>(),
|
||||
robot_spheres.data_ptr<float>(),
|
||||
joint_map_type.data_ptr<int8_t>(),
|
||||
joint_map.data_ptr<int16_t>(), link_map.data_ptr<int16_t>(),
|
||||
store_link_map.data_ptr<int16_t>(),
|
||||
link_sphere_map.data_ptr<int16_t>(),
|
||||
link_chain_map.data_ptr<int16_t>(), batch_size, n_spheres,
|
||||
link_chain_map.data_ptr<int16_t>(),
|
||||
joint_offset_map.data_ptr<float>(),
|
||||
batch_size, n_spheres,
|
||||
n_links, n_joints, store_n_links);
|
||||
}));
|
||||
}
|
||||
@@ -1417,22 +1422,25 @@ std::vector<torch::Tensor>kin_fused_backward_16t(
|
||||
{
|
||||
//
|
||||
AT_DISPATCH_FLOATING_TYPES(
|
||||
grad_nlinks_pos.scalar_type(), "kin_fused_backward_16t", ([&] {
|
||||
kin_fused_backward_kernel3<scalar_t, float, false, true, 128, true>
|
||||
grad_spheres.scalar_type(), "kin_fused_backward_16t", ([&] {
|
||||
kin_fused_backward_kernel3<scalar_t, double, false, true, 128, true>
|
||||
<< < blocksPerGrid, threadsPerBlock, sharedMemSize, stream >> > (
|
||||
grad_out.data_ptr<scalar_t>(),
|
||||
grad_nlinks_pos.data_ptr<scalar_t>(),
|
||||
grad_nlinks_quat.data_ptr<scalar_t>(),
|
||||
grad_out.data_ptr<float>(),
|
||||
grad_nlinks_pos.data_ptr<float>(),
|
||||
grad_nlinks_quat.data_ptr<float>(),
|
||||
grad_spheres.data_ptr<scalar_t>(),
|
||||
global_cumul_mat.data_ptr<scalar_t>(),
|
||||
joint_vec.data_ptr<scalar_t>(),
|
||||
fixed_transform.data_ptr<scalar_t>(),
|
||||
robot_spheres.data_ptr<scalar_t>(),
|
||||
global_cumul_mat.data_ptr<float>(),
|
||||
joint_vec.data_ptr<float>(),
|
||||
fixed_transform.data_ptr<float>(),
|
||||
robot_spheres.data_ptr<float>(),
|
||||
joint_map_type.data_ptr<int8_t>(),
|
||||
joint_map.data_ptr<int16_t>(), link_map.data_ptr<int16_t>(),
|
||||
joint_map.data_ptr<int16_t>(),
|
||||
link_map.data_ptr<int16_t>(),
|
||||
store_link_map.data_ptr<int16_t>(),
|
||||
link_sphere_map.data_ptr<int16_t>(),
|
||||
link_chain_map.data_ptr<int16_t>(), batch_size, n_spheres,
|
||||
link_chain_map.data_ptr<int16_t>(),
|
||||
joint_offset_map.data_ptr<float>(),
|
||||
batch_size, n_spheres,
|
||||
n_links, n_joints, store_n_links);
|
||||
}));
|
||||
}
|
||||
@@ -1447,7 +1455,8 @@ matrix_to_quaternion(torch::Tensor out_quat,
|
||||
)
|
||||
{
|
||||
using namespace Curobo::Kinematics;
|
||||
|
||||
CHECK_INPUT(out_quat);
|
||||
CHECK_INPUT_GUARD(in_rot);
|
||||
// we compute the warp threads based on number of boxes:
|
||||
|
||||
// TODO: verify this math
|
||||
|
||||
@@ -807,6 +807,7 @@ lbfgs_cuda_fuse(torch::Tensor step_vec, torch::Tensor rho_buffer,
|
||||
{
|
||||
using namespace Curobo::Optimization;
|
||||
|
||||
|
||||
// call first kernel:
|
||||
//const bool use_experimental = true;
|
||||
const bool use_fixed_m = true;
|
||||
|
||||
@@ -15,23 +15,13 @@
|
||||
#include <torch/extension.h>
|
||||
|
||||
#include "helper_math.h"
|
||||
#include <cuda_fp16.h>
|
||||
#include <cuda_bf16.h>
|
||||
|
||||
#include <vector>
|
||||
#include <torch/torch.h>
|
||||
#include <c10/cuda/CUDAGuard.h>
|
||||
#include "check_cuda.h"
|
||||
#include "cuda_precisions.h"
|
||||
|
||||
// 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)
|
||||
|
||||
#if defined(CUDA_VERSION) && CUDA_VERSION >= 11080 && TORCH_VERSION_MAJOR >= 2 && TORCH_VERSION_MINOR >= 2
|
||||
#include <cuda_fp8.h>
|
||||
#endif
|
||||
#define M 4
|
||||
#define VOXEL_DEBUG true
|
||||
#define VOXEL_UNOBSERVED_DISTANCE -1000.0
|
||||
@@ -63,8 +53,7 @@ namespace Curobo
|
||||
}
|
||||
|
||||
|
||||
#if defined(CUDA_VERSION) && CUDA_VERSION >= 11080 && TORCH_VERSION_MAJOR >= 2 && TORCH_VERSION_MINOR >= 2
|
||||
|
||||
#if CHECK_FP8
|
||||
__device__ __forceinline__ float
|
||||
get_array_value(const at::Float8_e4m3fn *grid_features, const int voxel_idx)
|
||||
{
|
||||
@@ -704,16 +693,21 @@ float4 &sum_pt)
|
||||
int3 &xyz_grid,
|
||||
float &interpolated_distance,
|
||||
int &voxel_idx)
|
||||
{
|
||||
{
|
||||
|
||||
|
||||
// convert location to index: can use floor to cast to int.
|
||||
// to account for negative values, add 0.5 * bounds.
|
||||
const float3 loc_grid = make_float3(loc_grid_params.x, loc_grid_params.y, loc_grid_params.z);
|
||||
const float3 sphere = make_float3(loc_sphere.x, loc_sphere.y, loc_sphere.z);
|
||||
// xyz_loc = make_int3(floorf((sphere + 0.5 * loc_grid) / loc_grid_params.w));
|
||||
xyz_loc = make_int3(floorf((sphere) / loc_grid_params.w) + (0.5 * loc_grid/loc_grid_params.w));
|
||||
xyz_grid = make_int3(floorf(loc_grid / loc_grid_params.w));
|
||||
//xyz_loc = make_int3(floorf((sphere + 0.5 * loc_grid) / loc_grid_params.w));
|
||||
const float inv_voxel_size = 1.0/loc_grid_params.w;
|
||||
//xyz_loc = make_int3(sphere * inv_voxel_size) + make_int3(0.5 * loc_grid * inv_voxel_size);
|
||||
|
||||
xyz_loc = make_int3((sphere + 0.5 * loc_grid) * inv_voxel_size);
|
||||
|
||||
//xyz_loc = make_int3(sphere / loc_grid_params.w) + make_int3(floorf(0.5 * loc_grid/loc_grid_params.w));
|
||||
xyz_grid = make_int3((loc_grid * inv_voxel_size)) + 1;
|
||||
|
||||
// find next nearest voxel to current point and then do weighted interpolation:
|
||||
voxel_idx = xyz_loc.x * xyz_grid.y * xyz_grid.z + xyz_loc.y * xyz_grid.z + xyz_loc.z;
|
||||
@@ -732,8 +726,8 @@ float4 &sum_pt)
|
||||
|
||||
|
||||
float3 delta = sphere - voxel_origin;
|
||||
int3 next_loc = make_int3(floorf((make_float3(xyz_loc) + normalize(delta))));
|
||||
float ratio = length(delta)/loc_grid_params.w;
|
||||
int3 next_loc = make_int3(((make_float3(xyz_loc) + normalize(delta))));
|
||||
float ratio = length(delta) * inv_voxel_size;
|
||||
|
||||
int next_voxel_idx = next_loc.x * xyz_grid.y * xyz_grid.z + next_loc.y * xyz_grid.z + next_loc.z;
|
||||
|
||||
@@ -2752,7 +2746,7 @@ sphere_obb_clpt(const torch::Tensor sphere_position, // batch_size, 3
|
||||
else
|
||||
{
|
||||
|
||||
#if defined(CUDA_VERSION) && CUDA_VERSION >= 11080 && TORCH_VERSION_MAJOR >= 2 && TORCH_VERSION_MINOR >= 2
|
||||
#if CHECK_FP8
|
||||
const auto fp8_type = torch::kFloat8_e4m3fn;
|
||||
#else
|
||||
const auto fp8_type = torch::kHalf;
|
||||
@@ -3217,7 +3211,7 @@ sphere_voxel_clpt(const torch::Tensor sphere_position, // batch_size, 3
|
||||
|
||||
int blocksPerGrid = (bnh_spheres + threadsPerBlock - 1) / threadsPerBlock;
|
||||
|
||||
#if defined(CUDA_VERSION) && CUDA_VERSION >= 11080 && TORCH_VERSION_MAJOR >= 2 && TORCH_VERSION_MINOR >= 2
|
||||
#if CHECK_FP8
|
||||
const auto fp8_type = torch::kFloat8_e4m3fn;
|
||||
#else
|
||||
const auto fp8_type = torch::kHalf;
|
||||
@@ -3352,7 +3346,7 @@ swept_sphere_voxel_clpt(const torch::Tensor sphere_position, // batch_size, 3
|
||||
|
||||
int blocksPerGrid = (bnh_spheres + threadsPerBlock - 1) / threadsPerBlock;
|
||||
|
||||
#if defined(CUDA_VERSION) && CUDA_VERSION >= 11080 && TORCH_VERSION_MAJOR >= 2 && TORCH_VERSION_MINOR >= 2
|
||||
#if CHECK_FP8
|
||||
const auto fp8_type = torch::kFloat8_e4m3fn;
|
||||
#else
|
||||
const auto fp8_type = torch::kHalf;
|
||||
|
||||
@@ -43,100 +43,10 @@ def rotation_matrix_to_quaternion(in_mat, out_quat):
|
||||
|
||||
|
||||
class KinematicsFusedFunction(Function):
|
||||
@staticmethod
|
||||
def _call_cuda(
|
||||
link_pos_out,
|
||||
link_quat_out,
|
||||
robot_sphere_out,
|
||||
global_cumul_mat,
|
||||
angle,
|
||||
fixed_transform,
|
||||
robot_spheres,
|
||||
link_map,
|
||||
joint_map,
|
||||
joint_map_type,
|
||||
store_link_map,
|
||||
link_sphere_map,
|
||||
):
|
||||
b_shape = link_pos_out.shape
|
||||
b_size = b_shape[0]
|
||||
n_spheres = robot_sphere_out.shape[1]
|
||||
r = kinematics_fused_cu.forward(
|
||||
link_pos_out,
|
||||
link_quat_out,
|
||||
robot_sphere_out,
|
||||
global_cumul_mat,
|
||||
angle,
|
||||
fixed_transform,
|
||||
robot_spheres,
|
||||
link_map,
|
||||
joint_map,
|
||||
joint_map_type,
|
||||
store_link_map,
|
||||
link_sphere_map,
|
||||
b_size,
|
||||
n_spheres,
|
||||
False,
|
||||
)
|
||||
out_link_pos = r[0]
|
||||
out_link_quat = r[1]
|
||||
out_spheres = r[2]
|
||||
global_cumul_mat = r[3]
|
||||
return out_link_pos, out_link_quat, out_spheres, global_cumul_mat
|
||||
|
||||
@staticmethod
|
||||
def _call_backward_cuda(
|
||||
grad_out,
|
||||
link_pos_out,
|
||||
link_quat_out,
|
||||
robot_sphere_out,
|
||||
global_cumul_mat,
|
||||
angle,
|
||||
fixed_transform,
|
||||
robot_spheres,
|
||||
link_map,
|
||||
joint_map,
|
||||
joint_map_type,
|
||||
store_link_map,
|
||||
link_sphere_map,
|
||||
link_chain_map,
|
||||
sparsity_opt=True,
|
||||
):
|
||||
b_shape = link_pos_out.shape
|
||||
b_size = b_shape[0]
|
||||
n_spheres = robot_sphere_out.shape[1]
|
||||
if grad_out.is_contiguous():
|
||||
grad_out = grad_out.view(-1)
|
||||
else:
|
||||
grad_out = grad_out.reshape(-1)
|
||||
r = kinematics_fused_cu.backward(
|
||||
grad_out,
|
||||
link_pos_out,
|
||||
link_quat_out,
|
||||
robot_sphere_out,
|
||||
global_cumul_mat,
|
||||
angle,
|
||||
fixed_transform,
|
||||
robot_spheres,
|
||||
link_map,
|
||||
joint_map,
|
||||
joint_map_type,
|
||||
store_link_map,
|
||||
link_sphere_map,
|
||||
link_chain_map,
|
||||
b_size,
|
||||
n_spheres,
|
||||
sparsity_opt,
|
||||
False,
|
||||
)
|
||||
out_q = r[0].view(b_size, -1)
|
||||
|
||||
return out_q
|
||||
|
||||
@staticmethod
|
||||
def forward(
|
||||
ctx,
|
||||
# link_mats: torch.Tensor,
|
||||
link_pos: torch.Tensor,
|
||||
link_quat: torch.Tensor,
|
||||
b_robot_spheres: torch.tensor,
|
||||
@@ -150,9 +60,17 @@ class KinematicsFusedFunction(Function):
|
||||
store_link_map: torch.Tensor,
|
||||
link_sphere_map: torch.Tensor,
|
||||
link_chain_map: torch.Tensor,
|
||||
joint_offset_map: torch.Tensor,
|
||||
grad_out: torch.Tensor,
|
||||
use_global_cumul: bool = True,
|
||||
):
|
||||
link_pos, link_quat, b_robot_spheres, global_cumul_mat = KinematicsFusedFunction._call_cuda(
|
||||
ctx.use_global_cumul = use_global_cumul
|
||||
b_shape = link_pos.shape
|
||||
b_size = b_shape[0]
|
||||
n_spheres = b_robot_spheres.shape[1]
|
||||
n_joints = joint_seq.shape[-1]
|
||||
|
||||
r = kinematics_fused_cu.forward(
|
||||
link_pos,
|
||||
link_quat,
|
||||
b_robot_spheres,
|
||||
@@ -165,7 +83,17 @@ class KinematicsFusedFunction(Function):
|
||||
joint_map_type,
|
||||
store_link_map,
|
||||
link_sphere_map,
|
||||
joint_offset_map, # offset_joint_map
|
||||
b_size,
|
||||
n_joints,
|
||||
n_spheres,
|
||||
True,
|
||||
)
|
||||
out_link_pos = r[0]
|
||||
out_link_quat = r[1]
|
||||
out_spheres = r[2]
|
||||
global_cumul_mat = r[3]
|
||||
|
||||
ctx.save_for_backward(
|
||||
joint_seq,
|
||||
fixed_transform,
|
||||
@@ -176,11 +104,11 @@ class KinematicsFusedFunction(Function):
|
||||
store_link_map,
|
||||
link_sphere_map,
|
||||
link_chain_map,
|
||||
joint_offset_map,
|
||||
grad_out,
|
||||
global_cumul_mat,
|
||||
)
|
||||
|
||||
return link_pos, link_quat, b_robot_spheres
|
||||
return out_link_pos, out_link_quat, out_spheres
|
||||
|
||||
@staticmethod
|
||||
def backward(ctx, grad_out_link_pos, grad_out_link_quat, grad_out_spheres):
|
||||
@@ -197,9 +125,11 @@ class KinematicsFusedFunction(Function):
|
||||
store_link_map,
|
||||
link_sphere_map,
|
||||
link_chain_map,
|
||||
joint_offset_map,
|
||||
grad_out,
|
||||
global_cumul_mat,
|
||||
) = ctx.saved_tensors
|
||||
|
||||
grad_joint = KinematicsFusedFunction._call_backward_cuda(
|
||||
grad_out,
|
||||
grad_out_link_pos,
|
||||
@@ -215,7 +145,9 @@ class KinematicsFusedFunction(Function):
|
||||
store_link_map,
|
||||
link_sphere_map,
|
||||
link_chain_map,
|
||||
joint_offset_map,
|
||||
True,
|
||||
use_global_cumul=ctx.use_global_cumul,
|
||||
)
|
||||
|
||||
return (
|
||||
@@ -233,53 +165,10 @@ class KinematicsFusedFunction(Function):
|
||||
None,
|
||||
None,
|
||||
None,
|
||||
None,
|
||||
None,
|
||||
)
|
||||
|
||||
|
||||
class KinematicsFusedGlobalCumulFunction(Function):
|
||||
@staticmethod
|
||||
def _call_cuda(
|
||||
link_pos_out,
|
||||
link_quat_out,
|
||||
robot_sphere_out,
|
||||
global_cumul_mat,
|
||||
angle,
|
||||
fixed_transform,
|
||||
robot_spheres,
|
||||
link_map,
|
||||
joint_map,
|
||||
joint_map_type,
|
||||
store_link_map,
|
||||
link_sphere_map,
|
||||
):
|
||||
b_shape = link_pos_out.shape
|
||||
b_size = b_shape[0]
|
||||
n_spheres = robot_sphere_out.shape[1]
|
||||
# print(n_spheres)
|
||||
# print(angle)
|
||||
r = kinematics_fused_cu.forward(
|
||||
link_pos_out,
|
||||
link_quat_out,
|
||||
robot_sphere_out,
|
||||
global_cumul_mat,
|
||||
angle,
|
||||
fixed_transform,
|
||||
robot_spheres,
|
||||
link_map,
|
||||
joint_map,
|
||||
joint_map_type,
|
||||
store_link_map,
|
||||
link_sphere_map,
|
||||
b_size,
|
||||
n_spheres,
|
||||
True,
|
||||
)
|
||||
out_link_pos = r[0]
|
||||
out_link_quat = r[1]
|
||||
out_spheres = r[2]
|
||||
global_cumul_mat = r[3]
|
||||
return out_link_pos, out_link_quat, out_spheres, global_cumul_mat
|
||||
|
||||
@staticmethod
|
||||
def _call_backward_cuda(
|
||||
grad_out,
|
||||
@@ -296,18 +185,18 @@ class KinematicsFusedGlobalCumulFunction(Function):
|
||||
store_link_map,
|
||||
link_sphere_map,
|
||||
link_chain_map,
|
||||
joint_offset_map,
|
||||
sparsity_opt=True,
|
||||
use_global_cumul=False,
|
||||
):
|
||||
b_shape = link_pos_out.shape
|
||||
b_shape = grad_out.shape
|
||||
b_size = b_shape[0]
|
||||
# b_size = link_mat_out.shape[0]
|
||||
n_spheres = robot_sphere_out.shape[1]
|
||||
n_joints = angle.shape[-1]
|
||||
if grad_out.is_contiguous():
|
||||
grad_out = grad_out.view(-1)
|
||||
else:
|
||||
grad_out = grad_out.reshape(-1)
|
||||
# create backward tensors?
|
||||
|
||||
r = kinematics_fused_cu.backward(
|
||||
grad_out,
|
||||
link_pos_out,
|
||||
@@ -323,126 +212,17 @@ class KinematicsFusedGlobalCumulFunction(Function):
|
||||
store_link_map,
|
||||
link_sphere_map,
|
||||
link_chain_map,
|
||||
joint_offset_map, # offset_joint_map
|
||||
b_size,
|
||||
n_joints,
|
||||
n_spheres,
|
||||
sparsity_opt,
|
||||
True,
|
||||
use_global_cumul,
|
||||
)
|
||||
out_q = r[0] # .view(angle.shape)
|
||||
out_q = out_q.view(b_size, -1)
|
||||
out_q = r[0].view(b_size, -1)
|
||||
|
||||
return out_q
|
||||
|
||||
@staticmethod
|
||||
def forward(
|
||||
ctx,
|
||||
# link_mats: torch.Tensor,
|
||||
link_pos: torch.Tensor,
|
||||
link_quat: torch.Tensor,
|
||||
b_robot_spheres: torch.tensor,
|
||||
global_cumul_mat: torch.Tensor,
|
||||
joint_seq: torch.Tensor,
|
||||
fixed_transform: torch.tensor,
|
||||
robot_spheres: torch.tensor,
|
||||
link_map: torch.tensor,
|
||||
joint_map: torch.Tensor,
|
||||
joint_map_type: torch.Tensor,
|
||||
store_link_map: torch.Tensor,
|
||||
link_sphere_map: torch.Tensor,
|
||||
link_chain_map: torch.Tensor,
|
||||
grad_out: torch.Tensor,
|
||||
):
|
||||
if joint_seq.is_contiguous():
|
||||
joint_seq = joint_seq.view(-1)
|
||||
else:
|
||||
joint_seq = joint_seq.reshape(-1)
|
||||
(
|
||||
link_pos,
|
||||
link_quat,
|
||||
b_robot_spheres,
|
||||
global_cumul_mat,
|
||||
) = KinematicsFusedGlobalCumulFunction._call_cuda(
|
||||
link_pos,
|
||||
link_quat,
|
||||
b_robot_spheres,
|
||||
global_cumul_mat,
|
||||
joint_seq,
|
||||
fixed_transform,
|
||||
robot_spheres,
|
||||
link_map,
|
||||
joint_map,
|
||||
joint_map_type,
|
||||
store_link_map,
|
||||
link_sphere_map,
|
||||
)
|
||||
ctx.save_for_backward(
|
||||
joint_seq,
|
||||
fixed_transform,
|
||||
robot_spheres,
|
||||
link_map,
|
||||
joint_map,
|
||||
joint_map_type,
|
||||
store_link_map,
|
||||
link_sphere_map,
|
||||
link_chain_map,
|
||||
grad_out,
|
||||
global_cumul_mat,
|
||||
)
|
||||
|
||||
return link_pos, link_quat, b_robot_spheres
|
||||
|
||||
@staticmethod
|
||||
def backward(ctx, grad_out_link_pos, grad_out_link_quat, grad_out_spheres):
|
||||
grad_joint = None
|
||||
|
||||
if ctx.needs_input_grad[4]:
|
||||
(
|
||||
joint_seq,
|
||||
fixed_transform,
|
||||
robot_spheres,
|
||||
link_map,
|
||||
joint_map,
|
||||
joint_map_type,
|
||||
store_link_map,
|
||||
link_sphere_map,
|
||||
link_chain_map,
|
||||
grad_out,
|
||||
global_cumul_mat,
|
||||
) = ctx.saved_tensors
|
||||
grad_joint = KinematicsFusedGlobalCumulFunction._call_backward_cuda(
|
||||
grad_out,
|
||||
grad_out_link_pos,
|
||||
grad_out_link_quat,
|
||||
grad_out_spheres,
|
||||
global_cumul_mat,
|
||||
joint_seq,
|
||||
fixed_transform,
|
||||
robot_spheres,
|
||||
link_map,
|
||||
joint_map,
|
||||
joint_map_type,
|
||||
store_link_map,
|
||||
link_sphere_map,
|
||||
link_chain_map,
|
||||
True,
|
||||
)
|
||||
|
||||
return (
|
||||
None,
|
||||
None,
|
||||
None,
|
||||
None,
|
||||
grad_joint,
|
||||
None,
|
||||
None,
|
||||
None,
|
||||
None,
|
||||
None,
|
||||
None,
|
||||
None,
|
||||
None,
|
||||
None,
|
||||
)
|
||||
|
||||
|
||||
def get_cuda_kinematics(
|
||||
link_pos_seq,
|
||||
@@ -458,41 +238,28 @@ def get_cuda_kinematics(
|
||||
store_link_map,
|
||||
link_sphere_idx_map, # sphere idx map
|
||||
link_chain_map,
|
||||
joint_offset_map,
|
||||
grad_out_q,
|
||||
use_global_cumul: bool = True,
|
||||
):
|
||||
if use_global_cumul:
|
||||
link_pos, link_quat, robot_spheres = KinematicsFusedGlobalCumulFunction.apply(
|
||||
link_pos_seq,
|
||||
link_quat_seq,
|
||||
batch_robot_spheres,
|
||||
global_cumul_mat,
|
||||
q_in,
|
||||
fixed_transform,
|
||||
link_spheres_tensor,
|
||||
link_map, # tells which link is attached to which link i
|
||||
joint_map, # tells which joint is attached to a link i
|
||||
joint_map_type, # joint type
|
||||
store_link_map,
|
||||
link_sphere_idx_map, # sphere idx map
|
||||
link_chain_map,
|
||||
grad_out_q,
|
||||
)
|
||||
else:
|
||||
link_pos, link_quat, robot_spheres = KinematicsFusedFunction.apply(
|
||||
link_pos_seq,
|
||||
link_quat_seq,
|
||||
batch_robot_spheres,
|
||||
global_cumul_mat,
|
||||
q_in,
|
||||
fixed_transform,
|
||||
link_spheres_tensor,
|
||||
link_map, # tells which link is attached to which link i
|
||||
joint_map, # tells which joint is attached to a link i
|
||||
joint_map_type, # joint type
|
||||
store_link_map,
|
||||
link_sphere_idx_map, # sphere idx map
|
||||
link_chain_map,
|
||||
grad_out_q,
|
||||
)
|
||||
# if not q_in.is_contiguous():
|
||||
# q_in = q_in.contiguous()
|
||||
link_pos, link_quat, robot_spheres = KinematicsFusedFunction.apply(
|
||||
link_pos_seq,
|
||||
link_quat_seq,
|
||||
batch_robot_spheres,
|
||||
global_cumul_mat,
|
||||
q_in,
|
||||
fixed_transform,
|
||||
link_spheres_tensor,
|
||||
link_map, # tells which link is attached to which link i
|
||||
joint_map, # tells which joint is attached to a link i
|
||||
joint_map_type, # joint type
|
||||
store_link_map,
|
||||
link_sphere_idx_map, # sphere idx map
|
||||
link_chain_map,
|
||||
joint_offset_map,
|
||||
grad_out_q,
|
||||
use_global_cumul,
|
||||
)
|
||||
return link_pos, link_quat, robot_spheres
|
||||
|
||||
@@ -263,13 +263,16 @@ class WorldCollisionConfig:
|
||||
cache: Optional[Dict[Obstacle, int]] = None
|
||||
n_envs: int = 1
|
||||
checker_type: CollisionCheckerType = CollisionCheckerType.PRIMITIVE
|
||||
max_distance: Union[torch.Tensor, float] = 0.01
|
||||
max_distance: Union[torch.Tensor, float] = 0.1
|
||||
max_esdf_distance: Union[torch.Tensor, float] = 1000.0
|
||||
|
||||
def __post_init__(self):
|
||||
if self.world_model is not None and isinstance(self.world_model, list):
|
||||
self.n_envs = len(self.world_model)
|
||||
if isinstance(self.max_distance, float):
|
||||
self.max_distance = self.tensor_args.to_device([self.max_distance])
|
||||
if isinstance(self.max_esdf_distance, float):
|
||||
self.max_esdf_distance = self.tensor_args.to_device([self.max_esdf_distance])
|
||||
|
||||
@staticmethod
|
||||
def load_from_dict(
|
||||
|
||||
@@ -18,7 +18,7 @@ import torch.autograd.profiler as profiler
|
||||
|
||||
# CuRobo
|
||||
from curobo.geom.sdf.world import CollisionQueryBuffer, WorldCollisionConfig
|
||||
from curobo.geom.sdf.world_mesh import WorldMeshCollision
|
||||
from curobo.geom.sdf.world_voxel import WorldVoxelCollision
|
||||
from curobo.geom.types import Cuboid, Mesh, Sphere, SphereFitType, WorldConfig
|
||||
from curobo.types.camera import CameraObservation
|
||||
from curobo.types.math import Pose
|
||||
@@ -33,7 +33,7 @@ except ImportError:
|
||||
from abc import ABC as Mapper
|
||||
|
||||
|
||||
class WorldBloxCollision(WorldMeshCollision):
|
||||
class WorldBloxCollision(WorldVoxelCollision):
|
||||
"""World Collision Representaiton using Nvidia's nvblox library.
|
||||
|
||||
This class depends on pytorch wrapper for nvblox.
|
||||
@@ -127,6 +127,7 @@ class WorldBloxCollision(WorldMeshCollision):
|
||||
collision_query_buffer: CollisionQueryBuffer,
|
||||
weight,
|
||||
activation_distance,
|
||||
compute_esdf: bool = False,
|
||||
):
|
||||
d = self._blox_mapper.query_sphere_sdf_cost(
|
||||
query_spheres,
|
||||
@@ -196,6 +197,7 @@ class WorldBloxCollision(WorldMeshCollision):
|
||||
collision_query_buffer,
|
||||
weight=weight,
|
||||
activation_distance=activation_distance,
|
||||
compute_esdf=compute_esdf,
|
||||
)
|
||||
|
||||
if ("primitive" not in self.collision_types or not self.collision_types["primitive"]) and (
|
||||
@@ -227,6 +229,7 @@ class WorldBloxCollision(WorldMeshCollision):
|
||||
activation_distance: torch.Tensor,
|
||||
env_query_idx=None,
|
||||
return_loss: bool = False,
|
||||
**kwargs,
|
||||
):
|
||||
if "blox" not in self.collision_types or not self.collision_types["blox"]:
|
||||
return super().get_sphere_collision(
|
||||
|
||||
@@ -48,11 +48,10 @@ class WorldVoxelCollision(WorldMeshCollision):
|
||||
dims = voxel_cache["dims"]
|
||||
voxel_size = voxel_cache["voxel_size"]
|
||||
feature_dtype = voxel_cache["feature_dtype"]
|
||||
n_voxels = int(
|
||||
math.floor(dims[0] / voxel_size)
|
||||
* math.floor(dims[1] / voxel_size)
|
||||
* math.floor(dims[2] / voxel_size)
|
||||
)
|
||||
grid_shape = VoxelGrid(
|
||||
"test", pose=[0, 0, 0, 1, 0, 0, 0], dims=dims, voxel_size=voxel_size
|
||||
).get_grid_shape()[0]
|
||||
n_voxels = grid_shape[0] * grid_shape[1] * grid_shape[2]
|
||||
|
||||
voxel_params = torch.zeros(
|
||||
(self.n_envs, n_layers, 4),
|
||||
@@ -77,6 +76,12 @@ class WorldVoxelCollision(WorldMeshCollision):
|
||||
dtype=feature_dtype,
|
||||
)
|
||||
|
||||
if feature_dtype in [torch.float32, torch.float16, torch.bfloat16]:
|
||||
voxel_features[:] = -1.0 * self.max_esdf_distance
|
||||
else:
|
||||
voxel_features = (voxel_features.to(dtype=torch.float16) - self.max_esdf_distance).to(
|
||||
dtype=feature_dtype
|
||||
)
|
||||
self._voxel_tensor_list = [voxel_params, voxel_pose, voxel_enable, voxel_features]
|
||||
self.collision_types["voxel"] = True
|
||||
self._env_voxel_names = [[None for _ in range(n_layers)] for _ in range(self.n_envs)]
|
||||
@@ -84,14 +89,14 @@ class WorldVoxelCollision(WorldMeshCollision):
|
||||
def load_collision_model(
|
||||
self, world_model: WorldConfig, env_idx=0, fix_cache_reference: bool = False
|
||||
):
|
||||
self._load_collision_model_in_cache(
|
||||
self._load_voxel_collision_model_in_cache(
|
||||
world_model, env_idx, fix_cache_reference=fix_cache_reference
|
||||
)
|
||||
return super().load_collision_model(
|
||||
world_model, env_idx=env_idx, fix_cache_reference=fix_cache_reference
|
||||
)
|
||||
|
||||
def _load_collision_model_in_cache(
|
||||
def _load_voxel_collision_model_in_cache(
|
||||
self, world_config: WorldConfig, env_idx: int = 0, fix_cache_reference: bool = False
|
||||
):
|
||||
"""TODO:
|
||||
@@ -396,9 +401,10 @@ class WorldVoxelCollision(WorldMeshCollision):
|
||||
|
||||
b, h, n, _ = query_sphere.shape # This can be read from collision query buffer
|
||||
use_batch_env = True
|
||||
env_query_idx_voxel = env_query_idx
|
||||
if env_query_idx is None:
|
||||
use_batch_env = False
|
||||
env_query_idx = self._env_n_voxels
|
||||
env_query_idx_voxel = self._env_n_voxels
|
||||
dist = SdfSphereVoxel.apply(
|
||||
query_sphere,
|
||||
collision_query_buffer.voxel_collision_buffer.distance_buffer,
|
||||
@@ -406,13 +412,13 @@ class WorldVoxelCollision(WorldMeshCollision):
|
||||
collision_query_buffer.voxel_collision_buffer.sparsity_index_buffer,
|
||||
weight,
|
||||
activation_distance,
|
||||
self.max_distance,
|
||||
self.max_esdf_distance,
|
||||
self._voxel_tensor_list[3],
|
||||
self._voxel_tensor_list[0],
|
||||
self._voxel_tensor_list[1],
|
||||
self._voxel_tensor_list[2],
|
||||
self._env_n_voxels,
|
||||
env_query_idx,
|
||||
env_query_idx_voxel,
|
||||
self._voxel_tensor_list[0].shape[1],
|
||||
b,
|
||||
h,
|
||||
@@ -424,12 +430,8 @@ class WorldVoxelCollision(WorldMeshCollision):
|
||||
sum_collisions,
|
||||
compute_esdf,
|
||||
)
|
||||
|
||||
if (
|
||||
"primitive" not in self.collision_types
|
||||
or not self.collision_types["primitive"]
|
||||
or "mesh" not in self.collision_types
|
||||
or not self.collision_types["mesh"]
|
||||
if ("primitive" not in self.collision_types or not self.collision_types["primitive"]) and (
|
||||
"mesh" not in self.collision_types or not self.collision_types["mesh"]
|
||||
):
|
||||
return dist
|
||||
d_prim = super().get_sphere_distance(
|
||||
@@ -443,9 +445,10 @@ class WorldVoxelCollision(WorldMeshCollision):
|
||||
compute_esdf=compute_esdf,
|
||||
)
|
||||
if compute_esdf:
|
||||
|
||||
d_val = torch.maximum(dist.view(d_prim.shape), d_prim)
|
||||
else:
|
||||
d_val = d_val.view(d_prim.shape) + d_prim
|
||||
d_val = dist.view(d_prim.shape) + d_prim
|
||||
|
||||
return d_val
|
||||
|
||||
@@ -473,9 +476,10 @@ class WorldVoxelCollision(WorldMeshCollision):
|
||||
raise ValueError("cannot return loss for classification, use get_sphere_distance")
|
||||
b, h, n, _ = query_sphere.shape
|
||||
use_batch_env = True
|
||||
env_query_idx_voxel = env_query_idx
|
||||
if env_query_idx is None:
|
||||
use_batch_env = False
|
||||
env_query_idx = self._env_n_voxels
|
||||
env_query_idx_voxel = self._env_n_voxels
|
||||
dist = SdfSphereVoxel.apply(
|
||||
query_sphere,
|
||||
collision_query_buffer.voxel_collision_buffer.distance_buffer,
|
||||
@@ -483,13 +487,13 @@ class WorldVoxelCollision(WorldMeshCollision):
|
||||
collision_query_buffer.voxel_collision_buffer.sparsity_index_buffer,
|
||||
weight,
|
||||
activation_distance,
|
||||
self.max_distance,
|
||||
self.max_esdf_distance,
|
||||
self._voxel_tensor_list[3],
|
||||
self._voxel_tensor_list[0],
|
||||
self._voxel_tensor_list[1],
|
||||
self._voxel_tensor_list[2],
|
||||
self._env_n_voxels,
|
||||
env_query_idx,
|
||||
env_query_idx_voxel,
|
||||
self._voxel_tensor_list[0].shape[1],
|
||||
b,
|
||||
h,
|
||||
@@ -501,11 +505,8 @@ class WorldVoxelCollision(WorldMeshCollision):
|
||||
True,
|
||||
False,
|
||||
)
|
||||
if (
|
||||
"primitive" not in self.collision_types
|
||||
or not self.collision_types["primitive"]
|
||||
or "mesh" not in self.collision_types
|
||||
or not self.collision_types["mesh"]
|
||||
if ("primitive" not in self.collision_types or not self.collision_types["primitive"]) and (
|
||||
"mesh" not in self.collision_types or not self.collision_types["mesh"]
|
||||
):
|
||||
return dist
|
||||
d_prim = super().get_sphere_collision(
|
||||
@@ -552,9 +553,10 @@ class WorldVoxelCollision(WorldMeshCollision):
|
||||
)
|
||||
b, h, n, _ = query_sphere.shape
|
||||
use_batch_env = True
|
||||
env_query_idx_voxel = env_query_idx
|
||||
if env_query_idx is None:
|
||||
use_batch_env = False
|
||||
env_query_idx = self._env_n_voxels
|
||||
env_query_idx_voxel = self._env_n_voxels
|
||||
|
||||
dist = SdfSweptSphereVoxel.apply(
|
||||
query_sphere,
|
||||
@@ -563,14 +565,14 @@ class WorldVoxelCollision(WorldMeshCollision):
|
||||
collision_query_buffer.voxel_collision_buffer.sparsity_index_buffer,
|
||||
weight,
|
||||
activation_distance,
|
||||
self.max_distance,
|
||||
self.max_esdf_distance,
|
||||
speed_dt,
|
||||
self._voxel_tensor_list[3],
|
||||
self._voxel_tensor_list[0],
|
||||
self._voxel_tensor_list[1],
|
||||
self._voxel_tensor_list[2],
|
||||
self._env_n_voxels,
|
||||
env_query_idx,
|
||||
env_query_idx_voxel,
|
||||
self._voxel_tensor_list[0].shape[1],
|
||||
b,
|
||||
h,
|
||||
@@ -583,12 +585,8 @@ class WorldVoxelCollision(WorldMeshCollision):
|
||||
return_loss,
|
||||
sum_collisions,
|
||||
)
|
||||
|
||||
if (
|
||||
"primitive" not in self.collision_types
|
||||
or not self.collision_types["primitive"]
|
||||
or "mesh" not in self.collision_types
|
||||
or not self.collision_types["mesh"]
|
||||
if ("primitive" not in self.collision_types or not self.collision_types["primitive"]) and (
|
||||
"mesh" not in self.collision_types or not self.collision_types["mesh"]
|
||||
):
|
||||
return dist
|
||||
d_prim = super().get_swept_sphere_distance(
|
||||
@@ -641,9 +639,10 @@ class WorldVoxelCollision(WorldMeshCollision):
|
||||
b, h, n, _ = query_sphere.shape
|
||||
|
||||
use_batch_env = True
|
||||
env_query_idx_voxel = env_query_idx
|
||||
if env_query_idx is None:
|
||||
use_batch_env = False
|
||||
env_query_idx = self._env_n_voxels
|
||||
env_query_idx_voxel = self._env_n_voxels
|
||||
dist = SdfSweptSphereVoxel.apply(
|
||||
query_sphere,
|
||||
collision_query_buffer.voxel_collision_buffer.distance_buffer,
|
||||
@@ -651,14 +650,14 @@ class WorldVoxelCollision(WorldMeshCollision):
|
||||
collision_query_buffer.voxel_collision_buffer.sparsity_index_buffer,
|
||||
weight,
|
||||
activation_distance,
|
||||
self.max_distance,
|
||||
self.max_esdf_distance,
|
||||
speed_dt,
|
||||
self._voxel_tensor_list[3],
|
||||
self._voxel_tensor_list[0],
|
||||
self._voxel_tensor_list[1],
|
||||
self._voxel_tensor_list[2],
|
||||
self._env_n_voxels,
|
||||
env_query_idx,
|
||||
env_query_idx_voxel,
|
||||
self._voxel_tensor_list[0].shape[1],
|
||||
b,
|
||||
h,
|
||||
@@ -671,11 +670,8 @@ class WorldVoxelCollision(WorldMeshCollision):
|
||||
return_loss,
|
||||
True,
|
||||
)
|
||||
if (
|
||||
"primitive" not in self.collision_types
|
||||
or not self.collision_types["primitive"]
|
||||
or "mesh" not in self.collision_types
|
||||
or not self.collision_types["mesh"]
|
||||
if ("primitive" not in self.collision_types or not self.collision_types["primitive"]) and (
|
||||
"mesh" not in self.collision_types or not self.collision_types["mesh"]
|
||||
):
|
||||
return dist
|
||||
d_prim = super().get_swept_sphere_collision(
|
||||
@@ -695,5 +691,15 @@ class WorldVoxelCollision(WorldMeshCollision):
|
||||
def clear_cache(self):
|
||||
if self._voxel_tensor_list is not None:
|
||||
self._voxel_tensor_list[2][:] = 0
|
||||
self._voxel_tensor_list[-1][:] = -1.0 * self.max_distance
|
||||
if self._voxel_tensor_list[3].dtype in [torch.float32, torch.float16, torch.bfloat16]:
|
||||
self._voxel_tensor_list[3][:] = -1.0 * self.max_esdf_distance
|
||||
else:
|
||||
self._voxel_tensor_list[3][:] = (
|
||||
self._voxel_tensor_list[3].to(dtype=torch.float16) * 0.0
|
||||
- self.max_esdf_distance
|
||||
).to(dtype=self._voxel_tensor_list[3].dtype)
|
||||
self._env_n_voxels[:] = 0
|
||||
print(self._voxel_tensor_list)
|
||||
|
||||
def get_voxel_grid_shape(self, env_idx: int = 0, obs_idx: int = 0):
|
||||
return self._voxel_tensor_list[3][env_idx, obs_idx].shape
|
||||
|
||||
@@ -577,22 +577,24 @@ class VoxelGrid(Obstacle):
|
||||
if self.feature_tensor is not None:
|
||||
self.feature_dtype = self.feature_tensor.dtype
|
||||
|
||||
def create_xyzr_tensor(
|
||||
self, transform_to_origin: bool = False, tensor_args: TensorDeviceType = TensorDeviceType()
|
||||
):
|
||||
def get_grid_shape(self):
|
||||
bounds = self.dims
|
||||
low = [-bounds[0] / 2, -bounds[1] / 2, -bounds[2] / 2]
|
||||
high = [bounds[0] / 2, bounds[1] / 2, bounds[2] / 2]
|
||||
trange = [h - l for l, h in zip(low, high)]
|
||||
x = torch.linspace(
|
||||
low[0], high[0], int(math.floor(trange[0] / self.voxel_size)), device=tensor_args.device
|
||||
)
|
||||
y = torch.linspace(
|
||||
low[1], high[1], int(math.floor(trange[1] / self.voxel_size)), device=tensor_args.device
|
||||
)
|
||||
z = torch.linspace(
|
||||
low[2], high[2], int(math.floor(trange[2] / self.voxel_size)), device=tensor_args.device
|
||||
)
|
||||
grid_shape = [
|
||||
1 + int(high[i] / self.voxel_size) - (int(low[i] / self.voxel_size))
|
||||
for i in range(len(low))
|
||||
]
|
||||
return grid_shape, low, high
|
||||
|
||||
def create_xyzr_tensor(
|
||||
self, transform_to_origin: bool = False, tensor_args: TensorDeviceType = TensorDeviceType()
|
||||
):
|
||||
trange, low, high = self.get_grid_shape()
|
||||
|
||||
x = torch.linspace(low[0], high[0], trange[0], device=tensor_args.device)
|
||||
y = torch.linspace(low[1], high[1], trange[1], device=tensor_args.device)
|
||||
z = torch.linspace(low[2], high[2], trange[2], device=tensor_args.device)
|
||||
w, l, h = x.shape[0], y.shape[0], z.shape[0]
|
||||
xyz = (
|
||||
torch.stack(torch.meshgrid(x, y, z, indexing="ij")).permute((1, 2, 3, 0)).reshape(-1, 3)
|
||||
@@ -603,11 +605,12 @@ class VoxelGrid(Obstacle):
|
||||
xyz = pose.transform_points(xyz.contiguous())
|
||||
r = torch.zeros_like(xyz[:, 0:1]) + (self.voxel_size * 0.5)
|
||||
xyzr = torch.cat([xyz, r], dim=1)
|
||||
|
||||
return xyzr
|
||||
|
||||
def get_occupied_voxels(self, feature_threshold: Optional[float] = None):
|
||||
if feature_threshold is None:
|
||||
feature_threshold = -1.0 * self.voxel_size
|
||||
feature_threshold = -0.5 * self.voxel_size
|
||||
if self.xyzr_tensor is None or self.feature_tensor is None:
|
||||
log_error("Feature tensor or xyzr tensor is empty")
|
||||
xyzr = self.xyzr_tensor.clone()
|
||||
|
||||
@@ -144,6 +144,7 @@ class GraphConfig:
|
||||
graph_file: str = "graph.yml",
|
||||
self_collision_check: bool = True,
|
||||
use_cuda_graph: bool = True,
|
||||
seed: Optional[int] = None,
|
||||
):
|
||||
graph_data = load_yaml(join_path(get_task_configs_path(), graph_file))
|
||||
base_config_data = load_yaml(join_path(get_task_configs_path(), base_cfg_file))
|
||||
@@ -182,6 +183,8 @@ class GraphConfig:
|
||||
arm_base_cg_rollout = ArmBase(cfg_cg)
|
||||
else:
|
||||
arm_base_cg_rollout = arm_base
|
||||
if seed is not None:
|
||||
graph_data["graph"]["seed"] = seed
|
||||
graph_cfg = GraphConfig.from_dict(
|
||||
graph_data["graph"],
|
||||
tensor_args,
|
||||
@@ -911,7 +914,7 @@ class GraphPlanBase(GraphConfig):
|
||||
i = self.i
|
||||
if x_set is not None:
|
||||
if x_set.shape[0] == 0:
|
||||
log_warn("no valid configuration found")
|
||||
log_info("no valid configuration found")
|
||||
return
|
||||
|
||||
if connect_mode == "radius":
|
||||
|
||||
@@ -274,7 +274,7 @@ class ArmReacher(ArmBase, ArmReacherConfig):
|
||||
current_fn = self._link_pose_costs[k]
|
||||
if current_fn.enabled:
|
||||
# get link pose
|
||||
current_pose = link_poses[k]
|
||||
current_pose = link_poses[k].contiguous()
|
||||
current_pos = current_pose.position
|
||||
current_quat = current_pose.quaternion
|
||||
|
||||
|
||||
@@ -417,6 +417,7 @@ class Goal(Sequence):
|
||||
class RolloutConfig:
|
||||
tensor_args: TensorDeviceType
|
||||
sum_horizon: bool = False
|
||||
sampler_seed: int = 1312
|
||||
|
||||
|
||||
class RolloutBase:
|
||||
@@ -436,7 +437,7 @@ class RolloutBase:
|
||||
self.tensor_args,
|
||||
up_bounds=self.action_bound_highs,
|
||||
low_bounds=self.action_bound_lows,
|
||||
seed=1312,
|
||||
seed=self.sampler_seed,
|
||||
)
|
||||
|
||||
@abstractmethod
|
||||
|
||||
@@ -261,7 +261,6 @@ class Pose(Sequence):
|
||||
position=clone_if_not_none(self.position),
|
||||
quaternion=clone_if_not_none(self.quaternion),
|
||||
normalize_rotation=False,
|
||||
# rotation=clone_if_not_none(self.rotation),
|
||||
)
|
||||
|
||||
def to(
|
||||
@@ -452,6 +451,13 @@ class Pose(Sequence):
|
||||
def compute_local_pose(self, world_pose: Pose) -> Pose:
|
||||
return self.inverse().multiply(world_pose)
|
||||
|
||||
def contiguous(self) -> Pose:
|
||||
return Pose(
|
||||
position=self.position.contiguous() if self.position is not None else None,
|
||||
quaternion=self.quaternion.contiguous() if self.quaternion is not None else None,
|
||||
normalize_rotation=False,
|
||||
)
|
||||
|
||||
|
||||
def quat_multiply(q1, q2, q_res):
|
||||
a_w = q1[..., 0]
|
||||
|
||||
@@ -512,6 +512,26 @@ class JointState(State):
|
||||
def shape(self):
|
||||
return self.position.shape
|
||||
|
||||
def index_dof(self, idx: int):
|
||||
# get index of joint names:
|
||||
velocity = acceleration = jerk = None
|
||||
new_index = idx
|
||||
position = torch.index_select(self.position, -1, new_index)
|
||||
if self.velocity is not None:
|
||||
velocity = torch.index_select(self.velocity, -1, new_index)
|
||||
if self.acceleration is not None:
|
||||
acceleration = torch.index_select(self.acceleration, -1, new_index)
|
||||
if self.jerk is not None:
|
||||
jerk = torch.index_select(self.jerk, -1, new_index)
|
||||
joint_names = [self.joint_names[x] for x in new_index]
|
||||
return JointState(
|
||||
position=position,
|
||||
joint_names=joint_names,
|
||||
velocity=velocity,
|
||||
acceleration=acceleration,
|
||||
jerk=jerk,
|
||||
)
|
||||
|
||||
|
||||
@get_torch_jit_decorator()
|
||||
def jit_js_scale(
|
||||
|
||||
@@ -499,7 +499,7 @@ class HaltonGenerator:
|
||||
def get_samples(self, num_samples, bounded=False):
|
||||
samples = self._get_samples(num_samples)
|
||||
if bounded:
|
||||
samples = samples * self.range_b + self.low_bounds
|
||||
samples = bound_samples(samples, self.range_b, self.low_bounds)
|
||||
return samples
|
||||
|
||||
@profiler.record_function("halton_generator/gaussian_samples")
|
||||
@@ -512,6 +512,12 @@ class HaltonGenerator:
|
||||
return gaussian_halton_samples
|
||||
|
||||
|
||||
@get_torch_jit_decorator()
|
||||
def bound_samples(samples: torch.Tensor, range_b: torch.Tensor, low_bounds: torch.Tensor):
|
||||
samples = samples * range_b + low_bounds
|
||||
return samples
|
||||
|
||||
|
||||
@get_torch_jit_decorator()
|
||||
def gaussian_transform(
|
||||
uniform_samples: torch.Tensor, proj_mat: torch.Tensor, i_mat: torch.Tensor, std_dev: float
|
||||
|
||||
@@ -842,7 +842,7 @@ class UsdHelper:
|
||||
usd_helper.add_world_to_stage(world_model, base_frame=base_frame)
|
||||
|
||||
animation_links = kin_model.kinematics_config.mesh_link_names
|
||||
animation_poses = kin_model.get_link_poses(q_traj.position, animation_links)
|
||||
animation_poses = kin_model.get_link_poses(q_traj.position.contiguous(), animation_links)
|
||||
# add offsets for visual mesh:
|
||||
for i, ival in enumerate(offsets):
|
||||
offset_pose = Pose.from_list(ival)
|
||||
|
||||
@@ -59,6 +59,7 @@ class IKSolverConfig:
|
||||
sample_rejection_ratio: int = 50
|
||||
tensor_args: TensorDeviceType = TensorDeviceType()
|
||||
use_cuda_graph: bool = True
|
||||
seed: int = 1531
|
||||
|
||||
@staticmethod
|
||||
@profiler.record_function("ik_solver/load_from_robot_config")
|
||||
@@ -94,6 +95,7 @@ class IKSolverConfig:
|
||||
collision_activation_distance: Optional[float] = None,
|
||||
high_precision: bool = False,
|
||||
project_pose_to_goal_frame: bool = True,
|
||||
seed: int = 1531,
|
||||
):
|
||||
if position_threshold <= 0.001:
|
||||
high_precision = True
|
||||
@@ -104,6 +106,11 @@ class IKSolverConfig:
|
||||
base_config_data = load_yaml(join_path(get_task_configs_path(), base_cfg_file))
|
||||
config_data = load_yaml(join_path(get_task_configs_path(), particle_file))
|
||||
grad_config_data = load_yaml(join_path(get_task_configs_path(), gradient_file))
|
||||
base_config_data["cost"]["pose_cfg"]["project_distance"] = project_pose_to_goal_frame
|
||||
base_config_data["convergence"]["pose_cfg"]["project_distance"] = project_pose_to_goal_frame
|
||||
config_data["cost"]["pose_cfg"]["project_distance"] = project_pose_to_goal_frame
|
||||
grad_config_data["cost"]["pose_cfg"]["project_distance"] = project_pose_to_goal_frame
|
||||
|
||||
|
||||
if collision_cache is not None:
|
||||
base_config_data["world_collision_checker_cfg"]["cache"] = collision_cache
|
||||
@@ -247,6 +254,7 @@ class IKSolverConfig:
|
||||
rollout_fn=aux_rollout,
|
||||
tensor_args=tensor_args,
|
||||
use_cuda_graph=use_cuda_graph,
|
||||
seed=seed,
|
||||
)
|
||||
return ik_cfg
|
||||
|
||||
@@ -318,24 +326,21 @@ class IKResult(Sequence):
|
||||
class IKSolver(IKSolverConfig):
|
||||
def __init__(self, config: IKSolverConfig) -> None:
|
||||
super().__init__(**vars(config))
|
||||
# self._solve_
|
||||
self.batch_size = -1
|
||||
self._num_seeds = self.num_seeds
|
||||
self.init_state = JointState.from_position(
|
||||
self.solver.rollout_fn.retract_state.unsqueeze(0)
|
||||
)
|
||||
self.dof = self.solver.safety_rollout.d_action
|
||||
self._col = None # torch.arange(0, 1, device=self.tensor_args.device, dtype=torch.long)
|
||||
self._col = None
|
||||
|
||||
# self.fixed_seeds = self.solver.safety_rollout.sample_random_actions(100 * 200)
|
||||
# create random seeder:
|
||||
self.q_sample_gen = HaltonGenerator(
|
||||
self.dof,
|
||||
self.tensor_args,
|
||||
up_bounds=self.solver.safety_rollout.action_bound_highs,
|
||||
low_bounds=self.solver.safety_rollout.action_bound_lows,
|
||||
seed=1531,
|
||||
# store_buffer=1000,
|
||||
seed=self.seed,
|
||||
)
|
||||
|
||||
# load ik nn:
|
||||
@@ -913,21 +918,13 @@ class IKSolver(IKSolverConfig):
|
||||
seed_list = []
|
||||
if use_nn_seed and self.ik_nn_seeder is not None:
|
||||
num_random_seeds = num_seeds - 1
|
||||
in_data = torch.cat(
|
||||
(pose.position, pose.quaternion), dim=-1
|
||||
) # .to(dtype=torch.float32)
|
||||
nn_seed = self.ik_nn_seeder(in_data).view(
|
||||
batch, 1, self.dof
|
||||
) # .to(dtype=self.tensor_args.dtype)
|
||||
in_data = torch.cat((pose.position, pose.quaternion), dim=-1)
|
||||
nn_seed = self.ik_nn_seeder(in_data).view(batch, 1, self.dof)
|
||||
seed_list.append(nn_seed)
|
||||
# print("using nn seed")
|
||||
if num_random_seeds > 0:
|
||||
random_seed = self.q_sample_gen.get_gaussian_samples(num_random_seeds * batch).view(
|
||||
batch, num_random_seeds, self.dof
|
||||
)
|
||||
|
||||
# random_seed = self.fixed_seeds[:num_random_seeds*batch].view(batch, num_random_seeds,
|
||||
# self.solver.safety_rollout.d_action)
|
||||
random_seed = self.q_sample_gen.get_samples(
|
||||
num_random_seeds * batch, bounded=True
|
||||
).view(batch, num_random_seeds, self.dof)
|
||||
|
||||
seed_list.append(random_seed)
|
||||
coord_position_seed = torch.cat(seed_list, dim=1)
|
||||
@@ -944,11 +941,16 @@ class IKSolver(IKSolverConfig):
|
||||
metrics = self.rollout_fn.rollout_constraint(q.position.unsqueeze(1))
|
||||
return metrics
|
||||
|
||||
def sample_configs(self, n: int, use_batch_env=False) -> torch.Tensor:
|
||||
def sample_configs(
|
||||
self, n: int, use_batch_env=False, sample_from_ik_seeder: bool = False
|
||||
) -> torch.Tensor:
|
||||
"""
|
||||
Only works for environment=0
|
||||
"""
|
||||
samples = self.rollout_fn.sample_random_actions(n * self.sample_rejection_ratio)
|
||||
if sample_from_ik_seeder:
|
||||
samples = self.q_sample_gen.get_samples(n * self.sample_rejection_ratio, bounded=True)
|
||||
else:
|
||||
samples = self.rollout_fn.sample_random_actions(n * self.sample_rejection_ratio)
|
||||
metrics = self.rollout_fn.rollout_constraint(
|
||||
samples.unsqueeze(1), use_batch_env=use_batch_env
|
||||
)
|
||||
|
||||
@@ -205,6 +205,8 @@ class MotionGenConfig:
|
||||
jerk_scale: Optional[Union[List[float], float]] = None,
|
||||
optimize_dt: bool = True,
|
||||
project_pose_to_goal_frame: bool = True,
|
||||
ik_seed: int = 1531,
|
||||
graph_seed: int = 1531,
|
||||
):
|
||||
"""Helper function to create configuration from robot and world configuration.
|
||||
|
||||
@@ -436,6 +438,7 @@ class MotionGenConfig:
|
||||
store_debug=store_ik_debug,
|
||||
collision_activation_distance=collision_activation_distance,
|
||||
project_pose_to_goal_frame=project_pose_to_goal_frame,
|
||||
seed=ik_seed,
|
||||
)
|
||||
|
||||
ik_solver = IKSolver(ik_solver_cfg)
|
||||
@@ -448,6 +451,7 @@ class MotionGenConfig:
|
||||
base_config_data,
|
||||
graph_file,
|
||||
use_cuda_graph=use_cuda_graph,
|
||||
seed=graph_seed,
|
||||
)
|
||||
graph_cfg.interpolation_dt = interpolation_dt
|
||||
graph_cfg.interpolation_steps = interpolation_steps
|
||||
|
||||
@@ -172,10 +172,6 @@ class TrajOptSolverConfig:
|
||||
config_data["cost"]["pose_cfg"]["project_distance"] = project_pose_to_goal_frame
|
||||
grad_config_data["cost"]["pose_cfg"]["project_distance"] = project_pose_to_goal_frame
|
||||
|
||||
base_config_data["cost"]["pose_cfg"]["project_distance"] = project_pose_to_goal_frame
|
||||
base_config_data["convergence"]["pose_cfg"]["project_distance"] = project_pose_to_goal_frame
|
||||
config_data["cost"]["pose_cfg"]["project_distance"] = project_pose_to_goal_frame
|
||||
grad_config_data["cost"]["pose_cfg"]["project_distance"] = project_pose_to_goal_frame
|
||||
|
||||
config_data["model"]["horizon"] = traj_tsteps
|
||||
grad_config_data["model"]["horizon"] = traj_tsteps
|
||||
|
||||
@@ -108,6 +108,9 @@ class ReacherSolveState:
|
||||
goal_buffer.retract_state = retract_config
|
||||
goal_buffer.goal_state = goal_state
|
||||
goal_buffer.links_goal_pose = link_poses
|
||||
if goal_buffer.links_goal_pose is not None:
|
||||
for k in goal_buffer.links_goal_pose.keys():
|
||||
goal_buffer.links_goal_pose[k] = goal_buffer.links_goal_pose[k].contiguous()
|
||||
return goal_buffer
|
||||
|
||||
def update_goal_buffer(
|
||||
@@ -154,7 +157,7 @@ class ReacherSolveState:
|
||||
current_goal_buffer.goal_state.copy_(goal_state)
|
||||
if link_poses is not None:
|
||||
for k in link_poses.keys():
|
||||
current_goal_buffer.links_goal_pose[k].copy_(link_poses[k])
|
||||
current_goal_buffer.links_goal_pose[k].copy_(link_poses[k].contiguous())
|
||||
|
||||
return current_solve_state, current_goal_buffer, update_reference
|
||||
|
||||
|
||||
Reference in New Issue
Block a user