search

roboticslab-uc3m / asibot-main

ASIBOT assistive robot: super/meta repository.
http://roboticslab.uc3m.es/roboticslab/robot/asibot
GNU Lesser General Public License v2.1
4 stars 0 forks source link

How to add plannification in an asibotpy program? #32

Closed serna92 closed 6 years ago

serna92 commented 7 years ago

Hello,

I'm using Ubuntu 16.04.2 and I have a .py file with a simulation but I'm using intermediate targets to avoid obstacles. How can I add in this program the needed plannification in order to find a collisions free path?.

Thank you very much.

PeterBowman commented 7 years ago

@jgvictores could we use the YarpOpenraveControlboardCollision plugin for this matter? Or would it require some kind of path-planning algorithm?

jgvictores commented 7 years ago

@PeterBowman YarpOpenraveControlboardCollision is esentially a YARP Controlboard that rejects commands that result in collisions. Unfortunately, it does not provide path planning. The most valuable resource from the same repo is some example code to obtain a simplified collision model (and load a plugin) from Python, at openrave-YarpRobot-collision-sim.py.

@serna92 Using openravepy, I'd go for something like the BiRRT planner (python example here). Once you get something similar to the example working, omit robot.WaitForController(0) # wait and instead try to extract the plan from manipprob to send it to the robot using the commands you've been using until now.

While that should be more than sufficient, here are some alternatives, in growing degree of (perhaps unrequired) learning curve:

serna92 commented 7 years ago

Hello,

I did something like that in my code:

from AsibotPy import *
from openravepy import *

env = Environment()
env.SetViewer('qtcoin')
env.Load('definitivo/entornoAsibot/asibot_kitchen.env.xml')
robot = env.GetRobots()[0]

rpc = yarp.RpcClient()

rpc.open('/command/ravebot/world')

yarp.Network.connect('/command/ravebot/world', '/ravebot/world')

#######################################

res = yarp.Bottle()

cmd1 = yarp.Bottle()

cmd1.addString('world')
cmd1.addString('grab')
cmd1.addString('obj')
cmd1.addString('redCan')
cmd1.addInt(1)

cmd2 = yarp.Bottle()

cmd2.addString('world')
cmd2.addString('grab')
cmd2.addString('obj')
cmd2.addString('redCan')
cmd2.addInt(0)  

#######################################

home=[0,0,1.4,0,0]

P1=[0.3,0.9,0.6,90,0]
P_lata=[-0.15,0.7,0.5,90,0]
P_labios=[-0.2,0,0.6,90,0]

P2=[-0.15,0.7,0.25,90,0]
P3=[-0.15,0.8,0.25,90,0]

#######################################

simCart = CartesianClient()
simCart.open('/ravebot')       # use '/canbot' for real

#######################################

print 'hello, robot!'
simCart.movl(home)  # defaults to 20 s
simCart.wait()      # wait for movement

manipprob = interfaces.BaseManipulation(robot) # create the interface for basic manipulation programs
manipprob.MoveManipulator(goal=P_lata)
robot.WaitForController(0)

But when I execute it, I obtain the next output:

hello, robot! 2017-05-03 12:37:21,332 openrave [WARN] [kinbody.cpp:1535 KinBody::SetDOFValues] dof 3 value is not in limits 9.000000e+01<2.356194e+00 2017-05-03 12:37:21,594 openrave [WARN] [basemanipulation.cpp:481 MoveActiveJoints] jitter failed 0 Traceback (most recent call last): File "definitivo/programas/pick_can.py", line 72, in manipprob.MoveManipulator(goal=P_lata) File "/usr/local/lib/python2.7/dist-packages/openravepy/openravepy/interfaces/BaseManipulation.py", line 121, in MoveManipulator return self._MoveJoints('MoveManipulator',goal=goal,steplength=steplength,maxiter=maxiter,maxtries=maxtries,execute=execute,outputtraj=outputtraj,goals=goals,outputtrajobj=outputtrajobj,jitter=jitter,releasegil=releasegil) File "/usr/local/lib/python2.7/dist-packages/openravepy/openravepy/interfaces/BaseManipulation.py", line 166, in _MoveJoints raise PlanningError('MoveActiveJoints') openravepy.openravepy.openravepy_ext.PlanningError: Planning Error MoveActiveJoints QtDBus: cannot relay signals from parent QObject(0x7f3b04740210 "") unless they are emitted in the object's thread QThread(0x7f3b0400c820 ""). Current thread is QThread(0x22ab6d0 ""). QtDBus: cannot relay signals from parent QObject(0x7f3b04740210 "") unless they are emitted in the object's thread QThread(0x7f3b0400c820 ""). Current thread is QThread(0x22ab6d0 ""). 2017-05-03 12:37:21,601 openrave [WARN] [qtcoinrave.cpp:80 DestroyPlugin] SoQt releasing all memory Coin debug: socontexthandler_cleanup(): 160 context-bound resources not free'd before exit.

