A YARP Controlboard for each robot, which emulated the velocity of the robot joints.

[rsantos88]

I want change the code to emulate the velocity in each robot joint.
First, we want to work directly with a robot base pointer (more proper) and with that, we want to get the joint positions (get virtual encoder position) . I've been searching a function that allows me to get the opposite of void SetJointValues(const std::vector& values, bool checklimits = true) but I cant find something similar. This last function inherit from KinBody class.

[jgvictores]

How about trying to get to the joint using OpenRAVE::KinBody::GetJointFromDOFIndex, and then trying to access OpenRAVE::KinBody::Joint::GetValue()?

In fact, this approach would be for IEncoders, but maybe it would look more elegant on IPositionControl too.

[rsantos88]

I've implemented setEncoder and getEncoder functions.
The result is; when I do set pos 0 10 and I see the position returned with get encs, it shows:

Response: [is] encs (0.0 0.0 0.0 0.0 0.174533 0.0) [tsta] 0 1496050942.161305 [ok]

[jgvictores]

Okay, I'll try to put "all the meat on the grill" for this one. 😃

[jgvictores]

@rsantos88 Slow but steady, I've first been looking into #30. Now I'm going to play around with OpenRAVE::KinBody::JointPtr jointptr = probot->GetJointFromDOFIndex (j); like on your recent commits, directly on YarpOpenraveControlboard. Watch out for a fix-29-jointIndex branch. 😉

[jgvictores]

Poor results with:

    axes = manipulatorIDs.size();
    CD_DEBUG("manipulatorIDs.size(): %d\n",manipulatorIDs.size());

    vectorOfManipulatorPtr[manipulatorIndex]->GetChildJoints(manipulatorJoints);
    CD_DEBUG("manipulatorJoints.size(): %d\n",manipulatorJoints.size());

Anyway, the implementation of GetChildJoints is a bit cryptic. Here is some output of a single run with Teo:

[debug] DeviceDriverImpl.cpp:36 open(): manipulatorIDs.size(): 2
[debug] DeviceDriverImpl.cpp:39 open(): manipulatorJoints.size(): 14
...
[debug] DeviceDriverImpl.cpp:36 open(): manipulatorIDs.size(): 2
[debug] DeviceDriverImpl.cpp:39 open(): manipulatorJoints.size(): 0
...
[debug] DeviceDriverImpl.cpp:36 open(): manipulatorIDs.size(): 6
[debug] DeviceDriverImpl.cpp:39 open(): manipulatorJoints.size(): 0
...
[debug] DeviceDriverImpl.cpp:36 open(): manipulatorIDs.size(): 6
[debug] DeviceDriverImpl.cpp:39 open(): manipulatorJoints.size(): 0
...
[debug] DeviceDriverImpl.cpp:36 open(): manipulatorIDs.size(): 6
[debug] DeviceDriverImpl.cpp:39 open(): manipulatorJoints.size(): 0
...
[debug] DeviceDriverImpl.cpp:36 open(): manipulatorIDs.size(): 6
[debug] DeviceDriverImpl.cpp:39 open(): manipulatorJoints.size(): 0

Going for a different approach.

[jgvictores]

Did some tests at e4dd0ee, which will actually be good for #12. 😂

[jgvictores]

Note to self: "think about odevelocity".

[jgvictores]

More context: http://openrave.org/docs/0.8.0/interface_types/#interface-controller

[rsantos88]

Note: other important thing to fix is to keep the position. Now, if we want to move a limb (e.g right arm) and then we want to move other different, the first limb returns to the original position (zero position).

[rsantos88]

Continuing with this...

I've implemented setEncoder and getEncoder functions.
The result is; when I do set pos 0 10 and I see the position returned with get encs, it shows:
Response: [is] encs (0.0 0.0 0.0 0.0 0.174533 0.0) [tsta] 0 1496050942.161305 [ok]

If I want to send the correct position values, I need to know in what manipulator I am trying to connect through rpc yarp. @jgvictores do you know any way to get this?

[jgvictores]

@rsantos88 The best way to access individual joints now is as seen here:

vectorOfJointPtr[axis]->whatever()

We'd actually have to replicate this implementation around

openrave-yarp-plugins/yarpplugins/YarpOpenraveControlboard/IEncodersImpl.cpp

Line 44 in 4a13252

