search

roboticslab-uc3m / kinematics-dynamics

Kinematics and dynamics solvers and controllers.
https://robots.uc3m.es/kinematics-dynamics/
GNU Lesser General Public License v2.1
19 stars 12 forks source link

CCS segfaults on transform port configuration #184

Closed RaulFdzbis closed 4 years ago

RaulFdzbis commented 4 years ago

Getting this error when trying to run BasicCartesianControl on a computer with Bionic and Openrave 0.9:

[info] DeviceDriverImpl.cpp:20 open(): Subdevice BasicCartesianControl
[debug] DeviceDriverImpl.cpp:11 open(): BasicCartesianControl config: (H0 (0 1 0 0 0 0 1 -0.3469 1 0 0 0.4982 0 0 0 1)) (HN (0 0 -1 -0.0975 -1 0 0 0 0 1 0 0 0 0 0 1)) (angleRepr axisAngle) (device BasicCartesianControl) (from "/usr/local/share/teo-configuration-files/contexts/kinematics/fixedTrunk-rightArm-fetch.ini") (ik st) (link_0 (offset 0.0) (D 0.0) (A 0.0) (alpha -90.0) (mass 0) (cog 0 0 0) (inertia 0 0 0)) (link_1 (offset -90.0) (D 0.0) (A 0.0) (alpha -90.0) (mass 0) (cog 0 0 0) (inertia 0 0 0)) (link_2 (offset -90.0) (D -0.32901) (A 0.0) (alpha -90.0) (mass 1.750625) (cog 0 0 0) (inertia 0 0 0)) (link_3 (offset 0.0) (D 0.0) (A 0.0) (alpha 90.0) (mass 0) (cog 0 0 0) (inertia 0 0 0)) (link_4 (offset 0.0) (D -0.215) (A 0.0) (alpha -90.0) (mass 2.396) (cog 0 0 0) (inertia 0 0 0)) (link_5 (offset -90.0) (D 0.0) (A -0.09) (alpha 0.0) (mass 0.3) (cog 0 0 0) (inertia 0 0 0)) (local "/BasicCartesianControl/teoSim/rightArm") (name "/teoSim/rightArm/CartesianControl") (numLinks 6) (remote "/teoSim/rightArm") (single_threaded 1) (subdevice BasicCartesianControl).
yarp: Port /BasicCartesianControl/teoSim/rightArm/rpc:o active at tcp://192.168.1.57:10009/
yarp: Port /BasicCartesianControl/teoSim/rightArm/command:o active at tcp://192.168.1.57:10010/
yarp: Port /BasicCartesianControl/teoSim/rightArm/stateExt:i active at tcp://192.168.1.57:10011/
[ERROR]Problem connecting to /teoSim/rightArm/rpc:i, is the remote device available?
[ERROR]Problem connecting to /teoSim/rightArm/command:i, is the remote device available?
[ERROR]Problem connecting to /teoSim/rightArm/stateExt:o, is the remote device available?
yarpdev: ***ERROR*** driver <remote_controlboard> was found but could not open
[error] DeviceDriverImpl.cpp:55 open(): robot device not valid: remote_controlboard.
yarpdev: ***ERROR*** driver <BasicCartesianControl> was found but could not open
[error] DeviceDriverImpl.cpp:38 open(): cannot make <BasicCartesianControl>
[error] DeviceDriverImpl.cpp:49 open(): cartesianControlDevice not valid
yarpdev: ***ERROR*** driver <CartesianControlServer> was found but could not open
[ERROR]yarpdev: ***ERROR*** device not available.
[INFO]Suggestions:
[INFO]+ Do "yarpdev --list" to see list of supported devices.
[INFO]+ Or append "--verbose" option to get more information.

