Dex wrist goes to different positions given the same command

Ask
1

I’ve been using the joint trajectory interface to send commands to the Dex Wrist version of stretch. Here’s an example command:

---                                                                            
header:                                                                        
  seq: 84                                                                      
  stamp:                                                                       
    secs: 1663809518                                                           
    nsecs: 932527065                                                           
  frame_id: ''                                                                 
goal_id:                                                                       
  stamp:                                                                       
    secs: 1663809518                                                           
    nsecs: 932483673                                                           
  id: "/basic_controls-84-1663809518.932"
goal:                                                                          
  trajectory:                                                                  
    header:                                                                    
      seq: 0                                                                   
      stamp:                                                                   
        secs: 1663809518                                                       
        nsecs: 932443380                                                       
      frame_id: ''                                                             
    joint_names:                                                               
      - joint_lift                                                             
      - joint_arm_l0                                                           
      - joint_arm_l1                                                           
      - joint_arm_l2                                                           
      - joint_arm_l3                                                           
      - joint_gripper_finger_left                                              
      - joint_wrist_roll                                                       
      - joint_wrist_pitch                                                      
      - joint_wrist_yaw                                                        
      - joint_head_pan                                                         
      - joint_head_tilt                                                        
    points:                                                                    
      -                                                                        
        positions: [0.5137053394519911, 0.0, 0.0, 0.0, 0.0, -0.038557933294775415, -0.7439806821245359, -0.7761942786701345, -0.7676295526044397, -0.8244375296703885, -0.0010067811717669514]
        velocities: []                                                         
        accelerations: []                                                      
        effort: []                                                             
        time_from_start:                                                       
          secs: 0                                                              
          nsecs:         0                                                     
  path_tolerance: []                                                           
  goal_tolerance: []                                                           
  goal_time_tolerance:                                                         
    secs: 1                                                                    
    nsecs:         0

When I send this the first time, it goes to the “correct” position - arm extended, slightly angled, at a useful pre-grasp pose. When I send it a second time, the wrist rotates 180 degrees – I think it might be trying to rotate all the way around to get to the position. I see these errors on the wrist joint positions after trying to go to this position from that position:

wrist yaw:  3.5918979856853905
wrist pitch: 0.0012965181258930425
wrist roll:  3.858272009700533

Now I could just check and not re-send the command at a given position, but this also applies for nearby positions, which I need for my application (this is a pre-grasp pose for an object).

Is there anything I can do here? It seems like it must be some bug with the dex wrist controllers.

2

Hi @cpaxton, thank you for reporting this issue! I have enough info from your post to try recreating the behavior. I’ll investigate it today and get back to you.

1 Like
3

I tried this with just the 3 wrist commands as well, to verify it wasn’t some weird interaction between components. It’s reproducible just with dex wrist on my robot.

4

Verified on dev/noetic with two different Stretch robots. Any idea what I can do here?

5

Hi @cpaxton, let’s start by disabling the collision manager if it’s enabled. We enable it for robots configured with Dex Wrists to avoid accidental self-collisions, but it might be the culprit here. Open your user YAML and change the use_collision_manager parameter by making it look like this:

robot:
  use_collision_manager: 0

Confirm it’s disabled using stretch_params.py | grep use_collision_manager. Test your program again and let me know if you’re still seeing the same behavior.

In order to try to replicate the issue, I took the trajectory goal you’ve included in your post and written a ROS node that executes it twice. I tested it on my Stretch that has a Dex Wrist, but I couldn’t replicate the same behavior you’re getting. The node is called yaml_to_command and you can run it using:

wget https://gist.githubusercontent.com/hello-binit/138654a999d7858317f57592bedc2c2a/raw/8608b3d759d30eb209975dd72dfc7e049a91395f/yaml_to_command
chmod +x yaml_to_command
# launch stretch_driver in a separate terminal
roslaunch stretch_core stretch_driver.launch
./yaml_to_command
yaml_to_command source code

In case the link goes down, here’s the source code. Notice instream defined on line 13, which is a multiline string of the yaml representation of the joint trajectory command.

#!/usr/bin/env python3

