Robot collision sphere moved from robot while using RMPflow

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.

I am planning to build a custom robot with a Clearpath Husky mobile base attached with a UR arm on the top chassis and use the RMP flow algorithm to fetch objects.

It looks like you just need to tell RMPflow where the robot is. RMPflow as an algorithm is independent of Isaac Sim, and it needs to be told if the robot base is not at the origin. See here:
RMPflow World State. Since the UR is moving on every frame, you will need to tell RMPflow on every frame where it is: rmpflow.set_robot_base_pose(robot_base_translation,robot_base_orientation)

1 Like

Thanks for your response, Arudich.

I followed the tutorial you sent me, by setting the robot_base_translation and the robot_base_orientation to the UR “ur_arm_base_link” position and orientation, since the RMP flow algorithm is set for the UR arm, but it yielded no positive results.

RMP flow weird behaviour issue.
This is current demo. Can you please tell me how to fix this behaviour ? Tuning the parameters or some other way.

Thanks for your help.

This is the main python script running using python.sh of isaac sim.


from omni.isaac.kit import SimulationApp

simulation_app = SimulationApp({"headless": False})

from omni.isaac.franka.tasks import FollowTarget
from omni.isaac.motion_generation.lula import RmpFlow
from omni.isaac.motion_generation import ArticulationMotionPolicy
from omni.isaac.core import World
from omni.isaac.core.objects import sphere






from robot_utils.husky_controller import HuskyController
from follow_target_husky_ur3_rmpFlow import FollowTarget
from robot_utils.ur3_rmpF_controller import RMPFlowController
from customised_task import CustomTask
import numpy as np

from omni.isaac.core.utils.extensions import get_extension_path_from_name
import os

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()
task_params = my_world.get_task("task").get_params()
robot_name = task_params["robot_name"]["value"]
target_name = task_params["target_name"]["value"]
my_robot = my_world.scene.get_object(robot_name)

# RMPflow config files for supported robots are stored in the motion_generation extension under "/motion_policy_configs"
mg_extension_path = get_extension_path_from_name("omni.isaac.motion_generation")
rmp_config_dir = os.path.join(mg_extension_path, "motion_policy_configs")

#Initialize an RmpFlow object




my_robot.ur_initialize()

pth = "/home/bikram/.local/share/ov/pkg/isaac_sim-2022.2.1/standalone_examples/api/omni.isaac.husky_ur"
rmpflow = RmpFlow(
    robot_description_path = pth + "/husky_ur3/rmpflow/husky_ur3_robot_description.yaml",
    urdf_path = "./husky_ur3/husky_ur3.urdf",
    rmpflow_config_path = "./husky_ur3/rmpflow/husky_ur3_rmpflow_config.yaml",
    end_effector_frame_name = "ur_arm_flange",
    maximum_substep_size = 0.00334
)
#Use the ArticulationMotionPolicy wrapper object to connect rmpflow to the Franka robot articulation.
physics_dt = 1/60.
articulation_rmpflow = ArticulationMotionPolicy(my_robot,rmpflow,physics_dt)

#create and add an obstacle from omni.isaac.core.objects
sphere_obstacle = sphere.FixedSphere("/sphere_obstacle",radius=.03,position=np.array([0.1, 0.0, 0.5]),color=np.array([0.,0.,1.]))
rmpflow.add_obstacle(sphere_obstacle)



# rmpflow.set_ignore_state_updates(True)
rmpflow.visualize_collision_spheres()


articulation_controller = my_robot.get_articulation_controller()
while simulation_app.is_running():
    my_world.step(render=True)
    if my_world.is_playing():
        if my_world.current_time_step_index == 0:
            my_world.reset()

        observations = my_world.get_observations()

        #Set rmpflow target to be the current position of the target cube.
        y = -observations[target_name]["position"][1]
        rmpflow.set_end_effector_target(
            target_position=np.array([observations[target_name]["position"][0],y,observations[target_name]["position"][2]]),
            target_orientation=observations[target_name]["orientation"],
        )
        #Track any movements of the sphere obstacle
        rmpflow.update_world()

        #Track any movements of the robot base
        robot_base_translation,robot_base_orientation = my_robot.base_ur.get_world_pose()
        rmpflow.set_robot_base_pose(robot_base_translation,robot_base_orientation)

        actions = articulation_rmpflow.get_next_articulation_action()
        articulation_controller.apply_action(actions)


        print(my_task.target_reached())

simulation_app.close()


This is the customised task for the scene generation and adding objects to the scene.


