Speed Difference in control by webpage/joystick and sending waypoint

[pouyan-asg]

Description

Hello,

I am using Kinova Gen3. When I control the robot by sending cartesian waypoints, the speed is slower than controlling through the webpage. I record the results and it shows the speed is the highest value (maximum linear velocity=0.5, maximum angular velocity=99.99830) but in fact, I can see the highest value is far higher than it. How can I figure out this issue and increase the speed of movement based on waypoints?

Thank you very much for your time.

Expected behavior

Being faster (similar to joystick/webpage control)

[felixmaisonneuve]

Hi @pouyan-asg,

Different control mode have different velocity limits. You can configure each one indiviidually via the WebApp (possible throught the API too, but it is faster in the WebApp). To do so, go to "Speed Limits"

In the "Advanced" section, you can select each control mode

Select "Cartesian Waypoint Trajectory" (the control mode you are using) and set all all limit to max with the "Set max" button.

Your arm should now move faster.

Regards, Felix

[pouyan-asg]

Thanks for your answer. I set the speed to maximum value but the issue still remains. When I increase the speed from the "Pose" section (as the screenshot shows), the speed is really good but when I want to control the robot by sending waypoints, the speed is not the same. Can I reach to real maximum speed through programming (i.e. sending waypoints)? If not, may you explain the reason?

Thank you very much in advance.

[felixmaisonneuve]

Can you monitor the arm's velocity while it executes a cartesian waypoint trajectory? I can link you to the issue #122 where I provided a small code example to get/set the different velocity limits. This comment in particular.

[pouyan-asg]

Thanks for your detailed answer. I could print the kinematics limits and I could change joint speeds by the following code:

        success = True
        base = BaseClient(router)
        vari = ControlConfigClient(router)
        joint_speed_limits = JointSpeedSoftLimits()
        joint_speed_limits.control_mode = CARTESIAN_WAYPOINT_TRAJECTORY
        joint_acceleration_limits = JointAccelerationSoftLimits()
        joint_acceleration_limits.control_mode = ANGULAR_TRAJECTORY
        DOF = 7
        for i in range(DOF) :
            if i < 4 and DOF == 7 : # Actuators 0 to 3 are bigs on 7DOF
                joint_speed_limits.joint_speed_soft_limits.append(80 * 1)
                joint_acceleration_limits.joint_acceleration_soft_limits.append(297.9 * 1)
            elif i < 3 and DOF == 6 : # Actuators 0 to 2 are bigs on 6DOF
                joint_speed_limits.joint_speed_soft_limits.append(80 * 1)
                joint_acceleration_limits.joint_acceleration_soft_limits.append(297.9 * 1)
            else : # small actuators
                joint_speed_limits.joint_speed_soft_limits.append(70 * 1)
                joint_acceleration_limits.joint_acceleration_soft_limits.append(572.9 * 1)

        vari.SetJointSpeedSoftLimits(joint_speed_limits)
        vari.SetJointAccelerationSoftLimits(joint_acceleration_limits)
        kinematic_limits = vari.GetAllKinematicSoftLimits()
        print(kinematic_limits)

And the output for CARTESIAN_WAYPOINT_TRAJECTORY control mode is:

kinematic_limits_list {
  control_mode: CARTESIAN_WAYPOINT_TRAJECTORY
  twist_linear: 0.5
  twist_angular: 99.99832153320312
  joint_speed_limits: 80.0
  joint_speed_limits: 80.0
  joint_speed_limits: 80.0
  joint_speed_limits: 80.0
  joint_speed_limits: 70.0
  joint_speed_limits: 70.0
  joint_speed_limits: 70.0
}

In this way, I can move the robot at the highest speed. So, I can conclude that it can not be faster in cartesian waypoint control mode. Is it true? Because even controlled by API, the cartesian movements are slower than those controlling by Joystick or Webpage. Do you think is there any way to increase the cartesian speed even by low-level control?

Thank you very much for your time.

Regards,

[felixmaisonneuve]

Your velocity limits are correctly set. Angular trajectories can be slightly faster, but nothing very significant. If you notice a difference, it might be because you specified a duration for your waypoint list or velocity limits in your cartesian waypoint.

Can you share your code were you implement your waypoint list and your cartesian waypoints?

[pouyan-asg]

The speed that I can see when controlling by API is the same as controlling through Webpage (Highest speed I mean). The example code that I use for sending Cartesian Waypoint is as follows:

TIME_DURATION = 1000

