Creating V-REP plugin for Tasks

[ahundt]

I'm planning to create a V-REP plugin for Tasks. Ideally my hope is to be able to V-REP scenes into tasks, run a solver, then update V-REP with the output. I'll explain my plan here, could you let me know if there are any gotchas or misunderstandings of the design of tasks?

The plugin will be in my grl library. I already wrote code implementing a similar plugin for another constrained optimization library. Since V-REP plugins are C/C++ and performance is a concern for me I plan to integrate directly using that API rather than via python.

My plan is to implement a minimal working example for my use case which I described in a jorisv/PG issue then expanding from there.

Minimal example:

1. assume a simple serial link arm + end effector with a fixed base of which there are two instances, the "measured arm" which reflects the state of a real physical arm's sensor data or a simulated version of it, another "planned arm" reflecting the solver results.
2. Init: create a multiBody passing in the names of each joint in V-REP, initially assume revolute joints.
3. Init: create a multiBodyConfig for the arm using the current joint angles.
4. Init?: create tasks objects for my use case of this solver?
5. simulation starts running.
6. At each time step of the simulator, get the current "measured arm" and update the multiBodyConfig with that information.
7. Run the solver with this updated data. (Can this happen in <50ms for a simple position task and a 7dof arm? Should there be a separate thread?)
8. Update the "planned arm" with the solver results.
9. w/ real arm just return, with pure simulation either assign "measured arm" to planned arm, (can rbdyn do this forward simulation step? perhaps other suggestions?).

Essentially, I expect to build the scene in v-rep, read and transfer the relevant configuration to tasks via the V-REP Plugin API, use tasks as the solver, then update V-REP with the results, which will also command an actual real physical arm when it is connected (I have another plugin that does this).

Questions:

• Allow configuration of other joints/tasks/solvers from V-REP native lua scripting APIs? How?
• Perhaps use slower V-REP remote (socket) python APIs for configuration and the tasks python APIs?
• Could python interaction somehow be directly integrated with the C++ plugin component at the initialization stage while still allowing high runtime performance?
• Can meshes be handled/transferred between V-REP and Tasks? If so, how?
• https://github.com/jrl-umi3218/RBDyn/issues/18

[haudren]

This plan does seem good. We have developed code that could help you, but unfortunately, it cannot be released.

1. Unfortunately Tasks documentation is sparse at most, but I'd say you'll need:
2. A posture task to target some joint angles with low priority. This is useful to avoid singularities.
3. A positionTask or orientationTask or transformTask with high priority to control the end effector.
4. Most definitely yes ! We use Tasks in the real-time control loop for our 38-DoF humanoid robot, and it runs in <5ms easily.
5. I'm not sure what you mean, but rbydn has a eulerIntegration method that will do a "forward" step.

For your questions:

• I never used the lua API
• We used successfully the remote APIs, but in C++
• I am not sure for the pybindgen bindings, but it works with our new bindings.
• Similarly, Tasks does not care about meshes... It only cares about the kinematics structure, and eventually collision shapes. They are convex hulls, usually generated using qconvex Those will then be interpreted into collision objects by https://github.com/jrl-umi3218/sch-core

[ahundt]

Thanks, first I'm working to load the robot model's joints and links from V-REP into Tasks for a 7 dof kuka iiwa 14 R820 arm. There is a model built into V-REP, plus another available in the iiwa_stack library.

Here is my initial pseudocode to initialize everything, hopefully it is reasonable!

The key V-REP functions for the initial setup of just the joint model are:

• simGetJointMatrix()
• simGetShapeMassAndInertia()
• simGetJointInterval()

Key Objects in Tasks/RBDyn:

• Tasks/tests/arm.h as a reference
• RBDyn/src/Joint.h Constructor for setup
  • To use this constructor I must convert the V-REP joint matrix to a translation and quaternion, which requires an answer to https://github.com/jrl-umi3218/RBDyn/issues/16 about the data layout assumed in the joint.h QuatToE() function. Maybe this uses the V-REP in memory layout since an API used with V-REP exists?
• RBDyn/src/Body.h

Pseudocode to construct the arm:

std::tuple<rbd::MultiBody, rbd::MultiBodyConfig> makeArm(bool isFixed=true, const sva::PTransformd X_base=sva::PTransformd::Identity()) First create a MultiBodyGraph mbg, joint vector jv, body vector bv.

For each joint (and link between joints) i:

• Setup Joint Body
  • Get the joint inertia matrix
  • convert the inertia matrix data memory layout (is this necessary?) see "regarding inertias" below (or use identity for very first implementation pass)
  • construct a body object b_i with joint link name from V-REP insert into bv
  • add body to mbg.addBody(b_i)
• Setup Joint Object (if the next one exists)
  • Get the joint matrix with simGetJointMatrix()
  • convert joint matrix to joint_translation and joint_quaternion
  • convert joint_quaternion to joint_axis per Joint constructor API
  • construct a revolute joint with joint_axis (assume forward == true for V-REP joints )