from omni.isaac.core.tasks import BaseTask
from omni.isaac.core.scenes.scene import Scene



from abc import abstractmethod, ABC
from typing import Optional
import numpy as np


from omni.isaac.core.utils.prims import is_prim_path_valid
from omni.isaac.core.utils.rotations import euler_angles_to_quat
from omni.isaac.core.utils.string import find_unique_string_name
from omni.isaac.core.objects import DynamicCuboid, VisualCuboid
from omni.isaac.core.utils.rotations import euler_angles_to_quat
from omni.isaac.core.utils.stage import get_stage_units

from robot_utils.husky_ur_robot import Husky_UR_Robot


class CustomTask(ABC, BaseTask):
      def __init__(
                  self,
                  name,
                  offset: Optional[np.ndarray],
      ):
            

            super().__init__(name,offset = None)


            return
      





      def set_up_scene(self, scene):
        
        super().set_up_scene(scene)

        scene.add_default_ground_plane()
        

        self.jetbot_name = find_unique_string_name(
            initial_name="husky_ur", is_unique_fn=lambda x: not self.scene.object_exists(x)
        )
        # Find a unique prim path
        self.jetbot_prim_path = find_unique_string_name(
            initial_name="/World/Husky_UR", is_unique_fn=lambda x: not is_prim_path_valid(x)
        )


        self._robot = scene.add(
            Husky_UR_Robot(
                prim_path=self.jetbot_prim_path,
                name=self.jetbot_name,
                wheel_dof_names=["front_left_wheel", 'rear_left_wheel',"front_right_wheel", 'rear_right_wheel'],
                usd_path="/home/bikram/husky_ur3.usd",
            )
        )
        

        target_name = find_unique_string_name(
            initial_name="target_cube", is_unique_fn=lambda x: not self.scene.object_exists(x)
        )

        target_prim_path = find_unique_string_name(
            initial_name="/World/Target_Cube", is_unique_fn=lambda x: not is_prim_path_valid(x)
        )
        


      #   self._target = scene.add(
      #        VisualCuboid(
      #             prim_path=target_prim_path,
      #             name=target_name,
      #             position = np.array([0,0,0.7]),
      #             color = np.array([1,0,0]),
      #             size = 1,
      #             scale = np.array([0.03, 0.03, 0.03]) / get_stage_units(), 
      #        )
      #   )

        self.set_params(
              target_prim_path = target_prim_path,
              target_name = target_name,
              target_position=np.array([0,0,0.7]),
              target_orientation=np.array([1,0,0,0])
        )
 

        self._task_objects[self._robot.name] = self._robot
      #   self._task_objects[self._target.name] = self._target
        self._move_task_objects_to_their_frame()

        return
      

      def set_params(
                  self,
                  target_prim_path,
                  target_name,
                  target_position,
                  target_orientation,
      ) -> None:
            

            self._target = self.scene.add(
                  VisualCuboid(
                        name=target_name,
                        prim_path=target_prim_path,
                        position=target_position,
                        orientation=target_orientation,
                        color=np.array([1, 0, 0]),
                        size=1.0,
                        scale=np.array([0.03, 0.03, 0.03]) / get_stage_units(),
                  )
            )

            self._task_objects[self._target.name] = self._target
            return






      def get_params(self) -> dict:




            params_representation = dict()
            params_representation["target_prim_path"] = {"value" : self._target.prim_path, "modifiable": True}
            params_representation["target_name"] = {"value": self._target.name, "modifiable": True}
            position, orientation = self._target.get_local_pose()
            params_representation["target_position"] = {"value": position, "modifiable": True}
            params_representation["target_orientation"] = {"value": orientation, "modifiable": True}
            params_representation["robot_name"] = {"value": self._robot.name, "modifiable": False}


            return params_representation
      





      def get_observations(self) -> dict:



            joints_state = self._robot.get_joints_state()
            target_position, target_orientation = self._target.get_local_pose()
            
            return {
                  self._robot.name: {
                  "joint_positions": np.array(joints_state.positions),
                  "joint_velocities": np.array(joints_state.velocities),
                  },
                  self._target.name: {"position": np.array(target_position), "orientation": np.array(target_orientation)},
            }





      def target_reached(self) -> bool:


            end_effector_position, _ = self._robot.end_effector.get_world_pose()
            target_position, _ = self._target.get_world_pose()
            if np.mean(np.abs(np.array(end_effector_position) - np.array(target_position))) < (0.035 / get_stage_units()):
                  return True
            else:
                  return False




