jrl-umi3218 / rbdyn Goto Github PK

If I'm not mis-understanding, code like

jacWork_[i] = proj*jac;
jacVec_[i].fullJacobian(mb, jacWork_[i], jacFull_);
cmMat_.block(0, 0, 6, mb.nrDof()) += jacFull_;

could turn into

        auto add_in = []( const MultiBody& mb, const std::vector<int>& jointsPath, Eigen::MatrixXd& cmMat, Eigen::MatrixXd& jac )
        {
            int jacPos = 0;
            for(std::size_t index = 0; index < jointsPath.size(); ++index)
            {
                int j = jointsPath[index];
                int dof = mb.joint(j).dof();
                
                cmMat.block(0, mb.jointPosInDof(j), jac.rows(), dof) += jac.block(0, jacPos, jac.rows(), dof);
                
                jacPos += dof;
            }
        };
        
        const std::vector<int>& jointsPath = jacVec_[i].jointsPath();
        jacWork_[i] = proj*jac;
        add_in( mb, jointsPath, cmMat_, jacWork_[i] );

doing so bumped my frame rate from about 7 to 20 fps for a 50 something dof multibody

The title at the top of the webpage should read:
RBDyn provides a set of classes and functions to model the dynamics of rigid body systems.

Is the python binding for computeH() and computeC() (alternatively the get functions H() and C()) tested correctly?
When I try to use it I get nonsense while using the same robot model in c++ seems to give the correct matrices.
Escpecially in the code below the same (incorrect) matrices are printed both before and after computeH() is called. The urdf model seems to have been parsed correctly with joints, bodies etc correct. Am I doing something wrong?

self.rbd = rbdyn.parsers.from_urdf(urdf_model)
self.rbd.mbc.zero(self.rbd.mb)
fd = rbdyn.ForwardDynamics(self.rbd.mb)
rbdyn.forwardKinematics(self.rbd.mb, self.rbd.mbc)
rbdyn.forwardVelocity(self.rbd.mb, self.rbd.mbc)
a = np.squeeze(fd.H())
print(a)
fd.computeH(self.rbd.mb, self.rbd.mbc)
a = np.squeeze(fd.H())
print(a)

I am having troubles in creating rotation matrix from VREP in accordance with SVA. I am using HRP4 in VREP simulator. I suspect the trouble is due to the reason that: In SVA, sin components of the rotation matrix(Rx, Ry, Rz) has "-"(minus) sign with it.

Rotation matrix from:(SVA)
hrp4.mbc().bodyPosW[hrp4.mb().bodyIndexByName("r_wrist")].rotation() ---> (1)

0.905646 0.0867599 0.415063
-0.0150835 0.984816 -0.172943
-0.423765 0.150365 0.893204

Rotation Matrix from VREP for R_WRIST_R:
simGetObjectQuaternion(simGetObjectHandle("R_WRIST_R"), -1,quat); // x,y,z,w
Eigen::Quaterniond eigen_quat(quat[3],quat[0],quat[1],quat[2]); // w,x,y,z
eigen_quat.matrix(); // produces the below matrix ----> (2)
0.423763 -0.0151001 0.905647
-0.150361 0.984815 0.0867758
-0.893206 -0.172947 0.415058

(1) is almost ~same as (2), but few rows and columns in (2) are interchanged. I tried to perform rotations in different RPY order along different axis. But still couldn't make (1) = (2). Inverting (2) didn't help as well. Is there any way to handle this? I am not able to do tasks in VREP, as there is a mismatch between the VREP rotation and SVA rotation.

For a Free base joint I noticed bodies fall in the opposite direction to gravity, easy to fix just change sign of gravity in ForwardDynamics::computeC() to

sva::MotionVecd a_0(Eigen::Vector3d::Zero(), -mbc.gravity);

then if I set gravity to (0, -9.81, 0) objects fall down the y-axis rather than up it

Hi,

So i bump into a problem. I somewhat messed up with an external file that has wrong body names in it.
Then, i wanted to get the index by using mb.bodyIndexByName(wrongName).
The thing is that the function actually returns a valid bodyIndex (body 0).

Is this the desired behaviour? I would rather prefer the program to throw me an error.

Thanks :)

