Robotics StackExchange | Archived questions

Moveit Commander Orientation (Rethink Sawyer)

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.

Asked by rklutz on 2019-12-02 13:35:42 UTC

Comments

Answers

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

Asked by rklutz on 2019-12-03 19:22:59 UTC

Comments