def MyFunc(base):
    base_servo_mode = Base_pb2.ServoingModeInformation()
    base_servo_mode.servoing_mode = Base_pb2.SINGLE_LEVEL_SERVOING
    base.SetServoingMode(base_servo_mode)
    product = base.GetProductConfiguration()
    waypointsDefinition_d = []
    if(product.model == Base_pb2.ProductConfiguration__pb2.MODEL_ID_L53 
       or product.model == Base_pb2.ProductConfiguration__pb2.MODEL_ID_L31):
        if(product.model == Base_pb2.ProductConfiguration__pb2.MODEL_ID_L31):
            kTheta_x = 0
            kTheta_y = 180
            kTheta_z = 90
            waypointsDefinition_d = [[0.33, 0.29, 0.2, 0.0, kTheta_x, kTheta_y, kTheta_z],
                                    [0.33, 0.29, 0.15, 0.0, kTheta_x, kTheta_y, kTheta_z],
                                    [0.365, 0.29, 0.15, 0.0, kTheta_x, kTheta_y, kTheta_z],
                                    [0.40, 0.29, 0.15, 0.0, kTheta_x, kTheta_y, kTheta_z],
                                    [0.40, 0.29, 0.18, 0.0, kTheta_x, kTheta_y, kTheta_z],
                                    [0.40, 0.295, 0.15, 0.0, kTheta_x, kTheta_y, kTheta_z],
                                    [0.365, 0.295, 0.15, 0.0, kTheta_x, kTheta_y, kTheta_z+270],
                                    [0.33, 0.295, 0.15, 0.0, kTheta_x, kTheta_y, kTheta_z+270],
                                    [0.33, 0.295, 0.18, 0.0, kTheta_x, kTheta_y, kTheta_z+270],
                                    [0.33, 0.30, 0.15, 0.0, kTheta_x, kTheta_y, kTheta_z],
                                    [0.365, 0.30, 0.15, 0.0, kTheta_x, kTheta_y, kTheta_z],
                                    [0.40, 0.30, 0.15, 0.0, kTheta_x, kTheta_y, kTheta_z],
                                    [0.40, 0.30, 0.18, 0.0, kTheta_x, kTheta_y, kTheta_z],]
        else:
            kTheta_x = 90.0
            kTheta_y = 0.0
            kTheta_z = 90.0
            waypointsDefinition_d = [[0.3, 0.12, 0.1, 0.0, kTheta_x, kTheta_y, kTheta_z]]

    else:
        print("Product is not compatible to run this example please contact support with KIN number bellow")
        print("Product KIN is : " + product.kin())

    waypoints = Base_pb2.WaypointList()

    waypoints.duration = 0.0
    waypoints.use_optimal_blending = True

    index = 0
    for waypointDefinition in waypointsDefinition_d:
        waypoint = waypoints.waypoints.add()
        waypoint.name = "waypoint_" + str(index)   
        waypoint.cartesian_waypoint.CopyFrom(cartesian_coordinate(waypointDefinition))
        index = index + 1 

    result = base.ValidateWaypointList(waypoints);
    # print(result)

    if(len(result.trajectory_error_report.trajectory_error_elements) == 0):
        e = threading.Event()
        notification_handle = base.OnNotificationActionTopic(check_for_end_or_abort(e),
                                                                Base_pb2.NotificationOptions())

        base.ExecuteWaypointTrajectory(waypoints)

        finished = e.wait(TIME_DURATION)
        base.Unsubscribe(notification_handle)

        if finished:
            print("Drying Finished!")
        else:
            print("Timeout on action notification wait for non-optimized trajectory")

        return finished

    else:
        print("Error found in Drying :(") 
        result.trajectory_error_report.PrintDebugString(); 

[felixmaisonneuve]

I see your waypoints are quite close from each other. The movement might be slow because the arm doesn't have time to accelerate and deceleerate between each waypoint. It might has to do with the blending radius too, but optimal blending is implemented in a way to have the shortest time between waypoints.

I will test your code on a real arm this week to see if I experience something similar to what you are describing.

[pouyan-asg]

Thanks @felixmaisonneuve for reviewing the code. As a slight modification, I agree the points are so close, but the speed is not the fastest, even at more distances. For example, today, I recorded robot speed for various points. I considered going on a straight line with fixed values of Y, Z, theta x, theta y and theta z and only X was changed. The total distance was 50 cm. Here you can find the result of timing for various points (the number of points on this straight line from the same initial and end point):

total run time for 16 points: 4.942715644836426 total run time for 16 points: 4.940280437469482 total run time for 16 points: 4.945784568786621 total run time for 12 points: 4.942131519317627 total run time for 12 points: 4.944432020187378 total run time for 12 points: 4.942370891571045 total run time for 8 points: 4.915758371353149 total run time for 8 points: 4.942735910415649 total run time for 8 points: 4.918205976486206 total run time for 8 points: 4.942677021026611 total run time for 6 points: 4.943176507949829 total run time for 5 points: 4.914399862289429 total run time for 5 points: 4.9171435832977295 total run time for 4 points: 4.919594764709473 total run time for 4 points: 4.9460577964782715 total run time for 3 points: 4.915651321411133 total run time for 3 points: 4.919529914855957 total run time for 2 points: 4.918443202972412 total run time for 2 points: 4.920965909957886