serna92 commented 7 years ago

Hello,

I'm trying to get the joint values needed to reach a cartesian position using the command inv:

P_lata=[-0.15,0.7,0.5,90,0]

goal = []
simCart.inv(P_lata, goal)
traj = basemanip.MoveManipulator(goal = goal,execute = False,outputtrajobj = True)

But I get the next output:

Traceback (most recent call last): File "definitivo/programas/pick_can.py", line 69, in simCart.inv(P_lata, goal) File "/usr/local/lib/python2.7/dist-packages/AsibotPy.py", line 59, in inv for elem in range(0,data.size()): AttributeError: 'NoneType' object has no attribute 'size'

Am I inserting the parameters correctly in simCart.inv?

PeterBowman commented 7 years ago

@serna92 works for me. Did you remember to put simCart.open('/ravebot') before issuing that command? Keep an eye on the terminal on which cartesianServer is running, you'll see several debug messages printed by the callbacks of the RPC responder. Among them, look for:

[xRpcResponder] Got [inv] (-0.15 0.7 0.5 90.0 0.0)

Any failure should be clearly stated right there.

BTW I fixed the stat command at 4991b02, pull the changes and run make install to keep your scripts updated.

serna92 commented 7 years ago

Enabling debug I got the next message from openrave:

2017-05-08 15:10:55,030 openrave [DEBUG] [planningutils.cpp:65 planningutils::JitterActiveDOF] JitterActiveDOFs: self collision: (asibot:link0)x(asibot:link3), contacts=0!

Is this causing not to get a valid trajectory?

serna92 commented 7 years ago

Hello,

I have another question, I can't figure out how to get the same environment which is getting executed in cartesian server.

Currently, I load a copy of the environment with openrave but I would like to work with the same environment in order to make invisible objects which arent taking part in the simulation in that moment or to set them a different position before starting a simulation.

Is there way to extract the same env variable from cartesian server?

PeterBowman commented 7 years ago

Is this causing not to get a valid trajectory?

That debug line might mean that links 0 and 3 are overlapping in the simulator, but I'm not sure. In certain circumstances, OpenRAVE bloats the terminal output with warning messages which we usually don't care about. Regardless, did you manage to solve the simCart.inv issue? If so, how is the simulation responding to your commands?

Is there way to extract the same env variable from cartesian server?

Any 3D models are pulled from roboticslab-uc3m/asibot-openrave-models. The default environment is located here and gets installed on your machine at /usr/local/share/asibot/contexts/openrave/models. You may copy the whole openrave/models tree somewhere else, make your changes to the relevant files and point cartesianServer at the environment XML by passing an absolute path with the --env option, like so:

cartesianServer --env /absolute/path/to/my/new/env/model.xml

To get rid of the kitchen stuff and work with the manipulator, nothing else, try:

cartesianServer --env /usr/local/share/asibot/contexts/openrave/models/asibot.robot.xml

serna92 commented 7 years ago

Yes, I resolved the simCart.inv issue, I wasn't running cartesian server because I was using the environment loaded by openrave. Now, I want to do the opposite, make openrave to work with the environment loaded by cartesian server and being able to modify it just at the begining of the simulation. Instead of Environment.Load(environment_path).

