Possible way to restrict speed limit when command joint position or cartesian position

[ShaoxiongYao]

Summary

I am using the example position command in joint space or cartesian space: `action = Base_pb2.Action() action.name = "Example Cartesian action movement" action.application_data = ""

    cartesian_pose = action.reach_pose.target_pose
    cartesian_pose.x = x    # (meters)
    cartesian_pose.y = y    # (meters)
    cartesian_pose.z = z    # (meters)
    cartesian_pose.theta_x = theta_x # (degrees)
    cartesian_pose.theta_y = theta_y # (degrees)
    cartesian_pose.theta_z = theta_z # (degrees)

    e = threading.Event()
    self.notification_handle = self.base.OnNotificationActionTopic(
        check_for_end_or_abort(e),
        Base_pb2.NotificationOptions()
    )

    self.base.ExecuteAction(action)

However, it seems that the arm will reach the target pose at maximum speed by default. I wonder if we may set the maximum speed during the action execution.

Use case

The example can be used when maximum speed regulation is required. For example, when the robot arm is holding a cup of water in hand, directly moving the target pose with maximum speed will split the water around.

Alternative

If possible, I wonder if there is a maximum speed parameter we can set. I am ok to have a unified max speed parameter for all reaching actions.

Possible issues

No issues

Additional context

[felixmaisonneuve]

Hi @ShaoxiongYao,

You can specify "softLimits" (software limits). Each Control Mode (cartesian waypoint, joints waypoint, reach cartesian pose action, reach joint pose action, ...) can have its specified software limits.

Each limits are available in the web app (this makes it easy to quickly visualize what each limits are), but you can modify them via the API as well.

In the WebApp, in the "Configuration->Speed Limits" page and go to "Advanced". You can specify each control mode and see the limits

In your case, you will need to change the "Cartesian Trajectory" and "Angular Trajectory" limits (because you are using the reach pose actions)

In the API, you can use GetAllKinematicSoftLimits to visualize every soft limits. I have this python code at hand that does this :

        kinematic_limits = self.control_config.GetAllKinematicSoftLimits()
        print("soft limits : ")
        print(kinematic_limits)

After that, you can change the software limits by doing something like that :

        joint_speed_limits = ControlConfig_pb2.JointSpeedSoftLimits()
        joint_speed_limits.control_mode = ControlConfig_pb2.ANGULAR_TRAJECTORY
        joint_acceleration_limits = ControlConfig_pb2.JointAccelerationSoftLimits()
        joint_acceleration_limits.control_mode = ControlConfig_pb2.ANGULAR_TRAJECTORY
        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 * BIG_ACTUATOR_JOINT_SPEED_FACTOR)
                joint_acceleration_limits.joint_acceleration_soft_limits.append(297.9 * BIG_ACTUATOR_JOINT_ACCELERATION_FACTOR)
            elif i < 3 and DOF == 6 : # Actuators 0 to 2 are bigs on 6DOF
                joint_speed_limits.joint_speed_soft_limits.append(80 * BIG_ACTUATOR_JOINT_SPEED_FACTOR)
                joint_acceleration_limits.joint_acceleration_soft_limits.append(297.9 * BIG_ACTUATOR_JOINT_ACCELERATION_FACTOR)
            else : # small actuators
                joint_speed_limits.joint_speed_soft_limits.append(70 * SMALL_ACTUATOR_JOINT_SPEED_FACTOR)
                joint_acceleration_limits.joint_acceleration_soft_limits.append(572.9 * SMALL_ACTUATOR_JOINT_ACCELERATION_FACTOR)

        self.control_config.SetJointSpeedSoftLimits(joint_speed_limits)
        self.control_config.SetJointAccelerationSoftLimits(joint_acceleration_limits)

Note: every ..._SPEED_FACTOR is a factor from 0 to 1 that I can set to change the limit according to max speed.

This changes the limits for angular trajectories (ReachJointPose), but it is very similar for cartesian trajectories. You just need to change the limits control mode

Best regards, Felix

[ShaoxiongYao]

Thank you for your answer! I check this soft speed limit works well.

However, each command seems to persist for ~3s and stops before reaching the target pose. I wonder if there is a way to set the command duration or enforce to robot reaching the target end effector pose. I can do it by repeating the position command when the previous command is finished, but that seems to make the robots' motion discontinuous.

[felixmaisonneuve]

The arm stopping after ~3s is not normal. Can you try to create an Action to reach cartesian in te webapp and test if this happens in the webapp actions as well?

[ShaoxiongYao]

Hi sorry for a bit late reply, will try your suggestion tomorrow.

[ShaoxiongYao]

Hi,

I try to create an action in the web app and control the robot to a target configuration. I wonder if I have created the action correctly. But the robot only has a small motion and stopped when I click the action start button. A screenshot of the web app after the action execution is attached.

Yours, Shaoxiong

[ShaoxiongYao]

I also attach the code block to limit the robot arm's speed. I realized the action duration may vary depending on the chosen target end-effector pose, but not guaranteed to reach. It is based on 01-move_angular_and_cartesian.py.

feedback = self.base_cyclic.RefreshFeedback()

x = feedback.base.tool_pose_x             # (meters)
y = feedback.base.tool_pose_y             # (meters)
z = feedback.base.tool_pose_z             # (meters)
theta_x = feedback.base.tool_pose_theta_x # (degrees)
theta_y = feedback.base.tool_pose_theta_y # (degrees)
theta_z = feedback.base.tool_pose_theta_z # (degrees)

# target pose
x += dx
y += dy
z += dz
base_servo_mode = Base_pb2.ServoingModeInformation()
base_servo_mode.servoing_mode = Base_pb2.SINGLE_LEVEL_SERVOING
self.base.SetServoingMode(base_servo_mode)

action = Base_pb2.Action()
action.name = "Example Cartesian action movement"
action.application_data = ""

cartesian_pose = action.reach_pose.target_pose
cartesian_pose.x = x    # (meters)
cartesian_pose.y = y    # (meters)
cartesian_pose.z = z    # (meters)
cartesian_pose.theta_x = theta_x # (degrees)
cartesian_pose.theta_y = theta_y # (degrees)
cartesian_pose.theta_z = theta_z # (degrees)

e = threading.Event()
self.notification_handle = self.base.OnNotificationActionTopic(
    check_for_end_or_abort(e),
    Base_pb2.NotificationOptions()
)
self.base.ExecuteAction(action)

[felixmaisonneuve]

This seems very unusual. Can you share a screenshot of your arm's firmware versions? (the Systems->Upgrade page)

Also, you could try to do a reset to factory settings of the arm (In Configurations->Robot->Arm->Base->Restore Factory Settings)

[ShaoxiongYao]

Thank you for the reply. Here is a screenshot of the arm's firmware versions.

I also checked the safety states of actuators are normal. I tried to reset the factory setting, but when I attempt the same zero configuration action in webapp[Actions->Zero->Starts Action], the robot does not move.

[felixmaisonneuve]

You are using an older firmware package. It might be a good idea to upgrade it. You can find the Kortex2.3 package on our Kinova website. You can then upload it on your arm via the Systems->Upgrade page. It could be a good idea to update your python kortex package (instructions described in the readme)

I am not sure if it will fix the issue, but it could help.

When running an Action in the WebApp, you have to hold the "play" button. The action is not working at all?

In your code, can you add a print in your check_for_end_or_abort function to know if the action ends or if it aborts (something like that I think : print("EVENT : " + Base_pb2.ActionEvent.Name(notification.action_event)))

[ShaoxiongYao]

Thank you for the help. I am updating the firmware package now and see if that can help.

Regarding running Action in the WebApp, after I hold the "play" button, the arm will continuously move and reach the target. I tried different actions "Home", "Zero" and "Retract" and the arm all reaches the target.

[felixmaisonneuve]

If the WebApp action are working, the error is probably coming from your code. Have you tried running our examples "as is". There is a timeout in our cartesian example. Maybe you have changed it https://github.com/Kinovarobotics/kortex/blob/2218dd6bb9fa2d135f77d1f611ffe8d3cce2edf9/api_cpp/examples/102-High_level_movement/01-move_angular_and_cartesian.cpp#L221-L226

[felixmaisonneuve]

Hi @ShaoxiongYao,

Did you manage to resolve this issue? I will assume so and close this issue in a couple days if I don't hear from you.

Best, Felix

[ShaoxiongYao]

Hi @felixmaisonneuve, sorry I have been working on other parts. I am available now and will check this and give you an update soon.

[ShaoxiongYao]

Hi, sorry to ask the question again, but I realize the problem is not solved. I have updated the firmware to the latest version as well as the python API:

Basically, I run the example code as https://github.com/Kinovarobotics/kortex/blob/master/api_python/examples/102-Movement_high_level/01-move_angular_and_cartesian.py but the target end-effector pose is not reached.

I have a slightly modified code (only add print arguments and pose movement) at https://github.com/ShaoxiongYao/kortex/blob/master/api_python/examples/102-Movement_high_level/01-move_angular_and_cartesian.py.

And the output I had is:

Logging as admin on device 192.168.1.10
Starting Cartesian action movement ...
start z: 0.4277838170528412
target z: 0.5277838170528412
Executing action
Waiting for movement to finish ...
EVENT : ACTION_START
EVENT : ACTION_FEEDBACK
EVENT : ACTION_FEEDBACK
EVENT : ACTION_END
Cartesian movement completed
end z: 0.4545380771160126

I wonder if your team may run this set of codes on your machine and compare the outputs. I will sincerely appreciate your help!

[felixmaisonneuve]

I did the same experiment as you and everything worked fine. Here is the code I used :

def example_cartesian_action_movement(base, base_cyclic):

    print("Starting Cartesian action movement ...")
    action = Base_pb2.Action()
    action.name = "Example Cartesian action movement"
    action.application_data = ""

    feedback = base_cyclic.RefreshFeedback()

    print("old = " + str(feedback.base.tool_pose_z))
    print("new = " + str(feedback.base.tool_pose_z + 0.5))

    cartesian_pose = action.reach_pose.target_pose
    cartesian_pose.x = feedback.base.tool_pose_x          # (meters)
    cartesian_pose.y = feedback.base.tool_pose_y  # (meters)
    cartesian_pose.z = feedback.base.tool_pose_z + 0.5    # (meters)
    cartesian_pose.theta_x = feedback.base.tool_pose_theta_x # (degrees)
    cartesian_pose.theta_y = feedback.base.tool_pose_theta_y # (degrees)
    cartesian_pose.theta_z = feedback.base.tool_pose_theta_z # (degrees)

    e = threading.Event()
    notification_handle = base.OnNotificationActionTopic(
        check_for_end_or_abort(e),
        Base_pb2.NotificationOptions()
    )

    print("Executing action")
    base.ExecuteAction(action)

    print("Waiting for movement to finish ...")
    finished = e.wait(TIMEOUT_DURATION)
    base.Unsubscribe(notification_handle)

    feedback = base_cyclic.RefreshFeedback()
    print("final = " + str(feedback.base.tool_pose_z))

    if finished:
        print("Cartesian movement completed")
    else:
        print("Timeout on action notification wait")
    return finished

it is based on the 01-move_angular_and_cartesian.py. I commented out the angular movement to leave only the Home action command and the cartesian action command. This is the output from one of the test I did :

Logging as admin on device 10.0.101.184
Moving the arm to a safe position
EVENT : ACTION_START
EVENT : ACTION_END
Safe position reached
Starting Cartesian action movement ...
old = 0.4223249852657318
new = 0.9223249852657318
Executing action
Waiting for movement to finish ...
EVENT : ACTION_START
EVENT : ACTION_FEEDBACK
EVENT : ACTION_FEEDBACK
EVENT : ACTION_FEEDBACK
EVENT : ACTION_FEEDBACK
EVENT : ACTION_FEEDBACK
EVENT : ACTION_FEEDBACK
EVENT : ACTION_FEEDBACK
EVENT : ACTION_FEEDBACK
EVENT : ACTION_END
final = 0.9227738380432129
Cartesian movement completed

Can you share your code? I do not understand why your arm would not reach its destination.

[ShaoxiongYao]

Hi, again, I really appreciate your help. I think I found the source of the problem was the robot's speed limit. I have the exact script run on my machine as:

#! /usr/bin/env python3

###
# KINOVA (R) KORTEX (TM)
#
# Copyright (c) 2018 Kinova inc. All rights reserved.
#
# This software may be modified and distributed
# under the terms of the BSD 3-Clause license.
#
# Refer to the LICENSE file for details.
#
###

import sys
import os
import time
import threading

from kortex_api.autogen.client_stubs.BaseClientRpc import BaseClient
from kortex_api.autogen.client_stubs.BaseCyclicClientRpc import BaseCyclicClient

from kortex_api.autogen.messages import Base_pb2, BaseCyclic_pb2, Common_pb2

# Maximum allowed waiting time during actions (in seconds)
TIMEOUT_DURATION = 20

# Create closure to set an event after an END or an ABORT
def check_for_end_or_abort(e):
    """Return a closure checking for END or ABORT notifications

    Arguments:
    e -- event to signal when the action is completed
        (will be set when an END or ABORT occurs)
    """
    def check(notification, e = e):
        print("EVENT : " + \
              Base_pb2.ActionEvent.Name(notification.action_event))
        if notification.action_event == Base_pb2.ACTION_END \
        or notification.action_event == Base_pb2.ACTION_ABORT:
            e.set()
    return check

def example_cartesian_action_movement(base, base_cyclic):

    print("Starting Cartesian action movement ...")
    action = Base_pb2.Action()
    action.name = "Example Cartesian action movement"
    action.application_data = ""

    feedback = base_cyclic.RefreshFeedback()

    print("old = " + str(feedback.base.tool_pose_z))
    print("new = " + str(feedback.base.tool_pose_z + 0.2))

    cartesian_pose = action.reach_pose.target_pose
    cartesian_pose.x = feedback.base.tool_pose_x             # (meters)
    cartesian_pose.y = feedback.base.tool_pose_y             # (meters)
    cartesian_pose.z = feedback.base.tool_pose_z + 0.2       # (meters)
    cartesian_pose.theta_x = feedback.base.tool_pose_theta_x # (degrees)
    cartesian_pose.theta_y = feedback.base.tool_pose_theta_y # (degrees)
    cartesian_pose.theta_z = feedback.base.tool_pose_theta_z # (degrees)

    e = threading.Event()
    notification_handle = base.OnNotificationActionTopic(
        check_for_end_or_abort(e),
        Base_pb2.NotificationOptions()
    )

    print("Executing action")
    base.ExecuteAction(action)

    print("Waiting for movement to finish ...")
    finished = e.wait(TIMEOUT_DURATION)
    base.Unsubscribe(notification_handle)

    feedback = base_cyclic.RefreshFeedback()
    print("final = " + str(feedback.base.tool_pose_z))

    if finished:
        print("Cartesian movement completed")
    else:
        print("Timeout on action notification wait")
    return finished

if __name__ == '__main__':
    # Import the utilities helper module
    sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))
    import utilities

    # Parse arguments
    args = utilities.parseConnectionArguments()

    # Create connection to the device and get the router
    with utilities.DeviceConnection.createTcpConnection(args) as router:

        # Create required services
        base = BaseClient(router)
        base_cyclic = BaseCyclicClient(router)

        example_cartesian_action_movement(base, base_cyclic)

        # You can also refer to the 110-Waypoints examples if you want to execute
        # a trajectory defined by a series of waypoints in joint space or in Cartesian space

When I set the joint speed limits to be 20 deg/s on the web application interface, like:

I have the output, where robot exactly reach the target configuration:

Logging as admin on device 192.168.1.10
Starting Cartesian action movement ...
old = 0.42786848545074463
new = 0.6278684854507446
Executing action
Waiting for movement to finish ...
EVENT : ACTION_START
EVENT : ACTION_FEEDBACK
EVENT : ACTION_FEEDBACK
EVENT : ACTION_END
final = 0.6285478472709656
Cartesian movement completed

However, when I set the speed limit to 5 deg/s, like:

I have the output, where the robot cannot reach the target configuration:

Logging as admin on device 192.168.1.10
Starting Cartesian action movement ...
old = 0.4276808202266693
new = 0.6276808202266693
Executing action
Waiting for movement to finish ...
EVENT : ACTION_START
EVENT : ACTION_FEEDBACK
EVENT : ACTION_FEEDBACK
EVENT : ACTION_END
final = 0.4762071669101715
Cartesian movement completed

I wonder if you may reproduce the same issue on your side. I will try my best to respond more frequently when you have any updates.

[felixmaisonneuve]

I tested on my side and I do have the same bug.

The workaround I found is to change the linear velocity limit instead of the joints velocity limits for cartesian actions. The issue seems to be when your cartesian trajectory is limited by one of your joint velocity limits.

Go to the "Advanced" tab of your speed limits page in the web app, and select "Cartesian Trajectory". This is the velocity limits that will be applied when you are sending a Cartesian Action (action.reach_pose.target_pose). What you could do is lower the linear velocity and leave the joint velocities at their max values

If you are also sending Angular Trajectory, then you coudl change the velocity limits from the "Angular Trajectory" page

I will leave this issue opened and mark it as a bug.

[felixmaisonneuve]

After some discussions, this is a bug that was fixed for the waypoint system. If you change your code to use waypoints instead of Cartesian Actions, you should not see this bug anymore

[felixmaisonneuve]

To clarify, Joint speeds limits can prevent a cartesian action to reach its destination, but Linear limit work as intended. This bug does not occur for cartesian waypoints

[ShaoxiongYao]

Thank you for your reply. A quick test that the linear velocity limit workaround works on my side. I will get you back whether the cartesian waypoints work later.

[ShaoxiongYao]

I tested that both the cartesian and joint space waypoints work well. I set the joint speed limits to 5 deg/s on the web app and the arm reaches the target cartesian/joint configuration using waypoints.

[felixmaisonneuve]

Great,

In that case, I will close this issue

Regards, Felix