*v = dEncRaw[ manipulatorIDs[j] ] * 180.0 / M_PI;

.
Hopefully I'll be able to do this soon, and also play around with odevelocity and other controllers as mentioned above. Offtopic, maybe you can check what I commented with you in person regarding roboticslab-uc3m/teo-openrave-models#3 ? Thanks!!!

[rsantos88]

Offtopic, maybe you can check what I commented with you in person regarding roboticslab-uc3m/teo-openrave-models#3 ? Thanks!!!

Ok, @jgvictores no problem. Could you write what do you want in that issue? only to reference the results. Thanks!

[jgvictores]

Commented at roboticslab-uc3m/teo-openrave-models#3 (comment) 😊

[jgvictores]

Have advanced here, but haven't been documenting.
Main issue is that (in addition to odevelocity which seems to be hidden, broken and superseeded by idealvelocitycontroller which in turn needs ODE) apparently trajectories are in fact computed by planner/smoothers. The problem is that they work at robot level, we want to work at joint level.
My next intention is to create a single joint trajectory, but use lots of traj->Inserts commands before launching: not only target or origin-target, also intermediate positions.

[jgvictores]

Still working on this, here are some references:

• https://sourceforge.net/p/openrave/mailman/message/30261754/
• http://openrave-users-list.185357.n3.nabble.com/Time-Stamp-of-quot-waypoint-quot-and-Concept-of-quot-Group-quot-td4005240.html

[jgvictores]

Some progress at a522394 "very slow, only one limb (last loaded controller), but performs trajectory with intermediate positions"

[jgvictores]

Thanks to 7027438 really seeing what is happening, because program breaks without OpenRAVE::planningutils::SmoothActiveDOFTrajectory(ptraj,probot);, and now we are seeing how it affects ptraj:

• [debug] IPositionImpl.cpp:50 positionMove(): ptraj-prev

<trajectory>
<configuration>
<group name="joint_values teoSim 22 23 24 25 26 27" offset="0" dof="6" interpolation=""/>
</configuration>
<data count="2">
0 0 0 0 0 0 0 0 -0.436332 0 0 0 </data>
</trajectory>

• [debug] IPositionImpl.cpp:54 positionMove(): ptraj-post

<trajectory>
<configuration>
<group name="deltatime" offset="12" dof="1" interpolation=""/>
<group name="joint_velocities teoSim 22 23 24 25 26 27" offset="6" dof="6" interpolation="linear"/>
<group name="joint_values teoSim 22 23 24 25 26 27" offset="0" dof="6" interpolation="quadratic"/>
<group name="iswaypoint" offset="13" dof="1" interpolation="next"/>
</configuration>
<data count="28">
0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 -1.92401e-05 0 0 0 0 0 -0.0438636 0 0 0 0.000877271 1 0 0 -0.0187145 0 0 0 0 0 -1.36801 0 0 0 0.0264829 1 0 0 -0.0237412 0 0 0 0 0 -1.54082 0 0 0 0.00345617 1 0 0 -0.0292858 0 0 0 0 0 -1.71131 0 0 0 0.00340987 1 0 0 -0.138109 0 0 0 0 0 -3.7163 0 0 0 0.0400999 1 0 0 -0.218166 0 0 0 0 0 -4.67083 0 0 0 0.0190905 1 0 0 -0.279409 0 0 0 0 0 -3.96136 0 0 0 0.0141893 1 0 0 -0.300068 0 0 0 0 0 -3.6914 0 0 0 0.00539929 1 0 0 -0.3122 0 0 0 0 0 -3.52324 0 0 0 0.0033631 1 0 0 -0.312637 0 0 0 0 0 -3.51704 0 0 0 0.00012406 1 0 0 -0.320109 0 0 0 0 0 -3.40915 0 0 0 0.00215769 1 0 0 -0.323185 0 0 0 0 0 -3.36373 0 0 0 0.000908408 1 0 0 -0.33134 0 0 0 0 0 -3.24025 0 0 0 0.00246975 1 0 0 -0.339428 0 0 0 0 0 -3.11295 0 0 0 0.00254591 1 0 0 -0.368224 0 0 0 0 0 -2.60975 0 0 0 0.0100639 1 0 0 -0.375933 0 0 0 0 0 -2.45762 0 0 0 0.00304267 1 0 0 -0.391852 0 0 0 0 0 -2.10903 0 0 0 0.00697177 1 0 0 -0.394298 0 0 0 0 0 -2.05022 0 0 0 0.00117621 1 0 0 -0.401131 0 0 0 0 0 -1.87619 0 0 0 0.00348059 1 0 0 -0.401743 0 0 0 0 0 -1.85982 0 0 0 0.000327367 1 0 0 -0.421587 0 0 0 0 0 -1.2143 0 0 0 0.0129104 1 0 0 -0.421604 0 0 0 0 0 -1.2136 0 0 0 1.40322e-05 1 0 0 -0.423385 0 0 0 0 0 -1.13787 0 0 0 0.00151451 1 0 0 -0.435849 0 0 0 0 0 -0.219907 0 0 0 0.0183593 1 0 0 -0.436332 0 0 0 0 0 8.88178e-16 0 0 0 0.00439815 1 0 0 -0.436332 0 0 0 0 0 0 0 0 0 0 1 </data>
</trajectory>

