epfl-lasa / control-libraries Goto Github PK

To use the pinocchio library properly, several environment variables have to be configured (see here and the CMakeLists require several steps to include all of the pinocchio library (include the abolute path of the openrobot/include directory, among other things).

It would be nice to have proper instructions that explain this step by step and facilitate the use of the robot_model library.

It could be useful to have certain getter methods for main model attributes, e.g. list of frames and joints, number of joints etc., such that the user can actually find out what frames he could use.

Or, getter method to the pinocchio::Model and pinocchio::Data of the RobotModel::Model. This way, everything could be accessed. Otherwise, the RobotModel::Model is kind of a black box

Check that the unit tests cover all of the function behaviours and produce the expected results.

Even if the build or make stages fail, the CI action "Build and Test" can still pass!

See for example this build step failure:
https://github.com/epfl-lasa/control_libraries/runs/2167083931?check_suite_focus=true#step:3:207

Followed by this test runner failure:
https://github.com/epfl-lasa/control_libraries/runs/2167083931?check_suite_focus=true#step:3:391

The reason for this is that the action only fails if the script has a non-zero exit code. While I was testing this, the make or make test command could fail entirely, causing the action to abort with a non-zero exit code. Now, the make stages can actually "finish" successfully even though they don't complete the build or some tests fail.

The fix is to add some extra logic to the workflow script to check the output of each stage.

As with https://github.com/eeberhard/docker-clion-cpp-env, configure a remote development docker container that allows development, debugging and testing of this library from within a host IDE such as CLion.

https://github.com/epfl-lasa/control-libraries/blob/main/source/controllers/src/impedance/Impedance.cpp
https://github.com/epfl-lasa/control-libraries/blob/main/source/controllers/src/impedance/CompliantTwist.cpp

There is inconsistency how the different controller handle the state member value force_ and torque_.
In the Impdedance controller, using the compute_command method, the force and torque of the state just gets passed on (if none is given to the feedback state, and hence there is are force / torque in the state_error).
Conversely, when using the CompliantTwist controller, force and torque from the initial state get removed through several typecasts CompliantState -> CompliantTwist. This is rather hard to detect, but leads to different behavior.

Question poses itself, if the force/torque should really be passed on like this. Since when obtaining a state from Franka, the gravity comp might already be included. If this is not set zero, before applying an (feedback) impedance controller it results in overcompensating the graivity, hence an instable controller.

The clproto header still has h file extension and I'd prefer that to be hpp. Obviously that is a breaking change and quite unimportant so maybe we'll do it before we do the next major release.

The Jacobian matrix is responsible for transforming CartesianState to JointState and vice versa. Therefore, it should have internally all the info for those conversion such as robot name, frame name and reference frame.

At the moment, this is not the case as it has only access to the robot name. Conversion from JointState to CartesianState is then made with hardcoding the name and reference frame of the state.

I would suggest we refine that by enforcing specifically that a Jacobian has a reference frame and is associated to a frame. We can even have operators with CartesianPose to handle the changing of reference frame.

In PR #153, from_std_vector has been deprecated which I believe is a great move. However, the data(std::vector) is not yet implemented everywhere. For example, Ellipsoid should also be changed to use this. For uniformity, this change should be propagated in all the classes that were implementing it.

When I run the install.sh script it doesn't work- it seems like it's not checking for existing packages that could be installed (specifically here, the osqp package). This could be an issue with my installation of osqp that I was trying unsuccessfully to build from source for another package, but it is unclear if its an issue with the osqp directory existing or if its the lack of cmake file. Here is what the error is outputing:

fatal: destination path 'osqp' already exists and is not an empty directory.
mkdir: cannot create directory ‘build’: File exists
fatal: destination path 'osqp-eigen' already exists and is not an empty directory.
mkdir: cannot create directory ‘build’: File exists
-- pinocchio FOUND. pinocchio at /opt/openrobots/lib/libpinocchio.so
-- boost_filesystem FOUND. boost_filesystem at /usr/lib/x86_64-linux-gnu/libboost_filesystem.so
-- boost_serialization FOUND. boost_serialization at /usr/lib/x86_64-linux-gnu/libboost_serialization.so
-- boost_system FOUND. boost_system at /usr/lib/x86_64-linux-gnu/libboost_system.so
-- Boost version: 1.65.1
-- Found the following Boost libraries:
--   filesystem
--   serialization
--   system
-- hpp-fcl FOUND. hpp-fcl at /opt/openrobots/lib/libhpp-fcl.so
-- Boost version: 1.65.1
-- Found the following Boost libraries:
--   chrono
--   serialization
--   system
CMake Error at robot_model/CMakeLists.txt:5 (find_package):
  By not providing "FindOsqpEigen.cmake" in CMAKE_MODULE_PATH this project
  has asked CMake to find a package configuration file provided by
  "OsqpEigen", but CMake did not find one.

  Could not find a package configuration file provided by "OsqpEigen" with
  any of the following names:

    OsqpEigenConfig.cmake
    osqpeigen-config.cmake

  Add the installation prefix of "OsqpEigen" to CMAKE_PREFIX_PATH or set
  "OsqpEigen_DIR" to a directory containing one of the above files.  If
  "OsqpEigen" provides a separate development package or SDK, be sure it has
  been installed.


-- Configuring incomplete, errors occurred!

We should have one or two high-level examples that shows how to install and plug together components of this library in a project.

Show a dynamical system + controller working a control loop.

This example could incorporate a simple external simulator / robot interface from a different repo. The main thing is that is it plug-and-play, "hello world" style, that shows the pieces all working together.

In the end someone outside the team should test the example and verify it makes sense and is easy to use.

The DS functions need some basic documentation to describe their behaviour. The usability and the effect of various parameters on the resultant dynamics should be very clear. This could include some equations and graphs.

First of all, the inverse function is not completely implemented (related to #134).

Then, the inverse function for all subtypes return a CartesianState. And the inverse for a CartesianTwist and CartesianWrench does not make sense because you'd always need a pose to do an inverse. So they should be deleted.

For the CartesianPose it should return a pose again.

We need a function that gives us the homogeneous transformation matrix from one frame to another directly. This would be a function similar to getTransformation(sourceFrame, targetFrame, jointPositions).

For the sake of having a CI that finishes successfully, we disabled the build of the pinocchio tests in #317.

Once we've figured out a faster / better way to build those images for both ARM and AMD architecture, we should re-enable the build of the tests.

As for v2.1.x I believe all the current implementation should be working as expected. There is one TODO that we left out for too long, the wrench computation in the multiplication operator of CartesianState. The problem is I am not so certain what would be the math needed to derive this operation.

In the reference for getFrameJacobian, it says the following:

You must first call se3::computeJointJacobians followed by se3::framesForwardKinematics to update placement values in data structure.

However, in the definition of Model::compute_jacobian, it does not follow this pattern.

// compute the jacobian from the joint state
  pinocchio::Data::Matrix6x J(6, this->robot_model_.nq);
  J.setZero();
  pinocchio::computeFrameJacobian(this->robot_model_, this->robot_data_, joint_state.get_positions(), frame_id,
                                  pinocchio::LOCAL_WORLD_ALIGNED, J);

I suspect that it this could still give valid results for frames that are also joints, but probably not for frames that are not joints. This needs fixing (would be a very small scope PR).

Forward position kinematics and inverse velocity kinematics are already implemented, inverse position kinematics is a TODO, and forward velocity kinematics is maybe nothing that needs to be done (TBD).

This is a minor issue discovered by @buschbapti. In C++, the constructors of CartesianState and its derived classes take a string argument called "reference_frame". However, in the Python bindings for these classes, the argument name was shortened to just "reference". This leads to the following behavior:

state_representation.CartesianPose(name="foo", reference_frame="bar")  # does not work!
state_representation.CartesianPose(name="foo", reference="bar")  # does work

Of course, the simple solution is to just treat these like the positional arguments that they are intended to be, instead of referring to them by name.

state_representation.CartesianPose("foo", "bar")  # works, and is the intended usage

Still, for the sake of the docstrings and for consistency against the C++ API, the argument name should be updated. It's a small change in the source code, but would still constitute a breaking change for any places where the keyword argument reference was used in Python usage of CartesianState and derived classes.

Discussed in #81

For now it is only possible to load the urdf if it is in a file and not if it is in a string like the one generated by the xacro files in ros.

My suggestion is that we use fstream to create a new file containing the urdf_string at a certain urdf_path (selected by the user to make the code OS independent) and than we load it in the same manner of before. The urdf is normally loaded in the constructor for this reason we will need a constructor taking also the urdf_string.

#include <fstream>

Model::Model(const std::string &robot_name, const std::string &urdf_path, std::string urdf_string="") ...

if (urdf_string==""){
  std::ofstream tempFile;
  tempFile.open(urdf_path);
  tempFile << urdf_string << std::endl;
  tempFile.close();
}

Due to the many changes on develop, the run scripts of the demos don't work anymore and have to be fixed for the release. I open this as an issue such that we can discuss how we want to do that.

1. We can just fix them by specifying the correct Dockerfile and everything else stays the same
2. We can use the ros workspace images that we host as base images and use aica-docker commands and style. That raises the question about how we can copy and install the source though
3. ...?
4. Host the ros workspaces that have all of control libraries (source + clproto) built and ready to use

Hey people, I noticed that we don't have yet the derivative of the Jacobian matrix, there is a pinocchio function to get it. I can do it quickly, but I don't have a software for testing it. Can someone of you create a test for it?
The pinocchio function is:

getFrameJacobianTimeVariation

P.S. I need it for a controller.

Check that the unit tests cover all of the function behaviours and produce the expected results.

See this issue. Fixed joints are not stored as joints but as frames in pinocchio, so the kinematrics methods in robot_model should be improved. Otherwise, it's not possible to use these methods for fixed joints (e.g. compute the jacobian for the last link of the franka).

Also, I opened an issue on pinocchio, see here. The inertial information of certain links is not available in the pinocchio robot model, which requires that certain information from the urdf file have to be passed to the executables explicitely (for example end effector tool mass and center of mass).

https://github.com/epfl-lasa/control-libraries/blob/main/source/controllers/src/impedance/Impedance.cpp

The CartesianState-Impedance controller method compute_command uses the method BaseQuaternon.vec() to calulcate the angular-error. However, this is just the vector part of the a quaternion Q but has does not represent a the rotation angle, see:
https://eigen.tuxfamily.org/dox/classEigen_1_1QuaternionBase.html#a91f93bde88f52796cfcd92c3594f39e5

The actual rotation angle can be calculated as:
rotvec = Q.vec() / sin(cos(Q.w)) * (2 * cos(Q.w))

Hence, it yields a different result as for example the scipy library, when using "Rotation.as_rotvec()"
https://docs.scipy.org/doc/scipy/reference/generated/scipy.spatial.transform.Rotation.html

So as I was trying to have a ROS node publishing and subscribing for a demo, I ran into the same issue as @patr3 (the one he never created an issue for 😄).
I'm not gonna copy the whole error message, but it's (normally) easily reproducible when checking out feature/ros-publisher-subscriber and try to run run-demo.sh in demos/ros_examples.

I mark this as a bug because I think it's important we solve this issue in the near future.

We have an intruder file test_dynamical_systems.cpp on board of the dynamical_systems library on the develop branch. I guess this guy lost its wagon and should be brought back to the tests folder with its fellow comrade.

When using in a code:
state_representation::JointPositions jp = state_representation::JointPositions::Random("myrobot", 7);
in the resulting random configuration it's considered the fact that the robot has a rigid body (so the joints will maintain a constant relative distance between them) but the rotation angles of the final configuration don't take into account the joint limit, risking compenetration between the parts.
At first sight, this could seem not to be a problem, because one will never start with a random configuration of the arm. Anyway, later on, an unprepared user could force the robot to go into positions that are not physically possible with the risk of creating potential damage.

At the moment, there is a possibility to create an empty Model in robot_model library which will result in errors when using the functions. This constructor should either be made private, removed or checking in functions with errors throwing included.

As highlighted by #35, the test coverage for state_representation is not sufficient. A CI workflow is also needed to ensure a smoother process.

The VelocityImpedance controller as used by the CartesianTwist is in reality a passive controller, this can be check by looking at the underlying math (des == desired; fb == feedback)
q_des = w_des * dt ; q_fb = 0

tau_ctrl = K(q_des - q_fb) + D (w_des - w_fb) = (K *dt + D) w_des - D
using new dynamics which are given by: hat{w}_des = (K *dt + D) / D w_des we can rewrite the control law as a simple passive one:
tau_ctrl = D (hat{w}_des - w)

I think using the simplified control law, the controller can be simplified (in the number of operations), hence made simpler for a user to understand as well as lower computational cost.
Additionally, reducing the redundant parameter K will allow easier tuning.

This one had me stuck for quite a while... I wanted to adapt the gains of a Linear DS after construction, so that I can change the behaviour at runtime. So I used the set_gain() function. For some reason, I was having linking errors when trying to do it.

The thing is, I knew my usage was correct - I added a test case to control_libraries to set the gain of a constructed Linear DS. There, it was working flawlessly. Then I had a thought. The only difference between the test case and my usage was that the library was built in Debug rather than Release. And, I saw the problematic function was specified inline even though it was implemented in the source file.

Steps to reproduce:

• Make a simple test executable that constructs a Linear ds and then sets the gains afterwards

#include <state_representation/space/cartesian/CartesianState.hpp>
#include <dynamical_systems/Linear.hpp>

int main(int, char**) {
  state_representation::CartesianState attractor("A");
  dynamical_systems::Linear<state_representation::CartesianState> linear(attractor);

  linear.set_gains(1.0);

  std::vector<double> gains = {1, 2, 3, 4, 5, 6};
  linear.set_gain(gains);

  return 0;
}

• Build control libraries dynamical systems with -DCMAKE_BUILD_TYPE=Release
• Build the test executable
• The linking will fail with undefined function Linear::set_gain() on both overloaded variants.
• Rebuild control libraries dynamical systems with -DCMAKE_BUILD_TYPE=Debug
• Rebuild the test executable
• The linking should succeed and the build completes.

My theory is that because these public functions are using the inline directive, even though they are defined inside the source file, the compiler optimises their definitions away. After all, if it's only being used locally in the source file, there's no point keeping the definition available to other translation units. In debug release that optimisation may not happen, so the function definition is still available during the linking step.

A few comments on the JointState refactoring PR have been left unresolved for a future PR.

https://github.com/epfl-lasa/control-libraries/blob/main/source/controllers/src/impedance/Impedance.cpp
https://github.com/epfl-lasa/control-libraries/blob/main/source/controllers/src/impedance/CompliantTwist.cpp

There is inconsistency how the different controller handle the state member value force_ and torque_.
In the Impdedance controller, using the compute_command method, the force and torque of the state just gets passed on (if none is given to the feedback state, and hence there is are force / torque in the state_error).
Conversely, when using the CompliantTwist controller, force and torque from the initial state get removed through several typecasts CompliantState -> CompliantTwist. This is rather hard to detect, but leads to different behavior.

Question poses itself, if the force/torque should really be passed on like this. Since when obtaining a state from Franka, the gravity comp might already be included. If this is not set zero, before applying an (feedback) impedance controller it results in overcompensating the graivity, hence an instable controller.

The copy construction between states is currently misleading as it copies all the state variables (calling the copy constructor of the base CartesianState or JointState class). Desired behavior would be that each derived copy constructor set to 0 the state variables that are not used in the derived class.

Because of the nature of the quaternion operation behind the operator+, the operation is not commutative which is slightly weird for an addition. It can lead to weird errors such as:

current_pose = dt * twist + current_pose;

will not lead to desired behavior when

current_pose = current_pose + dt * twist;

does (see #26). This can be a behavior we have to accept but then make super clear that there are differences between them.

When including #include <ros/ros.h> as first header at compilation I get this error:

/opt/openrobots/include/pinocchio/container/boost-container-limits.hpp:29:7: error: #error "BOOST_MPL_LIMIT_LIST_SIZE value is lower than the value of PINOCCHIO_BOOST_MPL_LIMIT_CONTAINER_SIZE"
   29 |     # error "BOOST_MPL_LIMIT_LIST_SIZE value is lower than the value of PINOCCHIO_BOOST_MPL_LIMIT_CONTAINER_SIZE"
      |       ^~~~~ 

I believe this is coming from the include of pinocchio that needs to happen first.

https://github.com/epfl-lasa/control-libraries/blob/main/source/controllers/src/impedance/Impedance.cpp

The CartesianState-Impedance controller method compute_command uses the method BaseQuaternon.vec() to calulcate the angular-error. However, this is just the vector part of the a quaternion Q but has does not represent a the rotation angle, see:
https://eigen.tuxfamily.org/dox/classEigen_1_1QuaternionBase.html#a91f93bde88f52796cfcd92c3594f39e5

The actual rotation angle can be calculated as:
rotvec = Q.vec() / sin(cos(Q.w)) * (2 * cos(Q.w))

Hence, it yields a different result as for example the scipy library, when using "Rotation.as_rotvec()"
https://docs.scipy.org/doc/scipy/reference/generated/scipy.spatial.transform.Rotation.html

Release 4.0 removes invalid multiplication operators from the CartesianState and its derived classes (#195). However, this is not that easy to achieve with the Python bindings. As of now, the following operations are possible in python:

state *= ANYTHING
state * ANYTHING

pose *= ANYTHING
pose * ANYTHING

Remember that the desired result would be

state *= state
state * ANYTHING

pose *= pose
pose * ANYTHING

Every time I launch a program that contains the robot_model library (so pinocchio is required) I receive this error:

error while loading shared libraries: libpinocchio.so.2.6.0: cannot open shared object file: No such file or directory

The workaround I found is to send these commands every time I open a new shell:

sudo find / -name libpinocchio.so.2.6.0
LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/path/to/libpinocchio.so.2.6.0
export LD_LIBRARY_PATH

Or, better, to directly write the last two lines in the .bashrc file