import rospy
from std_srvs.srv import Trigger, TriggerRequest, TriggerResponse
from control_msgs.msg import FollowJointTrajectoryGoal
from trajectory_msgs.msg import JointTrajectoryPoint

import hello_helpers.hello_misc as hm
import argparse as ap
import time
import yaml

instream = """
goal:
  trajectory:
    header:
      seq: 0
      stamp:
        secs: 1663809518
        nsecs: 932443380
      frame_id: ''
    joint_names:
      - joint_lift
      - joint_arm_l0
      - joint_arm_l1
      - joint_arm_l2
      - joint_arm_l3
      - joint_gripper_finger_left
      - joint_wrist_roll
      - joint_wrist_pitch
      - joint_wrist_yaw
      - joint_head_pan
      - joint_head_tilt
    points:
      -
        positions: [0.5137053394519911, 0.0, 0.0, 0.0, 0.0, -0.038557933294775415, -0.7439806821245359, -0.7761942786701345, -0.7676295526044397, -0.8244375296703885, -0.0010067811717669514]
        velocities: []
        accelerations: []
        effort: []
        time_from_start:
          secs: 0
          nsecs:         0
  path_tolerance: []
  goal_tolerance: []
  goal_time_tolerance:
    secs: 1
    nsecs: 0
"""


class YamlToCommand(hm.HelloNode):

    def __init__(self):
        hm.HelloNode.__init__(self)
        self.rate = 10.0

    def issue_stow_command(self):
        stow_point = JointTrajectoryPoint()
        stow_point.time_from_start = rospy.Duration(0.000)
        stow_point.positions = [0.2, 0.0, 3.0, 0.0, 0.0]

        trajectory_goal = FollowJointTrajectoryGoal()
        trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw', 'joint_wrist_pitch', 'joint_wrist_roll']
        trajectory_goal.trajectory.points = [stow_point]
        trajectory_goal.trajectory.header.stamp = rospy.Time(0.0)
        trajectory_goal.trajectory.header.frame_id = 'base_link'

        self.trajectory_client.send_goal(trajectory_goal)
        rospy.loginfo('Sent stow goal = {0}'.format(trajectory_goal))

    def issue_yaml_command(self, valid_joints=['joint_gripper_finger_left', 'joint_lift', 'joint_wrist_yaw', 'joint_wrist_pitch', 'joint_wrist_roll', 'joint_head_pan', 'joint_head_tilt']):
        data = yaml.load(instream, Loader=yaml.FullLoader)
        valid_joints_indices = sorted([data['goal']['trajectory']['joint_names'].index(j) for j in valid_joints])

        points = []
        for point in data['goal']['trajectory']['points']:
            p = JointTrajectoryPoint()
            p.time_from_start = rospy.Duration(float(point['time_from_start']['secs']) + float(1e-9 * point['time_from_start']['nsecs']))
            p.positions = [point['positions'][i] for i in valid_joints_indices] if len(point['positions']) >= len(valid_joints_indices) else []
            p.velocities = [point['velocities'][i] for i in valid_joints_indices] if len(point['velocities']) >= len(valid_joints_indices) else []
            p.accelerations = [point['accelerations'][i] for i in valid_joints_indices] if len(point['accelerations']) >= len(valid_joints_indices) else []
            points.append(p)

        trajectory_goal = FollowJointTrajectoryGoal()
        trajectory_goal.trajectory.joint_names = [data['goal']['trajectory']['joint_names'][i] for i in valid_joints_indices]
        trajectory_goal.trajectory.points = points
        trajectory_goal.trajectory.header.stamp = rospy.Time(0.0)
        trajectory_goal.trajectory.header.frame_id = 'base_link'

        self.trajectory_client.send_goal(trajectory_goal)
        rospy.loginfo('Sent yaml goal = {0}'.format(trajectory_goal))

    def main(self):
        hm.HelloNode.main(self, 'yaml_to_command', 'yaml_to_command', wait_for_first_pointcloud=False)

        rospy.loginfo('stowing...')
        node.issue_stow_command()
        time.sleep(6)
        rospy.loginfo('issuing first command...')
        node.issue_yaml_command()
        time.sleep(6)
        rospy.loginfo('issuing second command...')
        node.issue_yaml_command()
        time.sleep(6)
        rospy.loginfo('done!')