If i am not wrong the problem comes from not finding the ports /teoSim/rightArm/*. After running OpenraveYarpPaintSquares the available ports are:

~$ yarp name list
registration name /OpenraveYarpPluginLoader/rpc:s ip 192.168.1.57 port 10002 type tcp
registration name /OpenraveYarpPluginLoader/state:o ip 192.168.1.57 port 10003 type tcp
registration name /openraveYarpPaintSquares/rpc:s ip 192.168.1.57 port 10008 type tcp
registration name /root ip 192.168.1.57 port 10000 type tcp
registration name /teoSim/rightLacqueyFetch/internalFinger/command:i ip 192.168.1.57 port 10005 type tcp
registration name /teoSim/rightLacqueyFetch/internalFinger/rpc:i ip 192.168.1.57 port 10004 type tcp
registration name /teoSim/rightLacqueyFetch/internalFinger/state:o ip 192.168.1.57 port 10006 type tcp
registration name /teoSim/rightLacqueyFetch/internalFinger/stateExt:o ip 192.168.1.57 port 10007 type tcp
registration name fallback ip 224.2.1.1 port 10000 type mcast
*** end of message

Are /teoSim/rightLacqueyFetch/* equivalent to /teoSim/rightArm/* ?. Should we change the name in the configuration files? Or am I doing something wrong?

jgvictores commented 4 years ago

Are /teoSim/rightLacqueyFetch/* equivalent to /teoSim/rightArm/* ?. Should we change the name in the configuration files? Or am I doing something wrong?

No, most probably teoSim died. Could you please restart your computer and copy the full output of launching teoSim ?

RaulFdzbis commented 4 years ago

Here you go:

~$ teoSim
2020-01-13 10:44:55,460 openrave [WARN] [plugindatabase.h:645 InterfaceBasePtr OpenRAVE::RaveDatabase::Create] Failed to create name viewerrecorder, interface module
[info] OpenraveYarpPluginLoader.cpp:22 OpenraveYarpPluginLoader(): Checking for yarp network...
[success] OpenraveYarpPluginLoader.cpp:25 OpenraveYarpPluginLoader(): Found yarp network.
yarp: Port /OpenraveYarpPluginLoader/rpc:s active at tcp://10.118.40.231:10002/
yarp: Port /OpenraveYarpPluginLoader/state:o active at tcp://10.118.40.231:10003/
[debug] OpenraveYarpPluginLoader.cpp:85 main(): [env openrave/teo_lacqueyFetch.robot.xml open --device controlboardwrapper2 --subdevice YarpOpenraveControlboard --robotIndex 0 --allManipulators]
[debug] OpenraveYarpPluginLoader.cpp:123 main(): env: 'openrave/teo_lacqueyFetch.robot.xml'
2020-01-13 10:44:55,498 openrave [WARN] [libopenrave.cpp:799 __cxx11::string OpenRAVE::RaveGlobal::FindLocalFile] could not find file "openrave/teo_lacqueyFetch.robot.xml"
2020-01-13 10:44:55,498 openrave [WARN] [environment-core.h:492 Load] load failed on file openrave/teo_lacqueyFetch.robot.xml
[debug] OpenraveYarpPluginLoader.cpp:133 main(): Could not load 'openrave/teo_lacqueyFetch.robot.xml' environment, attempting via yarp::os::ResourceFinder.
2020-01-13 10:44:56,235 openrave [INFO] [ForceSensor.cpp:59 ForceSensor] Creating a Force Sensor
2020-01-13 10:44:56,235 openrave [INFO] [ForceSensor.cpp:59 ForceSensor] Creating a Force Sensor
[success] OpenraveYarpPluginLoader.cpp:143 main(): Loaded '/usr/local/share/teo-openrave-models/openrave/teo_lacqueyFetch.robot.xml' environment.
[debug] OpenraveYarpPluginLoader.cpp:150 main(): open[0]: [open --device controlboardwrapper2 --subdevice YarpOpenraveControlboard --robotIndex 0 --allManipulators]
[info] OpenraveYarpPluginLoader.cpp:168 Open(): Checking for yarp network...
[success] OpenraveYarpPluginLoader.cpp:175 Open(): Found yarp network.
[debug] OpenraveYarpPluginLoader.cpp:182 Open(): config: (allManipulators) (device controlboardwrapper2) (robotIndex 0) (subdevice YarpOpenraveControlboard)
[info] OpenraveYarpPluginLoader.cpp:185 Open(): penv: 0x557c8a5b2780
[DEBUG]ControlBoardWrapper2: 'period' parameter missing, using default thread period = 20ms 
[DEBUG]opening controlBoardWrapper2 subdevice
[debug] DeviceDriverImpl.cpp:27 open(): config: (allManipulators) (device YarpOpenraveControlboard) (manipulatorIndex 0) (name "/teoSim/rightLacqueyFetch/thumb") (penv {128 39 91 138 124 85 0 0 160 42 91 138 124 85 0 0}) (robotIndex 0) (subdevice YarpOpenraveControlboard)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[0]: 34 (rightLacqueyFetch/ThumbProximal)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[1]: 35 (rightLacqueyFetch/ThumbDistal)
[debug] DeviceDriverImpl.cpp:80 open(): pcontrol: 0x557c8c0b6f50, IdealController
[info] DeviceDriverImpl.cpp:86 open(): Detected idealcontroller, switch to genericmulticontroller.
[debug] DeviceDriverImpl.cpp:99 open(): pcontrol: 0x557c8a808190, genericmulticontroller
[debug] DeviceDriverImpl.cpp:116 open(): Attach individual controller for manipulatorIDs[0]: 34
[debug] DeviceDriverImpl.cpp:116 open(): Attach individual controller for manipulatorIDs[1]: 35
[INFO]created device <YarpOpenraveControlboard>. See C++ class roboticslab::YarpOpenraveControlboard for documentation.
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 2 axes are present
[DEBUG]joints parameter is 2
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 2 axes are present
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 2 axes are present
[INFO]/teoSim/rightLacqueyFetch/thumb : no ROS initialization required 
[INFO]/teoSim/rightLacqueyFetch/thumb  initting YARP initialization 
yarp: Port /teoSim/rightLacqueyFetch/thumb/rpc:i active at tcp://10.118.40.231:10004/
yarp: Port /teoSim/rightLacqueyFetch/thumb/command:i active at tcp://10.118.40.231:10005/
yarp: Port /teoSim/rightLacqueyFetch/thumb/state:o active at tcp://10.118.40.231:10006/
yarp: Port /teoSim/rightLacqueyFetch/thumb/stateExt:o active at tcp://10.118.40.231:10007/
[INFO]created wrapper <controlboardwrapper2>. See C++ class ControlBoardWrapper for documentation.
[success] OpenraveYarpPluginLoader.cpp:358 Open(): Valid yarp plugin (id 0).
[DEBUG]ControlBoardWrapper2: 'period' parameter missing, using default thread period = 20ms 
[DEBUG]opening controlBoardWrapper2 subdevice
[debug] DeviceDriverImpl.cpp:27 open(): config: (allManipulators) (device YarpOpenraveControlboard) (manipulatorIndex 1) (name "/teoSim/rightLacqueyFetch/externalFinger") (penv {128 39 91 138 124 85 0 0 160 42 91 138 124 85 0 0}) (robotIndex 0) (subdevice YarpOpenraveControlboard)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[0]: 36 (rightLacqueyFetch/ExternalFingerProximal)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[1]: 37 (rightLacqueyFetch/ExternalFingerDistal)
[debug] DeviceDriverImpl.cpp:80 open(): pcontrol: 0x557c8a808190, genericmulticontroller
[info] DeviceDriverImpl.cpp:92 open(): Detected genericmulticontroller, which will be used.
[debug] DeviceDriverImpl.cpp:99 open(): pcontrol: 0x557c8a808190, genericmulticontroller
[debug] DeviceDriverImpl.cpp:116 open(): Attach individual controller for manipulatorIDs[0]: 36
[debug] DeviceDriverImpl.cpp:116 open(): Attach individual controller for manipulatorIDs[1]: 37
[INFO]created device <YarpOpenraveControlboard>. See C++ class roboticslab::YarpOpenraveControlboard for documentation.
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 2 axes are present
[DEBUG]joints parameter is 2
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 2 axes are present
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 2 axes are present
[INFO]/teoSim/rightLacqueyFetch/externalFinger : no ROS initialization required 
[INFO]/teoSim/rightLacqueyFetch/externalFinger  initting YARP initialization 
yarp: Port /teoSim/rightLacqueyFetch/externalFinger/rpc:i active at tcp://10.118.40.231:10008/
yarp: Port /teoSim/rightLacqueyFetch/externalFinger/command:i active at tcp://10.118.40.231:10009/
yarp: Port /teoSim/rightLacqueyFetch/externalFinger/state:o active at tcp://10.118.40.231:10010/
yarp: Port /teoSim/rightLacqueyFetch/externalFinger/stateExt:o active at tcp://10.118.40.231:10011/
[INFO]created wrapper <controlboardwrapper2>. See C++ class ControlBoardWrapper for documentation.
[success] OpenraveYarpPluginLoader.cpp:358 Open(): Valid yarp plugin (id 1).
[DEBUG]ControlBoardWrapper2: 'period' parameter missing, using default thread period = 20ms 
[DEBUG]opening controlBoardWrapper2 subdevice
[debug] DeviceDriverImpl.cpp:27 open(): config: (allManipulators) (device YarpOpenraveControlboard) (manipulatorIndex 2) (name "/teoSim/rightLacqueyFetch/internalFinger") (penv {128 39 91 138 124 85 0 0 160 42 91 138 124 85 0 0}) (robotIndex 0) (subdevice YarpOpenraveControlboard)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[0]: 38 (rightLacqueyFetch/InternalFingerProximal)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[1]: 39 (rightLacqueyFetch/InternalFingerDistal)
[debug] DeviceDriverImpl.cpp:80 open(): pcontrol: 0x557c8a808190, genericmulticontroller
[info] DeviceDriverImpl.cpp:92 open(): Detected genericmulticontroller, which will be used.
[debug] DeviceDriverImpl.cpp:99 open(): pcontrol: 0x557c8a808190, genericmulticontroller
[debug] DeviceDriverImpl.cpp:116 open(): Attach individual controller for manipulatorIDs[0]: 38
[debug] DeviceDriverImpl.cpp:116 open(): Attach individual controller for manipulatorIDs[1]: 39
[INFO]created device <YarpOpenraveControlboard>. See C++ class roboticslab::YarpOpenraveControlboard for documentation.
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 2 axes are present
[DEBUG]joints parameter is 2
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 2 axes are present
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 2 axes are present
[INFO]/teoSim/rightLacqueyFetch/internalFinger : no ROS initialization required 
[INFO]/teoSim/rightLacqueyFetch/internalFinger  initting YARP initialization 
yarp: Port /teoSim/rightLacqueyFetch/internalFinger/rpc:i active at tcp://10.118.40.231:10012/
yarp: Port /teoSim/rightLacqueyFetch/internalFinger/command:i active at tcp://10.118.40.231:10013/
yarp: Port /teoSim/rightLacqueyFetch/internalFinger/state:o active at tcp://10.118.40.231:10014/
yarp: Port /teoSim/rightLacqueyFetch/internalFinger/stateExt:o active at tcp://10.118.40.231:10015/
[INFO]created wrapper <controlboardwrapper2>. See C++ class ControlBoardWrapper for documentation.
[success] OpenraveYarpPluginLoader.cpp:358 Open(): Valid yarp plugin (id 2).
[DEBUG]ControlBoardWrapper2: 'period' parameter missing, using default thread period = 20ms 
[DEBUG]opening controlBoardWrapper2 subdevice
[debug] DeviceDriverImpl.cpp:27 open(): config: (allManipulators) (device YarpOpenraveControlboard) (manipulatorIndex 3) (name "/teoSim/leftLacqueyFetch/thumb") (penv {128 39 91 138 124 85 0 0 160 42 91 138 124 85 0 0}) (robotIndex 0) (subdevice YarpOpenraveControlboard)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[0]: 28 (leftLacqueyFetch/ThumbProximal)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[1]: 29 (leftLacqueyFetch/ThumbDistal)
[debug] DeviceDriverImpl.cpp:80 open(): pcontrol: 0x557c8a808190, genericmulticontroller
[info] DeviceDriverImpl.cpp:92 open(): Detected genericmulticontroller, which will be used.
[debug] DeviceDriverImpl.cpp:99 open(): pcontrol: 0x557c8a808190, genericmulticontroller
[debug] DeviceDriverImpl.cpp:116 open(): Attach individual controller for manipulatorIDs[0]: 28
[debug] DeviceDriverImpl.cpp:116 open(): Attach individual controller for manipulatorIDs[1]: 29
[INFO]created device <YarpOpenraveControlboard>. See C++ class roboticslab::YarpOpenraveControlboard for documentation.
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 2 axes are present
[DEBUG]joints parameter is 2
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 2 axes are present
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 2 axes are present
[INFO]/teoSim/leftLacqueyFetch/thumb : no ROS initialization required 
[INFO]/teoSim/leftLacqueyFetch/thumb  initting YARP initialization 
yarp: Port /teoSim/leftLacqueyFetch/thumb/rpc:i active at tcp://10.118.40.231:10016/
yarp: Port /teoSim/leftLacqueyFetch/thumb/command:i active at tcp://10.118.40.231:10017/
yarp: Port /teoSim/leftLacqueyFetch/thumb/state:o active at tcp://10.118.40.231:10018/
yarp: Port /teoSim/leftLacqueyFetch/thumb/stateExt:o active at tcp://10.118.40.231:10019/
[INFO]created wrapper <controlboardwrapper2>. See C++ class ControlBoardWrapper for documentation.
[success] OpenraveYarpPluginLoader.cpp:358 Open(): Valid yarp plugin (id 3).
[DEBUG]ControlBoardWrapper2: 'period' parameter missing, using default thread period = 20ms 
[DEBUG]opening controlBoardWrapper2 subdevice
[debug] DeviceDriverImpl.cpp:27 open(): config: (allManipulators) (device YarpOpenraveControlboard) (manipulatorIndex 4) (name "/teoSim/leftLacqueyFetch/externalFinger") (penv {128 39 91 138 124 85 0 0 160 42 91 138 124 85 0 0}) (robotIndex 0) (subdevice YarpOpenraveControlboard)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[0]: 30 (leftLacqueyFetch/ExternalFingerProximal)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[1]: 31 (leftLacqueyFetch/ExternalFingerDistal)
[debug] DeviceDriverImpl.cpp:80 open(): pcontrol: 0x557c8a808190, genericmulticontroller
[info] DeviceDriverImpl.cpp:92 open(): Detected genericmulticontroller, which will be used.
[debug] DeviceDriverImpl.cpp:99 open(): pcontrol: 0x557c8a808190, genericmulticontroller
[debug] DeviceDriverImpl.cpp:116 open(): Attach individual controller for manipulatorIDs[0]: 30
[debug] DeviceDriverImpl.cpp:116 open(): Attach individual controller for manipulatorIDs[1]: 31
[INFO]created device <YarpOpenraveControlboard>. See C++ class roboticslab::YarpOpenraveControlboard for documentation.
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 2 axes are present
[DEBUG]joints parameter is 2
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 2 axes are present
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 2 axes are present
[INFO]/teoSim/leftLacqueyFetch/externalFinger : no ROS initialization required 
[INFO]/teoSim/leftLacqueyFetch/externalFinger  initting YARP initialization 
yarp: Port /teoSim/leftLacqueyFetch/externalFinger/rpc:i active at tcp://10.118.40.231:10020/
yarp: Port /teoSim/leftLacqueyFetch/externalFinger/command:i active at tcp://10.118.40.231:10021/
yarp: Port /teoSim/leftLacqueyFetch/externalFinger/state:o active at tcp://10.118.40.231:10022/
yarp: Port /teoSim/leftLacqueyFetch/externalFinger/stateExt:o active at tcp://10.118.40.231:10023/
[INFO]created wrapper <controlboardwrapper2>. See C++ class ControlBoardWrapper for documentation.
[success] OpenraveYarpPluginLoader.cpp:358 Open(): Valid yarp plugin (id 4).
[DEBUG]ControlBoardWrapper2: 'period' parameter missing, using default thread period = 20ms 
[DEBUG]opening controlBoardWrapper2 subdevice
[debug] DeviceDriverImpl.cpp:27 open(): config: (allManipulators) (device YarpOpenraveControlboard) (manipulatorIndex 5) (name "/teoSim/leftLacqueyFetch/internalFinger") (penv {128 39 91 138 124 85 0 0 160 42 91 138 124 85 0 0}) (robotIndex 0) (subdevice YarpOpenraveControlboard)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[0]: 32 (leftLacqueyFetch/InternalFingerProximal)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[1]: 33 (leftLacqueyFetch/InternalFingerDistal)
[debug] DeviceDriverImpl.cpp:80 open(): pcontrol: 0x557c8a808190, genericmulticontroller
[info] DeviceDriverImpl.cpp:92 open(): Detected genericmulticontroller, which will be used.
[debug] DeviceDriverImpl.cpp:99 open(): pcontrol: 0x557c8a808190, genericmulticontroller
[debug] DeviceDriverImpl.cpp:116 open(): Attach individual controller for manipulatorIDs[0]: 32
[debug] DeviceDriverImpl.cpp:116 open(): Attach individual controller for manipulatorIDs[1]: 33
[INFO]created device <YarpOpenraveControlboard>. See C++ class roboticslab::YarpOpenraveControlboard for documentation.
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 2 axes are present
[DEBUG]joints parameter is 2
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 2 axes are present
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 2 axes are present
[INFO]/teoSim/leftLacqueyFetch/internalFinger : no ROS initialization required 
[INFO]/teoSim/leftLacqueyFetch/internalFinger  initting YARP initialization 
yarp: Port /teoSim/leftLacqueyFetch/internalFinger/rpc:i active at tcp://10.118.40.231:10024/
yarp: Port /teoSim/leftLacqueyFetch/internalFinger/command:i active at tcp://10.118.40.231:10025/
yarp: Port /teoSim/leftLacqueyFetch/internalFinger/state:o active at tcp://10.118.40.231:10026/
yarp: Port /teoSim/leftLacqueyFetch/internalFinger/stateExt:o active at tcp://10.118.40.231:10027/
[INFO]created wrapper <controlboardwrapper2>. See C++ class ControlBoardWrapper for documentation.
[success] OpenraveYarpPluginLoader.cpp:358 Open(): Valid yarp plugin (id 5).
[DEBUG]ControlBoardWrapper2: 'period' parameter missing, using default thread period = 20ms 
[DEBUG]opening controlBoardWrapper2 subdevice
[debug] DeviceDriverImpl.cpp:27 open(): config: (allManipulators) (device YarpOpenraveControlboard) (manipulatorIndex 6) (name "/teoSim/trunk") (penv {128 39 91 138 124 85 0 0 160 42 91 138 124 85 0 0}) (robotIndex 0) (subdevice YarpOpenraveControlboard)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[0]: 0 (AxialTrunk)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[1]: 1 (FrontalTrunk)
[debug] DeviceDriverImpl.cpp:80 open(): pcontrol: 0x557c8a808190, genericmulticontroller
[info] DeviceDriverImpl.cpp:92 open(): Detected genericmulticontroller, which will be used.
[debug] DeviceDriverImpl.cpp:99 open(): pcontrol: 0x557c8a808190, genericmulticontroller
[debug] DeviceDriverImpl.cpp:116 open(): Attach individual controller for manipulatorIDs[0]: 0
[debug] DeviceDriverImpl.cpp:116 open(): Attach individual controller for manipulatorIDs[1]: 1
[INFO]created device <YarpOpenraveControlboard>. See C++ class roboticslab::YarpOpenraveControlboard for documentation.
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 2 axes are present
[DEBUG]joints parameter is 2
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 2 axes are present
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 2 axes are present
[INFO]/teoSim/trunk : no ROS initialization required 
[INFO]/teoSim/trunk  initting YARP initialization 
yarp: Port /teoSim/trunk/rpc:i active at tcp://10.118.40.231:10028/
yarp: Port /teoSim/trunk/command:i active at tcp://10.118.40.231:10029/
yarp: Port /teoSim/trunk/state:o active at tcp://10.118.40.231:10030/
yarp: Port /teoSim/trunk/stateExt:o active at tcp://10.118.40.231:10031/
[INFO]created wrapper <controlboardwrapper2>. See C++ class ControlBoardWrapper for documentation.
[success] OpenraveYarpPluginLoader.cpp:358 Open(): Valid yarp plugin (id 6).
[DEBUG]ControlBoardWrapper2: 'period' parameter missing, using default thread period = 20ms 
[DEBUG]opening controlBoardWrapper2 subdevice
[debug] DeviceDriverImpl.cpp:27 open(): config: (allManipulators) (device YarpOpenraveControlboard) (manipulatorIndex 7) (name "/teoSim/head") (penv {128 39 91 138 124 85 0 0 160 42 91 138 124 85 0 0}) (robotIndex 0) (subdevice YarpOpenraveControlboard)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[0]: 2 (AxialNeck)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[1]: 3 (FrontalNeck)
[debug] DeviceDriverImpl.cpp:80 open(): pcontrol: 0x557c8a808190, genericmulticontroller
[info] DeviceDriverImpl.cpp:92 open(): Detected genericmulticontroller, which will be used.
[debug] DeviceDriverImpl.cpp:99 open(): pcontrol: 0x557c8a808190, genericmulticontroller
[debug] DeviceDriverImpl.cpp:116 open(): Attach individual controller for manipulatorIDs[0]: 2
[debug] DeviceDriverImpl.cpp:116 open(): Attach individual controller for manipulatorIDs[1]: 3
[INFO]created device <YarpOpenraveControlboard>. See C++ class roboticslab::YarpOpenraveControlboard for documentation.
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 2 axes are present
[DEBUG]joints parameter is 2
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 2 axes are present
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 2 axes are present
[INFO]/teoSim/head : no ROS initialization required 
[INFO]/teoSim/head  initting YARP initialization 
yarp: Port /teoSim/head/rpc:i active at tcp://10.118.40.231:10032/
yarp: Port /teoSim/head/command:i active at tcp://10.118.40.231:10033/
yarp: Port /teoSim/head/state:o active at tcp://10.118.40.231:10034/
yarp: Port /teoSim/head/stateExt:o active at tcp://10.118.40.231:10035/
[INFO]created wrapper <controlboardwrapper2>. See C++ class ControlBoardWrapper for documentation.
[success] OpenraveYarpPluginLoader.cpp:358 Open(): Valid yarp plugin (id 7).
[DEBUG]ControlBoardWrapper2: 'period' parameter missing, using default thread period = 20ms 
[DEBUG]opening controlBoardWrapper2 subdevice
[debug] DeviceDriverImpl.cpp:27 open(): config: (allManipulators) (device YarpOpenraveControlboard) (manipulatorIndex 8) (name "/teoSim/rightArm") (penv {128 39 91 138 124 85 0 0 160 42 91 138 124 85 0 0}) (robotIndex 0) (subdevice YarpOpenraveControlboard)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[0]: 4 (FrontalRightShoulder)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[1]: 5 (SagittalRightShoulder)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[2]: 6 (AxialRightShoulder)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[3]: 7 (FrontalRightElbow)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[4]: 8 (AxialRightWrist)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[5]: 9 (FrontalRightWrist)
[debug] DeviceDriverImpl.cpp:80 open(): pcontrol: 0x557c8a808190, genericmulticontroller
[info] DeviceDriverImpl.cpp:92 open(): Detected genericmulticontroller, which will be used.
[debug] DeviceDriverImpl.cpp:99 open(): pcontrol: 0x557c8a808190, genericmulticontroller
[debug] DeviceDriverImpl.cpp:116 open(): Attach individual controller for manipulatorIDs[0]: 4
[debug] DeviceDriverImpl.cpp:116 open(): Attach individual controller for manipulatorIDs[1]: 5
[debug] DeviceDriverImpl.cpp:116 open(): Attach individual controller for manipulatorIDs[2]: 6
[debug] DeviceDriverImpl.cpp:116 open(): Attach individual controller for manipulatorIDs[3]: 7
[debug] DeviceDriverImpl.cpp:116 open(): Attach individual controller for manipulatorIDs[4]: 8
[debug] DeviceDriverImpl.cpp:116 open(): Attach individual controller for manipulatorIDs[5]: 9
[INFO]created device <YarpOpenraveControlboard>. See C++ class roboticslab::YarpOpenraveControlboard for documentation.
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 6 axes are present
[DEBUG]joints parameter is 6
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 6 axes are present
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 6 axes are present
[INFO]/teoSim/rightArm : no ROS initialization required 
[INFO]/teoSim/rightArm  initting YARP initialization 
yarp: Port /teoSim/rightArm/rpc:i active at tcp://10.118.40.231:10036/
yarp: Port /teoSim/rightArm/command:i active at tcp://10.118.40.231:10037/
yarp: Port /teoSim/rightArm/state:o active at tcp://10.118.40.231:10038/
yarp: Port /teoSim/rightArm/stateExt:o active at tcp://10.118.40.231:10039/
[INFO]created wrapper <controlboardwrapper2>. See C++ class ControlBoardWrapper for documentation.
[success] OpenraveYarpPluginLoader.cpp:358 Open(): Valid yarp plugin (id 8).
[DEBUG]ControlBoardWrapper2: 'period' parameter missing, using default thread period = 20ms 
[DEBUG]opening controlBoardWrapper2 subdevice
[debug] DeviceDriverImpl.cpp:27 open(): config: (allManipulators) (device YarpOpenraveControlboard) (manipulatorIndex 9) (name "/teoSim/trunkAndRightArm") (penv {128 39 91 138 124 85 0 0 160 42 91 138 124 85 0 0}) (robotIndex 0) (subdevice YarpOpenraveControlboard)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[0]: 0 (AxialTrunk)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[1]: 1 (FrontalTrunk)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[2]: 4 (FrontalRightShoulder)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[3]: 5 (SagittalRightShoulder)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[4]: 6 (AxialRightShoulder)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[5]: 7 (FrontalRightElbow)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[6]: 8 (AxialRightWrist)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[7]: 9 (FrontalRightWrist)
[debug] DeviceDriverImpl.cpp:80 open(): pcontrol: 0x557c8a808190, genericmulticontroller
[info] DeviceDriverImpl.cpp:92 open(): Detected genericmulticontroller, which will be used.
[debug] DeviceDriverImpl.cpp:99 open(): pcontrol: 0x557c8a808190, genericmulticontroller
[debug] DeviceDriverImpl.cpp:109 open(): EXPERIMENTAL: Using existing individual controller for manipulatorIDs[0]: 0 (idealcontroller)
[debug] DeviceDriverImpl.cpp:109 open(): EXPERIMENTAL: Using existing individual controller for manipulatorIDs[1]: 1 (idealcontroller)
[debug] DeviceDriverImpl.cpp:109 open(): EXPERIMENTAL: Using existing individual controller for manipulatorIDs[2]: 4 (idealcontroller)
[debug] DeviceDriverImpl.cpp:109 open(): EXPERIMENTAL: Using existing individual controller for manipulatorIDs[3]: 5 (idealcontroller)
[debug] DeviceDriverImpl.cpp:109 open(): EXPERIMENTAL: Using existing individual controller for manipulatorIDs[4]: 6 (idealcontroller)
[debug] DeviceDriverImpl.cpp:109 open(): EXPERIMENTAL: Using existing individual controller for manipulatorIDs[5]: 7 (idealcontroller)
[debug] DeviceDriverImpl.cpp:109 open(): EXPERIMENTAL: Using existing individual controller for manipulatorIDs[6]: 8 (idealcontroller)
[debug] DeviceDriverImpl.cpp:109 open(): EXPERIMENTAL: Using existing individual controller for manipulatorIDs[7]: 9 (idealcontroller)
[INFO]created device <YarpOpenraveControlboard>. See C++ class roboticslab::YarpOpenraveControlboard for documentation.
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 8 axes are present
[DEBUG]joints parameter is 8
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 8 axes are present
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 8 axes are present
[INFO]/teoSim/trunkAndRightArm : no ROS initialization required 
[INFO]/teoSim/trunkAndRightArm  initting YARP initialization 
yarp: Port /teoSim/trunkAndRightArm/rpc:i active at tcp://10.118.40.231:10040/
yarp: Port /teoSim/trunkAndRightArm/command:i active at tcp://10.118.40.231:10041/
yarp: Port /teoSim/trunkAndRightArm/state:o active at tcp://10.118.40.231:10042/
yarp: Port /teoSim/trunkAndRightArm/stateExt:o active at tcp://10.118.40.231:10043/
[INFO]created wrapper <controlboardwrapper2>. See C++ class ControlBoardWrapper for documentation.
[success] OpenraveYarpPluginLoader.cpp:358 Open(): Valid yarp plugin (id 9).
[DEBUG]ControlBoardWrapper2: 'period' parameter missing, using default thread period = 20ms 
[DEBUG]opening controlBoardWrapper2 subdevice
[debug] DeviceDriverImpl.cpp:27 open(): config: (allManipulators) (device YarpOpenraveControlboard) (manipulatorIndex 10) (name "/teoSim/leftArm") (penv {128 39 91 138 124 85 0 0 160 42 91 138 124 85 0 0}) (robotIndex 0) (subdevice YarpOpenraveControlboard)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[0]: 10 (FrontalLeftShoulder)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[1]: 11 (SagittalLeftShoulder)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[2]: 12 (AxialLeftShoulder)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[3]: 13 (FrontalLeftElbow)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[4]: 14 (AxialLeftWrist)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[5]: 15 (FrontalLeftWrist)
[debug] DeviceDriverImpl.cpp:80 open(): pcontrol: 0x557c8a808190, genericmulticontroller
[info] DeviceDriverImpl.cpp:92 open(): Detected genericmulticontroller, which will be used.
[debug] DeviceDriverImpl.cpp:99 open(): pcontrol: 0x557c8a808190, genericmulticontroller
[debug] DeviceDriverImpl.cpp:116 open(): Attach individual controller for manipulatorIDs[0]: 10
[debug] DeviceDriverImpl.cpp:116 open(): Attach individual controller for manipulatorIDs[1]: 11
[debug] DeviceDriverImpl.cpp:116 open(): Attach individual controller for manipulatorIDs[2]: 12
[debug] DeviceDriverImpl.cpp:116 open(): Attach individual controller for manipulatorIDs[3]: 13
[debug] DeviceDriverImpl.cpp:116 open(): Attach individual controller for manipulatorIDs[4]: 14
[debug] DeviceDriverImpl.cpp:116 open(): Attach individual controller for manipulatorIDs[5]: 15
[INFO]created device <YarpOpenraveControlboard>. See C++ class roboticslab::YarpOpenraveControlboard for documentation.
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 6 axes are present
[DEBUG]joints parameter is 6
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 6 axes are present
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 6 axes are present
[INFO]/teoSim/leftArm : no ROS initialization required 
[INFO]/teoSim/leftArm  initting YARP initialization 
yarp: Port /teoSim/leftArm/rpc:i active at tcp://10.118.40.231:10044/
yarp: Port /teoSim/leftArm/command:i active at tcp://10.118.40.231:10045/
yarp: Port /teoSim/leftArm/state:o active at tcp://10.118.40.231:10046/
yarp: Port /teoSim/leftArm/stateExt:o active at tcp://10.118.40.231:10047/
[INFO]created wrapper <controlboardwrapper2>. See C++ class ControlBoardWrapper for documentation.
[success] OpenraveYarpPluginLoader.cpp:358 Open(): Valid yarp plugin (id 10).
[DEBUG]ControlBoardWrapper2: 'period' parameter missing, using default thread period = 20ms 
[DEBUG]opening controlBoardWrapper2 subdevice
[debug] DeviceDriverImpl.cpp:27 open(): config: (allManipulators) (device YarpOpenraveControlboard) (manipulatorIndex 11) (name "/teoSim/trunkAndLeftArm") (penv {128 39 91 138 124 85 0 0 160 42 91 138 124 85 0 0}) (robotIndex 0) (subdevice YarpOpenraveControlboard)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[0]: 0 (AxialTrunk)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[1]: 1 (FrontalTrunk)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[2]: 10 (FrontalLeftShoulder)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[3]: 11 (SagittalLeftShoulder)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[4]: 12 (AxialLeftShoulder)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[5]: 13 (FrontalLeftElbow)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[6]: 14 (AxialLeftWrist)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[7]: 15 (FrontalLeftWrist)
[debug] DeviceDriverImpl.cpp:80 open(): pcontrol: 0x557c8a808190, genericmulticontroller
[info] DeviceDriverImpl.cpp:92 open(): Detected genericmulticontroller, which will be used.
[debug] DeviceDriverImpl.cpp:99 open(): pcontrol: 0x557c8a808190, genericmulticontroller
[debug] DeviceDriverImpl.cpp:109 open(): EXPERIMENTAL: Using existing individual controller for manipulatorIDs[0]: 0 (idealcontroller)
[debug] DeviceDriverImpl.cpp:109 open(): EXPERIMENTAL: Using existing individual controller for manipulatorIDs[1]: 1 (idealcontroller)
[debug] DeviceDriverImpl.cpp:109 open(): EXPERIMENTAL: Using existing individual controller for manipulatorIDs[2]: 10 (idealcontroller)
[debug] DeviceDriverImpl.cpp:109 open(): EXPERIMENTAL: Using existing individual controller for manipulatorIDs[3]: 11 (idealcontroller)
[debug] DeviceDriverImpl.cpp:109 open(): EXPERIMENTAL: Using existing individual controller for manipulatorIDs[4]: 12 (idealcontroller)
[debug] DeviceDriverImpl.cpp:109 open(): EXPERIMENTAL: Using existing individual controller for manipulatorIDs[5]: 13 (idealcontroller)
[debug] DeviceDriverImpl.cpp:109 open(): EXPERIMENTAL: Using existing individual controller for manipulatorIDs[6]: 14 (idealcontroller)
[debug] DeviceDriverImpl.cpp:109 open(): EXPERIMENTAL: Using existing individual controller for manipulatorIDs[7]: 15 (idealcontroller)
[INFO]created device <YarpOpenraveControlboard>. See C++ class roboticslab::YarpOpenraveControlboard for documentation.
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 8 axes are present
[DEBUG]joints parameter is 8
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 8 axes are present
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 8 axes are present
[INFO]/teoSim/trunkAndLeftArm : no ROS initialization required 
[INFO]/teoSim/trunkAndLeftArm  initting YARP initialization 
yarp: Port /teoSim/trunkAndLeftArm/rpc:i active at tcp://10.118.40.231:10048/
yarp: Port /teoSim/trunkAndLeftArm/command:i active at tcp://10.118.40.231:10049/
yarp: Port /teoSim/trunkAndLeftArm/state:o active at tcp://10.118.40.231:10050/
yarp: Port /teoSim/trunkAndLeftArm/stateExt:o active at tcp://10.118.40.231:10051/
[INFO]created wrapper <controlboardwrapper2>. See C++ class ControlBoardWrapper for documentation.
[success] OpenraveYarpPluginLoader.cpp:358 Open(): Valid yarp plugin (id 11).
[DEBUG]ControlBoardWrapper2: 'period' parameter missing, using default thread period = 20ms 
[DEBUG]opening controlBoardWrapper2 subdevice
[debug] DeviceDriverImpl.cpp:27 open(): config: (allManipulators) (device YarpOpenraveControlboard) (manipulatorIndex 12) (name "/teoSim/rightLeg") (penv {128 39 91 138 124 85 0 0 160 42 91 138 124 85 0 0}) (robotIndex 0) (subdevice YarpOpenraveControlboard)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[0]: 16 (AxialRightHip)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[1]: 17 (SagittalRightHip)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[2]: 18 (FrontalRightHip)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[3]: 19 (FrontalRightKnee)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[4]: 20 (FrontalRightAnkle)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[5]: 21 (SagittalRightAnkle)
[debug] DeviceDriverImpl.cpp:80 open(): pcontrol: 0x557c8a808190, genericmulticontroller
[info] DeviceDriverImpl.cpp:92 open(): Detected genericmulticontroller, which will be used.
[debug] DeviceDriverImpl.cpp:99 open(): pcontrol: 0x557c8a808190, genericmulticontroller
[debug] DeviceDriverImpl.cpp:116 open(): Attach individual controller for manipulatorIDs[0]: 16
[debug] DeviceDriverImpl.cpp:116 open(): Attach individual controller for manipulatorIDs[1]: 17
[debug] DeviceDriverImpl.cpp:116 open(): Attach individual controller for manipulatorIDs[2]: 18
[debug] DeviceDriverImpl.cpp:116 open(): Attach individual controller for manipulatorIDs[3]: 19
[debug] DeviceDriverImpl.cpp:116 open(): Attach individual controller for manipulatorIDs[4]: 20
[debug] DeviceDriverImpl.cpp:116 open(): Attach individual controller for manipulatorIDs[5]: 21
[INFO]created device <YarpOpenraveControlboard>. See C++ class roboticslab::YarpOpenraveControlboard for documentation.
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 6 axes are present
[DEBUG]joints parameter is 6
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 6 axes are present
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 6 axes are present
[INFO]/teoSim/rightLeg : no ROS initialization required 
[INFO]/teoSim/rightLeg  initting YARP initialization 
yarp: Port /teoSim/rightLeg/rpc:i active at tcp://10.118.40.231:10052/
yarp: Port /teoSim/rightLeg/command:i active at tcp://10.118.40.231:10053/
yarp: Port /teoSim/rightLeg/state:o active at tcp://10.118.40.231:10054/
yarp: Port /teoSim/rightLeg/stateExt:o active at tcp://10.118.40.231:10055/
[INFO]created wrapper <controlboardwrapper2>. See C++ class ControlBoardWrapper for documentation.
[success] OpenraveYarpPluginLoader.cpp:358 Open(): Valid yarp plugin (id 12).
[DEBUG]ControlBoardWrapper2: 'period' parameter missing, using default thread period = 20ms 
[DEBUG]opening controlBoardWrapper2 subdevice
[debug] DeviceDriverImpl.cpp:27 open(): config: (allManipulators) (device YarpOpenraveControlboard) (manipulatorIndex 13) (name "/teoSim/leftLeg") (penv {128 39 91 138 124 85 0 0 160 42 91 138 124 85 0 0}) (robotIndex 0) (subdevice YarpOpenraveControlboard)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[0]: 22 (AxialLeftHip)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[1]: 23 (SagittalLeftHip)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[2]: 24 (FrontalLeftHip)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[3]: 25 (FrontalLeftKnee)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[4]: 26 (FrontalLeftAnkle)
[debug] DeviceDriverImpl.cpp:64 open(): Get JointPtr for manipulatorIDs[5]: 27 (SagittalLeftAnkle)
[debug] DeviceDriverImpl.cpp:80 open(): pcontrol: 0x557c8a808190, genericmulticontroller
[info] DeviceDriverImpl.cpp:92 open(): Detected genericmulticontroller, which will be used.
[debug] DeviceDriverImpl.cpp:99 open(): pcontrol: 0x557c8a808190, genericmulticontroller
[debug] DeviceDriverImpl.cpp:116 open(): Attach individual controller for manipulatorIDs[0]: 22
[debug] DeviceDriverImpl.cpp:116 open(): Attach individual controller for manipulatorIDs[1]: 23
[debug] DeviceDriverImpl.cpp:116 open(): Attach individual controller for manipulatorIDs[2]: 24
[debug] DeviceDriverImpl.cpp:116 open(): Attach individual controller for manipulatorIDs[3]: 25
[debug] DeviceDriverImpl.cpp:116 open(): Attach individual controller for manipulatorIDs[4]: 26
[debug] DeviceDriverImpl.cpp:116 open(): Attach individual controller for manipulatorIDs[5]: 27
[INFO]created device <YarpOpenraveControlboard>. See C++ class roboticslab::YarpOpenraveControlboard for documentation.
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 6 axes are present
[DEBUG]joints parameter is 6
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 6 axes are present
[info] IPositionControlImpl.cpp:15 getAxes(): Reporting 6 axes are present
[INFO]/teoSim/leftLeg : no ROS initialization required 
[INFO]/teoSim/leftLeg  initting YARP initialization 
yarp: Port /teoSim/leftLeg/rpc:i active at tcp://10.118.40.231:10056/
yarp: Port /teoSim/leftLeg/command:i active at tcp://10.118.40.231:10057/
yarp: Port /teoSim/leftLeg/state:o active at tcp://10.118.40.231:10058/
yarp: Port /teoSim/leftLeg/stateExt:o active at tcp://10.118.40.231:10059/
[INFO]created wrapper <controlboardwrapper2>. See C++ class ControlBoardWrapper for documentation.
[success] OpenraveYarpPluginLoader.cpp:358 Open(): Valid yarp plugin (id 13).
[success] OpenraveYarpPluginLoader.cpp:159 main(): Open ids: 0 1 2 3 4 5 6 7 8 9 10 11 12 13

Everythink looks ok I think. Maybe it's a problem of OpenraveYarpPaintSquares?. The command Im using to launch BasicCartesianControl is:

yarpdev --device BasicCartesianControl --name /teoSim/rightArm/CartesianControl --from /usr/local/share/teo-configuration-files/contexts/kinematics/fixedTrunk-rightArm-fetch.ini --local /BasicCartesianControl/teoSim/rightArm --remote /teoSim/rightArm --ik st --angleRepr axisAngle
jgvictores commented 4 years ago

Okay, this codebase is quite old. openraveYarpPaintSquares.py in fact requires you NOT to use teoSim.

Recommendations:

RaulFdzbis commented 4 years ago

Restarted my computer, used yarp clean just in case, and tried openraveYarpPaintSquares.py and still no working. Same errors and same ports available.

RaulFdzbis commented 4 years ago

As always was a problem of having multiple residual installations of the same teo-openrave-models package, thanks to @jgvictores for helping me to solve the problem.

RaulFdzbis commented 4 years ago

Still, BasicCartesianControl is not working. Now, when i try to run it, it gives me a segmentation fault error. This is the output of BasicCartesianControl:

~$ yarpdev --device BasicCartesianControl --name /teoSim/rightArm/CartesianControl --from /usr/local/share/teo-configuration-files/contexts/kinematics/fixedTrunk-rightArm-fetch.ini --local /BasicCartesianControl/teoSim/rightArm --remote /teoSim/rightArm --ik st --angleRepr axisAngle
[info] DeviceDriverImpl.cpp:20 open(): Subdevice BasicCartesianControl
[debug] DeviceDriverImpl.cpp:11 open(): BasicCartesianControl config: (H0 (0 1 0 0 0 0 1 -0.3469 1 0 0 0.4982 0 0 0 1)) (HN (0 0 -1 -0.0975 -1 0 0 0 0 1 0 0 0 0 0 1)) (angleRepr axisAngle) (device BasicCartesianControl) (from "/usr/local/share/teo-configuration-files/contexts/kinematics/fixedTrunk-rightArm-fetch.ini") (ik st) (link_0 (offset 0.0) (D 0.0) (A 0.0) (alpha -90.0) (mass 0) (cog 0 0 0) (inertia 0 0 0)) (link_1 (offset -90.0) (D 0.0) (A 0.0) (alpha -90.0) (mass 0) (cog 0 0 0) (inertia 0 0 0)) (link_2 (offset -90.0) (D -0.32901) (A 0.0) (alpha -90.0) (mass 1.750625) (cog 0 0 0) (inertia 0 0 0)) (link_3 (offset 0.0) (D 0.0) (A 0.0) (alpha 90.0) (mass 0) (cog 0 0 0) (inertia 0 0 0)) (link_4 (offset 0.0) (D -0.215) (A 0.0) (alpha -90.0) (mass 2.396) (cog 0 0 0) (inertia 0 0 0)) (link_5 (offset -90.0) (D 0.0) (A -0.09) (alpha 0.0) (mass 0.3) (cog 0 0 0) (inertia 0 0 0)) (local "/BasicCartesianControl/teoSim/rightArm") (name "/teoSim/rightArm/CartesianControl") (numLinks 6) (remote "/teoSim/rightArm") (single_threaded 1) (subdevice BasicCartesianControl).
yarp: Port /BasicCartesianControl/teoSim/rightArm/rpc:o active at tcp://10.118.40.231:10009/
yarp: Port /BasicCartesianControl/teoSim/rightArm/command:o active at tcp://10.118.40.231:10010/
yarp: Port /BasicCartesianControl/teoSim/rightArm/stateExt:i active at tcp://10.118.40.231:10011/
yarp: Sending output from /BasicCartesianControl/teoSim/rightArm/rpc:o to /teoSim/rightArm/rpc:i using tcp
yarp: Sending output from /BasicCartesianControl/teoSim/rightArm/command:o to /teoSim/rightArm/command:i using udp
yarp: Receiving input from /teoSim/rightArm/stateExt:o to /BasicCartesianControl/teoSim/rightArm/stateExt:i using udp
[INFO]created device <remote_controlboard>. See C++ class RemoteControlBoard for documentation.
[info] DeviceDriverImpl.cpp:107 open(): numRobotJoints: 6.
[info] DeviceDriverImpl.cpp:149 open(): Joint 0 limits: [-98.100000,106.000000] [-30.000000,30.000000]
[info] DeviceDriverImpl.cpp:149 open(): Joint 1 limits: [-75.500000,22.400000] [-30.000000,30.000000]
[info] DeviceDriverImpl.cpp:149 open(): Joint 2 limits: [-80.100000,57.000000] [-30.000000,30.000000]
[info] DeviceDriverImpl.cpp:149 open(): Joint 3 limits: [-99.600000,98.400000] [-30.000000,30.000000]
[info] DeviceDriverImpl.cpp:149 open(): Joint 4 limits: [-80.400000,99.600000] [-30.000000,30.000000]
[info] DeviceDriverImpl.cpp:149 open(): Joint 5 limits: [-115.100000,44.700000] [-30.000000,30.000000]
[debug] DeviceDriverImpl.cpp:135 open(): config: (H0 (0 1 0 0 0 0 1 -0.3469 1 0 0 0.4982 0 0 0 1)) (HN (0 0 -1 -0.0975 -1 0 0 0 0 1 0 0 0 0 0 1)) (angleRepr axisAngle) (device KdlSolver) (from "/usr/local/share/teo-configuration-files/contexts/kinematics/fixedTrunk-rightArm-fetch.ini") (ik st) (link_0 (offset 0.0) (D 0.0) (A 0.0) (alpha -90.0) (mass 0) (cog 0 0 0) (inertia 0 0 0)) (link_1 (offset -90.0) (D 0.0) (A 0.0) (alpha -90.0) (mass 0) (cog 0 0 0) (inertia 0 0 0)) (link_2 (offset -90.0) (D -0.32901) (A 0.0) (alpha -90.0) (mass 1.750625) (cog 0 0 0) (inertia 0 0 0)) (link_3 (offset 0.0) (D 0.0) (A 0.0) (alpha 90.0) (mass 0) (cog 0 0 0) (inertia 0 0 0)) (link_4 (offset 0.0) (D -0.215) (A 0.0) (alpha -90.0) (mass 2.396) (cog 0 0 0) (inertia 0 0 0)) (link_5 (offset -90.0) (D 0.0) (A -0.09) (alpha 0.0) (mass 0.3) (cog 0 0 0) (inertia 0 0 0)) (local "/BasicCartesianControl/teoSim/rightArm") (maxs (106.0 22.4 57.0 98.4 99.6 44.7)) (mins (-98.1 -75.5 -80.1 -99.6 -80.4 -115.1)) (name "/teoSim/rightArm/CartesianControl") (numLinks 6) (remote "/teoSim/rightArm") (single_threaded 1) (subdevice BasicCartesianControl).
[info] DeviceDriverImpl.cpp:139 open(): kinematics: none.ini [none.ini]
||| did not find none.ini
yarp: cannot read from 
[debug] DeviceDriverImpl.cpp:150 open(): fullConfig: (H0 (0 1 0 0 0 0 1 -0.3469 1 0 0 0.4982 0 0 0 1)) (HN (0 0 -1 -0.0975 -1 0 0 0 0 1 0 0 0 0 0 1)) (angleRepr axisAngle) (device KdlSolver) (from "/usr/local/share/teo-configuration-files/contexts/kinematics/fixedTrunk-rightArm-fetch.ini") (ik st) (link_0 (offset 0.0) (D 0.0) (A 0.0) (alpha -90.0) (mass 0) (cog 0 0 0) (inertia 0 0 0)) (link_1 (offset -90.0) (D 0.0) (A 0.0) (alpha -90.0) (mass 0) (cog 0 0 0) (inertia 0 0 0)) (link_2 (offset -90.0) (D -0.32901) (A 0.0) (alpha -90.0) (mass 1.750625) (cog 0 0 0) (inertia 0 0 0)) (link_3 (offset 0.0) (D 0.0) (A 0.0) (alpha 90.0) (mass 0) (cog 0 0 0) (inertia 0 0 0)) (link_4 (offset 0.0) (D -0.215) (A 0.0) (alpha -90.0) (mass 2.396) (cog 0 0 0) (inertia 0 0 0)) (link_5 (offset -90.0) (D 0.0) (A -0.09) (alpha 0.0) (mass 0.3) (cog 0 0 0) (inertia 0 0 0)) (local "/BasicCartesianControl/teoSim/rightArm") (maxs (106.0 22.4 57.0 98.4 99.6 44.7)) (mins (-98.1 -75.5 -80.1 -99.6 -80.4 -115.1)) (name "/teoSim/rightArm/CartesianControl") (numLinks 6) (remote "/teoSim/rightArm") (single_threaded 1) (subdevice BasicCartesianControl).
[info] DeviceDriverImpl.cpp:154 open(): numLinks: 6 [1]
[info] DeviceDriverImpl.cpp:166 open(): gravity: 0.0 0.0 -9.81 [0.0 0.0 -9.81]
[info] DeviceDriverImpl.cpp:182 open(): H0:
 0.000000    1.000000    0.000000    0.000000
 0.000000    0.000000    1.000000   -0.346900
 1.000000    0.000000    0.000000    0.498200
 0.000000    0.000000    0.000000    1.000000
[ 1.000000   0.000000    0.000000    0.000000
 0.000000    1.000000    0.000000    0.000000
 0.000000    0.000000    1.000000    0.000000
 0.000000    0.000000    0.000000    1.000000]
[info] DeviceDriverImpl.cpp:209 open(): Added: link_0 (offset 0.000000) (D 0.000000) (A 0.000000) (alpha -90.000000) (mass 0.000000) (cog 0.000000 0.000000 0.000000) (inertia 0.000000 0.000000 0.000000)
[info] DeviceDriverImpl.cpp:209 open(): Added: link_1 (offset -90.000000) (D 0.000000) (A 0.000000) (alpha -90.000000) (mass 0.000000) (cog 0.000000 0.000000 0.000000) (inertia 0.000000 0.000000 0.000000)
[info] DeviceDriverImpl.cpp:209 open(): Added: link_2 (offset -90.000000) (D -0.329010) (A 0.000000) (alpha -90.000000) (mass 1.750625) (cog 0.000000 0.000000 0.000000) (inertia 0.000000 0.000000 0.000000)
[info] DeviceDriverImpl.cpp:209 open(): Added: link_3 (offset 0.000000) (D 0.000000) (A 0.000000) (alpha 90.000000) (mass 0.000000) (cog 0.000000 0.000000 0.000000) (inertia 0.000000 0.000000 0.000000)
[info] DeviceDriverImpl.cpp:209 open(): Added: link_4 (offset 0.000000) (D -0.215000) (A 0.000000) (alpha -90.000000) (mass 2.396000) (cog 0.000000 0.000000 0.000000) (inertia 0.000000 0.000000 0.000000)
[info] DeviceDriverImpl.cpp:209 open(): Added: link_5 (offset -90.000000) (D 0.000000) (A -0.090000) (alpha 0.000000) (mass 0.300000) (cog 0.000000 0.000000 0.000000) (inertia 0.000000 0.000000 0.000000)
[info] DeviceDriverImpl.cpp:280 open(): HN:
 0.000000    0.000000   -1.000000   -0.097500
-1.000000    0.000000    0.000000    0.000000
 0.000000    1.000000    0.000000    0.000000
 0.000000    0.000000    0.000000    1.000000
[ 1.000000   0.000000    0.000000    0.000000
 0.000000    1.000000    0.000000    0.000000
 0.000000    0.000000    1.000000    0.000000
 0.000000    0.000000    0.000000    1.000000]
[info] DeviceDriverImpl.cpp:282 open(): Chain number of segments (post- H0 and HN): 8
[info] DeviceDriverImpl.cpp:283 open(): Chain number of joints (post- H0 and HN): 6
[INFO]created device <KdlSolver>. See C++ class roboticslab::KdlSolver for documentation.
[info] DeviceDriverImpl.cpp:175 open(): numSolverJoints: 6.
[INFO]created device <BasicCartesianControl>. See C++ class roboticslab::BasicCartesianControl for documentation.
yarp: Port /teoSim/rightArm/CartesianControl/rpc:s active at tcp://10.118.40.231:10016/
yarp: Port /teoSim/rightArm/CartesianControl/command:i active at tcp://10.118.40.231:10017/
yarp: Port /teoSim/rightArm/CartesianControl/state:o active at tcp://10.118.40.231:10018/
Segmentation fault (core dumped)
jgvictores commented 4 years ago
  1. I would have the intuition it's related to the 6 vs 8 (not sure if joints or links) discrepancy.
  2. Among several runs, I did observe a [error] ICartesianControlImpl.cpp:35 stat(): getEncoders failed.

@PeterBowman Any ideas?

jgvictores commented 4 years ago

@PeterBowman thanks!

RaulFdzbis commented 4 years ago

I have just test it and is working like a charm, thanks!