stack-of-tasks / sot-dynamic-pinocchio Goto Github PK

View Code? Open in Web Editor NEW

5.0 13.0 14.0 936 KB

Encapsulate Pinocchio in SoT

License: BSD 2-Clause "Simplified" License

Shell 0.77% CMake 2.98% C++ 74.56% Python 21.68%

sot-dynamic-pinocchio's Introduction

SoT Dynamic Pinocchio

Encapsulate Pinocchio in SoT

This software provides robot dynamic computation for dynamic-graph by using pinocchio.

Setup

To compile this package, it is recommended to create a separate build directory:

mkdir _build
cd _build
cmake [OPTIONS] ..
make install

Please note that CMake produces a CMakeCache.txt file which should be deleted to reconfigure a package from scratch.

Dependencies

The matrix abstract layer depends on several packages which have to be available on your machine.

Libraries:
- dynamic-graph (>= 3.0.0)
- sot-core (>= 3.0.0)
- pinocchio (>= 1.1.2)
System tools:
- CMake (>=2.6)
- pkg-config
- usual compilation tools (GCC/G++, make, etc.)

sot-dynamic-pinocchio's People

Contributors

Stargazers

Watchers

Forkers

kevin-giraud-esclasse thomasfla theophanel olivier-stasse proyan ksyy nim65s jviereck fbailly noelieramuzat gabrielebndn florent-lamiraux rascof fangyuan-21

sot-dynamic-pinocchio's Issues

CoM velocity?

It would be nice to have an output signal for the CoM velocity.
This is not a priority at all (there are easy workarounds we implemented) but still it would be cleaner.

Bug in `DynamicPinocchio::computeJacobians`

Hi there,

looking at the change in b4ff119#diff-831036c6c653e73bea0ed8671360385bR671

it looks like the

pinocchio::JointIndex id = isFrame
    ? m_model->frames[(pinocchio::JointIndex)jointId].parent
    : (pinocchio::JointIndex)jointId;

is wrong. Reading the old code, I think the two lines should be swapped:

pinocchio::JointIndex id = isFrame
    ? (pinocchio::JointIndex)jointId
    : m_model->frames[(pinocchio::JointIndex)jointId].parent;

What do you think?

Return momenta before calling computation

sot-dynamic-pinocchio/src/sot-dynamic-pinocchio.cpp

Line 976 in 9c7ae90

return Momenta;

Return joint velocity in world aligned coordinates

Pinocchio has a way to compute the joint velocities in the world aligned frame. It would be great if this would be possible with the dg binding as well.

missing r2d2.urdf

Hi,

Running tests shows:

ERROR: test_build_robot_from_urdf (__main__.HumanoidRobotTest)
----------------------------------------------------------------------
Traceback (most recent call last):
  File "…/tests/python/humanoid_robot.py", line 44, in test_build_robot_from_urdf
    Robot("test_build_robot_from_string", urdfFile=self.r2d2_urdf_file)
  File "…/tests/python/humanoid_robot.py", line 15, in __init__
    self.loadModelFromUrdf(urdfFile)
  File "…/dynamic_graph/sot/dynamic_pinocchio/humanoid_robot.py", line 207, in loadModelFromUrdf
    self.pinocchioModel = pinocchio.buildModelFromUrdf(urdfFile, rootJointType())
ValueError: The file …/sot-dynamic-pinocchio/tests/python/r2d2.urdf does not contain a valid URDF model.

Should I have a r2d2.urdf file anywhere ?

[package.xml] Change the maintainer to @nim65s

The maintainer should be changed to @nim65s.
Indeed the package is failing on the rosbuild farm due to lapack detection problem.
https://build.ros.org/job/Mbin_uB64__sot-dynamic-pinocchio__ubuntu_bionic_amd64__binary/70/

Thanks in advance for your help @nim65s !

humandorobot.timeStep set to 0.005 s

We should change line:
https://github.com/stack-of-tasks/sot-dynamic-pinocchio/blame/c8f53f9b42e1d7cfed97c2f737d9172429bdb506/src/dynamic_graph/sot/dynamics_pinocchio/humanoid_robot.py#L150

A lot of people are reading it to make computation -> wrong !

[CMake] document or remove custom_cmake/python.cmake

Hi,

This file seems useless. I suggest to remove it and use the one from the jrl cmake submodule.

New release

Hello @nim65s,

do you know when the devel branch will be released ?

Missing EIGEN_MAKE_ALIGNED_OPERATOR_NEW

@NoelieRamuzat, I wanted to open this issue on your fork, but it seems when you create a fork issues cannot be posted there by default and needs to be enabled, so I am posting here.

