Issues with Posed Based Cartesian Trajectory Controller

[LAYERED-pierrechass]

Affected ROS Driver version(s)

latest

Used ROS distribution.

Noetic

Which combination of platform is the ROS driver running on.

Linux

How is the UR ROS Driver installed.

Build both the ROS driver and UR Client Library from source

Which robot platform is the driver connected to.

URSim in docker

Robot SW / URSim version(s)

NA

How is the ROS driver used.

Through the robot teach pendant using External Control URCap

Issue details

Summary

When trying to use the pose_based_cartesian_traj_controller I get an error from URSim stating that the velocity to reach the joint target is exceeding the joint velocity limits followed by an error saying that the robot cannot reach the requested pose.

Issue details

I tried to copy paste the example given and change the coordinates to make the point reachable, while it suppresses the 2nd error message about pose not being reachable the first one still remains. I am quite new to all of this and it is hard for me to troubleshoot what's happening. Here is the code I used when trying to input my user defined points:

EDIT: I tried to start from the robot being in home position and with cartesian points not too far away from the home position while keeping the same orientation.

def send_cartesian_trajectory(self):
        """Creates a Cartesian trajectory and sends it using the selected action server"""
        self.switch_controller(self.cartesian_trajectory_controller)

        # make sure the correct controller is loaded and activated
        goal = FollowCartesianTrajectoryGoal()
        trajectory_client = actionlib.SimpleActionClient(
            "{}/follow_cartesian_trajectory".format(self.cartesian_trajectory_controller),
            FollowCartesianTrajectoryAction,
        )

        # Wait for action server to be ready
        timeout = rospy.Duration(5)
        if not trajectory_client.wait_for_server(timeout):
            rospy.logerr("Could not reach controller action server.")
            sys.exit(-1)

        # The following list are arbitrary positions
        # Change to your own needs if desired
        pose_list = [
            geometry_msgs.Pose(
                geometry_msgs.Vector3(0.0, -0.25, 0.8),
                geometry_msgs.Quaternion(-0.7068252, 0, 0, 0.7073883),
            ),
            geometry_msgs.Pose(
                geometry_msgs.Vector3(0.20, -0.25, 0.8),
                geometry_msgs.Quaternion(-0.7068252, 0, 0, 0.7073883),
            ),
        ]
        duration_list = [4.0, 8.0]
        for i, pose in enumerate(pose_list):
            point = CartesianTrajectoryPoint()
            point.pose = pose
            point.time_from_start = rospy.Duration(duration_list[i])
            goal.trajectory.points.append(point)
        goal.goal_time_tolerance = rospy.Duration(100.0)

        rospy.loginfo("Executing trajectory using the {}".format(self.cartesian_trajectory_controller))
        trajectory_client.send_goal(goal)
        trajectory_client.wait_for_result()

        result = trajectory_client.get_result()

        rospy.loginfo("Trajectory execution finished in state {}".format(result.error_code))

Steps to Reproduce

1. Launch URSim in docker with external control enabled as explained here: docker run --rm -it -p 5900:5900 -p 6080:6080 -v ${HOME}/.ursim/urcaps:/urcaps -v ${HOME}/.ursim/programs:/ursim/programs --name ursim universalrobots/ursim_e-series
2. Configure URSim (localhost IP, program, ..)
3. Install Universal_Robots_ROS_Driver
4. Follow the instructions to do a test_move and select the pose based cartesian trajectory controller.

Expected Behavior

That execute the example without any problem.

Actual Behavior

Error messages (cf pictures)

Workaround Suggestion

I have tried visualizing the trajectory using the read-only controller as explained here and it seems correct, however I think there is a discrepancy between the Rviz position/frame of the robot and URSim, I am not sure if it could cause the error or not...

This issue is more a request for support than anything else as I am sure that I am doing something wrong here but not sure what exactly, it is not clear to me how to use this controller properly.

Pictures of the error messages:

Relevant log output

No response

Accept Public visibility

• [X] I agree to make this context public

[ThomasDuvinage]

I confirm to have the same behavior on a real robot for both UR5e and UR10.

Affected ROS Driver version(s) latest

Used ROS distribution. Noetic

Polyscope version UR5e : 5.14 UR10 : 3.15.4

Which combination of platform is the ROS driver running on.

Linux

UR10

UR5e

You can check the code to control the robot here.

Thanks for your support

[LAYERED-pierrechass]

This actually seems related to this issue? https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers/issues/210

[ZhefeiGong]

@LAYERED-pierrechass Hey did you find a solution? I had the same problem with the pose based cartesian trajectory controller. Any advice will be appreciated !!!

[LAYERED-pierrechass]

Hey, unfortunately I only found a workaround by using the JointBasedCartesianTrajectoryController, for my purpose it did the job.

[anxiousfruit]

Hi, @stefanscherzinger @fmauch While trying out my Cartesian path on a ur5e with both the pose based cartesian controller and the joint based cartesian controller, my path (calculated through moveit with an eef of 0.01) causes the robot to swing around wildly (like in https://github.com/UniversalRobots/Universal_Robots_ROS_Driver/issues/540) or move to joints that are way off the calculated cartesian path. To solve this, I recloned the driver and the UR library through an all source build, expecting that the solution from [https://github.com/UniversalRobots/Universal_Robots_ROS_Driver/issues/540] has been implemented in the main branch. My question is has: check to ensure that the shortest path is followed when interpolating between two orientations been implemented into the main branch or do I need to resource and make the changes to the interpolation script myself? Or do I need to calculate my moveit path at a definition smaller than 0.01, so that the robot does not move to random points and instead follows my trajectory?