Why the trajectory go like this

[dev4l]

Hello,

I try to figure out why the robot trajectory is doing strange thinks sometimes.

Here is a video of the behaviour:

https://streamable.com/nnqnvi

I use the "extended" robot arm position as initial position and the "ready "position as target. I also specify a joint constraint before starting the plan.

Here is the code I use to specify the constrainst and to set the plan.

def set_robot_position(self, pose_info):

        self.set_robot_joint_constrainst()

        print('get_path_constraints {}'.format(self.move_group.get_path_constraints()))

        pose_goal = geometry_msgs.msg.Pose()
        pose_goal.position.x = pose_info["position"][0]
        pose_goal.position.y = pose_info["position"][1]
        pose_goal.position.z = pose_info["position"][2]

        pose_goal.orientation.x = pose_info["orientation"][0]
        pose_goal.orientation.y = pose_info["orientation"][1]
        pose_goal.orientation.z = pose_info["orientation"][2]
        pose_goal.orientation.w = pose_info["orientation"][3]

        self.move_group.set_start_state_to_current_state()
        self.move_group.set_pose_target(pose_goal)

        self.move_group.set_max_velocity_scaling_factor(0.2)
        #self.move_group.set_max_acceleration_scaling_factor(0.5)
        if pose_info.__contains__('speed'):
            self.move_group.set_max_velocity_scaling_factor(pose_info['speed'])

        succeeded, plan, _, error_code = self.move_group.plan()
        self.display_trajectory(plan)
        self.execute_plan(plan)
        # self.move_group.go(wait=True)
        self.move_group.stop()
        self.move_group.clear_pose_targets()
        current_pose = self.move_group.get_current_pose().pose

        return self.all_close(pose_goal, current_pose, 0.01)

   def set_robot_joint_constrainst(self):
        constraints = Constraints()
        constraints.name = "Keep gripper horizontal"

        current_joint = self.move_group.get_current_joint_values()

        # Create an orientation constraint for the right gripper
        jcm = JointConstraint()
        jcm.joint_name = 'panda_link1'
        jcm.weight = 1.0
        jcm.position = 0.0;
        jcm.tolerance_above = 0.3;
        jcm.tolerance_below = -0.3;

        joint_constraint_list = []
        joint_constraint_list.append(jcm)

        constraint_list = moveit_msgs.msg.Constraints()
        constraint_list.name = 'middle_of_travel'
        constraint_list.joint_constraints = joint_constraint_list
        self.move_group.set_path_constraints(constraint_list)

Can someone explain me how to solve the behaviour because the constrainst don't seems to be used.

[gollth]

This is not an issue related to franka_ros in general. I'd suggest you consult the MoveIt docs and open an issue there if you cannot solve your problem.
All the best