Entities StateIntegrator and PinocchioDevice do not have EIGEN_MAKE_ALIGNED_OPERATOR_NEW. I think it should be added. See here for a reference. (PinocchioDevicealso concerns this repo as it is present in https://github.com/stack-of-tasks/sot-dynamic-pinocchio/tree/topic/pinocchio-device)

[CMake] lapack link fail on buster

From CI in #81 https://gitlab.laas.fr/stack-of-tasks/sot-dynamic-pinocchio/-/jobs/91082 :

/usr/bin/ld: cannot find -llapack
collect2: error: ld returned 1 exit status

Regression in createJacobianEndEffWorld?

When creating a signal via createJacobianEndEffWorld, the computed Jacobian is in the LOCAL and not LOCAL_WORLD_ALIGNED frame. This used to work before, so I guess the code regressed somewhere.

The underlying function computeGenericEndeffJacobian was modified here:

8fdf588

but I cannot spot anything in there.

Does someone has an idea where this regression could come from?

Here some example outputs to show the regression

robot_dg.createJacobianEndEffWorld('jac_cnt_FL', 'FL_FOOT')
np.array(robot_dg.jac_cnt_FL.value)

array([[ 0.46,  0.25, -0.85, -0.13,  0.12, -0.03, -0.05, -0.05, -0.16,
         0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ],
       [ 0.  ,  0.96,  0.28,  0.15, -0.06,  0.19,  0.13,  0.  ,  0.  ,
         0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ],
       [ 0.89, -0.13,  0.45,  0.07, -0.19, -0.19,  0.03, -0.12,  0.  ,
         0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ],
       [ 0.  ,  0.  ,  0.  ,  0.46,  0.25, -0.85,  0.46,  0.  ,  0.  ,
         0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ],
       [ 0.  ,  0.  ,  0.  ,  0.  ,  0.96,  0.28,  0.  ,  1.  ,  1.  ,
         0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ],
       [ 0.  ,  0.  ,  0.  ,  0.89, -0.13,  0.45,  0.89,  0.  ,  0.  ,
         0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ,  0.  ]])

And for comparison from Pinocchio:

endeff_id = robot_pin.model.getFrameId('FL_FOOT')

pin.forwardKinematics(robot_pin.model, robot_pin.data, q, v)
pin.updateFramePlacement(robot_pin.model, robot_pin.data, endeff_id)
pin.computeJointJacobians(robot_pin.model, robot_pin.data, q)

print(pin.getFrameJacobian(robot_pin.model, robot_pin.data, endeff_id, pin.ReferenceFrame.LOCAL))

[[ 0.46  0.25 -0.85 -0.13  0.12 -0.03 -0.05 -0.05 -0.16  0.    0.    0.
   0.    0.    0.    0.    0.    0.  ]
 [ 0.    0.96  0.28  0.15 -0.06  0.19  0.13 -0.   -0.    0.    0.    0.
   0.    0.    0.    0.    0.    0.  ]
 [ 0.89 -0.13  0.45  0.07 -0.19 -0.19  0.03 -0.12  0.    0.    0.    0.
   0.    0.    0.    0.    0.    0.  ]
 [ 0.    0.    0.    0.46  0.25 -0.85  0.46  0.    0.    0.    0.    0.
   0.    0.    0.    0.    0.    0.  ]
 [ 0.    0.    0.    0.    0.96  0.28  0.    1.    1.    0.    0.    0.
   0.    0.    0.    0.    0.    0.  ]
 [ 0.    0.    0.    0.89 -0.13  0.45  0.89  0.    0.    0.    0.    0.
   0.    0.    0.    0.    0.    0.  ]]

print(pin.getFrameJacobian(robot_pin.model, robot_pin.data, endeff_id, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED))

[[ 1.    0.    0.    0.   -0.11 -0.18  0.   -0.13 -0.07  0.    0.    0.
   0.    0.    0.    0.    0.    0.  ]
 [ 0.    1.    0.    0.11  0.    0.2   0.11  0.   -0.04  0.    0.    0.
   0.    0.    0.    0.    0.    0.  ]
 [ 0.    0.    1.    0.18 -0.2   0.    0.09 -0.01  0.14  0.    0.    0.
   0.    0.    0.    0.    0.    0.  ]
 [ 0.    0.    0.    1.    0.    0.    1.    0.    0.    0.    0.    0.
   0.    0.    0.    0.    0.    0.  ]
 [ 0.    0.    0.    0.    1.    0.    0.    0.96  0.96  0.    0.    0.
   0.    0.    0.    0.    0.    0.  ]
 [ 0.    0.    0.    0.    0.    1.    0.    0.28  0.28  0.    0.    0.
   0.    0.    0.    0.    0.    0.  ]]

Get jacobian rotated in the world frame

Right now the jacobians computed by the dynamic-pinocchio entity are rotated in the local frame of the frame they are computed at. Is there a way to get the jacobians at the frame location but oriented wrt. the world coordinate axis? This is as our force measurements are specified in world coordinates and therefore there must be a rotation to compute the effect of $J.T * \lambda$ in the dynamics equation.

Unable to run locally compiled code

When building sot-dynamic-pinocchio from source and installing it with prefix /opt/openrobots, my python code is not able to find all the required libraries.

Does the installation expect libraries to be versioned? Is there a way to achieve this when I install a self-build version?

fix build on eigen 3.2 with Eigen::DenseIndex

Since b06763f, this package can't build on 14.04, as Eigen 3.2 does not have Eigen::Index.

Switching to BSD v2

Dear all,
We will soon switch to BSD License v2.0 to ease our collaborations.
Let me know if there is a problem with this.
Cheers,
Olivier.

Wrong initialization of the limits

The following line is not initializing properly the orientation of the base:

sot-dynamic-pinocchio/src/sot-dynamic-pinocchio.cpp

Line 269 in e50a87a

res(fillingIndex) = std::numeric_limits<double>::min();

As min means the smallest postive value, if the real limit is negative this is not correct.
It happens to be a problem in the release v3.5.1.

Double init.py

Hi @nim65s

In 62982b5, you added the installation of a second init.py file, together with some python files related to romeo. The init.py file overwrites the one installed by src/CMakeLists.txt.

Is this on purpose ? If yes, I should remove src/dynamic_graph/.../init.py. If no, which one should we remove ?