Custom Robot Arm not reaching target while using RMP Flow | Imprecise control of robotic arm

Autonomous Machines Robotics - Isaac Isaac Gym
1

I want to integrate a UR3 (Universal Robot) arm on a movable Clearpath Husky base to create a custom mobile manipulator and I would like to use the RMP flow algorithm of Nvidia to make the robot arm reach a target position, I had previously posted on the forum regarding the issue I am facing, however I made some changes in the python script

Current situation, please check out the YouTube video in the given link.
Issue video-Read the description of the video as well

While using controlling the robotic arm using the RMP flow the Lula is considering a different kinematic object(structure) and not the robot while generating articulation actions.

This is the running Python script for the video given above.



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 omni.isaac.core.utils.extensions import get_extension_path_from_name





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


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()


rmpflow = RmpFlow(
    robot_description_path = "./husky_ur3/rmpflow/config_2.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()

rmpflow.visualize_end_effector_position()

# rmpflow.set_ignore_state_updates(True)




# bad_proportional_gains = articulation_controller.get_gains()[0]/50
# articulation_controller.set_gains(kps = bad_proportional_gains)


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"],
        )


        rmpflow_end_effector_prim = rmpflow.get_end_effector_as_prim()
        x,y = rmpflow_end_effector_prim.get_world_pose()
        
        # print(x,y)
        # print(rmpflow.get_end_effector_as_prim())
        #print(my_robot.get_end_effector)

        #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()

You can check how I hard coded the cube y cordinate as -y to make the end effector move in the while loop. As you can see in the video while the actual end effector is moving towards +y direction the lula end effector is moving towards -y direction.

  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"],
    )

I would be obliged if you help me out in setting up the robot and use RMP flow to reach a target object.

If you require any other code you can check the topic here

Robot collision sphere moved from robot while using RMPflow

Thanks in advance.

2

Hi,

I think the code structure is correct so far. I have a similar task implemented with RMPflow in the same structure. what happens if you simply pass the position and orientation of the observation to the method rmpflow.set_end_effector_target()?

Something like this:

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.
        rmpflow.set_end_effector_target(
            target_position=observations[target_name]["position"],
            target_orientation=observations[target_name]["orientation"],
        )

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

Robin

3

The robotic arm moves towards +y if the target is moved towards -y, but the Lula end effector moves towards the direction of the target cube. Check the gray cuboid in the video.

Here is the link for the video.
video

4

What problem do you have with the implemented RMP flow algo ??

Regards,
T.

5

sorry it was poorly written. I had no problems with the implementation. I tried it with a UR10 and a UR5e.

Can you provide the files for the RMPFlow() object? (robot_description, urdf_path, rmpflow_config_path).

6

Sure, here you go.

husky_ur3_config_files.zip (12.0 KB)

7

robot_util_files.zip (9.3 KB)
These are the .py files and it also contains the USD of the scene.

8

I have looked at the files, I have not noticed anything out of the ordinary, could you try replacing the config_2.yaml with the following content?

# 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,-0.785,-1.5707,-2.356,-1.5707,-1.5707]

# Most dimensions of the cspace have a direct corresponding element
# in the URDF. This list of rules defines how unspecified coordinates
# should be extracted or how values in the URDF should be overwritten.

cspace_to_urdf_rules:
    - {name: front_left_wheel, rule: fixed, value: 0.0}
    - {name: front_right_wheel, rule: fixed, value: 0.0}
    - {name: rear_left_wheel, rule: fixed, value: 0.0}
    - {name: rear_right_wheel, rule: fixed, value: 0.0}

# Lula uses collision spheres to define the robot geometry in order to avoid
# collisions with external obstacles.  If no spheres are specified, Lula will
# not be able to avoid obstacles.

