search

roboticslab-uc3m / teo-openrave-models

TEO OpenRAVE models.
http://roboticslab.uc3m.es/roboticslab/robot/teo-humanoid
GNU Lesser General Public License v2.1
0 stars 0 forks source link

Think about openrave-database-generators #3

Closed jgvictores closed 4 years ago

jgvictores commented 7 years ago

Think about http://openrave.org/docs/latest_stable/databases/#openrave-database-generators

jgvictores commented 7 years ago

This issue is becoming increasingly necessary (e.g. https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/99).

jgvictores commented 7 years ago

More specifically: http://openrave.org/docs/latest_stable/openravepy/databases.kinematicreachability/

rsantos88 commented 7 years ago

This is the reachability of Teo's right arm with the original model: snapshot_with_limits To know if it's considering the joint limits, I've tried to remove all the<limitsdeg> tags along the teo_rightArm.kinbody.xml file, but when I generate the reachability again, it shows the same result... snapshot_without_limits

jgvictores commented 7 years ago

@rsantos88 Looks great, thanks!

  1. Could you specify the command line you used to generate this output?
  2. Is there any output file generated? Does it seem useful or somehow human readable? Maybe you could upload it somewhere or copy+paste a fragment?
rsantos88 commented 7 years ago

Yes, the command line used is:

openrave.py --database kinematicreachability --robot=/home/teo/repos/teo-openrave-models/openrave/teo/teo.robot.xml --numthreads=4 --manipname=rightArm

This will take some time to generate the reachability... you've to wait... Then, it will create some files in /home/user/.openrave/. When the process finishes, you can see the result writing the same line adding show parameter:

openrave.py --database kinematicreachability --robot=/home/teo/repos/teo-openrave-models/openrave/teo/teo.robot.xml --numthreads=4 --manipname=rightArm --show

Remember that you need to install mayavi2 if you don't have it installed .

rsantos88 commented 7 years ago

Ok, possibly this is the solution to the problem (see)

rsantos88 commented 7 years ago

Changed the "circular" joints to false, the reachability result is... snapshot_with_real_limits max reachability: 0.91666666666666663 changes to: 0.069444444444444448

jgvictores commented 7 years ago

Great job! Could you commit the updated model?

It seems like an improvement. Would this model work with teoSim etc?

rsantos88 commented 7 years ago

The limits works well apparently. However, when you exceed the limits in teoSim, openrave warns to you with a message like this: [kinbody.cpp:1515 SetDOFValues] dof 4 value is not in limits 1.566438e+00<1.221730e+00 and the model stops respecting the limit configured in the xml, but if you want to know the actual position, it returns the extraimited position value. This problem is referenced at [YarpOpenraveControlboard] could get limits from openrave] issue. Even so, @jgvictores do you want to update the model with this changes? The changes are done in (b1db54994f409bb04bdb5cd845c8d370cc501add)

jgvictores commented 7 years ago

I'm not a big fan of duplicate files, but stranded branches and open issues do not help either. I'm strongly considering having both versions with different names on develop.

jgvictores commented 7 years ago

@rsantos88 Do you think you could create 2 more manipulators? Something like fullRightArm and fullLeftArm that include the trunk (initially, including all joint limits).

rsantos88 commented 7 years ago

