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. :wink:

[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 e4dd0eea8807e672869673ba80563cab883f0897, which will actually be good for #12. :joy:

[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 https://github.com/roboticslab-uc3m/openrave-yarp-plugins/blob/4a13252be8e4548132f7b66b493711fabe359a7f/yarpplugins/YarpOpenraveControlboard/IEncodersImpl.cpp#L44. 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 https://github.com/roboticslab-uc3m/teo-openrave-models/issues/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 https://github.com/roboticslab-uc3m/teo-openrave-models/issues/3#issuecomment-311086684 😊

[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 a522394a2b590efc1318d155b8195fadb55236a0 "very slow, only one limb (last loaded controller), but performs trajectory with intermediate positions"

[jgvictores]

Thanks to 702743801ebab57c131798e416f3d25f011bedfd 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 de33aef62ee3e8d57c2e490bb7d63177c4fe4ae9 "faster, performs trajectory with intermediate positions, all good but still lots to do because for one limb for now".

[jgvictores]

Going to implement rather crudely via code like: https://github.com/roboticslab-uc3m/openrave-yarp-plugins/blob/3b22288bb2567d937da0489841402677b6ad6fb7/libraries/YarpPlugins/YarpOpenraveControlboard/IPositionImpl.cpp#L51-L65

However, take into account the following interesting functions for the future: https://github.com/roboticslab-uc3m/openrave-yarp-plugins/blob/3b22288bb2567d937da0489841402677b6ad6fb7/libraries/YarpPlugins/YarpOpenraveControlboard/IPositionImpl.cpp#L47-L49

[jgvictores]

Just to recap on code for debugging trajectories, the xml-like view:

CD_DEBUG("begin{ptraj-serialize}\n");
ptraj->serialize(std::cout);
CD_DEBUG("end{ptraj-serialize}\n");

And the internal structure (Groups of ConfigurationSpecification):

OpenRAVE::ConfigurationSpecification activeConfigurationSpecification = probot->GetActiveConfigurationSpecification();
for (size_t i = 0; i < activeConfigurationSpecification._vgroups.size(); i++)
{
    OpenRAVE::ConfigurationSpecification::Group g = activeConfigurationSpecification._vgroups[i];
    CD_DEBUG("[%d] %s, %d, %d, %s\n",i,g.name.c_str(), g.offset, g.dof, g.interpolation.c_str());
}

In addition to seeing the robot active DOF:

std::vector<int> activeDOFIndices = probot->GetActiveDOFIndices();
for(size_t i=0; i<manipulatorIDs.size(); i++)
{
    CD_DEBUG("activeDOFIndices[%d]: %d\n",i,activeDOFIndices[i]);
}

[jgvictores]

WIP at issue-29-traj.

Next step is to use probot->GetController() intelligently to create a single multicontroller.

[jgvictores]

WIP at issue-29-traj-multicontroller.

[jgvictores]

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

[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 ca4929f90b217f2bdca999c15e3a7fa9d85dea20 (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 099f69e8b1650a7045af78dbd44217130e2c50d1, 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 486e76509aa036509f2ad0cb0a517d65ff04eb7c.

[jgvictores]

Must work on https://github.com/roboticslab-uc3m/openrave-yarp-plugins/issues/29#issuecomment-347620426

[jgvictores]

:tada::tada::tada::tada::tada::tada::tada: Finally closed and merged into develop at 02809770071873b2a5df9d3894b3a1a4f11a146d. :tada::tada::tada::tada::tada::tada::tada::tada::tada:

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

[jgvictores]

PS: The "big one" was 274cbc297c0bf28163d20cdbb153ba9cceb1356b