And this is the robot class function that I created inheriting the Wheeled Robot class.





'''

This is a modified robot class for the Husky + UR3 configured mobile manipulator



'''

import carb
import numpy as np


from typing import Optional
from omni.isaac.core.robots.robot import Robot
from omni.isaac.wheeled_robots.robots.wheeled_robot import WheeledRobot
from omni.isaac.core.utils.stage import add_reference_to_stage
from omni.isaac.core.prims.rigid_prim import RigidPrim
from omni.isaac.core.utils.nucleus import get_assets_root_path
from omni.isaac.manipulators.grippers.surface_gripper import SurfaceGripper



class Husky_UR_Robot(WheeledRobot):
      def __init__(
                  self,
                  prim_path : str,
                  name: Optional[str] = None,
                  wheel_dof_names: Optional[str] = None,
                  wheel_dof_indices: Optional[int] = None,
                  usd_path: Optional[str] = None,
                  position: Optional[np.ndarray] = None,
                  orientation: Optional[np.ndarray] = None,
                  end_effector_prim_name: Optional[str] = None,
                  attach_gripper: bool = False,
                  gripper_usd: Optional[str] = "default",
      ) -> None:
            
            self._prim_path = prim_path
            self._usd_path = usd_path
            self._name = name
           
            self._end_effector = None
            self._gripper = None
            self._end_effector_prim_name = end_effector_prim_name
            self._end_effector = None
            self._gripper = None
            self._base_ur = None
            self._base_ur_prim_path = None


            if self._usd_path is None:
                  add_reference_to_stage("/home/bikram/husky_ur3.usd", prim_path)
            else:
                   add_reference_to_stage(self._usd_path, prim_path)

            
            
            add_reference_to_stage(self._usd_path, prim_path)
            
            assets_root_path = get_assets_root_path()
            
            if self._end_effector_prim_name is None:
                  self._end_effector_prim_path = prim_path + "/ur_arm_flange"
            else:
                  self._end_effector_prim_path = prim_path + "/" + end_effector_prim_name


            self._base_ur_prim_path = prim_path + "/" + "ur_arm_base_link"



            super().__init__(
                  prim_path = prim_path,
                  name = name,
                  wheel_dof_indices = wheel_dof_indices,
                  wheel_dof_names = wheel_dof_names,
                  position = position,
                  orientation = orientation,
            )
            
            if attach_gripper:
                  gripper_usd = assets_root_path + "/Isaac/Robots/UR10/Props/short_gripper.usd"
                  self._gripper = SurfaceGripper(
                        end_effector_prim_path=self._end_effector_prim_path, translate=0.1611, direction="x"
                  )
                  #self._gripper.initialize()
            self._attach_gripper = attach_gripper


            self._base_ur = RigidPrim(prim_path = self._base_ur_prim_path, name = self.name + "_base_ur")
            




      def print_prim_path(self):
            print(self._end_effector_prim_path)




      def ur_initialize(self,physics_sim_view = None) -> None:
            if self._attach_gripper:
                  self._gripper.initialize(physics_sim_view,articulation_num_dofs = self.num_dof)
            self._end_effector = RigidPrim(prim_path = self._end_effector_prim_path,name = self.name+"_end_effector")
            self._end_effector.initialize(physics_sim_view)
            self._base_ur.initialize(physics_sim_view)
            for i in range(10000):
                  print(i)
            return
      
      




      def post_reset(self) -> None:
            WheeledRobot.post_reset(self)
            if self._end_effector != None:
                  self._end_effector.post_reset()
            if self._gripper !=  None:
                  self._gripper.post_reset()
            return
      

      @property
      def end_effector(self) -> RigidPrim:
            return self._end_effector
      


      @property
      def end_effector_position(self):
            return self._end_effector.get_world_pose()
      


      @property
      def base_ur(self) -> RigidPrim:
            return self._base_ur

This code looks right to me

robot_base_translation,robot_base_orientation = my_robot.base_ur.get_world_pose()
rmpflow.set_robot_base_pose(robot_base_translation,robot_base_orientation)

but you should check to make sure that robot_base_translation,robot_base_orientation match what you actually see on the stage (or if they seem to match where RMPflow believes the robot to be). I can’t be sure from the script you shared. No other tuning parameters should have any relevance to RMPflow knowing where the robot is in the world.

1 Like

Hey @arudich if I dont use the RMP flow algorithm and only use the IK solver, how should I write the Robot class for the robot given above. I need to move the wheels using an

apply_wheel_actions(ArticulationAction) function