if __name__ == '__main__':
    try:
        parser = ap.ArgumentParser(description='')
        args, unknown = parser.parse_known_args()
        node = YamlToCommand()
        node.main()
    except KeyboardInterrupt:
        rospy.loginfo('interrupt received, so shutting down')

Please let me know if you’re able to see the issue when you run this node (with the collision manager both on and off). You can switch in other joint trajectory commands by changing the instream variable.

Finally, I also tried commanding that robot pose twice through python, but no dice there either. Here’s the script:

Command robot pose twice through Stretch Body 
import stretch_body.robot
import sys
import time

r = stretch_body.robot.Robot()
r.startup()

if not r.is_calibrated():
    print('home the robot first')
    sys.exit(1)
r.stow()
time.sleep(0.5)

def move_all():
    r.lift.move_to(0.5137053394519911)
    r.push_command()
    r.end_of_arm.get_joint('wrist_roll').move_to(-0.7439806821245359)
    r.end_of_arm.get_joint('wrist_yaw').move_to(-0.7676295526044397)
    r.end_of_arm.get_joint('wrist_pitch').move_to(-0.7761942786701345)
    r.head.get_joint('head_pan').move_to(-0.8244375296703885)
    r.head.get_joint('head_tilt').move_to(-0.0010067811717669514)

move_all()
time.sleep(5)
move_all()
time.sleep(5)

r.stop()
6

Also, I forgot to confirm you’re running the latest software. Confirm Ubuntu 20.04:

$ lsb_release -d -s
Ubuntu 20.04.4 LTS

Upgrade and confirm Stretch python packages:

$ pip3 install -U hello-robot-stretch-body
$ pip3 list | grep hello
hello-robot-stretch-body               0.4.8
hello-robot-stretch-body-tools         0.4.4
hello-robot-stretch-factory            0.3.8
hello-robot-stretch-tool-share         0.2.5

Upgrade and confirm Stretch ROS:

$ roscd stretch_core/..
$ git pull
$ git log
commit a1eb95b2a07ca19d5563b3096f3d218f8c900066 (HEAD -> dev/noetic, origin/dev/noetic)
Author: Binit Shah <bshah@hello-robot.com>
Date:   Wed Sep 14 13:10:40 2022 -0700

    Quieter export_urdf.sh script

[...]
$ git status
On branch dev/noetic
Your branch is up to date with 'origin/dev/noetic'.

Untracked files:
  (use "git add <file>..." to include in what will be committed)
	stretch_description/meshes/link_straight_gripper.STL
	stretch_description/meshes/link_wrist_pitch.STL
	stretch_description/meshes/link_wrist_roll.STL
	stretch_description/meshes/link_wrist_yaw_bottom.STL
	stretch_description/urdf/stretch_dex_wrist.xacro

nothing added to commit but untracked files present (use "git add" to track)
7

Thanks Binit, I’m on the latest code.

I think I accidentally sent you the “failure” position instead of the one that causes the bug. I modified your yaml_to_command to duplicate the issue:

#!/usr/bin/env python3

import rospy
from std_srvs.srv import Trigger, TriggerRequest, TriggerResponse
from control_msgs.msg import FollowJointTrajectoryGoal
from trajectory_msgs.msg import JointTrajectoryPoint

import hello_helpers.hello_misc as hm
import argparse as ap
import time
import yaml

instream = """
goal:
  trajectory:
    header:
      seq: 0
      stamp:
        secs: 1663809518
        nsecs: 932443380
      frame_id: ''
    joint_names: 
      - joint_wrist_pitch
      - joint_wrist_roll
      - joint_gripper_finger_right
      - joint_gripper_finger_left
      - joint_right_wheel
      - joint_left_wheel
      - joint_lift
      - joint_arm_l3
      - joint_arm_l2
      - joint_arm_l1
      - joint_arm_l0
      - joint_wrist_yaw
      - joint_head_pan
      - joint_head_tilt
    points:
      -
        positions: [-1.043106935762236, 3.057223710256083, 0.2198363721102607, 0.2198363721102607, 0.0, 0.0, 1.050007834455481, 0.015847170769254296, 0.015847170769254296, 0.015847170769254296, 0.015847170769254296, 2.788649240643772, -1.5684182117949244, -0.7746702007144751]
        velocities: []
        accelerations: []
        effort: []
        time_from_start:
          secs: 0
          nsecs:         0
  path_tolerance: []
  goal_tolerance: []
  goal_time_tolerance:
    secs: 1
    nsecs: 0
"""


