Ask Your Question
0

Moveit Commander Orientation (Rethink Sawyer)

asked 2019-12-02 12:35:42 -0600

rklutz gravatar image

I'm using Moveit Commander to drive my robot, (Rethink Sawyer, following this tutorial) and I'm having trouble getting the pose orientation to apply. Here is the code:

  def move_to_point (self, position, orientation):
    pose = Pose()
    pose.position.x = position[0]
    pose.position.y = position[1]
    pose.position.z = position[2]
    pose.orientation.x = orientation[0]
    pose.orientation.y = orientation[1]
    pose.orientation.z = orientation[2]
    pose.orientation.w = orientation[3]
    self.group.clear_pose_targets()
    self.group.set_pose_target(pose)
    try:
        plan = self.group.go(wait=True)
        #result = self.group.execute(plan, wait=True)
        self.group.stop()
        self.group.clear_pose_targets()
        return plan
    except moveit_commander.MoveItCommanderException as err:
        print err

Regardless of what I set as the orientation, the robot's end-effector moves to [0,0,0,1] as it's final orientation. Investigating further by looking at the /move_group/goal topic I can see that it sets the two following constraints:

   constraint_region: 
          primitives: 
            - 
              type: 2
              dimensions: [0.0001]
          primitive_poses: 
            - 
              position: 
                x: 0.35
                y: -0.3
                z: -0.05
              orientation: 
                x: 0.0
                y: 0.0
                z: 0.0
                w: 1.0
          meshes: []
          mesh_poses: []
        weight: 1.0
    orientation_constraints: 
      - 
        header: 
          seq: 0
          stamp: 
            secs: 0
            nsecs:         0
          frame_id: "base"
        orientation: 
          x: 0.0
          y: 1.0
          z: 0.0
          w: 0.0
        link_name: "right_hand"
        absolute_x_axis_tolerance: 0.001
        absolute_y_axis_tolerance: 0.001
        absolute_z_axis_tolerance: 0.001
        weight: 1.0

Notice in the primitive_poses field there is an orientation field that is set to [0,0,0,1] (which I don't want) and the robot seems to be ignoring the constraint set in the orientation_constraints field.

Thanks for the help.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2019-12-03 18:22:59 -0600

rklutz gravatar image

The problem was that my quaternions weren't normalized and MoveIt fell back to identity. Should have read the messages in rqt_console.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2019-12-02 12:35:42 -0600

Seen: 11 times

Last updated: yesterday