How to set velocities of a mobile robot with /cmd_vel?

Omniverse Isaac Sim
,
1

Hi, I have a mobile robot with omniwheels and want to control its movement using /cmd_vel. Modelling the omniwheels is complicated so I want to set its velocities directly using /cmd_vel. How can I do that without using virtual odom joints (e.g. odom_x, odom_y, odom_z) or using RosTeleport? If this functionality doesn’t exist yet then I would like to implement one and contribute it, where should I start?

2

The easiest way to control the wheels would be to create a RosJointState component and send velocity commands to the individual wheel joints. Similar to the turtlebot sample, the wheel joints would need to have drives configured for velocity control
https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/sample_ros_turtlebot.html#velocity-control-setup

3

I know, but the problem is my robot has mecanum wheels, which can move sideway (Mecanum wheel - Wikipedia). So I would like to control the base with /cmd_vel directly (e.g. move in x or y with 10cm/s) without using the wheel physics - which is very hard to model.

4

One option would be to have a stage with gravity disabled on a specific robot or globally and then use rospy to receive Twist commands and apply velocities to the rigid bodies. (see example below)

The RosTeleport component doesn’t support velocities in the 2021.1.1 release but this will be resolved in 2021.2

I’ve attached an example usd stage with a rigid body prim and a script that can be run in the script editor
The following command can be used to create a dummy message to test

rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear:  {x: 1.0, y: 0.0, z: 0.0}, angular: {x: 0.0,y: 0.0,z: 1.0}}'

velocity_control_test.usd (3.6 KB)

The script below will create a rosnode+subscriber and set the rigid body velocity from the latest message.

import asyncio
import omni
from omni.isaac.dynamic_control import _dynamic_control
import rospy
from geometry_msgs.msg import Twist

try:
    rospy.init_node("isaac_sim_test_cmd", anonymous=True, disable_signals=True, log_level=rospy.ERROR)
except rospy.exceptions.ROSException as e:
    print("Node has already been initialized, do nothing")
pass

class rospy_cube_handler():
    def __init__(self):
        self.dc = _dynamic_control.acquire_dynamic_control_interface()
        self.sub = rospy.Subscriber("/cmd_vel", Twist, self.msg_callback)
        self.cube_handle = self.dc.get_rigid_body("/World/Cube")
        self.meters_per_unit = UsdGeom.GetStageMetersPerUnit(omni.usd.get_context().get_stage())
        self.message = None
    def shutdown(self):
        self.sub.unregister()
        self.sub = None

    def msg_callback(self, data):
        self.message = data
        
    def update(self):
        if self.message is not None:
            lin = self.message.linear
            ang = self.message.angular
            self.dc.wake_up_rigid_body(self.cube_handle)
            self.dc.set_rigid_body_linear_velocity(self.cube_handle, (lin.x/self.meters_per_unit,lin.y/self.meters_per_unit,lin.z/self.meters_per_unit))
            self.dc.set_rigid_body_angular_velocity(self.cube_handle, (ang.x,ang.y,ang.z))
async def my_task():
    await omni.kit.app.get_app().next_update_async()
    
    handler = rospy_cube_handler()
    
    print("RUN")
    # run for a few hundred frames
    for frame in range(600):
        await omni.kit.app.get_app().next_update_async()
        handler.update()
    handler.shutdown()
    print("DONE")

omni.timeline.get_timeline_interface().play()
asyncio.ensure_future(my_task())

2 Likes
5

Thanks a lot, that is exactly what I’m looking for and I’m gladly to hear that it will be resolved in 2021.2. I will test this on my use case and see what happens.

6

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