As you can see, the times are roughly the same. Also, I moved this distance by WebApp (i.e. Pose virtual joystick control), and the time was approximately 2 sec.

I think maybe this data is helpful.

I appreciate your opinion in this regard, and Thank you very much for your detailed guidance.

[felixmaisonneuve]

I did some testing on this today.

If the velocity limits are set correctly and your waypoint uses the optimal duration, your arm should move at maximum velocity. I tested it by monitoring the twist linear velocity during multiple sequences (moving only along 1 axis like you did)

I used the provided example for cartesian waypoints and did some small modifications here is an example of waypoints I used (moving only on the 'Y' axis)

 waypointsDefinition = ( (0.6, -0.5, 0.33,  0.0, kTheta_x, kTheta_y, kTheta_z),
                                    (0.6, 0.5, 0.33, 0.0, kTheta_x, kTheta_y, kTheta_z))

I replaced the finished = e.wait(TIMEOUT_DURATION) with this to print the arm's linear velocity during trajectory execution

        velocity = 1
        while(velocity > 0.00001):
            time.sleep(0.1)
            feedback = base_cyclic.RefreshFeedback()
            velocity = math.sqrt(feedback.base.tool_twist_linear_x * feedback.base.tool_twist_linear_x + feedback.base.tool_twist_linear_y * feedback.base.tool_twist_linear_y + feedback.base.tool_twist_linear_z * feedback.base.tool_twist_linear_z)
            print(velocity)

For a movement of 0.2 meter (from -0.1 to 0.1 in Y axis), the arm does not reach the maximum velocity:

For a movement of 0.5 meter (from -0.25 to 0.25 in Y axis), the arm barely reaches the maximum velocity (I assume it gets close to 0.5m/s, but my sampling rate is not high enough to read it):

For a movement of 1 meter (from -0.5 to 0.5 in Y axis), the arm reaches the maximum velocity:

I also played a bit with cartesian joystick and cartesian actions movements as well. I compared waypoints to them for similar movements. Waypoints and reach cartesian actions gave similar execution times, so I suspect they have the same acceleration. Cartesian Joystick movements were faster however. The arm was moving at the same velocity (0.5m/s), but it reached it a lot faster. So what you are seeing is not the arm going slower, but the arm's acceleration that is slower.

There is no way to change this acceleration. I am not sure why the arm accelerates faster for joystick movements. Probably because, in joystick commands, you don't have to worry about stopping at the exact position and bigger acceerations when stopping lead to inconsistant positionning. You could try to use Twist commands if you need to go faster, but I think your arm is already at its max capacity for cartesian movements.

Any feedback or question about the tests I did is welcomed

Best regards, Felix

[pouyan-asg]

Thanks, @felixmaisonneuve for your tests and comprehensive explanation.

I can summarize everything that due to the small distance between waypoints, the robot can not accelerate well and it seems that it is slower. Also, another thing is that my original waypoints are close and in various directions, so it has a direct effect on acceleration too. I will test the Twist command method to see how the speed improves. I will share the results here after this week's tests.

[pouyan-asg]

Hi @felixmaisonneuve ,

I had this opportunity to perform simple tests based on the twist command method. I considered roughly 20 cm movement along the X-axis from 0.25 to 0.46 with Y and Z constants. I recorded the run time with both the Twist command method and the Cartesian waypoint method, and below you can see the results:

Sending Cartesian waypoints: 1.7224712371826172 1.6940269470214844 1.6963779926300049 1.703066349029541 1.6938984394073486

Twist Command with "twist.linear_x=0.2": 1.4764068126678467 1.4750845432281494 1.50118088722229 1.529653549194336 1.475438117980957

As we can see, the run time in the Twist command is lower than the previous one. My questions are:

• In general, will the difference between run time be in this range, or can it decrease even more for twist command?

• We can only consider the amount the robot should move for the twist command, not the exact points. Is it correct? For example, in my test, I coded like below for time.sleep(1) and the robot moved 20 cm.

    command.reference_frame = Base_pb2.CARTESIAN_REFERENCE_FRAME_MIXED
    command.duration = 0

    twist = command.twist
    twist.linear_x = 0.2
    twist.linear_y = 0.0
    twist.linear_z = 0.0
    twist.angular_x = 0
    twist.angular_y = 0
    twist.angular_z = 0

    base.SendTwistCommand(command)
    time.sleep(1)
    base.Stop()

Thank you very much for your time and consideration.

[felixmaisonneuve]

For a similar distances (0.2m), I expect the difference in time for cartesian waypoints and artesian velocities commands to be similar, yes. But if you change one or more velocity limit (either for waypoints or velocity control or both), I expect this difference to change. Also, this difference will be different for different distances.

We can only consider the amount the robot should move for the twist command, not the exact points? Yes You cannot expect to be super precise on the positions/distances, but it should be about right

[pouyan-asg]

Thank you very much for your guidance and help. I close the issue now, but if I have any other questions, I will ask again here.

Regards