How to improve the smoothness of velocities produced by Pink?

[dbdxnuliba]

Given a trajectory for arm , the generated joint speed fluctuates greatly, how to increase the smoothness of the joint speed generated by pink?
could you please give an example to produce smooth joint velocity

such as the demo arm_ur3.py ,produce joint velocity curve :

@stephane-caron @simeon-ned
We can see the joint velocity generated by pink is not smooth ,so how can we improve the velocity smooth generated by pink,
many thanks !

code is here

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
#
# SPDX-License-Identifier: Apache-2.0
# Copyright 2022 Stéphane Caron

"""Universal Robots UR3 arm tracking a moving target."""
import os
import sys
sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))

import meshcat_shapes
import numpy as np
import qpsolvers
from loop_rate_limiters import RateLimiter
import os
import sys
sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), '../')))

import pink
from pink import solve_ik
from pink.tasks import FrameTask, PostureTask
from pink.utils import custom_configuration_vector
from pink.visualization import start_meshcat_visualizer

try:
    from robot_descriptions.loaders.pinocchio import load_robot_description
except ModuleNotFoundError:
    raise ModuleNotFoundError(
        "Examples need robot_descriptions, "
        "try ``pip install robot_descriptions``"
    )


if __name__ == "__main__":
    robot = load_robot_description("ur3_description", root_joint=None)
    viz = start_meshcat_visualizer(robot)

    end_effector_task = FrameTask(
        "ee_link",
        position_cost=1.0,  # [cost] / [m]
        orientation_cost=1.0,  # [cost] / [rad]
        lm_damping=1.0,  # tuned for this setup
    )

    posture_task = PostureTask(
        cost=1e-3,  # [cost] / [rad]
    )

    tasks = [end_effector_task, posture_task]

    q_ref = custom_configuration_vector(
        robot,
        shoulder_lift_joint=1.0,
        shoulder_pan_joint=1.0,
        elbow_joint=1.0,
    )
    configuration = pink.Configuration(robot.model, robot.data, q_ref)
    for task in tasks:
        task.set_target_from_configuration(configuration)
    viz.display(configuration.q)

    viewer = viz.viewer
    meshcat_shapes.frame(viewer["end_effector_target"], opacity=0.5)
    meshcat_shapes.frame(viewer["end_effector"], opacity=1.0)

    # Select QP solver
    solver = qpsolvers.available_solvers[0]
    if "quadprog" in qpsolvers.available_solvers:
        solver = "quadprog"

    rate = RateLimiter(frequency=200.0)
    dt = rate.period
    t = 0.0  # [s]
    #save data
    vel_vec = []
    q_vec = []
    cnt = 0
    cnt_max = 5000
    while True and cnt < cnt_max:
        # Update task targets
        end_effector_target = end_effector_task.transform_target_to_world
        end_effector_target.translation[1] = 0.5 + 0.1 * np.sin(2.0 * t)
        end_effector_target.translation[2] = 0.2

        # Update visualization frames
        viewer["end_effector_target"].set_transform(end_effector_target.np)
        viewer["end_effector"].set_transform(
            configuration.get_transform_frame_to_world(
                end_effector_task.frame
            ).np
        )

        # Compute velocity and integrate it into next configuration
        velocity = solve_ik(configuration, tasks, dt, solver=solver)
        configuration.integrate_inplace(velocity, dt)
        vel_vec.append(velocity)
        q_vec.append(configuration.q)
        

        # Visualize result at fixed FPS
        viz.display(configuration.q)
        rate.sleep()
        t += dt
        cnt = cnt +1 
    #plot the data
    import matplotlib.pyplot as plt
    velocity_data = np.array(vel_vec)
    configuration_data = np.array(q_vec)
    plt.figure()
    plt.plot(velocity_data)
    plt.title("Velocity data")
    plt.figure()
    plt.plot(configuration_data)
    plt.title("Configuration data")
    plt.show()

[stephane-caron]

Your observation is right, there is no velocity regularization by default in Pink, i.e. velocities are allowed to jump and continuity is not guaranteed. This is because Pink is designed as a first-order differential IK, working on configurations $q$, whereas velocity smoothness is a property enforced with at least a second order state $(q, v)$ (configuration and velocity).

First-order hacks

There are at least two ways to make velocities smoother, but they require somehow moving to the second order:

1. Edit: tried below.
   • Soft: create a task named e.g. SmoothVelocityTask or LowAccelerationTask that would penalize variations of the computed velocity from the previous one.
   • The DampingTask favors lower velocities, and thus avoids large jumps and oscillations in this example. Edit: here is an example: arm_ur3_velocity_damping.py
2. Hard: create a limit named e.g. AccelerationLimit that guarantees a bounded acceleration via $\pm v \ominus v_{prev} \leq a_{max} \delta t$ in the underlying QP.
   • Barriers, being more expressive than limits, could implement this just as well.
   • Edit: There is a little more to this question, as detailed for instance in this paper there is a second first-order inequality to add: $v \leq \sqrt{2 a_{\max} (q_{\max} \ominus q)}$. This is implemented in the acceleration limit.

Both options can be implemented, but they both require passing the previous velocity $v_{prev}$ as a way to consider a second-order state. This goes against the current design of Pink as a first-order IK (although it could be hacked into it, for instance by making a task with an extra $v_{prev}$ attribute for the soft option, or similarly a limit for the hard option).

Second-order differential IK

A better way to add this feature would be to refactor Pink as a second-order differential IK. This would imply not only replacing the Configuration class (wrapping configurations $q$) with a State class (wrapping pairs $(q, v)$ of configurations and velocities), but also replacing the first-order task equation:

$$J(q) v = - \alpha e(q)$$

With a second-order one, typically:

$$J(q) a + \dot{J}(q) v = -\alpha e(q) - 2 \sqrt{\alpha} \dot{e}(q, v)$$

This would yield a better differential IK (as the term $\dot{J}(q) v$ approximates better the nonlinear dynamics of the system locally), but in terms of code it would be a significant rewriting of the library.

[stephane-caron]

Low-acceleration task (soft)

I've implemented and tested a LowAccelerationTask (soft first-order hack in the list above) in 4efaf9f, but it turns out in this example you can get most of the smoothing you'd like with just a damping task. Here is a complete example and the outcome (posture then damping task):

arm_ur3_velocity_damping.py

Funnily the elbow switches to the other side in this instance. The LowAccelerationTask is not a good idea in this instance because it doesn't dissipate energy, so it tends to create oscillations when its cost is high enough. Acceleration limits (not implemented yet) along with a lower-cost damping task make more sense in my opinion.

In summary, to go back to your initial question:

could you please give an example to produce smooth joint velocity

Here is one: arm_ur3_velocity_damping.py

[stephane-caron]

Acceleration limit (hard)

Finally, I've implemented and tested an AccelerationLimit (hard first-order hack in the list above) in this PR. It reminded me there is a little more to this question, as detailed for instance in this paper there is a second first-order inequality to add:

$$ -\sqrt{2 a_{\max} (q \ominus q_{\min})} \leq v \leq \sqrt{2 a_{\max} (q_{\max} \ominus q)} $$

This is now implemented in the AccelerationLimit. I've then updated the velocity-damping example to arm_ur3_velocity_smoothing.py, adding acceleration limits and reducing the task gain for some extra velocity shaving. The outcome looks like this:

Hoping this helps!

[dbdxnuliba]

@stephane-caron ，thanks for your details reply，during use the pink, the first-order hacks
indeed has discontinuities in speed, which can cause significant impacts when used on real robots. I look forward to you achieving Second-order differential IK