and another function

apply_arm_actions(ArticulationAction) function

to move the arm.

I have written the “apply_wheel_actions” and this works but I didnt get any example for the arm control actions from the UR10 class.

I have wrote the " apply_wheel_actions". Can you please give me any doc as a guide to write the articulation action function for the arm “apply_arm_action” ?

'''

This is a trial of writing the Husky_UR3_robot class.




'''


import carb
from typing import Optional
import numpy as np


from omni.isaac.core.utils.types import ArticulationAction
from omni.isaac.core.robots.robot import Robot
from omni.isaac.core.utils.prims import get_prim_at_path, define_prim
from omni.isaac.core.utils.stage import add_reference_to_stage



class Husky_UR3_Robot(Robot):
    def __init__(self,
                 prim_path : str,
                 usd_path: str,
                 name : Optional[str],
                 position: Optional[np.ndarray] = None,
                 orientation: Optional[np.ndarray] = None,                 
    ) -> None:

        add_reference_to_stage(usd_path, prim_path)
        super().__init__(prim_path=prim_path, name=name, position=position, orientation=orientation, articulation_controller=None)
        self._husky_base_joint_names = ["front_left_wheel",
                                      "front_right_wheel",
                                      "rear_left_wheel",
                                      "rear_right_wheel"]
        self._husky_dof_idxs = []
        self._ur_joint_names = ["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"]
        self._ur_arm_dof_idxs = []
        self._num_dof = len(self._husky_base_joint_names) + len(self._ur_joint_names)




    @property
    def husky_wheel_dof_indices(self) -> list:
        return self._husky_dof_idxs
    








    def apply_wheel_actions(self, actions : ArticulationAction) -> None:

        actions_length = actions.get_length()
        if actions_length is not None and actions_length != self._num_wheel_dof:
            raise Exception("ArticulationAction passed should be the same length as the number of wheels")
        joint_actions = ArticulationAction()

        if actions.joint_positions is not None:
            joint_actions.joint_positions = np.zeros(self._num_dof)
            for i in range(self._num_wheel_dof):
                joint_actions.joint_positions[self._husky_dof_idxs[i]] = actions.joint_postions[i]

        if actions.joint_velocities is not None:
            joint_actions.joint_velocities = np.zeros(self._num_dof)
            for i in range(self._num_wheel_dof):
                joint_actions.joint_velocities[self._husky_dof_idxs[i]] = actions.joint_velocities[i]
        
        if actions.joint_efforts is not None:
            joint_actions.joint_efforts = np.zeros(self._num_dof)
            for i in range(self._num_wheel_dof):
                joint_actions.joint_efforts[self._husky_dof_idxs[i]] = actions.joint_efforts[i]
        self.apply_action(control_actions = joint_actions)
                
















    def initialize(self, physics_sim_view=None) -> None:


        super().initialize(physics_sim_view=physics_sim_view)
        if self._husky_base_joint_names is not None:
            self._husky_dof_idxs = [
                self.get_dof_index(self._husky_base_joint_names[i]) for i in range(len(self._husky_base_joint_names))
            ]
        elif self._husky_dof_idxs is None:
            carb.log_error("need to have either wheel names or wheel indices")

        self._num_wheel_dof = len(self._husky_dof_idxs)

        return




    def post_reset(self) -> None:

        Robot.post_reset(self)
        if self._articulation_controller is not None:
            self._articulation_controller.switch_control_mode(mode="velocity")
        return

Yes the tuning parameters doesnt have any relevance to the custom robot since the previous tuning was for a UR10 arm and I am using a UR3, so it would be similar. Yes but I should check robot_base_translation,robot_base_orientation if its matching with where my actual base of the robot is.

Thank you, @arudich.

I would be obliged if you check out my previous reply or ping someone who knows about this customization.

It sounds like you could benefit from using ArticulationSubset from omni.isaac.core.articulations

from omni.isaac.core.articulations import ArticulationSubset
art_subset = ArticulationSubset(self, self._ur_joint_names)

Then you can do
art_subset.apply_action(joint_positions: np.array, joint_velocities: np.array)

The point of ArticulationSubset is to handle the mapping between joint names and joint indices. So you pass it an Articulation (which in your class is selfRobot inherits from Articulation) and you pass it the joint names you want to control. Then you can send commands to just those joints. I.e. the ArticulationSubset.apply_action() would expect a 6-d action for the UR10 joints.

Thanks for your help. I am grateful.

This topic was automatically closed 14 days after the last reply. New replies are no longer allowed.