class YamlToCommand(hm.HelloNode):

    def __init__(self):
        hm.HelloNode.__init__(self)
        self.rate = 10.0

    def issue_stow_command(self):
        stow_point = JointTrajectoryPoint()
        stow_point.time_from_start = rospy.Duration(0.000)
        stow_point.positions = [1.05, 0.0, 3.0, 0.0, 0.0]

        trajectory_goal = FollowJointTrajectoryGoal()
        trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw', 'joint_wrist_pitch', 'joint_wrist_roll']
        trajectory_goal.trajectory.points = [stow_point]
        trajectory_goal.trajectory.header.stamp = rospy.Time(0.0)
        trajectory_goal.trajectory.header.frame_id = 'base_link'

        self.trajectory_client.send_goal(trajectory_goal)
        rospy.loginfo('Sent stow goal = {0}'.format(trajectory_goal))

    def issue_yaml_command(self, valid_joints=['joint_gripper_finger_left', 'joint_lift', 'joint_wrist_yaw', 'joint_wrist_pitch', 'joint_wrist_roll', 'joint_head_pan', 'joint_head_tilt']):
        data = yaml.load(instream, Loader=yaml.FullLoader)
        valid_joints_indices = sorted([data['goal']['trajectory']['joint_names'].index(j) for j in valid_joints])

        points = []
        for point in data['goal']['trajectory']['points']:
            p = JointTrajectoryPoint()
            p.time_from_start = rospy.Duration(float(point['time_from_start']['secs']) + float(1e-9 * point['time_from_start']['nsecs']))
            p.positions = [point['positions'][i] for i in valid_joints_indices] if len(point['positions']) >= len(valid_joints_indices) else []
            p.velocities = [point['velocities'][i] for i in valid_joints_indices] if len(point['velocities']) >= len(valid_joints_indices) else []
            p.accelerations = [point['accelerations'][i] for i in valid_joints_indices] if len(point['accelerations']) >= len(valid_joints_indices) else []
            points.append(p)

        trajectory_goal = FollowJointTrajectoryGoal()
        trajectory_goal.trajectory.joint_names = [data['goal']['trajectory']['joint_names'][i] for i in valid_joints_indices]
        trajectory_goal.trajectory.points = points
        trajectory_goal.trajectory.header.stamp = rospy.Time(0.0)
        trajectory_goal.trajectory.header.frame_id = 'base_link'

        self.trajectory_client.send_goal(trajectory_goal)
        rospy.loginfo('Sent yaml goal = {0}'.format(trajectory_goal))

    def main(self):
        hm.HelloNode.main(self, 'yaml_to_command', 'yaml_to_command', wait_for_first_pointcloud=False)

        rospy.loginfo('stowing...')
        node.issue_stow_command()
        time.sleep(6)
        rospy.loginfo('issuing first command...')
        node.issue_yaml_command()
        time.sleep(6)
        rospy.loginfo('issuing second command...')
        node.issue_yaml_command()
        time.sleep(6)
        rospy.loginfo('done!')


if __name__ == '__main__':
    try:
        parser = ap.ArgumentParser(description='')
        args, unknown = parser.parse_known_args()
        node = YamlToCommand()
        node.main()
    except KeyboardInterrupt:
        rospy.loginfo('interrupt received, so shutting down')
1 Like
8

It is only occurring for me if the lift is > 1.

9

This also occurs even with the collision manager off, but again only if i modify the script so lift = 1.05, so hopefully i can work around it for now.

10

Hi @cpaxton, glad to hear you found a workaround. I’m intermittently able to reproduce the issue you’re seeing and I plan to debug it next week. I’ll follow up here when I’ve found a fix and made it available.

1 Like