I have a modiffied environment and i want, for example, make an object (dish, glass, bottle or redCan) invisible using Environment.GetKinbody(kinbody_name) and Kinbody.setVisible(False) or modify its position (I still haven't found the needed command).

The simulation works well, it also grabs and release the objects but now I want to make the object position variable then the robot have to get a free obstacles trajectory so that's why I want to import openravepy. I open an issue on openrave too:

http://openrave-users-list.185357.n3.nabble.com/Getting-raise-PlanningError-MoveActiveJoints-tp4027910.html

I will attach the environment I'm using.

entornoAsibot.zip

jgvictores commented 7 years ago

Hi @serna92 Regarding possibilities of using more roboticslab-uc3m developments, we have the intention of porting functionalities to plugins that can be loaded from openravepy. These are the related issues: https://github.com/roboticslab-uc3m/openrave-yarp-plugins/issues/27 and https://github.com/roboticslab-uc3m/openrave-yarp-plugins/issues/26 (both part of the openrave-yarp-plugins repository).

It would be good:

serna92 commented 7 years ago

As functionality I would like that given a target position the program can give the needed waypoints or execute a trajectory using movj before the aproaching to the object using movl. And also, be able to select and set visible/invisible objects which aren't taking part in the simulated task.

What I want to do in my TFM is to design an UI in Python which asks the user what task and where is/are the object/s which take part in the simulation. Then start the simulation hidding the rest of objects.

About OpenRAVE plugins, should I manage to create a method to implement the functionality that I need? I have few idea about OpenRAVE plugins but I can try from seeing examples in https://github.com/roboticslab-uc3m/openrave-yarp-plugins repository.

Any tip?.

jgvictores commented 7 years ago

As functionality I would like that given a target position the program can give the needed waypoints or execute a trajectory using movj before the aproaching to the object using movl.

As commented above, using openravepy, once you get something similar to the (python example here) working, omit robot.WaitForController(0) # wait and instead try to extract the plan from manipprob (maybe ipython's autocomplete can help you with the methods). Once extracted, you can send it to the robot using the commands you've been using until now.

And also, be able to select and set visible/invisible objects which aren't taking part in the simulated task.

You'd have to make your own plugin for that. I'd recommend branching https://github.com/roboticslab-uc3m/openrave-yarp-plugins, and working on a renamed copy of OpenraveYarpPaintSquares as a starting point.

serna92 commented 7 years ago

Hello, I have done a plugin which I want to test but I have problems to use it. Especifically with the line RaveCreateModule(env,'mymodule'). I would like to send as param the env loaded after running cartesian server --env myEnv. I mean to be able to work with the loaded scene instead of load it again with env.load('myEnv').

jgvictores commented 7 years ago
  1. As far as I understand and have tested, passing parameters to OpenRAVE works differently from command line than as from python code. Just to get something working, you can imitate how this python line can be caught by this C++ OpenRAVE plugin line.
  2. It would be much better if you could please upload your code somewhere on GitHub and refer to specific lines.
jgvictores commented 7 years ago

PS: Please forget about cartesianServer for now. It will be superseeded, and you are invoking plugins from python code.

serna92 commented 7 years ago

All what I have is uploaded in this repository https://github.com/serna92/AsibotSimulation.git.

I was working on the files pick_can_modified.py and OpenraveYarpObjManagement.cpp.

Knowing that cartesianServer will be superseded, It will be as easier as use env.Load('asibot_kitchen.env.xml'). But I guess I will still have problems with this line.

Then, do I have to remove AsibotPy and all the simCart commands and replace them by openravepy methods?

jgvictores commented 7 years ago

Important: Regarding OpenraveYarpObjManagement.cpp, could you include it in a fork and/or branch and Pull Request to https://github.com/roboticslab-uc3m/openrave-yarp-plugins ?

Then, do I have to remove AsibotPy and all the simCart commands and replace them by openravepy methods?

No, we'll have something similar to simCartin the future, and will keep you notified on any change of syntax.

serna92 commented 7 years ago

Hello,

I figured out my mistake. In order to get a valid trajectory I had to provide the goal joint values in radians.

I also got the waypoints of the trajectory but the commands movj and movl only accept cartesian coordinates and there isn't a VOCAB to do direct kinematics. My code

jgvictores commented 7 years ago

  1. You can follow this example to control in joint space.

  2. Just as a side note (as it makes no sense to do forward kinematics for its inversion in the next step), places where forward kinematics implementation is missing:

jgvictores commented 6 years ago

closed due to inactivity > 1 month