stephane-caron/pink

Python inverse kinematics based on Pinocchio

pink's Issues

Slowdown when the end effector approaches the target

Hi,

Thanks for this inspiring library! I've encountered an issue with the velocity QP formulation when the robot is near the target. Here's the problem I'm facing:

I'm using the following formulation:

$$\begin{array}{ccl} \dot{q}^{opt} = & \arg \min & || v^{*} - J(q)\dot{q} ||^2_2 + \omega_{reg}||\dot{q} - k_{p,reg} (q_{ref} - q) ||^2_2\\\ & s.t & A \dot{q} \leq b \\\ \end{array}$$

However, I've noticed that when the robot is near the target, it slows down significantly. I believe this slowdown occurs because the position error decreases, leading to a situation where we primarily minimize the end effector Cartesian velocity. Even when I increase the gain, the robot still slows down near the target, which can result in spending a lot of time before convergence or even failing to converge. This issue is especially noticeable in physical simulations and hardware tests.

Could you provide some advice or hints on how to overcome this problem?

Thank you!

Velocity vibrations in Go2 position barrier example

buy test the demo "python3 examples/barriers/go2_squat.py"
speed vibration occurs,how can we solve the problem ,thanks

CoM task

Hi, Stéphane, I am wondering if there is a CoM task for a humanoid robot in current framework.

Need to use pink to solve IK using a LIPM model.

Fix sign of error and right-minus in body task

        # TODO(scaron): fix sign of error and box minus

Global inverse kinematics

Dear pink team,

first of all, thanks for the library! I was just experimenting with pink for a variety of 6-DoF robots and I was experiencing an unexpected high number of ik procedures not converging to a solution although it existed. It seems to me around 50% of the runs I was performing got stuck in local minima (position/orientation tradeoff).

In pseudocode, my application looks roughly like this:

q = sample_random_q()
r = create_random_6dof_robot()
goal = r.forward_kinematics(q)
r.reset_to_random_position()

ik = pink_ik(r, goal)  # Basically following the example provided in the readme

I compared the results against my own implementation of an ik solver based on the damped least squares inverse of the frame jacobian. While pink is significantly faster when finding a solution, I only find a solution in ~50% of cases compared to ~90% for my own solver. Playing around with the damping factor didn't really seem to improve the results significantly.

I would love to use pink, it seems to be a fast and nice library for solving the ik with pinocchio-based simulations. However, in order to do so, I would need to have a way to significantly improve the ratio of solved ik problems. Do you have any suggestions on possible adaptions that could help? I would be happy to contribute, but I don't know which approaches would be the most promising here as I have rarely worked with the quadratic programming solvers used here.

Cheers, Jonathan

URDF support

Hello,

I've noticed that pink only supports URDF in the robot_description repo. Is there any way I could use my custom URDF?

Thank you,
Seung Hyeon

pink installation on mac

Hello Stephan,

Thank you for your effort in maintaining this nice repo!

While using pink, I get the following two questions for you.

1. I've installed pink on my mac which is intel OSX Monterey 12.5.1 and I am using anaconda virtual environment (python version 3.8).
   When I tried to run the upkie_crouching.py example, it kept complaining there is no module named pink.models. So, instead of running the script, I manually tried opening the python interpreter(python version 3.8) in the same anaconda environment and typed the code in upkie_crouching.py line by line, and it successfully imported all the modules. I don't know how this could be possible. Do you have anything in your mind?

2. Other than the aforementioned software issue, I have another question regarding the inverse kinematics solver interface (API).
   I have a 7-DoF robotic manipulator which has a holonomic constraint (q_1 = q_2) so it has 6 active joints with one passive joint. Given any cartesian tasks, I would like to solve the inverse geometry problem to get the joint positions satisfying the holonomic constraint.
   In this case, I think one way to solve the problem is by setting the holonomic constraint as a task in the cost function and giving the larger task gain compared to the cartesian task.
   Another way to solve the problem is using projected jacobian (J_cartesian_task * N_holonomic_constraint) with N = I - JJ_pseudo_inverse.
   Do you think those two methods sound okay to obtain the solution that I want? If so, can you point out which API in pink I should use to set the holonomic constraint as a cost in the QP (I think I could try the latter one by myself)?

Thank you,
Seung Hyeon

Display a TF tree using pinocchio model

Dear Caron:
I found this repo by using Pinocchio when I tried to learn more about Meshcat, and thanks a lot for your code, I get some inspiration for drawing a TF tree for a robot model.
My understanding of this code in pink

meshcat_shapes.frame(viewer["left_contact_target"], opacity=0.5)

is that we will replace the old object with a new frame.
My question is if we could just add the frame by using addGeometryObject?

Thanks for your help!
heaty

Posture task doesn't work with continuous joints

Continuous joints have nq=2, whereas the posture task assumes nq=1 for revolute joints so that the tangent twist between two joint configurations is simply their difference. This will need to be generalized.

• Example: see WIP_kinova_gen2_arm.py in the examples folder.
• Related: stack-of-tasks/pinocchio#1751
• Related: stack-of-tasks/pinocchio#794

how can we install pink by source code

how can we install pink by source code

Walking Pattern Generator Example

Hello, Stephane. What does Pink need for an example of a walking pattern generator like the one in Pymanoid?

Greetings

Update to the Pinocchio 3 API

Pink still relies on some parts of the Pinocchio 2 API that have changed in Pinocchio 3, for instance:

Traceback (most recent call last):
  File "pink/examples/barriers/go2_squat.py", line 19, in <module>
    from pink.visualization import start_meshcat_visualizer
  File "pink/examples/barriers/pink/visualization.py", line 14, in <module>
    ) -> pin.visualize.MeshcatVisualizer:
         ^^^^^^^^^^^^^
AttributeError: module 'pinocchio' has no attribute 'visualize'

Attribute Error : 'FrameTask' object has no attribute 'frame'

When you run the arm_ur5.py example it gives an error:

Attribute Error : 'FrameTask' object has no attribute 'frame'

end_effector_task.frame is not defined

The class Frame Task has no attribute frame in the examples/arm_ur5.py

The same error is persistent with other arm_ur3.py and other examples

How to apply velocity smooth constraint and weight

how could we aplly velocity smooth constraint and weight

Numerical stability of Draco3 example

@ymontmarin I just tested the new linear holonomic task on the humanoid_draco3.py example, and it seems it has become less numerically stable:

humanoid_draco3_issue.mp4

(The first run uses the latest version from PyPI, the second run is the current master branch.)

I don't think the gains should be reduced for the new task formulation? Or maybe this is because we are now including the free flyer in the task, whereas it was just q_2 = -q_1 before?

Joint limits for planar joints

The omnidirectional three-wheeled robot added by #14 does not work yet because of joint limits for its root planar joint.

This issue will be fixed by #12.

Configuration is defined only for FrameType::Body, not for other enum values.

Hey Stéphane,
Nice library :)
I tried to use the library for a simple config calculation for a user-defined frame in the end-effector. I used the frametype pinocchio::FrameType::OP_FRAME , but it seems that it is not supported yet (see https://github.com/tasts-robots/pink/blob/master/pink/configuration.py#L161). (The more generic implementation might be with getFrameId and existFrame and so on).

Should be an easy change to make, please let me know if you want me to do it.
Best,
Rohan

Build sparse matrices for sparse solvers

Currently using OSQP as solver yields conversion warnings.

CVXOPT does not handle infinity

When there is no velocity bound on a joint, Pink currently sets inequality constraints as $-\infty < v_i < \infty$. But with CVXOPT this approach yields ValueError: domain error.

Possible solutions:

• Trim large values (might not generalize well)
• Add some post-processing to remove redundant inequalities for CVXOPT specifically
• Avoid such inequalities altogether

Velocity limits without configuration limits

Models that have velocity but no configuration limits get no velocity inequality constraints in their QP currently.

Package name on PyPI

Hello @chenjiandongx, I see that the last release of the pink package on PyPI was from 2018, and that its homepage on GitHub is now a 404 page.

Our project here is also named pink (pin-pink on PyPI). If your project is not active any more, would you be OK with transferring https://pypi.org/project/pink so that we can release to that name?

This would help us reduce friction when users install our project ("I installed pink but it doesn't work!", to which we check "did you pip install pin-pink and not pink", and that usually is the issue...)

Thanks in advance for your consideration,

Proposal for Integration of Control Barrier Functions (CBFs) into pink

Hello Dr. Caron,

First of all, thank you for maintaining the pink repository. I found it extremely useful in practice.

My colleague @simeon-ned and I have been actively working on the concept of Control Barrier Functions (CBFs) in robotic applications. This approach integrates seamlessly into QP-like formulations and it was intriguing for us to incorporate it into pink to ensure safety limitations, specifically solving IK with inequality constraints:

$$h(q) \geq 0$$

We have added this functionality in pink. You can find our forked repository here: link. The notes on the related theory can be found in the example notebook.

So far, we have added a new Barriers abstract class, along with several practical barriers:

• PositionBarrier: PositionBarrier implementation
• ConfigurationBarrier: ConfigurationBarrier implementation (can be seen as an alternative to limits)
• Body Spherical Barrier: Body Spherical Barrier implementation (for self-collision avoidance)

Documentation for these features, based on our fork, can be found on our local copy of GitHub pages: Barriers Documentation.

To demonstrate the capabilities of these features, we have developed two examples:

1. Joint and End Effector Barriers in UR5 Robot:
   • UR5 Robot Example Code

ur5_example.mp4

2. Spherical Self Collisions in Two Arms Yumi Robot:
   • Yumi Robot Example Code

yumi_example.mp4

We are eager to contribute these enhancements to your repository, believing they will greatly benefit the community. We are prepared to develop comprehensive tests and consider the integration of more complex geometries for self-collision management using the HPP FCL library, pending your approval.

We look forward to your feedback and hope for a positive collaboration.

Best regards,

Ivan Domrachev

Adding holonomic constraints

Hello Dr. Caron,

Thank you for maintaining this nice repo.

I have a question regarding how to set a holonomic constraint when solving IK using the API you provided in this repo.
I've looked through the documentation of this repo, but I only found the body and posture task class, so my best guess is I could not solve the IK on the holonomic system given the current APIs.

My problem is: I have a 7-DOF manipulator in which one of the joints is passive(q_1) while satisfying the relation of q_1 = q_2 (one active joint) at the same time, so I would like to move this hard constraint to the cost function (by making it a soft constraint). Is there any task class that I can use to solve this problem?

I would really appreciate you if you could get me any ideas or suggestions.

Best,
Seung Hyeon

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

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()

Lie algebra formulation for linear holonomic task

Follow-up to #43 (comment). The linear holonomic task should be formulated properly in the Lie algebra to handle more general application cases.

Handle models without and with floating base

This feature is a low-hanging fruit, as pinocchio already provides a floating-base boolean.