ROS integration for custom motion profiles

Ask
1

@bshah @Mohamed_Fazil
I am looking into custom values of velocity and acceleration for each of the joints in Stretch RE2. I am currently using the stretch_params slow, default, and fast profiles per joint.
I am interested to integrate this with the home_robot_hw ROS stack. Since the ROS stack is built on top of the stretch Python libraries, I wanted to ask for help in integrating the following stretch Python library commands into ROS.

robot.arm.move_to(0.0, v_m, a_m)
OR 
robot.lift.move_to(point_3d['z'], v_m, a_m)

I have defined motion profiles for arm and lift in the following way:

motion_profiles = {
    joint: {
        'slow': {'vel_m': 0.05, 'accel_m': 0.05}, # robot_params._nominal_params[joint]['motion']['slow'],
        'default': {'vel_m': 0.08, 'accel_m': 0.08}, # robot_params._nominal_params[joint]['motion']['default'],
        'fast': {'vel_m': 0.12, 'accel_m': 0.15}, # robot_params._nominal_params[joint]['motion']['fast'],
        'very_fast': {'vel_m': 0.15, 'accel_m': 0.15},  # robot_params._nominal_params[joint]['motion']['trajectory_max'],
    }
for joint in ['lift', 'arm']}
2

Hi @Vidhi_Jain, since the home_robot_hw package is actually built on Stretch’s ROS package, you’ll want to interact with stretch_driver’s FollowJointTrajectory action server instead of using those move_to methods. This is easily done by creating a ROS node and using the move_to_pose() method. So you would roslaunch stretch_driver.launch, and then call move_to_pose() with custom_full_goal=True and a dictionary where the values are 4-float tuples. The tuples are (position_goal, velocity, acceleration, effort_threshold). For example:

# roslaunch the stretch launch file beforehand

import hello_helpers.hello_misc as hm
temp = hm.HelloNode.quick_create('temp')

motion_profiles = {
    'joint_lift': {
        'slow': {'vel_m': 0.05, 'accel_m': 0.05}, # robot_params._nominal_params[joint]['motion']['slow'],
        'default': {'vel_m': 0.08, 'accel_m': 0.08}, # robot_params._nominal_params[joint]['motion']['default'],
        'fast': {'vel_m': 0.12, 'accel_m': 0.15}, # robot_params._nominal_params[joint]['motion']['fast'],
        'very_fast': {'vel_m': 0.15, 'accel_m': 0.15},  # robot_params._nominal_params[joint]['motion']['trajectory_max'],
    }
for joint in ['joint_lift', 'joint_arm']}
    v = motion_profiles[joint]['slow']['vel_m']
    a = motion_profiles[joint]['slow']['accel_m']
    temp.move_to_pose({joint: (0.5, v, a, 40)}, custom_full_goal=True)

More documentation on creating a ROS HelloNode and using the move_to_pose() method is available here.

1 Like
3

Thanks, Binit! A small clarification question:
What do accel, accel_m, and accel_r mean in the stretch_body.robot_params.get_params() {joint_name} 'motion'?

4

Hey Vidhi, good question, the accel and accel_m parameters are the ones you care about. They control the acceleration of the trapezoidal motion profile that the joints on Stretch follow. accel pertains to the Dynamixel joints (e.g. the head joints or the wrist yaw/pitch/roll joints). For example:

In [10]: stretch_body.robot_params.RobotParams.get_params()[1]['wrist_yaw']['motion']
Out[10]: 
{'trajectory_vel_ctrl': 1,
 'trajectory_vel_ctrl_kP': 1.5,
 'default': {'accel': 3.0, 'vel': 2.0},
 'fast': {'accel': 5.0, 'vel': 2.5},
 'max': {'accel': 10, 'vel': 6.0},
 'slow': {'accel': 1.5, 'vel': 0.75},
 'trajectory_max': {'vel_r': 3.0, 'accel_r': 3.0}}

and accel_m pertains to the Stepper joints (e.g. arm, lift, and wheel motors). For example:

In [15]: stretch_body.robot_params.RobotParams.get_params()[1]['lift']['motion']
Out[15]: 
{'default': {'accel_m': 0.15, 'vel_m': 0.095},
 'fast': {'accel_m': 0.2, 'vel_m': 0.12},
 'max': {'accel_m': 0.3, 'vel_m': 0.15},
 'slow': {'accel_m': 0.05, 'vel_m': 0.05},
 'trajectory_max': {'vel_m': 0.2, 'accel_m': 0.3}}

You can ignore the trajectory_max dictionaries (which include vel_r/accel_r/vel_m/accel_m) since those parameters relate to splined motion profile, which is a different way of moving the robot’s joints.

2 Likes