The robot arm is a UR3 arm.
The collision sphere has moved from the robot arm after I configured the UR3 yaml file as the robot yaml file. The UR arm is fixed on the base of the robot.
This is the Robot description YAML file
# The robot description defines the generalized coordinates and how to map those
# to the underlying URDF dofs.
api_version: 1.0
# Defines the generalized coordinates. Each generalized coordinate is assumed
# to have an entry in the URDF.
# Lula will only use these joints to control the robot position.
cspace:
- ur_arm_shoulder_pan_joint
- ur_arm_shoulder_lift_joint
- ur_arm_elbow_joint
- ur_arm_wrist_1_joint
- ur_arm_wrist_2_joint
- ur_arm_wrist_3_joint
default_q: [
0.0,-1.0,0.9,0.0,0.0,0.0
]
# Most dimensions of the cspace have a direct corresponding element
# in the URDF. This list of rules defines how unspecified coordinates
# should be extracted or how values in the URDF should be overwritten.
cspace_to_urdf_rules:
# Lula uses collision spheres to define the robot geometry in order to avoid
# collisions with external obstacles. If no spheres are specified, Lula will
# not be able to avoid obstacles.
collision_spheres:
- ur_arm_shoulder_link:
- "center": [-0.0, 0.0, -0.02]
"radius": 0.055
- "center": [0.01, -0.019, -0.0]
"radius": 0.045
- "center": [0.004, -0.007, 0.019]
"radius": 0.05
- ur_arm_upper_arm_link:
- "center": [0.003, 0.002, 0.104]
"radius": 0.052
- "center": [-0.232, 0.002, 0.112]
"radius": 0.043
- "center": [-0.121, -0.001, 0.12]
"radius": 0.042
- "center": [-0.163, 0.002, 0.118]
"radius": 0.041
- "center": [-0.086, 0.001, 0.121]
"radius": 0.041
- "center": [-0.02, 0.014, 0.121]
"radius": 0.041
- "center": [-0.026, -0.019, 0.126]
"radius": 0.035
- "center": [-0.238, 0.0, 0.146]
"radius": 0.04
- ur_arm_forearm_link:
- "center": [-0.013, 0.001, 0.04]
"radius": 0.042
- "center": [-0.214, -0.002, 0.035]
"radius": 0.039
- "center": [-0.171, -0.0, 0.027]
"radius": 0.036
- "center": [-0.083, 0.0, 0.029]
"radius": 0.036
- "center": [0.009, -0.006, 0.054]
"radius": 0.034
- "center": [-0.204, 0.006, 0.003]
"radius": 0.036
- "center": [-0.103, 0.002, 0.028]
"radius": 0.035
- "center": [0.006, 0.01, 0.054]
"radius": 0.034
- "center": [-0.213, 0.005, 0.043]
"radius": 0.037
- "center": [-0.022, -0.002, 0.025]
"radius": 0.033
- "center": [-0.137, 0.001, 0.027]
"radius": 0.036
- "center": [-0.05, 0.0, 0.034]
"radius": 0.039
- ur_arm_wrist_1_link:
- "center": [0.0, -0.009, -0.002]
"radius": 0.041
- "center": [-0.003, 0.019, 0.001]
"radius": 0.037
- "center": [0.006, 0.007, -0.024]
"radius": 0.033
- ur_arm_wrist_2_link:
- "center": [-0.0, 0.0, -0.015]
"radius": 0.041
- "center": [-0.0, 0.012, 0.001]
"radius": 0.039
- "center": [-0.0, -0.018, -0.001]
"radius": 0.04
- ur_arm_wrist_3_link:
- "center": [0.0, 0.002, -0.025]
"radius": 0.035
This is the RMPflow config file for the Robot
# Copyright (c) 2020, NVIDIA CORPORATION. All rights reserved.
#
# NVIDIA CORPORATION and its licensors retain all intellectual property
# and proprietary rights in and to this software, related documentation
# and any modifications thereto. Any use, reproduction, disclosure or
# distribution of this software and related documentation without an express
# license agreement from NVIDIA CORPORATION is strictly prohibited.
api_version: 1.0
joint_limit_buffers: [.01, .01, .01, .01, .01, .01]
rmp_params:
cspace_target_rmp:
metric_scalar: 50.
position_gain: 100.
damping_gain: 50.
robust_position_term_thresh: .5
inertia: 1.
cspace_trajectory_rmp:
p_gain: 80.
d_gain: 10.
ff_gain: .25
weight: 50.
cspace_affine_rmp:
final_handover_time_std_dev: .25
weight: 2000.
joint_limit_rmp:
metric_scalar: 1000.
metric_length_scale: .01
metric_exploder_eps: 1e-3
metric_velocity_gate_length_scale: .01
accel_damper_gain: 200.
accel_potential_gain: 1.
accel_potential_exploder_length_scale: .1
accel_potential_exploder_eps: 1e-2
joint_velocity_cap_rmp:
max_velocity: 2.15
velocity_damping_region: 0.5
damping_gain: 300.
metric_weight: 100.
target_rmp:
accel_p_gain: 80.
accel_d_gain: 120.
accel_norm_eps: .075
metric_alpha_length_scale: .05
min_metric_alpha: .01
max_metric_scalar: 10000.
min_metric_scalar: 2500.
proximity_metric_boost_scalar: 20.
proximity_metric_boost_length_scale: .02
accept_user_weights: false
axis_target_rmp:
accel_p_gain: 200.
accel_d_gain: 40.
metric_scalar: 10.
proximity_metric_boost_scalar: 3000.
proximity_metric_boost_length_scale: .05
accept_user_weights: false
collision_rmp:
damping_gain: 50.
damping_std_dev: .04
damping_robustness_eps: 1e-2
damping_velocity_gate_length_scale: .01
repulsion_gain: 1200.
repulsion_std_dev: .01
metric_modulation_radius: .5
metric_scalar: 10000.
metric_exploder_std_dev: .02
metric_exploder_eps: .001
damping_rmp:
accel_d_gain: 30.
metric_scalar: 50.
inertia: 100.
canonical_resolve:
max_acceleration_norm: 50.
projection_tolerance: .01
verbose: false
body_cylinders:
- name: ur_arm_base_link
pt1: [0, 0, 0.15]
pt2: [0, 0, 0]
radius: .065
body_collision_controllers:
- name: ur_arm_wrist_2_link
radius: .04
- name: ur_arm_wrist_3_link
radius: .04
- name: ur_arm_tool0
radius: .04
The robot usd is
husky_ur3.usd (5.4 KB)
and the robot urdf is
husky_ur.urdf (28.1 KB)
<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from husky.urdf.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<!--
Software License Agreement (BSD)
\file husky.urdf.xacro
\authors Paul Bovbel <pbovbel@clearpathrobotics.com>, Devon Ash <dash@clearpathrobotics.com>
\copyright Copyright (c) 2015, Clearpath Robotics, Inc., 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.
* Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote
products derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR-
RANTIES, 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, IN-
DIRECT, 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.
-->
<robot name="husky">
<material name="dark_grey">
<color rgba="0.2 0.2 0.2 1.0"/>
</material>
<material name="medium_grey">
<color rgba="0.6 0.6 0.6 1.0"/>
</material>
<material name="light_grey">
<color rgba="0.8 0.8 0.8 1.0"/>
</material>
<material name="yellow">
<color rgba="0.8 0.8 0.0 1.0"/>
</material>
<material name="black">
<color rgba="0.15 0.15 0.15 1.0"/>
</material>
<material name="white">
<color rgba="1.0 1.0 1.0 1.0"/>
</material>
<material name="red">
<color rgba="1.0 0.0 0.0 1.0"/>
</material>
<joint name="arm_mount_joint" type="fixed">
<parent link="top_plate_front_link"/>
<child link="ur_arm_base_link"/>
<origin rpy="0 0 3.1415927" xyz="-0.105 0.0 0.0"/>
</joint>
<!--
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="ur_arm_shoulder_pan_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="ur_arm_shoulder_pan_joint">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="ur_arm_shoulder_pan_motor">
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="ur_arm_shoulder_lift_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="ur_arm_shoulder_lift_joint">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="ur_arm_shoulder_lift_motor">
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="ur_arm_elbow_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="ur_arm_elbow_joint">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="ur_arm_elbow_motor">
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="ur_arm_wrist_1_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="ur_arm_wrist_1_joint">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="ur_arm_wrist_1_motor">
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="ur_arm_wrist_2_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="ur_arm_wrist_2_joint">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="ur_arm_wrist_2_motor">
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="ur_arm_wrist_3_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="ur_arm_wrist_3_joint">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="ur_arm_wrist_3_motor">
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<!-- links: main serial chain -->
<link name="ur_arm_base_link"/>
<link name="ur_arm_base_link_inertia">
<visual>
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
<geometry>
<mesh filename="package://ur_description/meshes/ur3/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="package://ur_description/meshes/ur3/collision/base.stl"/>
</geometry>
</collision>
<inertial>
<mass value="2.0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.0030531654454" ixy="0.0" ixz="0.0" iyy="0.0030531654454" iyz="0.0" izz="0.005625"/>
</inertial>
</link>
<link name="ur_arm_shoulder_link">
<visual>
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
<geometry>
<mesh filename="package://ur_description/meshes/ur3/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="package://ur_description/meshes/ur3/collision/shoulder.stl"/>
</geometry>
</collision>
<inertial>
<mass value="2.0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.008093163429399999" ixy="0.0" ixz="0.0" iyy="0.008093163429399999" iyz="0.0" izz="0.005625"/>
</inertial>
</link>
<link name="ur_arm_upper_arm_link">
<visual>
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.1198"/>
<geometry>
<mesh filename="package://ur_description/meshes/ur3/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.1198"/>
<geometry>
<mesh filename="package://ur_description/meshes/ur3/collision/upperarm.stl"/>
</geometry>
</collision>
<inertial>
<mass value="3.42"/>
<origin rpy="0 1.5707963267948966 0" xyz="-0.121825 0.0 0.12"/>
<inertia ixx="0.021728483221103233" ixy="0.0" ixz="0.0" iyy="0.021728483221103233" iyz="0.0" izz="0.00961875"/>
</inertial>
</link>
<link name="ur_arm_forearm_link">
<visual>
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.0275"/>
<geometry>
<mesh filename="package://ur_description/meshes/ur3/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.0275"/>
<geometry>
<mesh filename="package://ur_description/meshes/ur3/collision/forearm.stl"/>
</geometry>
</collision>
<inertial>
<mass value="1.26"/>
<origin rpy="0 1.5707963267948966 0" xyz="-0.106625 0.0 0.0275"/>
<inertia ixx="0.006546806443776375" ixy="0.0" ixz="0.0" iyy="0.006546806443776375" iyz="0.0" izz="0.00354375"/>
</inertial>
</link>
<link name="ur_arm_wrist_1_link">
<visual>
<!-- TODO: Move this to a parameter -->
<origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.085"/>
<geometry>
<mesh filename="package://ur_description/meshes/ur3/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.085"/>
<geometry>
<mesh filename="package://ur_description/meshes/ur3/collision/wrist1.stl"/>
</geometry>
</collision>
<inertial>
<mass value="0.8"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.0016106408557434" ixy="0.0" ixz="0.0" iyy="0.0016106408557434" iyz="0.0" izz="0.00225"/>
</inertial>
</link>
<link name="ur_arm_wrist_2_link">
<visual>
<origin rpy="0 0 0" xyz="0 0 -0.085"/>
<geometry>
<mesh filename="package://ur_description/meshes/ur3/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.085"/>
<geometry>
<mesh filename="package://ur_description/meshes/ur3/collision/wrist2.stl"/>
</geometry>
</collision>
<inertial>
<mass value="0.8"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.0015721733711303997" ixy="0.0" ixz="0.0" iyy="0.0015721733711303997" iyz="0.0" izz="0.00225"/>
</inertial>
</link>
<link name="ur_arm_wrist_3_link">
<visual>
<origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.082"/>
<geometry>
<mesh filename="package://ur_description/meshes/ur3/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.082"/>
<geometry>
<mesh filename="package://ur_description/meshes/ur3/collision/wrist3.stl"/>
</geometry>
</collision>
<inertial>
<mass value="0.35"/>
<origin rpy="0 0 0" xyz="0.0 0.0 -0.02"/>
<inertia ixx="0.00013626661215999998" ixy="0.0" ixz="0.0" iyy="0.00013626661215999998" iyz="0.0" izz="0.0001792"/>
</inertial>
</link>
<!-- joints: main serial chain -->
<joint name="ur_arm_base_link-base_link_inertia" type="fixed">
<parent link="ur_arm_base_link"/>
<child link="ur_arm_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="ur_arm_shoulder_pan_joint" type="revolute">
<parent link="ur_arm_base_link_inertia"/>
<child link="ur_arm_shoulder_link"/>
<origin rpy="0 0 0" xyz="0 0 0.1519"/>
<axis xyz="0 0 1"/>
<limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
<dynamics damping="0" friction="0"/>
</joint>
<joint name="ur_arm_shoulder_lift_joint" type="revolute">
<parent link="ur_arm_shoulder_link"/>
<child link="ur_arm_upper_arm_link"/>
<origin rpy="1.570796327 0 0" xyz="0 0 0"/>
<axis xyz="0 0 1"/>
<limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
<dynamics damping="0" friction="0"/>
</joint>
<joint name="ur_arm_elbow_joint" type="revolute">
<parent link="ur_arm_upper_arm_link"/>
<child link="ur_arm_forearm_link"/>
<origin rpy="0 0 0" xyz="-0.24365 0 0"/>
<axis xyz="0 0 1"/>
<limit effort="28.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="3.141592653589793"/>
<dynamics damping="0" friction="0"/>
</joint>
<joint name="ur_arm_wrist_1_joint" type="revolute">
<parent link="ur_arm_forearm_link"/>
<child link="ur_arm_wrist_1_link"/>
<origin rpy="0 0 0" xyz="-0.21325 0 0.11235"/>
<axis xyz="0 0 1"/>
<limit effort="12.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="6.283185307179586"/>
<dynamics damping="0" friction="0"/>
</joint>
<joint name="ur_arm_wrist_2_joint" type="revolute">
<parent link="ur_arm_wrist_1_link"/>
<child link="ur_arm_wrist_2_link"/>
<origin rpy="1.570796327 0 0" xyz="0 -0.08535 -1.750557762378351e-11"/>
<axis xyz="0 0 1"/>
<limit effort="12.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="6.283185307179586"/>
<dynamics damping="0" friction="0"/>
</joint>
<joint name="ur_arm_wrist_3_joint" type="revolute">
<parent link="ur_arm_wrist_2_link"/>
<child link="ur_arm_wrist_3_link"/>
<origin rpy="1.570796326589793 3.141592653589793 3.141592653589793" xyz="0 0.0819 -1.679797079540562e-11"/>
<axis xyz="0 0 1"/>
<limit effort="12.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="6.283185307179586"/>
<dynamics damping="0" friction="0"/>
</joint>
<!-- ROS-Industrial 'base' frame: base_link to UR 'Base' Coordinates transform -->
<link name="ur_arm_base"/>
<joint name="ur_arm_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="ur_arm_base_link"/>
<child link="ur_arm_base"/>
</joint>
<!-- ROS-Industrial 'flange' frame: attachment point for EEF models -->
<link name="ur_arm_flange"/>
<joint name="ur_arm_wrist_3-flange" type="fixed">
<parent link="ur_arm_wrist_3_link"/>
<child link="ur_arm_flange"/>
<origin rpy="0 -1.5707963267948966 -1.5707963267948966" xyz="0 0 0"/>
</joint>
<!-- ROS-Industrial 'tool0' frame: all-zeros tool frame -->
<link name="ur_arm_tool0"/>
<joint name="ur_arm_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="ur_arm_flange"/>
<child link="ur_arm_tool0"/>
</joint>
<!-- Base link is the center of the robot's bottom plate -->
<link name="base_link">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://husky_description/meshes/base_link.dae"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.0 0 0.061875"/>
<geometry>
<box size="0.9874 0.5709 0.12375"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.17562499999999998"/>
<geometry>
<box size="0.7899200000000001 0.5709 0.10375"/>
</geometry>
</collision>
</link>
<!-- Base footprint is on the ground under the robot -->
<link name="base_footprint"/>
<joint name="base_footprint_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 -0.13228"/>
<parent link="base_link"/>
<child link="base_footprint"/>
</joint>
<!-- Inertial link stores the robot's inertial information -->
<link name="inertial_link">
<inertial>
<mass value="46.034"/>
<origin xyz="-0.00065 -0.085 0.062"/>
<inertia ixx="0.6022" ixy="-0.02364" ixz="-0.1197" iyy="1.7386" iyz="-0.001544" izz="2.0296"/>
</inertial>
</link>
<joint name="inertial_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="base_link"/>
<child link="inertial_link"/>
</joint>
<link name="front_left_wheel_link">
<inertial>
<mass value="2.637"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.04411" iyz="0" izz="0.02467"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://husky_description/meshes/wheel.dae"/>
</geometry>
</visual>
<collision>
<origin rpy="1.570795 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.1143" radius="0.1651"/>
</geometry>
</collision>
</link>
<gazebo reference="front_left_wheel_link">
<mu1 value="1.0"/>
<mu2 value="1.0"/>
<kp value="10000000.0"/>
<kd value="1.0"/>
<fdir1 value="1 0 0"/>
</gazebo>
<joint name="front_left_wheel" type="continuous">
<parent link="base_link"/>
<child link="front_left_wheel_link"/>
<origin rpy="0 0 0" xyz="0.256 0.2854 0.03282"/>
<axis rpy="0 0 0" xyz="0 1 0"/>
</joint>
<transmission name="front_left_wheel_trans" type="SimpleTransmission">
<type>transmission_interface/SimpleTransmission</type>
<actuator name="front_left_wheel_motor">
<mechanicalReduction>1</mechanicalReduction>
</actuator>
<joint name="front_left_wheel">
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
</joint>
</transmission>
<link name="front_right_wheel_link">
<inertial>
<mass value="2.637"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.04411" iyz="0" izz="0.02467"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://husky_description/meshes/wheel.dae"/>
</geometry>
</visual>
<collision>
<origin rpy="1.570795 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.1143" radius="0.1651"/>
</geometry>
</collision>
</link>
<gazebo reference="front_right_wheel_link">
<mu1 value="1.0"/>
<mu2 value="1.0"/>
<kp value="10000000.0"/>
<kd value="1.0"/>
<fdir1 value="1 0 0"/>
</gazebo>
<joint name="front_right_wheel" type="continuous">
<parent link="base_link"/>
<child link="front_right_wheel_link"/>
<origin rpy="0 0 0" xyz="0.256 -0.2854 0.03282"/>
<axis rpy="0 0 0" xyz="0 1 0"/>
</joint>
<transmission name="front_right_wheel_trans" type="SimpleTransmission">
<type>transmission_interface/SimpleTransmission</type>
<actuator name="front_right_wheel_motor">
<mechanicalReduction>1</mechanicalReduction>
</actuator>
<joint name="front_right_wheel">
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
</joint>
</transmission>
<link name="rear_left_wheel_link">
<inertial>
<mass value="2.637"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.04411" iyz="0" izz="0.02467"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://husky_description/meshes/wheel.dae"/>
</geometry>
</visual>
<collision>
<origin rpy="1.570795 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.1143" radius="0.1651"/>
</geometry>
</collision>
</link>
<gazebo reference="rear_left_wheel_link">
<mu1 value="1.0"/>
<mu2 value="1.0"/>
<kp value="10000000.0"/>
<kd value="1.0"/>
<fdir1 value="1 0 0"/>
</gazebo>
<joint name="rear_left_wheel" type="continuous">
<parent link="base_link"/>
<child link="rear_left_wheel_link"/>
<origin rpy="0 0 0" xyz="-0.256 0.2854 0.03282"/>
<axis rpy="0 0 0" xyz="0 1 0"/>
</joint>
<transmission name="rear_left_wheel_trans" type="SimpleTransmission">
<type>transmission_interface/SimpleTransmission</type>
<actuator name="rear_left_wheel_motor">
<mechanicalReduction>1</mechanicalReduction>
</actuator>
<joint name="rear_left_wheel">
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
</joint>
</transmission>
<link name="rear_right_wheel_link">
<inertial>
<mass value="2.637"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.04411" iyz="0" izz="0.02467"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://husky_description/meshes/wheel.dae"/>
</geometry>
</visual>
<collision>
<origin rpy="1.570795 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.1143" radius="0.1651"/>
</geometry>
</collision>
</link>
<gazebo reference="rear_right_wheel_link">
<mu1 value="1.0"/>
<mu2 value="1.0"/>
<kp value="10000000.0"/>
<kd value="1.0"/>
<fdir1 value="1 0 0"/>
</gazebo>
<joint name="rear_right_wheel" type="continuous">
<parent link="base_link"/>
<child link="rear_right_wheel_link"/>
<origin rpy="0 0 0" xyz="-0.256 -0.2854 0.03282"/>
<axis rpy="0 0 0" xyz="0 1 0"/>
</joint>
<transmission name="rear_right_wheel_trans" type="SimpleTransmission">
<type>transmission_interface/SimpleTransmission</type>
<actuator name="rear_right_wheel_motor">
<mechanicalReduction>1</mechanicalReduction>
</actuator>
<joint name="rear_right_wheel">
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
</joint>
</transmission>
<!--
CHASSIS: Spawn and attach default chassis.
Currently, no customization available.
-->
<!-- Spawn Husky chassis -->
<link name="top_chassis_link">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://husky_description/meshes/top_chassis.dae"/>
</geometry>
</visual>
</link>
<!-- Attach chassis to the robot -->
<joint name="top_chassis_joint" type="fixed">
<parent link="base_link"/>
<child link="top_chassis_link"/>
</joint>
<link name="front_bumper_link">
<visual>
<geometry>
<mesh filename="package://husky_description/meshes/bumper.dae"/>
</geometry>
</visual>
</link>
<!-- Attach front bumper -->
<joint name="front_bumper" type="fixed">
<origin rpy="0 0 0" xyz="0.48 0 0.091"/>
<parent link="base_link"/>
<child link="front_bumper_link"/>
</joint>
<link name="rear_bumper_link">
<visual>
<geometry>
<mesh filename="package://husky_description/meshes/bumper.dae"/>
</geometry>
</visual>
</link>
<!-- Attach rear bumper -->
<joint name="rear_bumper" type="fixed">
<origin rpy="0 0 3.14159" xyz="-0.48 0 0.091"/>
<parent link="base_link"/>
<child link="rear_bumper_link"/>
</joint>
<!-- Spawn user rails -->
<link name="user_rail_link">
<visual>
<geometry>
<mesh filename="package://husky_description/meshes/user_rail.dae"/>
</geometry>
</visual>
</link>
<!-- Attach user rails to base link -->
<joint name="user_rail" type="fixed">
<origin rpy="0 0 0" xyz="0.272 0 0.245"/>
<parent link="base_link"/>
<child link="user_rail_link"/>
</joint>
<!-- Spawn default top plate -->
<link name="top_plate_link">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://husky_description/meshes/top_plate.dae"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://husky_description/meshes/top_plate.stl"/>
</geometry>
</collision>
</link>
<!-- Attach default top plate -->
<joint name="top_plate_joint" type="fixed">
<parent link="base_link"/>
<child link="top_plate_link"/>
<origin rpy="0 0 0" xyz="0.0812 0 0.245"/>
</joint>
<!-- Default top plate front link -->
<link name="top_plate_front_link"/>
<joint name="top_plate_front_joint" type="fixed">
<parent link="top_plate_link"/>
<child link="top_plate_front_link"/>
<origin rpy="0 0 0" xyz="0.36367 0 0.00639"/>
</joint>
<!-- Default top plate rear link-->
<link name="top_plate_rear_link"/>
<joint name="top_plate_rear_joint" type="fixed">
<parent link="top_plate_link"/>
<child link="top_plate_rear_link"/>
<origin rpy="0 0 0" xyz="-0.36633 0 0.00639"/>
</joint>
<!--
IMU Link: Standard location to add an IMU (i.e. UM7 or Microstrain)
-->
<link name="imu_link"/>
<joint name="imu_joint" type="fixed">
<origin rpy="0 -1.5708 3.1416" xyz="0.19 0 0.149"/>
<parent link="base_link"/>
<child link="imu_link"/>
</joint>
<gazebo reference="imu_link">
</gazebo>
<gazebo>
<plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
<robotNamespace>/</robotNamespace>
<legacyModeNS>true</legacyModeNS>
</plugin>
</gazebo>
<gazebo>
<plugin filename="libhector_gazebo_ros_imu.so" name="imu_controller">
<robotNamespace>/</robotNamespace>
<updateRate>50.0</updateRate>
<bodyName>base_link</bodyName>
<topicName>imu/data</topicName>
<accelDrift>0.005 0.005 0.005</accelDrift>
<accelGaussianNoise>0.005 0.005 0.005</accelGaussianNoise>
<rateDrift>0.005 0.005 0.005 </rateDrift>
<rateGaussianNoise>0.005 0.005 0.005 </rateGaussianNoise>
<headingDrift>0.005</headingDrift>
<headingGaussianNoise>0.005</headingGaussianNoise>
</plugin>
</gazebo>
<gazebo>
<plugin filename="libhector_gazebo_ros_gps.so" name="gps_controller">
<robotNamespace>/</robotNamespace>
<updateRate>40</updateRate>
<bodyName>base_link</bodyName>
<frameId>base_link</frameId>
<topicName>navsat/fix</topicName>
<velocityTopicName>navsat/vel</velocityTopicName>
<referenceLatitude>49.9</referenceLatitude>
<referenceLongitude>8.9</referenceLongitude>
<referenceHeading>90</referenceHeading>
<referenceAltitude>0</referenceAltitude>
<drift>0.0001 0.0001 0.0001</drift>
</plugin>
</gazebo>
<!-- This file is a placeholder which is included by default from
husky.urdf.xacro. If a robot is being customized and requires
additional URDF, set the HUSKY_URDF_EXTRAS environment variable
to the full path of the file you would like included. -->
<!-- This file is a placeholder which is included by default from
husky.urdf.xacro. If a robot is being customized and requires
additional URDF, set the HUSKY_URDF_EXTRAS environment variable
to the full path of the file you would like included. -->
</robot>
The python script running is
from omni.isaac.kit import SimulationApp
simulation_app = SimulationApp({"headless": False})
from robot_utils.husky_controller import HuskyController
from follow_target_husky_ur3_rmpFlow import FollowTarget
from robot_utils.ur3_rmpF_controller import RMPFlowController
from omni.isaac.core import World
import numpy as np
from customised_task import CustomTask
my_world = World(stage_units_in_meters=1.0)
my_task = CustomTask(name = "task",offset=None)
my_world.add_task(my_task)
my_world.reset()
my_params = my_world.get_task("task").get_params()
robot_name= my_params["robot_name"]["value"]
target_name = my_params["target_name"]["value"]
# print(my_params)
my_robot = my_world.scene.get_object(robot_name)
my_robot.ur_initialize()
my_controller = RMPFlowController(name="target_follower_controller", robot_articulation=my_robot, attach_gripper=False)
rmpflow = my_controller.get_motion_policy()
rmpflow.set_ignore_state_updates(True)
rmpflow.visualize_collision_spheres()
articulation_controller = my_robot.get_articulation_controller()
husky_control = HuskyController(name="simple_control", wheel_radius=0.03, wheel_base=0.1125)
while simulation_app.is_running():
my_world.step(render=True)
#print("code ok")
if my_world.is_playing():
if my_world.current_time_step_index == 0:
my_world.reset()
my_controller.reset()
observations = my_world.get_observations()
#print(observations)
#my_robot.apply_wheel_actions(husky_control.forward(command=[0.05, 0]))
pos_robot , or_robot = my_robot.get_world_pose()
print("Position and Orientation of Husky:{}{}".format(pos_robot,or_robot))
pos_end , or_end = my_robot.end_effector_position
print("Position and Orientation of end:{}{}".format(pos_end,or_end))
x = observations[target_name]["position"][0]
y = -observations[target_name]["position"][1]
actions = my_controller.forward(
target_end_effector_position=observations[target_name]["position"],
target_end_effector_orientation=observations[target_name]["orientation"],
)
# print("position...")
print("target pos, or")
print(observations[target_name]["position"])
print(observations[target_name]["orientation"])
articulation_controller.apply_action(actions)
print(my_task.target_reached())
simulation_app.close()
Please look into this.
Please tell me if you require any other code or material.