Translation of teoSim's DOF indices

[rsantos88]

It's very interesting to be able to appreciate in the execution of teoSim, if there is some moment in which one of the joints surpasses a joint limit, being able it to warn with a warning message. This function is done by teoSim through openrave but with a different joint index: DOF (degrees of freedom).

Example:

2019-11-25 12:32:36,281 openrave [WARN] [idealcontroller.cpp:526 _ReportError] robot teoSim dof 17 is violating lower limit -2.478368e-01 < -2.481280e-01, time=0.000000
2019-11-25 12:32:36,281 openrave [WARN] [kinbody.cpp:1576 KinBody::SetDOFValues] dof 17 value is not in limits -2.481280e-01<-2.478368e-01

DOF 17 corresponds to CAN-ID 5 or YARP-ID 1. It would be interesting to change or add a warning message with an ID understandable by us.

[jgvictores]

2019-11-25 12:32:36,281 openrave [WARN] [idealcontroller.cpp:526 _ReportError] robot teoSim dof 17 is violating lower limit -2.478368e-01 < -2.481280e-01, time=0.000000

https://github.com/rdiankov/openrave/blob/abe85928e74381d3d3d33db73693d92c9fbd7a1c/plugins/basecontrollers/idealcontroller.cpp#L480

2019-11-25 12:32:36,281 openrave [WARN] [kinbody.cpp:1576 KinBody::SetDOFValues] dof 17 value is not in limits -2.481280e-01<-2.478368e-01

I see this in OpenRAVE v0.9.0, but exact message is not currently in master. Looks like now somewhere around https://github.com/rdiankov/openrave/blob/master/src/libopenrave/kinbody.cpp#L1549-L1570

From a quick glance, it seems that the first lines call _ReportError() which may be set to throw exceptions. I have no idea of the exception mechanism of OpenRAVE but it seems like it may have to be activated at program-level (and we might see OpenRAVE breaking all the time due to unhandled exceptions). Thus:

1. I could generate a patch for either of the two to also cite the name of the axis. Maybe even PR of it, though it may take some time.
2. Further research: 2.1 To implement this in an external layer (may lead to an inefficient implementation calling check redundantly) 2.2 Explore the exception mechanism.

I'd go for (1). :smile:

[rsantos88]

This is a simple script that I made to move a joint with a specific DOF:

from openravepy import *
import math

env = Environment() # create openrave environment
env.SetViewer('qtcoin') # attach viewer (optional)
env.Load('/usr/local/share/teo-openrave-models/contexts/openrave/teo_lacqueyFetch.robot.xml') # load a simple scene
robot = env.GetRobots()[0] # get the first robot

while 1:
    dof = float(input("DOF: "))
    val = float(input("value: "))
    deg = val * math.pi/180
    robot.SetDOFValues([deg],[dof]) # set joint 0 to value 0.5      

At least we'll be able to see graphically which joint it is.

[jgvictores]

SimulationStep -> _SetDOFValues -> _ReportError -> throw openrave_exception(s,ORE_Assert)

[jgvictores]

SimulationStep

Makes sense in the light of: [environment-core.h:2568 _SimulationThread] simulation thread exception: openrave (Assert): robot teoSim dof 40 is violating max velocity displacement 6.544984694978731e-03 > 5.236987755982988e-03, time=0.060000

[jgvictores]

[warning] YarpOpenraveControlboard.hpp:66 run(): openrave (Assert): robot teoSim dof 11 is violating lower limit -4.171337e-01 < -7.853982e-01, time=12.500000 via https://github.com/roboticslab-uc3m/openrave-yarp-plugins/compare/571abb9d8c6b987c9d25fcca01f82113c4cde425..008c43f736f8aa26f70909aed139ffc42e585018

[jgvictores]

@rsantos88

When we run teoSim, it uses libraries/OpenravePlugins/OpenraveYarpPluginLoader to load several libraries/YarpPlugins/YarpOpenraveControlboard. At init, we iterate through the joints. In this fragment of output, you can see that dof 17 of OpenRAVE is actually /teoSim/rightLeg joint 1 out of 0-5.

[debug] DeviceDriverImpl.cpp:27 open(): config: (allManipulators) (device YarpOpenraveControlboard) (manipulatorIndex 12) (name "/teoSim/rightLeg") (penv {112 75 180 1 0 0 0 0 16 229 186 1 0 0 0 0}) (robotIndex 0) (subdevice YarpOpenraveControlboard)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[0]: 16
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[1]: 17 
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[2]: 18
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[3]: 19
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[4]: 20
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[5]: 21

