MoveIt Cartesian Waypoint Acceleration

Description

Using a Gen3 robot, I am unable to make the robot follow a cartesian path of waypoints using the move group command interface.

Version

ROS distribution : Noetic

Branch and commit you are using : Noetic-Devel, Release 2.3.0

Steps to reproduce

Modified the example_move_it_trajectories.py (no issues with that file and what was included in it) file to include a number of waypoints and an execution. The rviz visualization shows the robot completing the trajectory and it creates a valid plan, but the controller fails each time it is run. Does this robot support move_group trajectories?

I know there are the other example trajectories, but I would like to use the collision avoidance of moveit when working in my specific robot configuration.

I am using the hard_joint_limits and have scaled the max acceleration and velocity to 0.1 in the Cartesian path plan. Is there something else to be done to enable this to work?

Code example (if necessary)

     def plan_cartesian_path(self, scale=1):
        # Copy class variables to local variables to make the web tutorials more clear.
        # In practice, you should use the class variables directly unless you have a good
        # reason not to.
        arm_group = self.arm_group
        arm_group.set_max_acceleration_scaling_factor(0.1)
        arm_group.set_max_velocity_scaling_factor(0.1)

        ## BEGIN_SUB_TUTORIAL plan_cartesian_path
        ##
        ## Cartesian Paths
        ## ^^^^^^^^^^^^^^^
        ## You can plan a Cartesian path directly by specifying a list of waypoints
        ## for the end-effector to go through. If executing  interactively in a
        ## Python shell, set scale = 1.0.
        ##
        waypoints = []

        wpose = arm_group.get_current_pose().pose
        wpose.position.x += .2
        #wpose.position.y += .04
        wpose.position.z += .1
        waypoints.append(copy.deepcopy(wpose))

        wpose.position.x += .1
        #wpose.position.y += .04
        wpose.position.z += .1
        waypoints.append(copy.deepcopy(wpose))

        print(waypoints)

        (plan, fraction) = arm_group.compute_cartesian_path(
            waypoints, 0.01, 0.00  # waypoints to follow  # eef_step
        )  # jump_threshold

        print(plan)

        return arm_group.execute(plan, wait=True)

Then executed in def main():

  if success:
    rospy.loginfo("Exectuing waypoint trajectory")
    success &= example.plan_cartesian_path()
    print (success)

Error Output

In the terminal running the example launch file:

[ INFO] [1671641688.742604778]: ABORTED: Solution found but controller failed during execution
False
[ERROR] [1671641688.745563]: The example encountered an error.

In the terminal running the kortex_driver:

Execution request received
[ INFO] [1671641688.714493395]: New goal received.
[ INFO] [1671641688.714575220]: Joint Trajectory Goal is accepted.
[ERROR] [1671641688.741291263]: Joint Trajectory failed validation in the arm.
[ERROR] [1671641688.741384319]: Error 1 : Invalid Acceleration - Provided Waypoints Lead to Go Over Maximum Acceleration on Actuator (0) and Segment (10).
[ERROR] [1671641688.741408192]: Error 2 : Invalid Acceleration - Provided Waypoints Lead to Go Over Maximum Acceleration on Actuator (3) and Segment (25).
[ INFO] [1671641688.741727586]: Controller 'gen3_joint_trajectory_controller' successfully finished
[ WARN] [1671641688.741880228]: Controller handle gen3_joint_trajectory_controller reports status ABORTED
[ INFO] [1671641688.741932772]: Completed trajectory execution with status ABORTED ...
[ INFO] [1671641688.742056997]: Execution completed: ABORTED

Thank you!

Kinovarobotics / ros_kortex