Yay!

[jgvictores]

Also interesting is how:

[debug] IPositionImpl.cpp:55 positionMove(): 0, joint_values teoSim 22 23 24 25 26 27, 

becomes:

[debug] IPositionImpl.cpp:77 positionMove(): 0, deltatime, 
[debug] IPositionImpl.cpp:77 positionMove(): 1, joint_velocities teoSim 22 23 24 25 26 27, linear
[debug] IPositionImpl.cpp:77 positionMove(): 2, joint_values teoSim 22 23 24 25 26 27, quadratic
[debug] IPositionImpl.cpp:77 positionMove(): 3, iswaypoint, next

[jgvictores]

Same in different format, when good:

[debug] IPositionImpl.cpp:37 positionMove(): [0] joint_values teoSim 22 23 24 25 26 27, 0, 6, 

Becomes:

[debug] IPositionImpl.cpp:88 positionMove(): [0] deltatime, 12, 1, 
[debug] IPositionImpl.cpp:88 positionMove(): [1] joint_velocities teoSim 22 23 24 25 26 27, 6, 6, linear
[debug] IPositionImpl.cpp:88 positionMove(): [2] joint_values teoSim 22 23 24 25 26 27, 0, 6, quadratic
[debug] IPositionImpl.cpp:88 positionMove(): [3] iswaypoint, 13, 1, next

[jgvictores]

Nice stuff at de33aef "faster, performs trajectory with intermediate positions, all good but still lots to do because for one limb for now".

[jgvictores]

First functional version of multicontrol with 1 dof trajectories and all robot joints active, at b83c3ec.

[jgvictores]

The mentioned multicontroller works, but currently trajectories are all duration 1 (which I guess is in seconds). Next step is to introduce speed. Next will be implementing genRefSpeeds parameter that sets movement to immediate if below a genMinVels.

[jgvictores]

WIP at issue-29-traj-vel.

[jgvictores]

Quite a bit done at ca4929f (merge of issue-29-traj-vel into issue-29-traj; work can continue on issue-29-traj-vel).

What's done:

• Implemented velocity regulation in position control mode, based on refSpeed (no longer all 1 second duration). Timed and working.

What's left to do:

• Right now immediate movement is done when refSpeed is 0. However, there may be cases where we want refSpeed to be 0, for instance to block a joint. Should recover genMinVels and genMaxVels and work with that.

[jgvictores]

Further advances on same branch(es) most importantly involve 099f69e that implements a getMaxVel (from OpenRAVE model) functionality.

[jgvictores]

Note that we've used GetMaxVel at 099f69e, which is only a getter (no setter). OpenRAVE JointInfo has:

boost::array<dReal,3> _vmaxvel;                  ///< the soft maximum velocity (rad/s) to move the joint when planning
boost::array<dReal,3> _vhardmaxvel;              ///< the hard maximum velocity, robot cannot exceed this velocity. used for verification checking

Which look pretty public. Will see if can access directly to be able to set in addition to get.

[jgvictores]

Ended up using newly-discoverd VelLimit setter and getter functions at 486e765.

[jgvictores]

Must work on #29 (comment)

[jgvictores]

🎉🎉🎉🎉🎉🎉🎉 Finally closed and merged into develop at 0280977. 🎉🎉🎉🎉🎉🎉🎉🎉🎉

Will now be closing related issues and opening new ones documenting the few identified open issues.

[jgvictores]

PS: The "big one" was 274cbc2