Is quatToE() organized in wxyz or xyzw format? Can the docs/comments be updated to reflect this?

The new version of the Joint constructor takes an axis, but V-REP supplies a transformation matrix between joints. I believe this is because V-REP uses joint objects that are placed in the scene graph, and for revolute joints the axis is placed at the origin of the joint object around the Z axis. I think this is similar to how joints are defined in URDF files. Would simply converting the rotation component to angle axis form and using that axis as the Joint constructor parameter work?

To extend the constructor to take a full Eigen matrix converted from the one V-REP supplies, are there any gotchas, suggestions, or other example code I might find useful?

On Windows (I'm using MSVC v143 if this information is relevant), if set the cmake option BUILD_SHARED_LIBS to OFF, then the compilation would fail. The errors look like this:

After some debugging, I found the root cause is this macro block in RBDyn

and this macro block in RBDynParsers

are both NOT properly activated even if BUILD_SHARED_LIBS=OFF, because the macro RBDYN_STATIC and RBDYN_PARSERS_STATIC are never defined in cmake. This caused the __declspec(dllimport) Windows shared library directive to be appended in the compiled static library RBDyn.lib and RBDynParsers.lib, which caused the linker errors.

Making the following changes solved the issue, please review and PR the fix, thanks.

I'm hoping to clarify what the EulerIntegration step parameter means in the typical case, although I know many of the class and algorithm parameters are technically unitless and may work with other units.

Could it be considered a time step in seconds?

Hi all,

As stated in the title, I just want to make sure I will get the analytical Jacobian from the RBDyn, not the geometric Jacobian.

Assume the pose of the robot is given by $P = [x,y,z,r,p,y]^{T}$ (where r,p,y are Euler angles), is the Jacobian we get from RBDyn the same as: \partial{P}/\partial{t} (t is time?).

Many thanks in advance.

The RBDynParsers target should be added to the RBDyn target. target_link_libraries(RBDyn::RBDyn), should link libRBDyn.so and libRBDynParser.so

Which tag/commit of SpaceVecAlg was used, when releasing each RBDyn tag?

When I replaced the joints of the robot, I encountered a problem. That is, when I replaced the last fixed joint of a serial mechanical arm with a free joint, the calculation result of the inertial matrix of the robot was wrong, but the gravity matrix was correct. I checked the modified body chain and joint chain, and it is also correct.
What special treatment needs to be done after removing the joint? Any suggestion？ @gergondet

CMakeList.txt is missing find_package(Boost)

Is there anything available for serialization/deserialization of robots (MultiBodies), their configurations, their constraints, etc?

I encountered an offender today: fullJacobian expects to be given a res output parameter. This is not pythonic, and in most cases the output should be created on the fly.

This is of course low priority.

Hi! I met with some problems with the Center Of Mass Jacobian for a humanoid robot.

It is easy to use CoMJacobian to calculate the CoM Jacobian, but it uses more than 1.5 milliseconds on an I5 Intel processor, and I want it to be faster, so I want to speed up the calculation by generating the CoM Jacobian code like this library do:
https://github.com/cdsousa/SymPyBotics

rbt.kin.J[-1]
Matrix([
[0, 0],
[0, 0],
[0, 0],
[0, -cos(q1)],
[0, -sin(q1)],
[1, 0]])

Above is a two dof jacobian matrix. May I get a matrix like this, and I can use mbc.q to directly calculate the CoM Jacobian? If it is possible, how could I get the 3x18 CoM Jacobian matrix?

Thanks a lot!

How hard would it be to implement a function like the following?

GetTransformRelativeTo(multibodyGraph g, multiBodyConfig c, string objectname, string baseObjectName)

In V-REP I often use a function just like the above to get the transform from the frame of any object to any other. I'd love to have an equivalent of RBDyn, do you think this might be easy to implement?

Here is the equivalent V-REP function I use grl.getTransformBetweenHandles:

--- Get the pose of one object relative to another
---
--- @param objectpositionHandle The Object you want to get the position of
--- @param relativeToBaseFrameHandle The object or frame the position should be defined in, nil for world
--- @return position,orientation a 3 vector and quaternion value pair
grl.getTransformBetweenHandles=function(objectPositionHandle, relativeToBaseFrameHandle)
	local p=simGetObjectPosition(objectPositionHandle,relativeToBaseFrameHandle)
	local o=simGetObjectQuaternion(objectPositionHandle,relativeToBaseFrameHandle)
	return p,o
end

I am trying to do a set point test with left foot as base and right foot as end effector for HRP-4.
I am able to reach the set point (x,y,z,R,P,Y), only when I use the joints in right foot(by setting left foot joint columns to zero).
If I use the entire kinematic chain including the joints from left foot, I am not able to reach the set point. The legs make some very inappropriate motions.
Here is a little pseudo code:
hrp4_mb is a multi-body with left foot L_ANKLE_R_LINK as base body (With help from @gergondet @haudren )
hrp4_mbc is corresponding MultiBodyConfig

rbd::Jacobian jac_LF(hrp4_mb, "R_ANKLE_R");
const Eigen::MatrixXd& jac_matrix = jac_LF.jacobian(hrp4_mb, hrp4_mbc);
Eigen::MatrixXd J(6, hrp4_mb.nrDof());

jac_LF.fullJacobian(hrp4_mb, jac_matrix, J);
Eigen::Vector3d trans hrp4_mbc.bodyPosW[hrp4_mb.bodyIndexByName("R_ANKLE_R_LINK")].translation();
Eigen::Matrix3d rot hrp4_mbc.bodyPosW[hrp4_mb.bodyIndexByName("R_ANKLE_R_LINK")].rotation();

Eigen::VectorXd desired(6), current(6);
desired << R,P,Y,x,y,z,;
current << rot.eulerAngles(2,1,0),trans;

Eigen::Matrix6d GAIN_MATRIX;
velocityVector << GAIN_MATRIX * (desired - current)
Eigen::MatrixXd q_dot = pesudoInverse(J) * velocityVector;

while (timeStep < maxTimeStep)
{
	q = q + (0.05*q_dot);
	set new q to hrp4_mbc.q;
	rbd::forwardKinematics
	rbd::forwardVelocity
	get translation and rotation for right foot ("R_ANKLE_R_LINK")
	velocityVector_temp = GAIN_MATRIX *(desired - current_temp) // FK for rightfoot
	rbd::Jacobian
	get fullJacobian J_temp
	q = pesudoInverse(J_temp)*velocityVector_temp
	timeStep = timeStep + 0.05 // 0.05 to match VREP simulation
}

Can someone kindly point out the mistake I am doing here.

It has been noticed that while using jrl-umi3218/github-actions/build-package-native@master your other-gpg-keys 0x892EA6EE273707C6495A6FB6220D644C64666806 is present in plaintext. Please ensure that secrets are encrypted or not passed as plain text in github workflows.

Hello,

I am trying to use the Jacobian transpose to calculate the part of the robot joint torque that is caused by an external force applied on a robot body: tau = J^T f_{ext}. However, I am facing the problem that the computation results do not match with analogous computations via Inverse Dynamics (ID).

First, I compute robot torque via ID with no external force acting on any of the robot links and save the results:

// Robot configuration
Eigen::Vector3d jointPos(0.1, 0.2, 0.3);
Eigen::Vector3d jointVel(0.0, 0.1, 0.0);
rbd::vectorToParam(jointPos, robot_module->mbc.q);
rbd::vectorToParam(jointVel, robot_module->mbc.alpha);

// Update robot configuration
rbd::forwardKinematics(robot_module->mb, robot_module->mbc);
rbd::forwardVelocity(robot_module->mb, robot_module->mbc);

// Compute joint torque
rbd::InverseDynamics id(robot_module->mb);
id.inverseDynamics(robot_module->mb, robot_module->mbc);

// Save results
Eigen::Vector3d tauNoExtForce;
rbd::paramToVector(robot_module->mbc.jointTorque, tauNoExtForce);

Then, I reset the robot state and add an external force to one of the links, compute the joint torques via ID and save the result:

// Reset robot configuration
robot_module->mbc.zero(robot_module->mb);
rbd::vectorToParam(jointPos, robot_module->mbc.q);
rbd::vectorToParam(jointVel, robot_module->mbc.alpha);
rbd::forwardKinematics(robot_module->mb, robot_module->mbc);
rbd::forwardVelocity(robot_module->mb, robot_module->mbc);

// Add external force to a body
robot_module->mbc.force[linkIndex] = sva::ForceVecd(Eigen::Vector3d(0.,0.,0.), Eigen::Vector3d(0.0,0.,6.0));

// Compute joint torques
id.inverseDynamics(robot_module->mb, robot_module->mbc);

// Save results
Eigen::Vector3d tauWithExtForce;
rbd::paramToVector(robot_module->mbc.jointTorque, tauWithExtForce);

From these two ID results, I get that the portion of the joint torque caused by the external force is:

Eigen::Vector3d tauDiff = tauWithExtForce - tauNoExtForce;

Now, I try to compute the joint torques by multiplying the external force with Jacobian transpose of the link where force was applied

// Compute Jacobian of the link where external force was applied
rbd::Jacobian jac(robot_module->mb, linkName);
Eigen::MatrixXd jacMat = jac.jacobian(robot_module->mb, robot_module->mbc);
Eigen::MatrixXd fullJacMat(6, robot_module->mb.nrDof());
jac.fullJacobian(robot_module->mb, jacMat, fullJacMat);

// Compute joint torques generated by external force via Jacobian (tau = J^T f)
Eigen::MatrixXd extTauFromJac = fullJacMat.transpose() * robot_module->mbc.force[linkIndex].vector();

However, when I compare tauDiff and extTauFromJac, the results are very different (normally, they should be only differ in sign). Here is a sample output:

[info] Joint torque due to external force (from InvDyn difference):
[info] -1.21789e-17            0     -1.58771
[info] Joint torque due to external force (from Jacobian):
[info]        0 -1.58771        0

Could you please tell me what is the reason of this mismatch? Perhaps, I misunderstand something or maybe something is missing in my code? I attach here the zip of a small project containing all computations to reproduce these results: rbdyn_force.zip

Thank you in advance for helping to clarify this!

what are the advantages of this package?

I am trying to install RBDYN library so I can run the tutorials in python. Following is my system configuration:

Dell Alienware Area 51 Desktop
Ubuntu 16.04 LTS
cmake version 3.5.1
doxygen version 1.8.11
gcc (Ubuntu 5.4.0-6ubuntu1~16.04.10) 5.4.0 20160609
Cython version 0.28.5
libboost1.58-dev:amd64
#define EIGEN_WORLD_VERSION 3
#define EIGEN_MAJOR_VERSION 2
#define EIGEN_MINOR_VERSION 92
SpaceVecAlg Installed
Python 3.5.2

I have installed cython using pip3 install command. After completing the installation, when I import rbdyn I get the following error:

Traceback (most recent call last):
File "", line 1, in
File "/home/username/Programs/RBDyn/binding/python/rbdyn/init.py", line 1, in
from .rbdyn import *
ImportError: /home/username/Programs/RBDyn/binding/python/rbdyn/rbdyn.cpython-35m-x86_64-linux-gnu.so: undefined symbol: _ZN3rbd15InverseDynamics25sInverseDynamicsNoInertiaERKNS_9MultiBodyERNS_15MultiBodyConfigE

Please help.

Excuse me!
I use RBDyn to controll my humanoid robot.I set the torso as a floating base,add ground force,directly use ID to calculate the joint torque.But i derive also has the floating base joint 6 freedom torque,how i use this torque,because the floating base joint not actuators.
Could i director use ID to calculate the floating base robot's inverse dynamics?

make: *** No targets specified and no makefile found. Stop.
this error appears at almost all jrl libraries .and i can find any solution for it .
i was following this tutorial to setup JRL for implemntation at my humonoid
https://scaron.info/doc/lipm_walking_controller/build.html

any help , thanks in advance .

When creating models can joint limits be specified at the rbdyn level? I'm following the tutorial and I don't see a mention of them there or when I search the code for "limit".

Please remove the hard-coded add_library(SHARED) when building RBDyn and RBdyn parser library. User's should be able to specify the library type using -DBUILD_SHARED_LIBS=ON, this is the normal cmake convention.

In the FD and ID libraries, the fictitious acceleration due to gravity is set as follows:

sva::MotionVecd a_0(Eigen::Vector3d::Zero(), mbc.gravity);

However, in Roy Featherstone's book, p94, and p95 equations (5.15) (5.19), it uses a_0 = - a_g; just curious, why the libraries chose to reverse the sign?

It's a little confusing to users because we have to set mbc.gravity = {0, 0, 9.81} instead of mbc.gravity = {0, 0, -9.81}, which is what people would normally assume. Are there any benefits outweighing this confusion?

In Featherstone's book, there is two competing conventions for representing the translation part in planar joints and free joints.
Fig. 4.9 and table 4.1 (pp. 78-79) consider that the translations are expressed in successor frame (FS), but example 4.5 (p. 81) makes the case for having (translation) velocity variables expressed in FS but position variables expressed in parent frame (FP). This is to get better numerical stability while keeping simple motion subspace matrices S.

From the code of Joint::pose, planar joints are using the convention of Fig. 4.9, while free joints use that of example 4.5. I would push for having better coherency and change the convention of planar joint to the latter option: we would gain not only on the numerical behavior front, but also it is also, imho, more intuitive for the user to express the translation independently of the rotation.

In itself the change is drastic as it can break client code. In practice, it seems that the planar joint is not used much.
I changed locally Joint::pose for planar joints, and it didn't break any test. It seems to me that this is the only place the code need to change, but I don't know the whole library well enough, so there might be other places to amend. Maybe @jorisv has some advice on that.

It would be better to take a decision before addressing #35 to avoid additional work.

I'm interested in trying out the IDIM module on my robot arm. I looked at your papers and the comments and it seems an important aspect is dealing with the rubber feet on your bot. Can these algorithms be used for the dynamics of an arm without such a rubber component?

I'd normally just jump in and try things, but I also assume this will require physically moving the robot to make the estimates so I want to ensure I proceed safely. Any advice would be appreciated!

I know that rbd::Jacobian::fullJacobian columns are arranged in the order of rbd::MultiBody::jointPosInDof. Is it the same for CoM Jacobian rbd::CoMJacobian::CoMJacobian

Hello! Hope you're having a great week!

As the title indicated, I'm wondering if the Planar joint type only supports the default motion axes as listed in Prof. Featherstone's textbook P79, Table 4.1?

Since I noticed that there's this following part of code in src/RBDyn/RBDyn/Joint.h for constructing a Joint class:

inline void Joint::constructJoint(Type t, const Eigen::Vector3d & a)
{
  type_ = t;

  switch(t)
  {
    // ... input vector3d `a` for axis not used ...
    case Planar:
      S_ = Eigen::Matrix<double, 6, 3>::Zero();
      S_.block<3, 3>(2, 0).setIdentity();
      S_ *= dir_;
      params_ = 3;
      dof_ = 3;
      break;
    // ... input vector3d `a` for axis not used ...
  }
}

If that's the case, then, fixing it here other things should automatically work, is that right? Like other dynamics/kinematics computation would all be correct as long as the motion subspace is correctly configured?

Thank you very much in advance!

Following #33, integration for planar joints needs to be looked at

I might submit a pull request for an Eigen::Quaternion version of quatToE(). Would that be accepted and if so, where should I put it? (in case the current version depends on an old version of eigen without Quaternions.)

Hello,

A pain point in using SVA/RBDyn is that it is not very clear when and/or why one should use joint (or body) IDs over indexes. For example, even recently @stanislas-brossette found a bug related to computing a jacobian using an index rather than an id.

To alleviate the problem, I started to make id and index two separate classes using boost's strong_typedef macro on my fork. However, I am now wondering if IDs should be exposed in the API:

• The main reason to have IDs is to be able to convert configurations between different MultiBodies that have been generated from the same MultiBodyGraph.
• The Jacobian takes an ID as parameter but throws it away immediately
• Most Tasks take an ID to compute a Jacobian but throw it away similarly

Thus, I wonder if would not be simpler to simply drop the IDs from the public API and only expose the indexes.

@jorisv : Is there a fundamental design problem that I missed ?
@bchretien : I know you are using quite a bit of robots created with different roots, do you see any problem with the above ?

Cheers,
Hervé