Ok @jgvictores , this is the visual result after calculate the reachability with the corrections of (#4) issue: new_reachability_rightarm_trunk Here you have other nice top view: new_reachability_rightarm_trunk top I've added these two other manipulators in a different branch. If you want, we can merge it with develop branch. I attach here the database generated this weekend with the inverse reachability (rightArm + trunk): robot.4a0cf68d3eba8fe6aa3e92f28fa3c9ed.zip

rsantos88 commented 7 years ago

I append this updated info: openravepy.databases.kinematicreachability: show, max reachability: 0.1388888888888889

jgvictores commented 7 years ago

I understand it just involves a few extra lines in teo.robot.xml. It would be nice to see these changes in develop (not the full .zip though).

rsantos88 commented 7 years ago

lol.. it's not the changes of teo.robot.xml. It's just the inverse-reachability database generated through the new manipulator rightArm_trunk. I've upload this database with the porpouse of see the results if you want without doing this process (you can read more here). You can also generate database but it takes to much time ;)

jgvictores commented 7 years ago

Yes, what I meant is it would be good to merge these and these lines of teo.robot.xml from the reachabilityWithTrunk branch to develop.

rsantos88 commented 7 years ago

Ok, done it (#7)

jgvictores commented 7 years ago

Great, thanks! I haven't deleted the branch, in case there was anything you were still working on. Feel free to delete as required!

rsantos88 commented 7 years ago

Ok, it's already deleted :)

rsantos88 commented 7 years ago

I want to find a utility to this database, so I am trying first to follow the inverse reachability tutorial but I've too many problems. I've written my issue here, so if you know how to solve it I'll be very grateful :)

rsantos88 commented 6 years ago

I'll close this issue. , Currently, i am learning and using this database generator to generate a optimized and stable analytic inverse kinematics solvers for rightArmTrunk and leftArmTrunk and load them when we use a TEO model for OpenRave

jgvictores commented 6 years ago

Could you point to the corresponding repositories, code, models?

jgvictores commented 6 years ago

Reopening.

Will close after adding:

rsantos88 commented 6 years ago

Ok, I've done the script here

It creates the database reachability correctly but it breaks and doesn't show the graphic result with this error:

2018-04-26 16:57:51,145 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link teoSim/r1Man with 0 geometries (env 1) (userdatakey fclcollision0x1e99ce0)
2018-04-26 16:57:51,167 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link teoSim/r2Man with 0 geometries (env 1) (userdatakey fclcollision0x1e99ce0)
2018-04-26 16:57:51,180 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link teoSim/r5dummy with 0 geometries (env 1) (userdatakey fclcollision0x1e99ce0)
2018-04-26 16:57:51,195 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link teoSim/r5Man with 0 geometries (env 1) (userdatakey fclcollision0x1e99ce0)
2018-04-26 16:57:51,203 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link teoSim/r6dummy with 0 geometries (env 1) (userdatakey fclcollision0x1e99ce0)
2018-04-26 16:57:51,222 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link teoSim/r6Man with 0 geometries (env 1) (userdatakey fclcollision0x1e99ce0)
2018-04-26 16:57:51,272 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link teoSim/r1Man with 0 geometries (env 1) (userdatakey fclcollision0x1e99ce0)
2018-04-26 16:57:51,293 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link teoSim/r2Man with 0 geometries (env 1) (userdatakey fclcollision0x1e99ce0)
2018-04-26 16:57:51,307 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link teoSim/r5dummy with 0 geometries (env 1) (userdatakey fclcollision0x1e99ce0)
2018-04-26 16:57:51,322 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link teoSim/r5Man with 0 geometries (env 1) (userdatakey fclcollision0x1e99ce0)
2018-04-26 16:57:51,331 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link teoSim/r6dummy with 0 geometries (env 1) (userdatakey fclcollision0x1e99ce0)
2018-04-26 16:57:51,348 openrave [WARN] [fclspace.h:248 FCLSpace::KinBodyInfoPtr fclrave::FCLSpace::InitKinBody] Initializing link teoSim/r6Man with 0 geometries (env 1) (userdatakey fclcollision0x1e99ce0)
Set for kinematic reachability of: trunkAndRightArm. Change the above code line for trunkAndLeftArm, etc...
openravepy.databases.inversekinematics: GetDefaultIndices, found 3-intersection centered on index 5
openravepy.databases.inversekinematics: GetDefaultIndices, found 3-intersection centered on index 7
Could load existing kinematicreachability of trunkAndRightArm, not generating!
openravepy.databases.kinematicreachability: show, max reachability: 0.20833333333333334
2018-04-26 16:57:53,737 openrave [INFO] [OpenraveYarpForceEstimator.cpp:419 DestroyPlugin] destroying plugin
2018-04-26 16:57:53,738 openrave [INFO] [OpenraveYarpCoupled.cpp:421 DestroyPlugin] destroying plugin
2018-04-26 16:57:53,738 openrave [INFO] [OpenraveYarpPaintSquares.cpp:396 DestroyPlugin] destroying plugin
2018-04-26 16:57:53,738 openrave [INFO] [OpenraveYarpWorldRpcResponder.cpp:674 DestroyPlugin] destroying plugin
2018-04-26 16:57:53,738 openrave [INFO] [OpenraveYarpPluginLoader.cpp:358 DestroyPlugin] destroying plugin
Traceback (most recent call last):
  File "generateReachability.py", line 31, in <module>
    reachabilitymodel.show()
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/databases/kinematicreachability.py", line 378, in show
    src = mlab.pipeline.scalar_field(reachability3d)
  File "/usr/lib/python2.7/dist-packages/mayavi/tools/sources.py", line 1136, in scalar_field
    data_source.reset(x=x, y=y, z=z, scalars=s)
  File "/usr/lib/python2.7/dist-packages/mayavi/tools/sources.py", line 358, in reset
    self.set(trait_change_notify=False, **traits)
  File "/usr/lib/python2.7/dist-packages/mayavi/tools/sources.py", line 113, in set
    super(MlabSource, self).set(trait_change_notify, **traits)
  File "/usr/lib/python2.7/dist-packages/traits/has_traits.py", line 1946, in trait_set
    for name, value in traits.items():
ValueError: The truth value of an array with more than one element is ambiguous. Use a.any() or a.all()

I've tried to follow the steps of the barrettsegway robot example:

Running the Generator

openrave.py --database kinematicreachability --robot=robots/barrettsegway.robot.xml

Showing the Reachability (uses mayavi2)

openrave.py --database kinematicreachability --robot=robots/barrettsegway.robot.xml --show

but it breaks with the same message:

teo@teo-oliver:~/repos/teo-openrave-models/scripts/python$ openrave.py --database kinematicreachability --robot=robots/barrettsegway.robot.xml --show
2018-04-26 17:01:59,304 openrave [WARN] [openravepy_robot.cpp:874 PyRobotBase::PyManipulatorPtr openravepy::PyRobotBase::SetActiveManipulator] SetActiveManipulator(int) is deprecated
openravepy.databases.inversekinematics: GetDefaultIndices, found 3-intersection centered on index 1
openravepy.databases.inversekinematics: GetDefaultIndices, found 3-intersection centered on index 5
openravepy.databases.kinematicreachability: show, max reachability: 0.91666666666666663
2018-04-26 17:02:01,595 openrave [INFO] [OpenraveYarpForceEstimator.cpp:419 DestroyPlugin] destroying plugin
2018-04-26 17:02:01,595 openrave [INFO] [OpenraveYarpCoupled.cpp:421 DestroyPlugin] destroying plugin
2018-04-26 17:02:01,595 openrave [INFO] [OpenraveYarpPaintSquares.cpp:396 DestroyPlugin] destroying plugin
2018-04-26 17:02:01,595 openrave [INFO] [OpenraveYarpWorldRpcResponder.cpp:674 DestroyPlugin] destroying plugin
2018-04-26 17:02:01,596 openrave [INFO] [OpenraveYarpPluginLoader.cpp:358 DestroyPlugin] destroying plugin
Traceback (most recent call last):
  File "/usr/local/bin/openrave.py", line 126, in <module>
    database.run(args=args)
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_0_9/databases/kinematicreachability.py", line 440, in run
    ReachabilityModel.RunFromParser(Model = lambda robot: ReachabilityModel(robot=robot), parser = ReachabilityModel.CreateOptionParser(), *args,**kwargs)
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_0_9/databases/__init__.py", line 262, in RunFromParser
    model.show(options=options)
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_0_9/databases/kinematicreachability.py", line 378, in show
    src = mlab.pipeline.scalar_field(reachability3d)
  File "/usr/lib/python2.7/dist-packages/mayavi/tools/sources.py", line 1136, in scalar_field
    data_source.reset(x=x, y=y, z=z, scalars=s)
  File "/usr/lib/python2.7/dist-packages/mayavi/tools/sources.py", line 358, in reset
    self.set(trait_change_notify=False, **traits)
  File "/usr/lib/python2.7/dist-packages/mayavi/tools/sources.py", line 113, in set
    super(MlabSource, self).set(trait_change_notify, **traits)
  File "/usr/lib/python2.7/dist-packages/traits/has_traits.py", line 1946, in trait_set
    for name, value in traits.items():
ValueError: The truth value of an array with more than one element is ambiguous. Use a.any() or a.all()
jgvictores commented 4 years ago

May be of relevance for this issue some day: Adopt tag regarding robot grippers (https://github.com/roboticslab-uc3m/openrave-yarp-plugins/issues/86)

jgvictores commented 4 years ago

May be of relevance for this issue some day: Adopt tag regarding robot grippers (roboticslab-uc3m/openrave-yarp-plugins#86)

I confirm the <joints> tag (aka <gripperjoints> afaik) is required for the OpenRAVE grasping module (databases.grasping.GraspingModel(robot, target)) (else chokes on some kind of gripper release). Recovered for TEO at https://github.com/roboticslab-uc3m/teo-openrave-models/commit/083998bfde1d05dcd96b7626da8a8c3bc8edf719

jgvictores commented 4 years ago

BTW, added horizontal label to this issue. Should be split into issues per database generator, at least if we ever want to close it. ^^

jgvictores commented 4 years ago

Ok, I've done the script here

Rebased and merged at https://github.com/roboticslab-uc3m/teo-openrave-models/commit/1115cc4d78b283bb0b469f6865b7f007838bb8ef [ @rsantos88 thanks! ]

jgvictores commented 4 years ago

Generated a cool kinematicreachability linked at https://github.com/roboticslab-uc3m/teo-openrave-models/commit/673cdcada7fc3b43f95af61ec6eaa6c433145b0b, attaching screenshot below.

Some links from https://github.com/roboticslab-uc3m/teo-openrave-models/issues/3#issuecomment-315730184 and also some new ones:

And, yes, as stated above: for (...) kinematicreachability (...) --show, mayavi is required. On this PC it was as easy as a sudo -H pip2 install mayavi (yep, using sudo and not on pip3 yet ftw).

snapshot

jgvictores commented 4 years ago

A --show on inversereachability seems to place an object at a random position, then show how to reach it. teo-invreach teo-invreac2

jgvictores commented 4 years ago

To sum up what we can see regarding http://openrave.org/docs/latest_stable/databases/#openrave-database-generators as of https://github.com/roboticslab-uc3m/teo-openrave-models/tree/e6401ac329d2c1716c95a6b4795560f7e60d7a40/scripts/bash#scriptsbash:

PeterBowman commented 3 years ago

Regarding reachability/manipulability, see also https://github.com/robotology/community/discussions/559.