• Link Bodies with Joint if i >0
  • mbg.linkbodies(bv[i-1].name(),PTransformd(joint_translation),bv[i].name(),jv[i].name());

        MultiBody mb = mbg.makeMultiBody(bv[0], isFixed, X_base);`

    MultiBodyConfig mbc(mb);
    mbc.zero(mb);

    return std::make_tuple(mb, mbc);

Question Regarding Inertias:

• Will I have to reorganize the memory layout of the matrices between V-REP and RBDyn? I know the matrix layouts of V-REP and Eigen definitely differ substantially, for example.

[haudren]

Your plan does seem reasonable! We have somehting similar, but it is URDF-based.

Regarding your last point: yes, most probably.

• RBDyn uses Eigen as the underlying matrix engine, hence any differences in memory layout between V-REP and Eigen should be reflected.
• Note that RBDyn may also use different conventions. For inertia matrices, the two most common problems are that the inertia may be expressed at the Center of Mass of the link or at the joint origin. The latter is to be used with RBDyn. To convert between the two, you can use sva::inertiaToOrigin.

[ahundt]

Thanks I will keep that in mind. Right now I've got the initialization MultiBody and MultiBodyConfig set up.

At the moment I'm assuming that the joint transforms V-REP gives me are independent of the current joint configuration at this time step.

• How do I initialize the configuration to the current position of the arm, or is the best way to achieve that simply add a posture task for the current configuration with a low weight?
• Are there any constraints on when one should/should not be updating the config?
• In particular, once the system becomes more complex it likely won't be trivial to just edit q, is there a way to access values per id?

[haudren]

• To initialize the configuration the best (and only) way is to edit q. All of RBDyn is built around the concept of filling the various members of mbc and letting the algorithms do the rest of the work.
• You can update the config anytime except during the solving phase (actually during the problem construction phase): this is impossible in a single-threaded environment but if you use a multi-threaded environment I would recommend adding locks to prevent the mbc from being edited twice, and from being edited during solving.
• Yes, there is. The easiest way is to do something like:

for(std::string& name : vrepJointNames)
{
    int id = mb.jointIndexByName(name);
    value = jointAngleFromVrep(name);
    mbc.q[id][0] = value;
}

[ahundt]

Okay that makes sense, thanks. Also, for QPSolver, is it best to initialize a new one at each time step I'm solving the constrained optimization problem or is it safe to create a persistent one, update the configuration, and re-run it multiple times?

[haudren]

No you should most definitely keep it! A typical use case is to solve in a loop and integrate the solution to see how the robot would evolve if it followed perfectly the reference. This can be achieved by repeadtedly running this code:

solver.solveNoMbcUpdate(mbs, mbcs);
for(int i = 0; i < mbs.size(); ++i)
{
    solver.updateMbc(mbcs[i], i);
    rbd::eulerIntegration(mbs[i], mbcs[i], timeStep);
    rbd::forwardKinematics(mbs[i], mbcs[i]);
    rbd::forwardVelocity(mbs[i], mbcs[i]);
}

I think that if you replace eulerIntegration by a function that sets the configuration from the current robot state, you should be goood to go.

Note: the solver may fail is the state is not correctly updated. Most notably, if you use a ContactConstraint in a mode other than acceleration, it will fail if the speed of the contacting bodies is not 0. Similarly, any constraint that takes the timeStep as parameter (joint limits, collisions...) may provoke solver failure as their value depends on current q/alpha if the update yields incompatible speeds/positions.

[ahundt]

Okay, I'll probably need to update what I have a bit to reflect what you mentioned above.

Right now I'm starting with simple position control, leaving orientation alone. I set up the robot handles and transforms from V-REP in this block. Then on each time step of V-REP I create a new QP and initialize it with the current arm pose, computing the forward kinematics.

Unfortunately I must have a problem with configuring the transforms, because I'm using a kuka iiwa arm and I'm giving it a target pose of about 0.5 m in front of it (along the x axis) and 0.2 m up (z axis). This should be very reachable and I initialize with

However, the solver finds a nearly zero set of joint angles jointAngles_dt: 0.0663391426, 0, 0, 0, -3.84553692e-25, 0, 0.

Additionally, if I uncomment the code to set the joint limit constraints it crashes immediately.

Any advice to debug these problems? Perhaps I should create a simplified zxz scene first and get that working...

[haudren]

Nothing looks obviously out of kilter here but I have some suggestions:

• You solve only 10 steps, it would be interesting to check if joint angles change over more iterations.
• Check how the world positions evolve with this change of jointAngles. All world transforms can be found in mbc.bodyPosW. You can also check the value of posTask.eval().
• If possible, visualize it.

I have three ideas as to what to could be wrong:

• You do not actually set the correct objective: you use

tasks::qp::PositionTask posTask(rbd_mbs_, simulatedRobotIndex, ikGroupTipName_,getObjectTransform(ikGroupTipHandle_).translation());

But this is simply a task to maintain position if I understand well. You should use your target position as the last argument to PositionTask.

• Transforms are given in the successor frame, which usually involves inverting the rotation given by V-REP/ROS/... (i.e. pt = sva::PTransformd(vrepRot.inverse(), vrepTrans)).
• Using the z-axis for all joints may have been an error, depending on what V-REP is really giving you.

For the failure when using limits: it crashes ? I.e. segfault ? Or solver failure ? For the first case, I'd say your problem is that your vector of limits is not properly constructed: it should contain empty vector for fixed joints. Check that your lower/upper bound are the same size as you vector q.

Example, for a fixed base, 3 joints, one fixed joint robot:

lbound : [[], [-1], [-1], [-1], []]
ubound : [[], [1], [1], [1], []]

You can check that your bounds are correct w.r.t your MultiBody by calling:

sParamToVector(mb, lbound)

It will throw with an explanation of where there is a mismatch.

[haudren]

Note that this error is also present on line 399:

rbd_mbcs_[0].q[i]={currentJointPosVec[i]};

Should probably be:

rbd_mbcs[0].q[mb.jointIndexByName(jointNames_[i])] = {currentJointPosVec[i]};

Moreover, if your objective is solely to compute Inverse Kinematics, you can probably use a pure RBDyn-based method (as presented in the tutorials, or in the simple IK that comes with RBDyn).

[ahundt]

I'm planning to do more than simple inverse kinematics, just trying to start simple. I fixed the goal passed into the task to be the actual tip target now. :-)

I've also asked how the transforms are structured on the V-REP forum. Hopefully that can help clarify the details if I hear back from them.

I plotted some poses as V-REP dummy objects as well, and it is clear that the transforms aren't being converted correctly between V-REP and Tasks. I need to look more carefully into what I'm loading, it may be the successor/predecessor relationship you describe, a row/column ordering issue, or something else. I'm in the process of debugging it.

I'm also getting this odd behavior where running the solver always produces the same joint angles, even if the target translation changes:

[2016-11-02 22:44:57.009632] [0x00007000005b2000] [trace]   jointAngles: 0.0663391426, 0.521225393, -0.0590483434, -1.33356845, -0.0266402438, 1.11842847, -0.0145333242
[2016-11-02 22:44:57.072251] [0x00007000005b2000] [trace]   target translation (vrep format):
 0.200361
-0.700025
  1.03355
[2016-11-02 22:44:57.073910] [0x00007000005b2000] [trace]   jointAngles: 0.0663391426, 0.521225393, -0.0590483434, -1.33356845, -0.0266402438, 1.11842847, -0.0145333242
[2016-11-02 22:44:57.155098] [0x00007000005b2000] [trace]   target translation (vrep format):
 0.201731
-0.695684
   1.0333

Even if the arm description isn't correct, shouldn't I be able to expect these values to change as they go through the loop? I did remove the eulerIntegration() step however, and I belive I have dynamics enabled in V-REP so in that case perhaps I do need the eulerIntegration() step to run.

[ahundt]

I've visualized the data and it looks like there is at least one mismatch between the world frames and the local frames. Could you advise me on where my initialization is going wrong?

Perhaps the way I'm initializing X_base is part of the problem, but it doesn't quite explain the way world coordinates entities are broken.

What I did is initialize the MultiBodyConfig with all joints at the 0 position, then try setting a bunch of dummy node (the yellow spheres) positions using the data from two arrays of member variables in MultiBodyConfig:

MultiBodyConfig::parentToSon() Locally correct

In the first one, I set the transforms all locally correct with respect to the first sphere via parentToSon() in my construct() header. This looks OK locally! Notice how it goes straight up just like the straight up arm.

MultiBodyConfig::bodyPosW() World Frame incorrect

When I only set the poses of each the world frame seems completely crazy using entries from bodyPosW() in my construct() header. You can't see the order from the pictures, but these dummies are jumping back and forth + up and down, you can see it in the actual printout of the frames for each joint below.

Here are the actual frames as set in the world coordinate system with bodyPosW():

           0            0            0            1
[2016-11-03 16:59:28.080048] [0x0000700000635000] [trace]   Dummy0 
3.9811e-07         -1          0     1.4059
         1 3.9811e-07          0 -0.0657862
         0          0          1   0.988837
         0          0          0          1
[2016-11-03 16:59:28.080189] [0x0000700000635000] [trace]   Dummy1 
-3.55963e-07 -3.42286e-08           -1      -1.4059
          -1 -4.21468e-08  3.55963e-07     0.988837
-4.21468e-08            1 -1.34359e-07     0.141714
           0            0            0            1
[2016-11-03 16:59:28.080302] [0x0000700000635000] [trace]   Dummy2  
4.82404e-07           -1   1.0013e-07       1.4059
           1  4.82403e-07 -8.42937e-08     0.362714
 8.42937e-08  -1.0013e-07            1     0.988837
           0            0            0            1
[2016-11-03 16:59:28.080437] [0x0000700000635000] [trace]   Dummy3 
-5.03477e-07  2.02816e-07            1      -1.4059
          -1  2.10733e-08 -5.03477e-07    -0.988837
-2.10734e-08           -1  3.02946e-07    -0.163714
           0            0            0            1
[2016-11-03 16:59:28.080549] [0x0000700000635000] [trace]   Dummy4  
5.87771e-07           -1 -1.68587e-07       1.4059
           1  5.87771e-07 -2.10733e-08   -0.0157859
 2.10735e-08  1.04348e-14            1     0.988837
           0            0            0            1
[2016-11-03 16:59:28.080666] [0x0000700000635000] [trace]   Dummy5 
-5.03477e-07 -3.42286e-08           -1      -1.4059
          -1 -2.10734e-08  5.03477e-07     0.923137
-2.10735e-08            1 -1.34359e-07     0.204714
           0            0            0            1
[2016-11-03 16:59:28.080794] [0x0000700000635000] [trace]   Dummy6 
 5.87771e-07           -1   1.0013e-07       1.4059
           1  5.87771e-07 -2.10735e-08     0.280814
 2.10735e-08 -2.68718e-07            1     0.988838
           0            0            0            1

[ahundt]

Also, in my case Z is up. Is that potentially part of the problem?

[ahundt]

@haudren Perhaps you may have been mistaken about the inverse?

Transforms are given in the successor frame, which usually involves inverting the rotation given by V-REP/ROS/... (i.e. pt = sva::PTransformd(vrepRot.inverse(), vrepTrans)).

I tried this change simply eliminating all inverses and the locations of the dummy nodes look dead on (some are inside the arm so not visible):

[ahundt]

Actually while closer, this is still not correct... still debugging...

[haudren]

Ok, it does look much better. Sorry about this, but it is a common problem we have with a lot of users. You can also try simply inverting the rotation for the base transform, it may be the cause of your problems.

Note that without calling forwardKinematics with a properly initialized mbc.q all entries will be garbage.

[ahundt]

Are you suggesting adding X_base.rotation() = X_base.inv().rotation();?

[haudren]

Yes indeed. However, I don't think you can do this with transforms:

X_base = sva.PTransformd(X_base.rotation().inverse(), X_base.translation());

Should do the trick

[ahundt]

That unfortunately didn't seem to do the trick, but I have a bit more debug data (without the inversion discussed above) printed from this revision. Here is the image:

I created dummy nodes to debug the and Initialized them as follows:

• Dummy1x is the Joint Handle frame stored by V-REP for joint x with the arm at all 0 joint angles.
• Dummy0x is the world frame for joint x supplied after initializing std::vector<rbd::MultiBodyConfig> rbd_mbcs_ and std::vector<rbd::MultiBody> rbd_mbs_.

These are the comparable DummyYX variables, where Y=0 means an RBDyn frame and Y=1 means a V-REP frame, all in world coordinates.

Joint 1

For the first frame the transforms are equal, in other words they match and both appear to be correct:

[2016-11-04 04:45:58.530351] [0x0000700000635000] [trace]   Dummy11 V-REP
5.39991e-07          -1           0   0.0308969
          1 5.39991e-07           0    -1.34075
          0           0           1    0.988837
          0           0           0           1
[2016-11-04 04:45:58.532756] [0x0000700000635000] [trace]   Dummy01 RBDyn
3.9811e-07         -1          0  0.0308977
         1 3.9811e-07          0   -1.34075
         0          0          1   0.988837
         0          0          0          1

Joint 2

Here is the first frame where they differ, the rotation is off. Converted to RPY format for the rotation this is V-Rep (0,-90,+90) while RBDyn (-90,0,90).

[2016-11-04 04:45:58.530550] [0x0000700000635000] [trace]   Dummy12 V-REP
-5.96047e-07 -1.19209e-07           -1    0.0308969
          -1 -1.19209e-07  4.76837e-07     -1.34075
-2.84217e-14            1 -2.38419e-07      1.19634
           0            0            0            1
[2016-11-04 04:45:58.532864] [0x0000700000635000] [trace]   Dummy02 RBDyn
-4.40257e-07            1 -4.21468e-08    0.0308977
 3.42285e-08  4.21469e-08            1     -1.34075
           1  4.40257e-07 -1.34359e-07      1.19634
           0            0            0            1

Joint 3

Third Frame Happens to match up again:

[2016-11-04 04:45:58.530739] [0x0000700000635000] [trace]   Dummy13 V-REP
 5.39991e-07           -1 -4.21469e-08    0.0308969
           1  5.39991e-07 -4.21468e-08     -1.34075
 4.21469e-08 -4.21468e-08            1      1.41734
           0            0            0            1
[2016-11-04 04:45:58.532980] [0x0000700000635000] [trace]   Dummy03 RBDyn
  4.6133e-07           -1 -1.05367e-07    0.0308977
           1   4.6133e-07  -1.0013e-07     -1.34075
 -1.0013e-07 -1.05367e-07            1      1.41734
           0            0            0            1

Joint 4 RPY RBDyn (+90,+0,-90) and V-REP (0,+90,-90) note this is similar to Joint 2:

[2016-11-04 04:45:58.530917] [0x0000700000635000] [trace]   Dummy14 V-REP
-7.15256e-07  2.68221e-07            1    0.0308968
          -1 -1.19209e-07 -5.66244e-07     -1.34075
-2.98024e-08           -1  1.19209e-07      1.61634
           0            0            0            1

[2016-11-04 04:45:58.533097] [0x0000700000635000] [trace]   Dummy04 RBDyn
-5.45624e-07            1  1.05367e-07    0.0308976
-2.02816e-07  1.05367e-07           -1     -1.34075
          -1 -5.45624e-07  3.02946e-07      1.61634
           0            0            0            1

Joint 5

Third Frame Happens to match up again:

[2016-11-04 04:45:58.531090] [0x0000700000635000] [trace]   Dummy15 V-REP
5.39991e-07          -1           0   0.0308968
          1 5.39991e-07           0    -1.34075
          0           0           1     1.79584
          0           0           0           1
[2016-11-04 04:45:58.533228] [0x0000700000635000] [trace]   Dummy05 RBDyn
 5.45624e-07           -1 -2.10735e-08    0.0308978
           1  5.45624e-07   8.6584e-15     -1.34075
 1.68587e-07 -2.10733e-08            1      1.79584
           0            0            0            1

Joint 6

First translation divergence, rotations also diverge: RPY V-REP (0,-90,-90) RBDyn (-90,0,-90)

[2016-11-04 04:45:58.533346] [0x0000700000635000] [trace]   Dummy06 RBDyn
-5.45624e-07            1 -6.32203e-08   -0.0348023
 3.42285e-08  6.32203e-08            1     -1.34075
           1  5.45624e-07 -1.34359e-07      2.01634
           0            0            0            1
[2016-11-04 04:45:58.531248] [0x0000700000635000] [trace]   Dummy16 V-REP
-5.96047e-07 -1.19209e-07           -1     0.096597
          -1 -5.68434e-14  5.96046e-07     -1.34075
-3.55271e-14            1 -1.19209e-07      2.01634
           0            0            0            1

Joint 7

Translations happen to match up again:

[2016-11-04 04:45:58.531470] [0x0000700000635000] [trace]   Dummy17 V-REP
5.39991e-07          -1           0   0.0308968
          1 5.39991e-07           0    -1.34075
          0           0           1     2.09244
          0           0           0           1
[2016-11-04 04:45:58.533465] [0x0000700000635000] [trace]   Dummy07 RBDyn
 5.45624e-07           -1 -2.10735e-08     0.030898
           1  5.45624e-07 -2.68718e-07     -1.34075
 -1.0013e-07 -2.10735e-08            1      2.09244
           0            0            0            1

So you mentioned that sometimes the first joint is what's off? Since a few of the joints in the middle happen to match up I think it is a slight difference in the way the transforms are being specified. Perhaps an Axis difference, Joint direction, both? Switching the joint direction didn't seem to make a difference, I tried that before as well.

I'll also try to get each transform relative to the previous joint for both V-REP and Tasks. That may finally make the exact difference obvious.

[ahundt]

Okay I believe this change outputs the relative frames, I know in V-REP these are each joint i+1 in the frame of joint i. In RBDyn, where I'm less certain, each frame is the contents of rbd_mbcs_[simulatedRobotIndex].parentToSon[...] converted into a 4x4 transform.

note: this picture is the same as the post above, there were no changes that should affect it

Base to Joint 1 note there is an additional world to base step not printed here

[2016-11-04 05:32:58.966386] [0x00007000006b8000] [trace]   Dummy11 V-REP JointInPrevFrame
2.58279e-05           1           0  -0.0808974
         -1 2.58279e-05          -0   0.0157479
         -0           0           1    0.148837
          0           0           0           1
[2016-11-04 05:32:58.970582] [0x00007000006b8000] [trace]   Dummy01 RBDyn ParentLinkToSon
2.58279e-05           1           0  -0.0808974
         -1 2.58279e-05           0   0.0157479
          0           0           1    0.148837
          0           0           0           1

Joint 1 to Joint 2

[2016-11-04 05:32:58.966683] [0x00007000006b8000] [trace]   Dummy12 V-REP JointInPrevFrame
          -1 -4.21469e-08 -4.21468e-08            0
-4.21469e-08  3.42285e-08            1            0
-4.21468e-08            1 -1.34359e-07       0.2075
           0            0            0            1
[2016-11-04 05:32:58.970753] [0x00007000006b8000] [trace]   Dummy02 RBDyn ParentLinkToSon
          -1 -4.21469e-08 -4.21468e-08            0
-4.21469e-08  3.42285e-08            1            0
-4.21468e-08            1 -1.34359e-07       0.2075
           0            0            0            1

Joint 2 to Joint 3

[2016-11-04 05:32:58.966984] [0x00007000006b8000] [trace]   Dummy13 V-REP JointInPrevFrame
          -1 -1.47514e-07  2.10734e-08            0
 2.10734e-08  3.42285e-08            1        0.221
-1.47514e-07            1 -1.34359e-07 -5.96046e-08
           0            0            0            1
[2016-11-04 05:32:58.970944] [0x00007000006b8000] [trace]   Dummy03 RBDyn ParentLinkToSon
          -1 -1.47514e-07  2.10734e-08            0
 2.10734e-08  3.42285e-08            1        0.221
-1.47514e-07            1 -1.34359e-07 -5.96046e-08
           0            0            0            1

Joint 3 to Joint 4

[2016-11-04 05:32:58.967378] [0x00007000006b8000] [trace]   Dummy14 V-REP JointInPrevFrame
          -1 -8.42937e-08  1.06581e-14            0
 1.06581e-14 -3.02946e-07           -1            0
 8.42937e-08           -1  2.02816e-07        0.199
           0            0            0            1
[2016-11-04 05:32:58.971177] [0x00007000006b8000] [trace]   Dummy04 RBDyn ParentLinkToSon
          -1 -8.42937e-08  1.06581e-14            0
 1.06581e-14 -3.02946e-07           -1            0
 8.42937e-08           -1  2.02816e-07        0.199
           0            0            0            1

Joint 4 to Joint 5

[2016-11-04 05:32:58.967740] [0x00007000006b8000] [trace]   Dummy15 V-REP JointInPrevFrame
          -1 -8.42937e-08  7.10543e-15            0
 7.10543e-15 -3.02946e-07           -1      -0.1795
 8.42937e-08           -1  3.42285e-08  -8.9407e-08
           0            0            0            1
[2016-11-04 05:32:58.971392] [0x00007000006b8000] [trace]   Dummy05 RBDyn ParentLinkToSon
          -1 -8.42937e-08  7.10543e-15            0
 7.10543e-15 -3.02946e-07           -1      -0.1795
 8.42937e-08           -1  3.42285e-08  -8.9407e-08
           0            0            0            1

Joint 5 to Joint 6

[2016-11-04 05:32:58.968060] [0x00007000006b8000] [trace]   Dummy16 V-REP JointInPrevFrame
          -1            0 -8.42937e-08            0
-8.42937e-08 -1.34359e-07            1   -0.0657001
           0            1 -1.34359e-07       0.2205
           0            0            0            1
[2016-11-04 05:32:58.971629] [0x00007000006b8000] [trace]   Dummy06 RBDyn ParentLinkToSon
          -1            0 -8.42937e-08            0
-8.42937e-08 -1.34359e-07            1   -0.0657001
           0            1 -1.34359e-07       0.2205
           0            0            0            1

Joint 6 to Joint 7

[2016-11-04 05:32:58.968403] [0x00007000006b8000] [trace]   Dummy17 V-REP JointInPrevFrame
          -1 -8.42937e-08            0            0
           0 -1.34359e-07            1    0.0761001
-8.42937e-08            1 -1.34359e-07    0.0657003
           0            0            0            1
[2016-11-04 05:32:58.971856] [0x00007000006b8000] [trace]   Dummy07 RBDyn ParentLinkToSon
          -1 -8.42937e-08            0            0
           0 -1.34359e-07            1    0.0761001
-8.42937e-08            1 -1.34359e-07    0.0657003
           0            0            0            1

As you can see these matrices are essentially identical in all cases, I guess this mean it comes down to a difference in how the multiplications are carried out for the forward kinematics? At this point I'm very uncertain of the next debug step considering that the world frames match up at several, but not all joints in this simple case with zero joint angles, but the frame to frame matrices seem to be identical.

I've also checked that my code to convert an Eigen::Affine3d to sva::PTransformd gives me the same matrix back after a round trip. I'm not certain what else might do the trick. @gergondet do you perhaps have any advice?

[haudren]

(Sorry it's Friday evening, I am in a bit of a hurry) What I am expecting is one of two cases:

• Either all the world transformations match
• Either all the world translations match, and all rotations are inverted (seems to be the common case).

I'll try looking a bit more into it, but it could be that you have to:

• Invert the rotations when creating the joints. Thus the parentToSon rotations will no longer match
• Invert the rotations when displaying them in V-REP.

Note that the way that transformations are composed in ForwardKinematics is simply to left-multiply the parentToSon together.

X_0^(n+1) = parentToSon(n)*X_0^n

[ahundt]

Okay, thanks! Don't worry if you're busy at the moment.

• Here is a commit that applies the inverse
  • In this commit I apply an inverse to the rotation component whenever switching from eigen to sva::PTransformd or when I'm switching back.
• github gist containing updated equivalent V-REP and RBDyn transorm world and joint-to-joint outputs with inverse() applied.
• V-REP simulation I'm using (tool is removed but otherwise the same): RoboneSimulation.ttt.zip

All the positions match up now but the world orientations differ, and if I try rotating joints the forward kinematics don't match up (tool is removed but otherwise the same):

I need to try another commit that tries converting to PTransformd via rpy format, though since I'm doing actual robot motion with these transforms rather than just visualization rpy could be vulnerable to gimbal lock aka singularities.

[haudren]

It seems to me that all you matrices are the same in your gist, but I may just be tired... I'll come back to you on those issues during the week-end or on Monday at the latest!

[ahundt]

That does look to be the case for the parent to son vs the link in prev link frame comparisons between V-REP and RBDyn.

Here is what's happening with links to the code for the RBDyn portion:

1. Run the RBDyn forwardKinematics() call
2. Get the PTransformd out in world coordinates and prev link coordinates
3. Converting world coords to an eigen::Affine3d and prev link to an eigen::Affine3d
4. Then printing out the world matrix and link to link matrix for each affine3d.

For the V-REP portion it is a touch simpler, though neither is very complex:

1. Get each joint in the world frame or get each joint in the previous joint frame
2. print joint to joint and print world to joint

I think the conversions between V-REP and Eigen::Affine3d in my code are good to my knowledge, since I've been using both together for some time without issue, and originally had to go through a debugging process like this to get there. The problem may be in my functions that convert between my Eigen coordinates to SpaceVecAlg / RBDyn / Tasks coordinates, and the setup of the axes of rotation. I'm taking a look at the Rigid Body Dynamics book to see if I can figure out how they are organized differently.

[ahundt]

NOTE: Corrections made

Joint alignment problem appears to be fixed:

Okay I've made another big leap! The joints now appear to line up so far with this commit. I do the inverses you describe, and something similar to the inverse commit in my previous post, but I put the rotations in a Quaternion before performing the inverse. Now every joint seems to align at the 0 joint angle!

New Joint angle and indexing problem:

In RBDyn is each joint expected to be at the current pose, then the link portion is after the joint, or is that configurable based on how MultiBodyGraph:::linkBodies is called?

I then tried starting at my initial starting joint angle, that's where you see the second robot in all the pictures. Unfortunately, there is a new problem, though it is much smaller. It seems the joint angle is rotating the joint previous to the one I desire to be rotated, rather than the one I'd like rotated, with updating the next index as shown in this commit as a partial workaround. All the poses look right except the first joint, which I skipped because of this offset problem. Fortunately the angle is very small for joint 1 (.06 radians) so it is easy to tell everything else is matching up closely. Here is an image, where you can see two frames at every joint.

Each Dummy and triad's arrows corresponds to RBDyn or V-REP as follows:

• V-REP - aligned with the previous link's body (before joint rotation is applied)
• RBDyn - aligned with the next link's body (after joint rotation is applied)

I suspect I can fix this problem by modifying the way I call MultiBodyGraph::linkBodies, but I'm not certain. Another possible alternative might be to weirdly rename all of the bodies to be named after the joint before all the transforms, and Perhaps on the first joint I'll have to add a dummy transform so that the names and effects of applying joint angles lines up with V-REP.

[ahundt]

It looks like RBDyn gives transforms after the joint rotation is applied, while V-REP gives transforms before.

Also, it looks like the indices of the RBDyn transforms are definitely off by one somehow, I'm still trying to debug if my initialization is the problem, it is simply a convention difference, or something else.

[ahundt]

There is definitely some sort of indexing problem, but I'm not sure why. I've not ruled out the possibility of this being an issue in RBDyn, though most likely it is my code. I have a hack that looks like it works around the problem, so I'll partially move on to the next problem and partially try to address the outstanding indexing problem.

Running the solver

So, I try to apply the same index hacks when I try to use the solver to get to a simple target pose. I will be adding more complicated functionality once the basics work. This is what happens at each time step after the initialization is complete:

1. I set the current position in the mbcs variable with the same index hack detailed above
2. I create a SetPointTask.
   • Am I correct feeding this a translation coordinate in the world frame?
   • All my masses are currently junk values (1kg, no translation/rotation), is that ok?
3. I add it to the solver
4. I run the solver
5. If I call EulerIntegration() here the arm just jumps around like crazy, if I don't it does not move at all.
   • I'm trying to move to a target 10-20 cm away well inside the arm's workspace.
   • I currently have eulerIntegration() commented, but can the solver results even be utilized without that call? If so, how? The test examples don't seem to show such a use case.
6. I call forwardKinematics() and forwardVelocity()
7. I update the simulation position, though this currently has no effect, see step 5.

Resolving the joint angle indexing off by 1 error

I link bodies in a super straightforward way as follows:

            for(std::size_t i = 0; i < rbd_bodyNames_.size()-1; i++)
            {
                sva::PTransformd to(getObjectPTransform(rbd_jointHandles_[i+1],rbd_jointHandles_[i]));
                sva::PTransformd from(sva::PTransformd::Identity());

                std::string prevBody = rbd_bodyNames_[i];
                std::string curJoint = rbd_jointNames_[i];
                std::string nextBody = rbd_bodyNames_[i+1];

                rbd_mbg_.linkBodies(prevBody, to, nextBody, from, curJoint);
            }

Then I try to access the joint names after computing forward kinematics, and when I visualize the data I have to offset by 1 for the frames to be correct:

            for (std::size_t i = 0; i < jointHandles_.size(); ++ i)
            {
                /// @todo TODO(ahundt) FIXME JOINT INDICES ARE OFF BY 1, Manually SETTING FIRST JOINT
                std::string jointName = jointNames_[i];
                std::size_t jointIdx = simArmMultiBody.jointIndexByName(jointName);
                if(i<jointHandles_.size()-1) rbd_mbcs_[simulatedRobotIndex].q[jointIdx][0] = initialJointAngles[i+1]; // should be initialJointAngles[i]
                simSetJointPosition(jointHandles_[i],initialJointAngles[i]);
            }
            std::string jointName = rbd_jointNames_[0]; /// @todo TODO(ahundt) HACK: This gets one joint after what's expected
            std::size_t jointIdx = simArmMultiBody.jointIndexByName(jointName);
            rbd_mbcs_[simulatedRobotIndex].q[jointIdx][0] = initialJointAngles[0];

These hacks get me good output poses for each frame given a set of input joint angles q to rbd_mbcs_ (a multibody vector).

[haudren]

I'll try to answer your concerns. First the "off-by-one" error: when you create a MultiBody from a MultiBodyGraph, a "root" joint is added as first joint. In your case, it is a fixed joint, but for a free-floating robot (for example humanoid) it would be a free joint. I would suspect that this is the source of your problems. Similarly, note that the order of the joints in a MultiBody may not be the same as the order in which you added joints to the MultiBodyGraph. This is why you should rely on the joint name to get the correct indices.

When you link bodies you will have this:

//         to              from 
// body --------> joint ---------> body

Meaning that if you set "from" to identity, the joint will be at the same position as the next body. This is the convention in all of our programs, and in URDFs. If you want to set a joint to be at the same position as the current body, simply swap "to" and "from" transforms.

Now onto the eulerIntegration problem: when you call solve(), the solver computes the optimal joint acceleration. So if you don't integrate it, the joint angles will not change. What is weird is that your joint angles jump all over the place, especially if you have only one task.

PositionTask does expect a world frame position. If your masses are really junk it may explain why the arm is jumping around.

However, I think the main point you are lacking is that your problem does not have a MotionConstr... Try adding a tasks::qp::MotionConstr with infinite torque limits, it should help!

[ahundt]

Whoops I was missing a line from the real code in my previous post where I initialized the to variable when I link bodies.

Off by 1 joint index

This is why you should rely on the joint name to get the correct indices.

Sorry I'm a bit confused isn't using the joint names how this code below is working? Maybe there is a different way I should be accessing it? I thought I was following the example you gave me above correctly. (except for the off by one fix)

/// @todo TODO(ahundt) FIXME JOINT INDICES ARE OFF BY 1, Manually SETTING FIRST JOINT
std::string jointName = jointNames_[i];
std::size_t jointIdx = simArmMultiBody.jointIndexByName(jointName);
 if(i<jointHandles_.size()-1) rbd_mbcs_[simulatedRobotIndex].q[jointIdx][0] = initialJointAngles[i+1]; // should be initialJointAngles[i]

Then setting the values uses the joint names as well:

/// @todo TODO(ahundt) FIXME JOINT INDICES ARE OFF BY 1, Manually SETTING FIRST JOINT
std::string jointName = jointNames_[i];
std::size_t jointIdx = simArmMultiBody.jointIndexByName(jointName);
if(i<jointHandles_.size()-1) rbd_mbcs_[simulatedRobotIndex].q[jointIdx][0] = initialJointAngles[i+1]; // should be initialJointAngles[i]
simSetJointPosition(jointHandles_[i],initialJointAngles[i]);

I have similar code for updating the V-REP simulation with the results and vice-versa. I believe I am using the joint name, but when I visualize the data it is giving me the previous joint. I confirmed this through testing.

Solver & EulerIntegration

Now onto the eulerIntegration problem: when you call solve(), the solver computes the optimal joint acceleration. So if you don't integrate it, the joint angles will not change. What is weird is that your joint angles jump all over the place, especially if you have only one task.

PositionTask does expect a world frame position. If your masses are really junk it may explain why the arm is jumping around. However, I think the main point you are lacking is that your problem does not have a MotionConstr... Try adding a tasks::qp::MotionConstr with infinite torque limits, it should help!

Interesting, I just put a mass of 1.0 for all joints just like the examples. Even if these are incorrect shouldn't the optimization converge on the objective function pointing to the goal pose? Perhaps because of the stiffness setting and the "long" distance it is accelerating the arm enormously?

Maybe I should just try the RBDyn IK to ensure things are working before going back to the Tasks IK?

I can also try adding a tasks::qp::MotionConstr as you advise.

Thanks again for all your help!

[ahundt]

When you link bodies you will have this:

//         to              from 
// body --------> joint ---------> body

Meaning that if you set "from" to identity, the joint will be at the same position as the next body. This is the convention in all of our programs, and in URDFs. If you want to set a joint to be at the same position as the current body, simply swap "to" and "from" transforms.

That's the behavior I expected, here is the behavior I'm getting:

// root  --------> joint -------> body ------>joint------>body ------>joint------>body
//                  "j0"          "b0"        "j1"       "b1"          "j2"        "b2"             

std::size_t jointIdx = simArmMultiBody.jointIndexByName("j2");
rbd_mbcs_[simulatedRobotIndex].q[jointIdx][0] = 0.6;
forwardKinematics(...)

In the above example, the rotation will occur around joint "j1".

[ahundt]

Okay if I set the goal to be the current position defined by the multibodyconfig it stays in place with eulerIntegration enabled! However, if I set the target to be even 0.0001m away it jumps around uncontrollably.

auto targetWorldTransform = rbd_mbcs_[simulatedRobotIndex].bodyPosW[simArmMultiBody.bodyIndexByName(ikGroupTipName_)].translation();
//targetWorldTransform.z() += 0.0001; // uncommenting this causes completely out of control motion
 BOOST_LOG_TRIVIAL(trace) << "target translation (rbdyn format):\n"<< targetWorldTransform;
tasks::qp::PositionTask posTask(rbd_mbs_, simulatedRobotIndex, ikGroupTipName_,targetWorldTransform);

I haven't added constraints like you describe yet, that's up next.

[ahundt]

I added code to test the basic RBDyn InverseKinematics object, here are the results:

Essentially, I'm back to square one. The frames are definitely still mismatched. It seems I found a "local minima" where things appeared to work before.

• The arm colliding with the table is where the V-REP arm goes with rbd::InverseKinematics configured to a pose on the left side of that bone you see there, there is a blue path you might be able to see in the full image if you zoom in.
• The dummy nodes are my conversion of the joint locations from MultiBodyConfig.bodyPosW.
• The other arm model is just in the initial starting configuration and not in use at the moment.

[ahundt]

Good news, I made additional debug utilities for the FK and it does actually work, here is a test pose with FK only based on this commit:

The problem seems to be in the IK configuration alone. I'll detail that shortly.

[haudren]

Ok, good to know that it is at least working with FK. IK has not been thoroughly tested and is the "basic" version, which may pose all sorts of problems.

[ahundt]

Okay I've verified that FK works above, and now I'm trying to debug the IK problem. It appears neither the rbd::InverseKinematics nor the tasks::QPSolver is working.

rbd::InverseKinematics testing

I've verified that if the IK goal is set to the current arm position, it succeeds:

InverseKinematics run with this commit.

However with a 1 line change where I move the goal up on the z axis by 0.0001 m I get the following:

commit with z axis incremented by .0001m

Nearly all the joint angles are near the max possible value:

rbd_mbcs_   std::__1::vector<rbd::MultiBodyConfig, std::__1::allocator<rbd::MultiBodyConfig> >  size=1  
[0] rbd::MultiBodyConfig        
q   std::__1::vector<std::__1::vector<double, std::__1::allocator<double> >, std::__1::allocator<std::__1::vector<double, std::__1::allocator<double> > > > size=10 
[0] std::__1::vector<double, std::__1::allocator<double> >  size=0  
[1] std::__1::vector<double, std::__1::allocator<double> >  size=1  
[0] double  -2.2665058827876667E+227    -2.2665058827876667E+227
[2] std::__1::vector<double, std::__1::allocator<double> >  size=1  
[0] double  -9.7529023542440811E+226    -9.7529023542440811E+226
[3] std::__1::vector<double, std::__1::allocator<double> >  size=1  
[0] double  -6.3415269706104561E+227    -6.3415269706104561E+227
[4] std::__1::vector<double, std::__1::allocator<double> >  size=1  
[0] double  5.2960979797770477E+227 5.2960979797770477E+227
[5] std::__1::vector<double, std::__1::allocator<double> >  size=1  
[0] double  -1.2281105742687497E+227    -1.2281105742687497E+227
[6] std::__1::vector<double, std::__1::allocator<double> >  size=1  
[0] double  -4.5237099037931837E+227    -4.5237099037931837E+227
[7] std::__1::vector<double, std::__1::allocator<double> >  size=1  
[0] double  -1.1479875065301878E+228    -1.1479875065301878E+228
[8] std::__1::vector<double, std::__1::allocator<double> >  size=1  
[0] double  1.9948198562334408E+228 1.9948198562334408E+228
[9] std::__1::vector<double, std::__1::allocator<double> >  size=0  

tasks::QPSolver testing

QP solver was run the settings in this commit, with the goal z position an increment of +0.0001 m above the initial pose of the arm.

QP first iteration appears to succeed

The joint angles look reasonable and similar to the initial joint angles:

q   std::__1::vector<std::__1::vector<double, std::__1::allocator<double> >, std::__1::allocator<std::__1::vector<double, std::__1::allocator<double> > > > size=10 
[0] std::__1::vector<double, std::__1::allocator<double> >  size=0  
[1] std::__1::vector<double, std::__1::allocator<double> >  size=1  
[0] double  0.066339142620563507    0.066339142620563507
[2] std::__1::vector<double, std::__1::allocator<double> >  size=1  
[0] double  0.52122539281845093 0.52122539281845093
[3] std::__1::vector<double, std::__1::allocator<double> >  size=1  
[0] double  -0.059048343449831009   -0.059048343449831009
[4] std::__1::vector<double, std::__1::allocator<double> >  size=1  
[0] double  -1.3335684537887573 -1.3335684537887573
[5] std::__1::vector<double, std::__1::allocator<double> >  size=1  
[0] double  -0.026640243828296661   -0.026640243828296661
[6] std::__1::vector<double, std::__1::allocator<double> >  size=1  
[0] double  1.1184284687042236  1.1184284687042236
[7] std::__1::vector<double, std::__1::allocator<double> >  size=1  
[0] double  -0.014533324167132378   -0.014533324167132378
[8] std::__1::vector<double, std::__1::allocator<double> >  size=1  
[0] double  0   0
[9] std::__1::vector<double, std::__1::allocator<double> >  size=0  

QPSolver q is totally junk by the third timestep

This would be with a goal pose of initial pose z +0.0003 m, assuming the other steps worked correctly:

q   std::__1::vector<std::__1::vector<double, std::__1::allocator<double> >, std::__1::allocator<std::__1::vector<double, std::__1::allocator<double> > > > size=10 
[0] std::__1::vector<double, std::__1::allocator<double> >  size=0  
[1] std::__1::vector<double, std::__1::allocator<double> >  size=1  
[0] double  1.2762778608967991E+249 1.2762778608967991E+249
[2] std::__1::vector<double, std::__1::allocator<double> >  size=1  
[0] double  1.0510523560326581E+249 1.0510523560326581E+249
[3] std::__1::vector<double, std::__1::allocator<double> >  size=1  
[0] double  -4.8048107704350082E+249    -4.8048107704350082E+249
[4] std::__1::vector<double, std::__1::allocator<double> >  size=1  
[0] double  6.0060134630437603E+248 6.0060134630437603E+248
[5] std::__1::vector<double, std::__1::allocator<double> >  size=1  
[0] double  -2.4024053852175041E+249    -2.4024053852175041E+249
[6] std::__1::vector<double, std::__1::allocator<double> >  size=1  
[0] double  1.1184284687046053  1.1184284687046053
[7] std::__1::vector<double, std::__1::allocator<double> >  size=1  
[0] double  -1.2012026926087521E+249    -1.2012026926087521E+249
[8] std::__1::vector<double, std::__1::allocator<double> >  size=1  
[0] double  -0.000000000000000000000000000023045807283067879    -0.000000000000000000000000000023045807283067879
[9] std::__1::vector<double, std::__1::allocator<double> >  size=0  

[ahundt]

It is also worth noting that the tasks::qp::QPSolver results in the post above violate the tasks::qp::JointLimitsConstr I set. I'm not accounting for the off by 1 index bug there, but even so all but one of the joints should still only find results within the constraint limits...

[haudren]

Have you checked if the solver actually succeeds ? i.e. if the calls to solver.solve() return true or false ? I'd say that if the calls return False, you will be integrating junk.

[ahundt]

Just added a BOOST_VERIFY() wrapper and the first two iterations succeed then on the third it fails. Since it will have only moved a total of 2mm at that point in theory it shouldn't fail on the third time, unless the non-q values like velocity, accel, etc start going crazy. Any thoughts?

[haudren]

Ok so that explains a bit better why the joint angles go crazy at some point.

If both IK and Tasks have these kinds of results however, is probably because something is wrong in the kinematic tree. You will need to check that:

• Predecessor and successor vectors are correctly set (In your case it should basically be range() vectors)
• Every mb.transform(i) is fine (i.e. no NaN or weird rotations)
• When you run IK, can you try to print out jacMat (IK.cpp:68) and the error vector v (IK.cpp:76) ?

Note that if you run IK and get weird results it is probably because the jacobian computation fails. The jacobian is only concerned with bodyPosW (set by FK from q) and motionSubspace (set by FV from the joint type)

[ahundt]

I assume mb.tranform(i) means the Xt_ member variable?

Ah, it does look like the last joint there is a bit junky, I put the full Xt_ data in the issue's gist:

Eigen::PlainObjectBase<Eigen::Matrix<double, 3, 3, 0, 3, 3> >   Eigen::PlainObjectBase<Eigen::Matrix<double, 3, 3, 0, 3, 3> >       
m_storage   Eigen::DenseStorage<double, 9, 3, 3, 0>     
m_data  Eigen::internal::plain_array<double, 9, 0, 0>       
array   double [9]      
[0] double  -5.141686914372488E+75  -5.141686914372488E+75
[1] double  6.0525043654517257E+69  6.0525043654517257E+69
[2] double  5.1431719935909431E+75  5.1431719935909431E+75
[3] double  4.1915689661375069E+73  4.1915689661375069E+73
[4] double  -1.028617361159347E+76  -1.028617361159347E+76
[5] double  -6.0525043654517257E+69 -6.0525043654517257E+69
[6] double  5.1430011890582994E+75  5.1430011890582994E+75
[7] double  4.1915689661375069E+73  4.1915689661375069E+73
[8] double  -5.1446575017536247E+75 -5.1446575017536247E+75
r_  sva::PTransform<double>::vector3_t      
Eigen::PlainObjectBase<Eigen::Matrix<double, 3, 1, 0, 3, 1> >   Eigen::PlainObjectBase<Eigen::Matrix<double, 3, 1, 0, 3, 1> >       
m_storage   Eigen::DenseStorage<double, 3, 3, 1, 0>     
m_data  Eigen::internal::plain_array<double, 3, 0, 0>       
array   double [3]      
[0] double  0.0000000000000000000000000000000000000098582804315653778   0.0000000000000000000000000000000000000098582804315653778
[1] double  0.000000000000000000000000000000000000000040178029569121155 0.000000000000000000000000000000000000000040178029569121155
[2] double  0.000000000000000039426468710209044 0.000000000000000039426468710209044

What initializes this particular variable?

Going through some of the other data as well...

[haudren]

Ok so this particular variable is the result of the fixed transforms "to" and "from" that you give when linking bodies in a MultiBodyGraph. Make sure you are not using anywhere an uninitialized Eigen::Matrix3d or sva::PTransformd.

Note that by using mb.transform(i, pt) you can set a particular static transform to a value. You can check if this solves your problem before tracing back the origin of this.

[haudren]

Ok I think I found the offensive code in SpaceVecAlg.hpp:30:

Eigen::Affine3d PTranformToEigenAffine(sva::PTransform<double> & ptransform)
{
    Eigen::Affine3d eigenTransform;
    eigenTransform = Eigen::Quaterniond(ptransform.rotation()).inverse();
    eigenTransform.translation() = ptransform.translation();
    return eigenTransform;
}

Should probably read:

eigenTransform.rotation() = Eigen::Quaterniond(...)

e/ Oh well I noticed that this is the opposite of what you're using for now...

[ahundt]

eigenTransform.rotation() = Eigen::Quaterniond(...)

Strangely, you can't assign to the rotation component of an Affine3d, that is what I tried at first but you can assign the Quaternion to the whole transform, and I know if it is an identity transform it will become just a rotation (translation will remain 0).

I did figure out that I'm going off the end of my set of joint handles, in other words one was uninitialized. I'm working around that now, but need to figure out what actual scene objects to tie it too which is making things more troublesome.

[ahundt]

Good news is with the added frame it no longer crashes! (still have the weird off by 1 error)

On to the next step! The arm either appears to move, but extraordinarily slowly. How do I configure the solver and eulerIntegration step to solve for completing the motion by the next V-REP time step? I tried passing the timestep of 50ms or 0.05 seconds as the eulerIntegration step parameter, but I've been running for 10 simulated minutes and its moved about 1mm per minute, rather than the hoped for 1mm per time step.

float simulationTimeStep = simGetSimulationTimeStep();

[haudren]

The arm's speed is influenced mostly by two things:

• The stiffness of the task
• The timestep

To go faster, you simply have to increase the stiffness. Basically, the solver is trying to minimize the following cost function for set-point tasks:

||\ddot{\epsilon} - s*\epsilon + 2*\sqrt(\epsilon)*\dot{\epsilon} ||^2

i.e. in your case, we attract the arm to the desired position by "pulling" on it with a critically damped spring-damper system. It will thus never reach exactly the position you asked as it tries to reach that point at zero speed. We usually setup a small threshold to declare a task complete.

If you want to actually follow a trajectory i.e. pass through a set of points at certain speeds, you will need a TrajectoryTask. If you want to make sure that some task is completed is some amount of iterations, you can use a TargetObjectiveTask but be aware that they tend to become very unstable as the number of iterations left reaches zero.

[ahundt]

Amazing! Things seem to work! Thanks!

Still have to work out the small bugs, but the big ones seem squashed. :-)

[haudren]

Happy to hear that :smile_cat: Do not hesitate if you have more problems!

[ahundt]

I'm trying to integrate DamperJointsLimitsConstr, and the solver succeeds for a few seconds and appears to move correctly, then it suddenly fails to find a solution after a few seconds (real time, 50ms time step). I used parameters based on the test code for the damper limits. Here are my changes to switch to DamperJointsLimitsConstr.

I tried substantially reducing the stiffness on the position and orientation tasks, and this let it run quite a bit longer but it still eventually fails.

Are there any particular configuration changes you recommend or an approach to work around this problem?

Here is the key code:

        // joint limit objects, initialize to current q so entries
        // will be reasonable, such as empty entries for fixed joints
        std::vector<std::vector<double> > lBound = simArmConfig.q;
        std::vector<std::vector<double> > uBound = simArmConfig.q;
        std::vector<std::vector<double> > lVelBound = simArmConfig.alpha;
        std::vector<std::vector<double> > uVelBound = simArmConfig.alpha;

        // for all joints
        for (std::size_t i=0 ; i < jointHandles_.size() ; i++)
        {
            /// limits must be organized as described in https://github.com/jrl-umi3218/Tasks/issues/10#issuecomment-257793242
            std::string jointName = jointNames_[i];
            std::size_t jointIdx = simArmMultiBody.jointIndexByName(jointName);
            jointIdx-=1; /// @todo TODO(ahundt) HACK FIXME JOINT INDICES ARE OFF BY 1
            lBound[jointIdx][0] = llimits[i];
            uBound[jointIdx][0] = ulimits[i];
            lVelBound[jointIdx][0] = -inf; /// @todo TODO(ahundt) Hardcoded infinite Velocity limits, set to real values
            uVelBound[jointIdx][0] = inf;
        }

        tasks::qp::DamperJointLimitsConstr dampJointConstr(rbd_mbs_, simulatedRobotIndex, {lBound, uBound},{lVelBound, uVelBound}, 0.125, 0.025, 0.1, simulationTimeStep);

        // Test add*Constraint
        dampJointConstr.addToSolver(solver);
        BOOST_VERIFY(solver.nrBoundConstraints() == 1);
        BOOST_VERIFY(solver.nrConstraints() == 1);

side note: The joint indexing bug was my fault and I've now fixed it in my code, sorry about that!

[ahundt]

DamperJointsLimitsConstr problem is resolved as well! When I was doing unrelated implementation steps it just so happens that I had one additional degree of freedom that I hadn't integrated yet that meant the solver could find solutions without errors for the damped joint constraints task.

[ahundt]

I'd like to write a small troubleshooting guide based on everything above. I expect there will be parts appropriate for each of Tasks, RBDyn, and SpaceVecAlg. What would be the best place to put it? Perhaps expand the README.md or do you recommend somewhere else?