Thanks to https://github.com/roboticslab-uc3m/openrave-yarp-plugins/commit/770fd8e7e6553287b5ba567f01e47090e6ead6de (I commited directly to develop) we also see the joint name (:smile:):

[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[1]: 17 (SagittalRightHip)

PS: I'm still working on the exceptions, but it requires re-implementing the thread, and some nasty string parsing. : )

[jgvictores]

Oh, well, here would be the patch for OpenRAVE 7c5f5e27eec2b2ef10aa63fbc519a998c276f908:

diff --git a/plugins/basecontrollers/idealcontroller.cpp b/plugins/basecontrollers/idealcontroller.cpp
index e923ca2..6dfc96b 100644
--- a/plugins/basecontrollers/idealcontroller.cpp
+++ b/plugins/basecontrollers/idealcontroller.cpp
@@ -477,10 +477,10 @@ private:
         for(size_t i = 0; i < _vlower[0].size(); ++i) {
             if( !_dofcircular[i] ) {
                 if( curvalues.at(i) < _vlower[0][i]-g_fEpsilonJointLimit ) {
-                    _ReportError(str(boost::format("robot %s dof %d is violating lower limit %e < %e, time=%f")%probot->GetName()%i%_vlower[0][i]%curvalues[i]%_fCommandTime));
+                    _ReportError(str(boost::format("robot %s dof %d (%s) is violating lower limit %e < %e, time=%f")%probot->GetName()%i%probot->GetJointFromDOFIndex(i)->GetName()%_vlower[0][i]%curvalues[i]%_fCommandTime));
                 }
                 if( curvalues.at(i) > _vupper[0][i]+g_fEpsilonJointLimit ) {
-                    _ReportError(str(boost::format("robot %s dof %d is violating upper limit %e > %e, time=%f")%probot->GetName()%i%_vupper[0][i]%curvalues[i]%_fCommandTime));
+                    _ReportError(str(boost::format("robot %s dof %d (%s) is violating upper limit %e > %e, time=%f")%probot->GetName()%i%probot->GetJointFromDOFIndex(i)->GetName()%_vupper[0][i]%curvalues[i]%_fCommandTime));
                 }
             }
         }

[jgvictores]

PS: I copied the above contents into this file (with that extension just for GitHub to allow me to upload to issue): diff.patch.log You can apply the patch on OpenRAVE tag v0.9.0 or similar, copying the file to the OpenRAVE source root, and then from the command line via: git apply diff.patch.log (then recompile and install OpenRAVE of course)

PS2: The exception via went slightly ashtray due to: 1) To re-implement the thread, my branch incorporated a hacked copypasta off https://github.com/rdiankov/openrave/blob/abe85928e74381d3d3d33db73693d92c9fbd7a1c/src/libopenrave-core/environment-core.h#L2576-L2672; 2) AFAIK, the exception throws a char array, so we'd have to parse something like a string robot teoSim dof 17 is violating lower limit -2.478368e-01 < -2.481280e-01, time=0.000000 that could be subject to change upstream (OpenRAVE).

[jgvictores]

Este mola más! 98-limit-output.patch.log

[rsantos88]

Genial! pienso que sería muy interesante antes de cerrar esta issue incluirlo en la documentación de instalación de openrave https://github.com/roboticslab-uc3m/installation-guides/blob/master/install-openrave.md/ o si lo ves mejor en https://github.com/roboticslab-uc3m/openrave-yarp-plugins/blob/develop/doc/openrave-yarp-plugins-install.md

[jgvictores]

• Commit: https://github.com/roboticslab-uc3m/installation-guides/commit/85e8592e8c904fe942c6787de6b26e00a67c0fe2
• Visualización: http://robots.uc3m.es/gitbook-installation-guides/install-openrave.html#source-code-hacks :blush:

[rsantos88]

Perfecto! Muchas gracias @jgvictores !! Cierro issue :)

[jgvictores]

Update (in separate patches because I believe the 2nd one is less compatible with new OpenRAVE versions; these are for tag v0.9.0):

• plugins/basecontrollers/idealcontroller.cpp: above 98-limit-output.patch.log
• src/libopenrave/kinbody.cpp: new 98-limit-output-2.patch.log