collision_spheres:
  - ur_arm_base_link_inertia:
    - "center": [0.0, 0.0, 0.0]
      "radius": 0.08
  - ur_arm_shoulder_link:
    - "center": [0.0, 0.0, 0.0]
      "radius": 0.07
    - "center": [0.013, 0.014, -0.07]
      "radius": 0.04
  - ur_arm_upper_arm_link:
    - "center": [-0.002, -0.003, 0.143]
      "radius": 0.07
    - "center": [-0.234, -0.0, 0.131]
      "radius": 0.06
    - "center": [-0.063, -0.002, 0.14]
      "radius": 0.067
    - "center": [-0.122, -0.001, 0.137]
      "radius": 0.065
    - "center": [-0.179, -0.001, 0.134]
      "radius": 0.062
  - ur_arm_forearm_link:
    - "center": [-0.0, -0.0, 0.045]
      "radius": 0.05
    - "center": [-0.201, -0.0, 0.03]
      "radius": 0.05
    - "center": [-0.04, -0.0, 0.042]
      "radius": 0.05
    - "center": [-0.08, -0.0, 0.039]
      "radius": 0.05
    - "center": [-0.12, -0.0, 0.036]
      "radius": 0.05
    - "center": [-0.161, -0.0, 0.033]
      "radius": 0.05
  - ur_arm_wrist_1_link:
    - "center": [0.0, 0.0, 0.0]
      "radius": 0.05
  - ur_arm_wrist_2_link:
    - "center": [0.0, 0.0, 0.0]
      "radius": 0.045
  - ur_arm_wrist_3_link:
    - "center": [0.0, 0.0, 0.0]
      "radius": 0.04
  - base_link:
    - "center": [-0.32, 0.145, -0.0]
      "radius": 0.05
    - "center": [-0.333, -0.142, -0.0]
      "radius": 0.05
    - "center": [-0.322, 0.097, -0.0]
      "radius": 0.05
    - "center": [-0.324, 0.049, -0.0]
      "radius": 0.05
    - "center": [-0.326, 0.002, -0.0]
      "radius": 0.05
    - "center": [-0.329, -0.046, -0.0]
      "radius": 0.05
    - "center": [-0.331, -0.094, -0.0]
      "radius": 0.05
    - "center": [0.339, -0.165, 0.0]
      "radius": 0.05
    - "center": [-0.256, -0.144, -0.0]
      "radius": 0.05
    - "center": [-0.179, -0.147, -0.0]
      "radius": 0.05
    - "center": [-0.101, -0.149, -0.0]
      "radius": 0.05
    - "center": [-0.024, -0.152, -0.0]
      "radius": 0.05
    - "center": [0.053, -0.155, 0.0]
      "radius": 0.05
    - "center": [0.131, -0.157, 0.0]
      "radius": 0.05
    - "center": [0.208, -0.16, 0.0]
      "radius": 0.05
    - "center": [0.285, -0.163, 0.0]
      "radius": 0.05
    - "center": [0.332, 0.142, 0.0]
      "radius": 0.05
    - "center": [-0.247, 0.145, -0.0]
      "radius": 0.05
    - "center": [-0.175, 0.144, -0.0]
      "radius": 0.05
    - "center": [-0.102, 0.144, -0.0]
      "radius": 0.05
    - "center": [-0.03, 0.144, -0.0]
      "radius": 0.05
    - "center": [0.043, 0.143, 0.0]
      "radius": 0.05
    - "center": [0.115, 0.143, 0.0]
      "radius": 0.05
    - "center": [0.187, 0.143, 0.0]
      "radius": 0.05
    - "center": [0.26, 0.143, 0.0]
      "radius": 0.05
    - "center": [0.338, -0.114, 0.0]
      "radius": 0.05
    - "center": [0.337, -0.063, 0.0]
      "radius": 0.05
    - "center": [0.336, -0.011, 0.0]
      "radius": 0.05
    - "center": [0.335, 0.04, 0.0]
      "radius": 0.05
    - "center": [0.334, 0.091, 0.0]
      "radius": 0.05
    - "center": [0.264, -0.112, 0.0]
      "radius": 0.05
    - "center": [0.19, -0.109, 0.0]
      "radius": 0.05
    - "center": [0.115, -0.107, 0.0]
      "radius": 0.05
    - "center": [0.041, -0.105, 0.0]
      "radius": 0.05
    - "center": [-0.034, -0.103, -0.0]
      "radius": 0.05
    - "center": [-0.108, -0.101, -0.0]
      "radius": 0.05
    - "center": [-0.182, -0.098, -0.0]
      "radius": 0.05
    - "center": [-0.257, -0.096, -0.0]
      "radius": 0.05
    - "center": [0.263, -0.061, 0.0]
      "radius": 0.05
    - "center": [0.189, -0.059, 0.0]
      "radius": 0.05
    - "center": [0.115, -0.057, 0.0]
      "radius": 0.05
    - "center": [0.041, -0.055, 0.0]
      "radius": 0.05
    - "center": [-0.033, -0.053, -0.0]
      "radius": 0.05
    - "center": [-0.107, -0.052, -0.0]
      "radius": 0.05
    - "center": [-0.181, -0.05, -0.0]
      "radius": 0.05
    - "center": [-0.255, -0.048, -0.0]
      "radius": 0.05
    - "center": [0.262, -0.01, 0.0]
      "radius": 0.05
    - "center": [0.189, -0.009, 0.0]
      "radius": 0.05
    - "center": [0.115, -0.007, 0.0]
      "radius": 0.05
    - "center": [0.042, -0.006, 0.0]
      "radius": 0.05
    - "center": [-0.032, -0.004, -0.0]
      "radius": 0.05
    - "center": [-0.106, -0.003, -0.0]
      "radius": 0.05
    - "center": [-0.179, -0.001, -0.0]
      "radius": 0.05
    - "center": [-0.253, 0.0, -0.0]
      "radius": 0.05
    - "center": [0.262, 0.041, 0.0]
      "radius": 0.05
    - "center": [0.188, 0.042, 0.0]
      "radius": 0.05
    - "center": [0.115, 0.043, 0.0]
      "radius": 0.05
    - "center": [0.042, 0.044, 0.0]
      "radius": 0.05
    - "center": [-0.031, 0.045, -0.0]
      "radius": 0.05
    - "center": [-0.105, 0.046, -0.0]
      "radius": 0.05
    - "center": [-0.178, 0.047, -0.0]
      "radius": 0.05
    - "center": [-0.251, 0.048, -0.0]
      "radius": 0.05
    - "center": [0.261, 0.092, 0.0]
      "radius": 0.05
    - "center": [0.188, 0.092, 0.0]
      "radius": 0.05
    - "center": [0.115, 0.093, 0.0]
      "radius": 0.05
    - "center": [0.042, 0.094, 0.0]
      "radius": 0.05
    - "center": [-0.031, 0.094, -0.0]
      "radius": 0.05
    - "center": [-0.103, 0.095, -0.0]
      "radius": 0.05
    - "center": [-0.176, 0.096, -0.0]
      "radius": 0.05
    - "center": [-0.249, 0.096, -0.0]
      "radius": 0.05
  - rear_right_wheel_link:
    - "center": [0.0, 0.0, 0.0]
      "radius": 0.16
  - rear_left_wheel_link:
    - "center": [0.0, 0.0, 0.0]
      "radius": 0.16
  - front_left_wheel_link:
    - "center": [0.0, 0.0, 0.0]
      "radius": 0.16
  - front_right_wheel_link:
    - "center": [0.0, 0.0, 0.0]
      "radius": 0.16
  - top_plate_link:
    - "center": [-0.247, 0.156, 0.003]
      "radius": 0.023
    - "center": [-0.06, 0.065, 0.003]
      "radius": 0.023
    - "center": [0.056, 0.255, 0.003]
      "radius": 0.023
    - "center": [0.013, 0.126, 0.003]
      "radius": 0.023
    - "center": [-0.324, 0.168, 0.003]
      "radius": 0.023
    - "center": [-0.046, -0.286, 0.003]
      "radius": 0.023
    - "center": [0.352, 0.154, 0.003]
      "radius": 0.023
    - "center": [-0.132, 0.084, 0.003]
      "radius": 0.023
    - "center": [0.32, 0.008, 0.003]
      "radius": 0.023
    - "center": [-0.292, 0.109, 0.003]
      "radius": 0.023
    - "center": [0.026, -0.078, 0.003]
      "radius": 0.023
    - "center": [-0.134, -0.124, 0.003]
      "radius": 0.023
    - "center": [-0.292, -0.181, 0.003]
      "radius": 0.023
    - "center": [-0.104, 0.039, 0.003]
      "radius": 0.023
    - "center": [-0.22, 0.083, 0.003]
      "radius": 0.023
    - "center": [-0.091, -0.238, 0.003]
      "radius": 0.023
    - "center": [-0.058, -0.006, 0.003]
      "radius": 0.023
    - "center": [0.159, 0.079, 0.003]
      "radius": 0.023
    - "center": [-0.192, -0.195, 0.003]
      "radius": 0.023
    - "center": [0.128, 0.111, 0.003]
      "radius": 0.023
    - "center": [-0.281, -0.224, 0.003]
      "radius": 0.023
    - "center": [-0.249, 0.067, 0.003]
      "radius": 0.023
    - "center": [-0.337, 0.24, 0.003]
      "radius": 0.023
    - "center": [-0.017, -0.209, 0.003]
      "radius": 0.023
    - "center": [-0.062, -0.209, 0.003]
      "radius": 0.023
    - "center": [0.332, -0.139, 0.003]
      "radius": 0.023
    - "center": [0.158, 0.05, 0.003]
      "radius": 0.023
    - "center": [-0.295, 0.155, 0.003]
      "radius": 0.023
    - "center": [-0.001, 0.083, 0.003]
      "radius": 0.023
    - "center": [0.058, 0.082, 0.003]
      "radius": 0.023
    - "center": [0.218, 0.271, 0.003]
      "radius": 0.023
    - "center": [0.131, 0.274, 0.003]
      "radius": 0.023
    - "center": [0.129, -0.12, 0.003]
      "radius": 0.023
    - "center": [0.234, -0.095, 0.003]
      "radius": 0.023
    - "center": [-0.291, 0.023, 0.003]
      "radius": 0.023
    - "center": [-0.073, 0.285, 0.003]
      "radius": 0.023
    - "center": [0.088, 0.227, 0.003]
      "radius": 0.023
    - "center": [-0.191, -0.238, 0.003]
      "radius": 0.023
    - "center": [0.115, 0.052, 0.003]
      "radius": 0.023
    - "center": [-0.339, 0.109, 0.003]
      "radius": 0.023
    - "center": [0.1, -0.067, 0.003]
      "radius": 0.023
    - "center": [0.244, 0.083, 0.003]
      "radius": 0.023
    - "center": [-0.279, 0.24, 0.003]
      "radius": 0.023
    - "center": [-0.044, 0.172, 0.003]
      "radius": 0.023
    - "center": [-0.031, 0.228, 0.003]
      "radius": 0.023
    - "center": [0.132, 0.2, 0.003]
      "radius": 0.023
    - "center": [-0.076, 0.186, 0.003]
      "radius": 0.023
    - "center": [-0.031, -0.241, 0.003]
      "radius": 0.023
    - "center": [0.248, 0.242, 0.003]
      "radius": 0.023
    - "center": [0.071, 0.156, 0.003]
      "radius": 0.023
    - "center": [0.278, -0.24, 0.003]
      "radius": 0.023
    - "center": [0.278, -0.166, 0.003]
      "radius": 0.023
    - "center": [-0.014, -0.123, 0.003]
      "radius": 0.023
    - "center": [-0.148, 0.286, 0.003]
      "radius": 0.023
    - "center": [-0.104, -0.095, 0.003]
      "radius": 0.023
    - "center": [-0.119, -0.036, 0.003]
      "radius": 0.023
    - "center": [0.293, 0.156, 0.003]
      "radius": 0.023
    - "center": [-0.351, 0.215, 0.003]
      "radius": 0.023
    - "center": [0.04, 0.037, 0.003]
      "radius": 0.023
    - "center": [-0.176, -0.107, 0.003]
      "radius": 0.023
    - "center": [-0.032, 0.113, 0.003]
      "radius": 0.023
    - "center": [-0.059, 0.021, 0.003]
      "radius": 0.023
    - "center": [0.074, -0.255, 0.003]
      "radius": 0.023
    - "center": [0.026, 0.139, 0.003]
      "radius": 0.023
    - "center": [-0.003, -0.136, 0.003]
      "radius": 0.023
    - "center": [-0.294, 0.225, 0.003]
      "radius": 0.023
    - "center": [-0.238, 0.269, 0.003]
      "radius": 0.023
    - "center": [-0.305, 0.255, 0.003]
      "radius": 0.023
    - "center": [0.332, -0.033, 0.003]
      "radius": 0.023
    - "center": [0.351, 0.2, 0.003]
      "radius": 0.023
    - "center": [0.217, 0.167, 0.003]
      "radius": 0.023
    - "center": [-0.014, 0.157, 0.003]
      "radius": 0.023
    - "center": [-0.295, -0.004, 0.003]
      "radius": 0.023
    - "center": [-0.31, -0.081, 0.003]
      "radius": 0.023
    - "center": [-0.028, 0.006, 0.003]
      "radius": 0.023
    - "center": [-0.238, -0.138, 0.003]
      "radius": 0.023
    - "center": [-0.249, 0.23, 0.003]
      "radius": 0.023
    - "center": [0.011, 0.259, 0.003]
      "radius": 0.023
    - "center": [-0.076, -0.169, 0.003]
      "radius": 0.023
    - "center": [0.146, 0.271, 0.003]
      "radius": 0.023
    - "center": [0.144, 0.289, 0.003]
      "radius": 0.023
    - "center": [-0.251, 0.274, 0.003]
      "radius": 0.023
    - "center": [-0.306, 0.181, 0.003]
      "radius": 0.023
    - "center": [-0.145, 0.094, 0.003]
      "radius": 0.023
    - "center": [0.297, 0.007, 0.003]
      "radius": 0.023
    - "center": [-0.222, 0.234, 0.003]
      "radius": 0.023
    - "center": [0.322, -0.1, 0.003]
      "radius": 0.023
    - "center": [0.32, 0.209, 0.003]
      "radius": 0.023
    - "center": [-0.173, -0.151, 0.003]
      "radius": 0.023
    - "center": [0.147, 0.034, 0.003]
      "radius": 0.023
    - "center": [0.339, -0.242, 0.003]
      "radius": 0.023
    - "center": [0.133, -0.017, 0.003]
      "radius": 0.023
    - "center": [-0.041, 0.08, 0.003]
      "radius": 0.023
    - "center": [-0.201, -0.152, 0.003]
      "radius": 0.023
    - "center": [0.069, -0.096, 0.003]
      "radius": 0.023
    - "center": [-0.049, 0.226, 0.003]
      "radius": 0.023
    - "center": [0.141, -0.051, 0.003]
      "radius": 0.023
    - "center": [0.204, -0.084, 0.003]
      "radius": 0.023
    - "center": [-0.266, 0.116, 0.003]
      "radius": 0.023
    - "center": [0.062, 0.037, 0.003]
      "radius": 0.023
    - "center": [0.277, -0.262, 0.003]
      "radius": 0.023
    - "center": [0.014, 0.181, 0.003]
      "radius": 0.023
    - "center": [0.101, 0.188, 0.003]
      "radius": 0.023
    - "center": [-0.168, 0.255, 0.003]
      "radius": 0.023
    - "center": [-0.105, -0.06, 0.003]
      "radius": 0.023
    - "center": [-0.206, -0.043, 0.003]
      "radius": 0.023
    - "center": [0.027, -0.149, 0.003]
      "radius": 0.023
    - "center": [0.059, -0.029, 0.003]
      "radius": 0.023
    - "center": [0.159, -0.046, 0.003]
      "radius": 0.023
    - "center": [0.13, 0.175, 0.003]
      "radius": 0.023
    - "center": [-0.32, -0.159, 0.003]
      "radius": 0.023
    - "center": [-0.263, -0.142, 0.003]
      "radius": 0.023
    - "center": [0.205, -0.045, 0.003]
      "radius": 0.023
    - "center": [-0.198, 0.167, 0.003]
      "radius": 0.023
    - "center": [0.221, -0.195, 0.003]
      "radius": 0.023
    - "center": [0.158, 0.25, 0.003]
      "radius": 0.023
    - "center": [0.175, 0.013, 0.003]
      "radius": 0.023
    - "center": [-0.339, -0.278, 0.003]
      "radius": 0.023
    - "center": [0.142, -0.158, 0.003]
      "radius": 0.023
    - "center": [-0.175, -0.275, 0.003]
      "radius": 0.023
    - "center": [0.17, -0.003, 0.003]
      "radius": 0.023
    - "center": [-0.063, -0.139, 0.003]
      "radius": 0.023
    - "center": [0.15, -0.107, 0.003]
      "radius": 0.023
    - "center": [0.249, 0.059, 0.003]
      "radius": 0.023
    - "center": [0.173, -0.175, 0.003]
      "radius": 0.023
    - "center": [0.284, -0.006, 0.003]
      "radius": 0.023
    - "center": [0.048, -0.268, 0.003]
      "radius": 0.023
    - "center": [0.185, -0.108, 0.003]
      "radius": 0.023
    - "center": [0.083, -0.195, 0.003]
      "radius": 0.023
    - "center": [0.183, -0.282, 0.003]
      "radius": 0.023
    - "center": [0.104, -0.238, 0.003]
      "radius": 0.023
    - "center": [0.1, -0.112, 0.003]
      "radius": 0.023
    - "center": [0.121, -0.05, 0.003]
      "radius": 0.023
    - "center": [-0.013, -0.19, 0.003]
      "radius": 0.023
    - "center": [0.219, -0.26, 0.003]
      "radius": 0.023
    - "center": [-0.192, -0.176, 0.003]
      "radius": 0.023
    - "center": [-0.147, -0.177, 0.003]
      "radius": 0.023
    - "center": [-0.276, -0.26, 0.003]
      "radius": 0.023
    - "center": [0.319, 0.073, 0.003]
      "radius": 0.023
    - "center": [-0.312, -0.12, 0.003]
      "radius": 0.023
    - "center": [-0.216, -0.166, 0.003]
      "radius": 0.023
    - "center": [0.075, 0.083, 0.003]
      "radius": 0.023
    - "center": [0.205, -0.187, 0.003]
      "radius": 0.023
    - "center": [0.33, -0.18, 0.003]
      "radius": 0.023
    - "center": [0.219, -0.0, 0.003]
      "radius": 0.023
    - "center": [0.352, -0.017, 0.003]
      "radius": 0.023
    - "center": [0.33, -0.077, 0.003]
      "radius": 0.023
    - "center": [0.291, 0.188, 0.003]
      "radius": 0.023
    - "center": [0.286, 0.066, 0.003]
      "radius": 0.023
    - "center": [-0.068, 0.139, 0.003]
      "radius": 0.023
    - "center": [0.071, -0.279, 0.003]
      "radius": 0.023
    - "center": [-0.261, -0.077, 0.003]
      "radius": 0.023
    - "center": [0.015, -0.292, 0.003]
      "radius": 0.023
    - "center": [-0.237, -0.294, 0.003]
      "radius": 0.023
    - "center": [0.029, -0.245, 0.003]
      "radius": 0.023
    - "center": [0.236, -0.167, 0.003]
      "radius": 0.023
    - "center": [0.275, -0.188, 0.003]
      "radius": 0.023
    - "center": [0.133, 0.01, 0.003]
      "radius": 0.023
    - "center": [-0.018, -0.045, 0.003]
      "radius": 0.023
    - "center": [0.287, -0.093, 0.003]
      "radius": 0.023
    - "center": [-0.331, -0.18, 0.003]
      "radius": 0.023
    - "center": [-0.091, 0.014, 0.003]
      "radius": 0.023
    - "center": [-0.315, -0.282, 0.003]
      "radius": 0.023
    - "center": [-0.127, -0.006, 0.003]
      "radius": 0.023
    - "center": [0.086, -0.129, 0.003]
      "radius": 0.023
    - "center": [-0.208, 0.147, 0.003]
      "radius": 0.023
    - "center": [-0.322, -0.234, 0.003]
      "radius": 0.023
    - "center": [-0.102, -0.075, 0.003]
      "radius": 0.023
    - "center": [-0.264, 0.205, 0.003]
      "radius": 0.023
    - "center": [0.136, -0.268, 0.003]
      "radius": 0.023
    - "center": [-0.062, -0.235, 0.003]
      "radius": 0.023
    - "center": [0.232, -0.245, 0.003]
      "radius": 0.023
    - "center": [0.026, 0.279, 0.003]
      "radius": 0.023
    - "center": [-0.339, 0.136, 0.003]
      "radius": 0.023
    - "center": [-0.1, 0.138, 0.003]
      "radius": 0.023
    - "center": [-0.115, 0.167, 0.003]
      "radius": 0.023
    - "center": [-0.221, 0.045, 0.003]
      "radius": 0.023
    - "center": [0.267, 0.287, 0.003]
      "radius": 0.023
    - "center": [-0.306, -0.149, 0.003]
      "radius": 0.023
    - "center": [-0.192, 0.216, 0.003]
      "radius": 0.023
    - "center": [0.17, -0.282, 0.003]
      "radius": 0.023
    - "center": [-0.363, 0.156, 0.003]
      "radius": 0.023
    - "center": [-0.357, -0.05, 0.003]
      "radius": 0.023
    - "center": [-0.347, 0.04, 0.003]
      "radius": 0.023
    - "center": [-0.329, 0.269, 0.003]
      "radius": 0.023
    - "center": [-0.221, 0.116, 0.003]
      "radius": 0.023
    - "center": [-0.22, -0.087, 0.003]
      "radius": 0.023
    - "center": [-0.262, -0.039, 0.003]
      "radius": 0.023
    - "center": [-0.11, -0.197, 0.003]
      "radius": 0.023
    - "center": [-0.231, -0.241, 0.003]
      "radius": 0.023
    - "center": [0.238, -0.283, 0.003]
      "radius": 0.023
    - "center": [-0.237, 0.002, 0.003]
      "radius": 0.023
    - "center": [-0.288, -0.018, 0.003]
      "radius": 0.023
    - "center": [-0.28, 0.132, 0.003]
      "radius": 0.023
    - "center": [-0.335, -0.148, 0.003]
      "radius": 0.023
    - "center": [0.078, 0.182, 0.003]
      "radius": 0.023
    - "center": [-0.19, 0.016, 0.003]
      "radius": 0.023
    - "center": [-0.11, 0.241, 0.003]
      "radius": 0.023
    - "center": [0.055, -0.162, 0.003]
      "radius": 0.023
    - "center": [-0.176, 0.003, 0.003]
      "radius": 0.023
    - "center": [-0.176, 0.178, 0.003]
      "radius": 0.023
    - "center": [0.069, 0.118, 0.003]
      "radius": 0.023
    - "center": [0.262, -0.011, 0.003]
      "radius": 0.023
    - "center": [0.259, 0.208, 0.003]
      "radius": 0.023
    - "center": [-0.148, 0.032, 0.003]
      "radius": 0.023
    - "center": [0.274, -0.221, 0.003]
      "radius": 0.023
    - "center": [0.29, 0.208, 0.003]
      "radius": 0.023
    - "center": [-0.103, 0.203, 0.003]
      "radius": 0.023
    - "center": [-0.173, 0.082, 0.003]
      "radius": 0.023
    - "center": [0.175, -0.083, 0.003]
      "radius": 0.023
    - "center": [0.304, 0.131, 0.003]
      "radius": 0.023
    - "center": [-0.273, 0.17, 0.003]
      "radius": 0.023
    - "center": [-0.246, -0.169, 0.003]
      "radius": 0.023
    - "center": [0.157, 0.145, 0.003]
      "radius": 0.023
    - "center": [0.196, -0.11, 0.003]
      "radius": 0.023
    - "center": [0.094, 0.171, 0.003]
      "radius": 0.023
    - "center": [-0.209, 0.213, 0.003]
      "radius": 0.023
    - "center": [-0.011, 0.022, 0.003]
      "radius": 0.023
    - "center": [0.284, 0.258, 0.003]
      "radius": 0.023
    - "center": [-0.242, 0.035, 0.003]
      "radius": 0.023
    - "center": [-0.324, 0.0, 0.003]
      "radius": 0.023
    - "center": [0.12, 0.154, 0.003]
      "radius": 0.023
    - "center": [0.256, -0.137, 0.003]
      "radius": 0.023
    - "center": [0.023, -0.111, 0.003]
      "radius": 0.023
    - "center": [-0.028, 0.172, 0.003]
      "radius": 0.023
    - "center": [0.331, -0.152, 0.003]
      "radius": 0.023
    - "center": [0.35, -0.026, 0.003]
      "radius": 0.023
    - "center": [0.159, 0.276, 0.003]
      "radius": 0.023
    - "center": [0.008, 0.099, 0.003]
      "radius": 0.023
    - "center": [0.343, -0.124, 0.003]
      "radius": 0.023
    - "center": [0.305, 0.158, 0.003]
      "radius": 0.023
    - "center": [0.133, -0.139, 0.003]
      "radius": 0.023
    - "center": [0.067, -0.24, 0.003]
      "radius": 0.023
    - "center": [-0.227, -0.138, 0.003]
      "radius": 0.023
    - "center": [-0.187, -0.271, 0.003]
      "radius": 0.023
    - "center": [-0.16, -0.103, 0.003]
      "radius": 0.023
    - "center": [0.032, 0.026, 0.003]
      "radius": 0.023
    - "center": [0.051, 0.026, 0.003]
      "radius": 0.023
    - "center": [0.058, -0.105, 0.003]
      "radius": 0.023
    - "center": [-0.319, -0.063, 0.003]
      "radius": 0.023
    - "center": [-0.239, -0.282, 0.003]
      "radius": 0.023
    - "center": [0.053, 0.067, 0.003]
      "radius": 0.023
    - "center": [-0.018, -0.229, 0.003]
      "radius": 0.023
    - "center": [-0.139, -0.169, 0.003]
      "radius": 0.023
    - "center": [-0.121, -0.112, 0.003]
      "radius": 0.023
    - "center": [-0.163, -0.176, 0.003]
      "radius": 0.023
    - "center": [0.115, 0.278, 0.003]
      "radius": 0.023
    - "center": [-0.119, -0.133, 0.003]
      "radius": 0.023
    - "center": [-0.287, -0.164, 0.003]
      "radius": 0.023
    - "center": [-0.057, -0.239, 0.003]
      "radius": 0.023
    - "center": [-0.353, -0.055, 0.003]
      "radius": 0.023
    - "center": [-0.276, -0.149, 0.003]
      "radius": 0.023
    - "center": [-0.041, 0.065, 0.003]
      "radius": 0.023
    - "center": [-0.11, -0.022, 0.003]
      "radius": 0.023
    - "center": [-0.145, 0.1, 0.003]
      "radius": 0.023
    - "center": [0.288, 0.073, 0.003]
      "radius": 0.023
    - "center": [-0.15, -0.18, 0.003]
      "radius": 0.023
    - "center": [-0.147, -0.127, 0.003]
      "radius": 0.023
    - "center": [0.109, -0.256, 0.003]
      "radius": 0.023
    - "center": [0.188, -0.191, 0.003]
      "radius": 0.023
    - "center": [0.218, -0.104, 0.003]
      "radius": 0.023
    - "center": [0.17, 0.256, 0.003]
      "radius": 0.023
    - "center": [0.293, -0.25, 0.003]
      "radius": 0.023
    - "center": [-0.043, 0.017, 0.003]
      "radius": 0.023
    - "center": [-0.246, -0.008, 0.003]
      "radius": 0.023
    - "center": [0.147, 0.246, 0.003]
      "radius": 0.023
    - "center": [0.258, 0.227, 0.003]
      "radius": 0.023
    - "center": [-0.249, 0.209, 0.003]
      "radius": 0.023
    - "center": [-0.338, -0.185, 0.003]
      "radius": 0.023
    - "center": [-0.066, 0.268, 0.003]
      "radius": 0.023
    - "center": [0.252, -0.216, 0.003]
      "radius": 0.023
    - "center": [0.095, 0.106, 0.003]
      "radius": 0.023
    - "center": [-0.152, 0.268, 0.003]
      "radius": 0.023
    - "center": [0.003, 0.223, 0.003]
      "radius": 0.023
    - "center": [0.362, -0.206, 0.003]
      "radius": 0.022
    - "center": [-0.256, -0.161, 0.003]
      "radius": 0.023
    - "center": [-0.332, -0.071, 0.003]
      "radius": 0.023
    - "center": [-0.143, 0.019, 0.003]
      "radius": 0.023
    - "center": [0.142, 0.231, 0.003]
      "radius": 0.023
    - "center": [0.05, 0.15, 0.003]
      "radius": 0.023
    - "center": [-0.356, 0.06, 0.003]
      "radius": 0.023
    - "center": [0.091, 0.03, 0.003]
      "radius": 0.023
    - "center": [0.045, 0.291, 0.005]
      "radius": 0.021
    - "center": [-0.024, 0.202, 0.003]
      "radius": 0.023
    - "center": [-0.111, 0.092, 0.003]
      "radius": 0.023
    - "center": [0.096, -0.275, 0.003]
      "radius": 0.023
    - "center": [0.151, 0.181, 0.003]
      "radius": 0.023
    - "center": [0.181, 0.209, 0.003]
      "radius": 0.023
    - "center": [-0.084, 0.027, 0.003]
      "radius": 0.023
    - "center": [-0.079, 0.263, 0.003]
      "radius": 0.023
    - "center": [0.023, 0.163, 0.003]
      "radius": 0.023
    - "center": [0.156, -0.023, 0.003]
      "radius": 0.023
    - "center": [0.051, -0.017, 0.003]
      "radius": 0.023
    - "center": [-0.009, -0.016, 0.003]
      "radius": 0.023
    - "center": [0.109, 0.164, 0.003]
      "radius": 0.023
    - "center": [-0.347, 0.013, 0.003]
      "radius": 0.023
    - "center": [0.016, -0.061, 0.003]
      "radius": 0.023
    - "center": [-0.342, -0.073, 0.003]
      "radius": 0.023
    - "center": [-0.083, 0.251, 0.003]
      "radius": 0.023
    - "center": [0.282, 0.282, 0.003]
      "radius": 0.023
    - "center": [-0.122, 0.103, 0.003]
      "radius": 0.023
    - "center": [0.067, 0.176, 0.003]
      "radius": 0.023
    - "center": [-0.187, -0.09, 0.003]
      "radius": 0.023
    - "center": [-0.267, 0.193, 0.003]
      "radius": 0.023
    - "center": [-0.038, 0.152, 0.003]
      "radius": 0.023
    - "center": [-0.098, -0.13, 0.003]
      "radius": 0.023
    - "center": [0.067, -0.288, 0.003]
      "radius": 0.023
    - "center": [-0.05, -0.176, 0.003]
      "radius": 0.023
    - "center": [-0.225, 0.158, 0.003]
      "radius": 0.023
    - "center": [-0.2, 0.203, 0.003]
      "radius": 0.023
    - "center": [-0.096, 0.234, 0.003]
      "radius": 0.023
    - "center": [0.067, 0.012, 0.003]
      "radius": 0.023
    - "center": [0.051, 0.003, 0.003]
      "radius": 0.023
    - "center": [0.294, 0.091, 0.003]
      "radius": 0.023
    - "center": [-0.171, 0.193, 0.003]
      "radius": 0.023
    - "center": [0.24, -0.017, 0.003]
      "radius": 0.023
    - "center": [0.266, 0.061, 0.003]
      "radius": 0.023
    - "center": [-0.245, 0.245, 0.003]
      "radius": 0.023
    - "center": [0.257, -0.032, 0.003]
      "radius": 0.023
    - "center": [-0.271, -0.011, 0.003]
      "radius": 0.023
    - "center": [-0.301, 0.207, 0.003]
      "radius": 0.023
    - "center": [-0.077, 0.203, 0.003]
      "radius": 0.023
    - "center": [0.207, 0.06, 0.003]
      "radius": 0.023
    - "center": [0.301, -0.057, 0.003]
      "radius": 0.023
    - "center": [0.297, -0.218, 0.003]
      "radius": 0.023
    - "center": [0.199, 0.145, 0.003]
      "radius": 0.023
    - "center": [0.09, -0.088, 0.003]
      "radius": 0.023
    - "center": [0.166, -0.214, 0.003]
      "radius": 0.023
    - "center": [-0.009, 0.117, 0.003]
      "radius": 0.023
    - "center": [0.27, 0.135, 0.003]
      "radius": 0.023
    - "center": [0.209, -0.118, 0.003]
      "radius": 0.023
    - "center": [0.122, -0.0, 0.003]
      "radius": 0.023
    - "center": [0.227, -0.162, 0.003]
      "radius": 0.023
    - "center": [-0.079, 0.224, 0.003]
      "radius": 0.023
    - "center": [-0.33, -0.06, 0.003]
      "radius": 0.023
    - "center": [-0.241, 0.193, 0.003]
      "radius": 0.023
    - "center": [-0.33, 0.016, 0.003]
      "radius": 0.023
    - "center": [-0.29, 0.187, 0.003]
      "radius": 0.023
    - "center": [0.069, -0.145, 0.003]
      "radius": 0.023
    - "center": [-0.1, -0.003, 0.003]
      "radius": 0.023
    - "center": [0.149, -0.191, 0.003]
      "radius": 0.023
    - "center": [-0.342, 0.062, 0.003]
      "radius": 0.023
    - "center": [0.097, -0.294, 0.003]
      "radius": 0.023
    - "center": [-0.301, -0.131, 0.003]
      "radius": 0.023
    - "center": [-0.314, 0.129, 0.003]
      "radius": 0.023
    - "center": [-0.157, -0.293, 0.003]
      "radius": 0.023
    - "center": [-0.246, 0.088, 0.003]
      "radius": 0.023
    - "center": [-0.124, -0.266, 0.003]
      "radius": 0.023
    - "center": [-0.17, -0.247, 0.003]
      "radius": 0.023
    - "center": [-0.282, -0.273, 0.003]
      "radius": 0.023
    - "center": [-0.142, -0.1, 0.003]
      "radius": 0.023
    - "center": [-0.201, -0.113, 0.003]
      "radius": 0.023
    - "center": [-0.304, 0.086, 0.003]
      "radius": 0.023
    - "center": [-0.1, -0.149, 0.003]
      "radius": 0.023
    - "center": [-0.112, 0.017, 0.003]
      "radius": 0.023
    - "center": [-0.196, 0.085, 0.003]
      "radius": 0.023
    - "center": [0.037, -0.232, 0.003]
      "radius": 0.023
    - "center": [-0.155, -0.046, 0.003]
      "radius": 0.023
    - "center": [-0.184, -0.071, 0.003]
      "radius": 0.023
    - "center": [0.225, 0.146, 0.003]
      "radius": 0.023
    - "center": [-0.214, -0.259, 0.003]
      "radius": 0.023
    - "center": [0.207, 0.129, 0.003]
      "radius": 0.023
    - "center": [-0.005, -0.025, 0.003]
      "radius": 0.023
    - "center": [0.279, -0.145, 0.003]
      "radius": 0.023
    - "center": [0.022, -0.027, 0.003]
      "radius": 0.023
    - "center": [0.06, 0.063, 0.003]
      "radius": 0.023
    - "center": [0.315, -0.042, 0.003]
      "radius": 0.023
    - "center": [0.327, 0.121, 0.003]
      "radius": 0.023
    - "center": [0.257, -0.175, 0.003]
      "radius": 0.023
    - "center": [0.151, 0.108, 0.003]
      "radius": 0.023
    - "center": [-0.007, 0.004, 0.003]
      "radius": 0.023
    - "center": [0.108, 0.086, 0.003]
      "radius": 0.023
    - "center": [0.272, -0.128, 0.003]
      "radius": 0.023
    - "center": [-0.063, -0.243, 0.003]
      "radius": 0.023
    - "center": [0.065, -0.202, 0.003]
      "radius": 0.023
    - "center": [-0.038, -0.114, 0.003]
      "radius": 0.023
    - "center": [0.194, -0.16, 0.003]
      "radius": 0.023
    - "center": [0.251, 0.004, 0.003]
      "radius": 0.023
    - "center": [-0.042, -0.101, 0.003]
      "radius": 0.023
    - "center": [0.285, 0.1, 0.003]
      "radius": 0.023
    - "center": [-0.126, 0.133, 0.003]
      "radius": 0.023
    - "center": [0.314, 0.101, 0.003]
      "radius": 0.023
    - "center": [0.177, 0.131, 0.003]
      "radius": 0.023
    - "center": [0.346, -0.185, 0.003]
      "radius": 0.023
    - "center": [0.2, 0.275, 0.003]
      "radius": 0.023
    - "center": [-0.101, 0.176, 0.003]
      "radius": 0.023
    - "center": [0.023, 0.254, 0.003]
      "radius": 0.023
    - "center": [0.17, -0.128, 0.003]
      "radius": 0.023
    - "center": [0.297, -0.162, 0.003]
      "radius": 0.023
    - "center": [-0.155, 0.173, 0.003]
      "radius": 0.023
    - "center": [0.33, 0.174, 0.003]
      "radius": 0.023
    - "center": [0.308, -0.206, 0.003]
      "radius": 0.023
    - "center": [0.346, -0.102, 0.003]
      "radius": 0.023
    - "center": [0.344, -0.218, 0.003]
      "radius": 0.023
    - "center": [-0.213, 0.291, 0.005]
      "radius": 0.021
    - "center": [-0.12, 0.291, 0.001]
      "radius": 0.021
    - "center": [0.353, 0.237, 0.003]
      "radius": 0.023
    - "center": [0.286, 0.12, 0.003]
      "radius": 0.023
    - "center": [0.36, -0.06, 0.003]
      "radius": 0.023
    - "center": [0.352, 0.116, 0.003]
      "radius": 0.023
