# Dex wrist goes to different positions given the same command
**URL:** https://forum.hello-robot.com/t/dex-wrist-goes-to-different-positions-given-the-same-command/493
**Category:** Ask
**Created:** [September 22, 2022, 2:25pm UTC](https://forum.hello-robot.com/t/dex-wrist-goes-to-different-positions-given-the-same-command/493 "2022-09-22T14:25:06Z")
**Posts on this page:** 10
**Page:** 1
### Author:  [@cpaxton](https://forum.hello-robot.com/u/cpaxton)
#### Post date: [September 22, 2022, 2:25pm UTC](https://forum.hello-robot.com/t/dex-wrist-goes-to-different-positions-given-the-same-command/493/1 "2022-09-22T14:25:06Z")
I’ve been using the joint trajectory interface to send commands to the Dex Wrist version of stretch. Here’s an example command:
```auto
---
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:
```auto
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.
---
### Author:  [@bshah](https://forum.hello-robot.com/u/bshah)
#### Post date: [September 22, 2022, 3:29pm UTC](https://forum.hello-robot.com/t/dex-wrist-goes-to-different-positions-given-the-same-command/493/2 "2022-09-22T15:29:18Z")
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.
---
### Author:  [@cpaxton](https://forum.hello-robot.com/u/cpaxton)
#### Post date: [September 22, 2022, 4:27pm UTC](https://forum.hello-robot.com/t/dex-wrist-goes-to-different-positions-given-the-same-command/493/3 "2022-09-22T16:27:13Z")
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.
---
### Author:  [@cpaxton](https://forum.hello-robot.com/u/cpaxton)
#### Post date: [September 22, 2022, 8:27pm UTC](https://forum.hello-robot.com/t/dex-wrist-goes-to-different-positions-given-the-same-command/493/4 "2022-09-22T20:27:34Z")
Verified on dev/noetic with two different Stretch robots. Any idea what I can do here?
---
### Author:  [@bshah](https://forum.hello-robot.com/u/bshah)
#### Post date: [September 22, 2022, 10:48pm UTC](https://forum.hello-robot.com/t/dex-wrist-goes-to-different-positions-given-the-same-command/493/5 "2022-09-22T22:48:25Z")
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:
```yaml
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`](https://gist.github.com/hello-binit/138654a999d7858317f57592bedc2c2a) and you can run it using:
```auto
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.
>
> ```python
> #!/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**
>
> ```python
> 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()
>
> ```
---
### Author:  [@bshah](https://forum.hello-robot.com/u/bshah)
#### Post date: [September 22, 2022, 11:50pm UTC](https://forum.hello-robot.com/t/dex-wrist-goes-to-different-positions-given-the-same-command/493/6 "2022-09-22T23:50:01Z")
Also, I forgot to confirm you’re running the latest software. Confirm Ubuntu 20.04:
```auto
$ lsb_release -d -s
Ubuntu 20.04.4 LTS
```
Upgrade and confirm Stretch python packages:
```auto
$ 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:
```auto
$ roscd stretch_core/..
$ git pull
$ git log
commit a1eb95b2a07ca19d5563b3096f3d218f8c900066 (HEAD -> dev/noetic, origin/dev/noetic)
Author: Binit Shah
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 ..." 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)
```
---
### Author:  [@cpaxton](https://forum.hello-robot.com/u/cpaxton)
#### Post date: [September 23, 2022, 2:51pm UTC](https://forum.hello-robot.com/t/dex-wrist-goes-to-different-positions-given-the-same-command/493/7 "2022-09-23T14:51:08Z")
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:
```auto
#!/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')
```
---
### Author:  [@cpaxton](https://forum.hello-robot.com/u/cpaxton)
#### Post date: [September 23, 2022, 2:52pm UTC](https://forum.hello-robot.com/t/dex-wrist-goes-to-different-positions-given-the-same-command/493/8 "2022-09-23T14:52:11Z")
It is only occurring for me if the lift is \> 1.
---
### Author:  [@cpaxton](https://forum.hello-robot.com/u/cpaxton)
#### Post date: [September 23, 2022, 2:57pm UTC](https://forum.hello-robot.com/t/dex-wrist-goes-to-different-positions-given-the-same-command/493/9 "2022-09-23T14:57:27Z")
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.
---
### Author:  [@bshah](https://forum.hello-robot.com/u/bshah)
#### Post date: [September 24, 2022, 1:33am UTC](https://forum.hello-robot.com/t/dex-wrist-goes-to-different-positions-given-the-same-command/493/10 "2022-09-24T01:33:54Z")
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.