9

Thanks for your reply.

I did what you asked I replaced the config_2.yaml with your code and now I dont need to hardcode the +y -y direction change as I mentioned above, but now a change in direction in x has been occurring. If the target moves +x the end effector moves -x.

Check out the video here.
video

I guess I have to change the positive negative values of the default_q vector and check which one gives me the proper directional movement. But how do I make the arm reach precisely to the target position ? Is there any way to do that ?

10

Did you create the ur3_robot_description (or config_2.yaml) file yourself with the Lula Robot Description Editor (Lula Robot Description Editor — Omniverse IsaacSim latest documentation), including the Husky base platform?

11

Yes I did.

I checked out the tutorial and followed the tutorial to make the robot description file. I kept the mobile robot joints ( the 4 wheel joints ) as fixed joints and the ur3 arm joints as active joints and generated collision spheres for the arm, the base of the mobile robot and the 4 wheels.

query2
query2440×733 25.9 KB

However I didnt do it for the small links (rear bumper link, front bumper link …you can see in the photo above) that were attached to the mobile robot base. But I dont think that has to do with anything with how Lula behaves. Does it ?

12

UR5_Test.zip (2.7 MB)

Here you find the config files and a code snippet for a RMPflow application with an UR5e. Maybe this helps.

13

Thank you. I will check it and let you know.