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

Some methods return an empty list (Python) #183

Closed rsantos88 closed 4 years ago

rsantos88 commented 4 years ago

Reviewing the python demo exampleCartesianControlClient.py, there are things that are not working properly. I've been doing different tests with ipython and the code of this branch (is basically the same with two positions). I can't get qd_vector with the inverse kinematics function to work properly in this line: https://github.com/roboticslab-uc3m/kinematics-dynamics/blob/f4b2d7c9fe0e6484e70d55acdc1e91c418f4da65/examples/python/exampleCartesianControlClient.py#L44

I put the robot with a start position: set poss (-10 0 0 -70 0 -10) I am running BasicCartesianControl with this parameters

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 --angleRepr axisAngle --ik st

The result is:

yarp: Port /cartesianControlExample/rpc:c active at tcp://2.2.2.107:10066/
yarp: Port /cartesianControlExample/command:o active at tcp://2.2.2.107:10067/
yarp: Sending output from /cartesianControlExample/rpc:c to /teoSim/rightArm/CartesianControl/rpc_transform:s using tcp
yarp: Sending output from /cartesianControlExample/command:o to /teoSim/rightArm/CartesianControl/command:i using udp
[warning] DeviceDriverImpl.cpp:49 open(): FK streaming not supported in --transform mode, using RPC instead.
[success] DeviceDriverImpl.cpp:78 open(): Connected to remote.
[INFO]created device <CartesianControlClient>. See C++ class roboticslab::CartesianControlClient for documentation.
> stat
< ccnc [0.4, -0.34, 0.13, -1.1044467652e-16, 1.0, 1.0996314441e-16, 90.0]
-- movement 1:
> inv [0.45, -0.34, 0.13, 0.0, 1.0, 0.0, 90.0]
< []
> movj [0.45, -0.34, 0.13, 0.0, 1.0, 0.0, 90.0]
< [fail]
-- movement 2:
> inv [0.4, -0.34, 0.13, 0.0, 1.0, 0.0, 90.0]
< []
> movj [0.4, -0.34, 0.13, 0.0, 1.0, 0.0, 90.0]
< [fail]
bye!
yarp: Removing output from /cartesianControlExample/rpc:c to /teoSim/rightArm/CartesianControl/rpc_transform:s
yarp: Removing output from /cartesianControlExample/command:o to /teoSim/rightArm/CartesianControl/command:i
PeterBowman commented 4 years ago

Please check the terminal that runs a BasicCartesianControl instance, perhaps you are commanding the robot out of joint limits.

rsantos88 commented 4 years ago

Please check the terminal that runs a BasicCartesianControl instance, perhaps you are commanding the robot out of joint limits.

If I try this using RPC commands, connecting directly with BasicCartesianControl, it works perfectly

PeterBowman commented 4 years ago

Which port do you connect to? /teoSim/rightArm/CartesianControl/rpc_transform:s?

rsantos88 commented 4 years ago

Testing the code with ipython:

In [1]: import yarp

In [2]: import kinematics_dynamics

In [3]: 

In [3]: yarp.Network.init()

In [4]: 

In [4]: if yarp.Network.checkNetwork() != True:
   ...:         print '[error] Please try running yarp server'
   ...:         raise SystemExit
   ...: 

In [5]: options = yarp.Property()

In [6]: options.put('device','CartesianControlClient')

In [7]: options.put('cartesianRemote','/teoSim/rightArm/CartesianControl')

In [8]: options.put('cartesianLocal','/cartesianControlExample')

In [9]: options.put('transform', 1)

In [10]: dd = yarp.PolyDriver(options)  # calls open -> connects
yarp: Port /cartesianControlExample/rpc:c active at tcp://2.2.2.107:10066/
yarp: Port /cartesianControlExample/command:o active at tcp://2.2.2.107:10067/
yarp: Sending output from /cartesianControlExample/rpc:c to /teoSim/rightArm/CartesianControl/rpc_transform:s using tcp
yarp: Sending output from /cartesianControlExample/command:o to /teoSim/rightArm/CartesianControl/command:i using udp
[warning] DeviceDriverImpl.cpp:49 open(): FK streaming not supported in --transform mode, using RPC instead.
[success] DeviceDriverImpl.cpp:78 open(): Connected to remote.
[INFO]created device <CartesianControlClient>. See C++ class roboticslab::CartesianControlClient for documentation.

In [11]: 

In [11]: if not dd.isValid():
   ....:         print 'Cannot open the device!'
   ....:         raise SystemExit
   ....: 

In [12]: cartesianControl = kinematics_dynamics.viewICartesianControl(dd)  # view the actual interface

In [13]: 

In [13]: print '> stat'
> stat

In [14]: x = yarp.DVector()

In [15]: ret, state, ts = cartesianControl.stat(x)

In [16]: print '<', yarp.decode(state), '[%s]' % ', '.join(map(str, x))
< ccnc [0.456365653832, -0.3469, 0.136854042983, -2.12657684958e-17, 1.0, 1.2060416625e-16, 90.0]

In [17]: 

In [17]: xd = [0,0]

In [18]: 

In [18]: xd[0] = [0.45, -0.34, 0.13, 0.0, 1.0, 0.0, 90.0]

In [19]: xd[1] = [0.40, -0.34, 0.13, 0.0, 1.0, 0.0, 90.0]

In [20]: xd_vector = yarp.DVector(xd[0])

In [21]: qd_vector = yarp.DVector()

In [22]: cartesianControl.inv(xd_vector,qd_vector)
Out[22]: True

In [23]: map(str, xd_vector)
Out[23]: ['0.45', '-0.34', '0.13', '0.0', '1.0', '0.0', '90.0']

In [24]: map(str, qd_vector)
Out[24]: []

In [25]: len(qd_vector)
Out[25]: 0

so, the IK function is returning true but not the resulting vector...

rsantos88 commented 4 years ago

Which port do you connect to? /teoSim/rightArm/CartesianControl/rpc_transform:s?

Yes

PeterBowman commented 4 years ago

Definitely works for me in current develop (https://github.com/roboticslab-uc3m/kinematics-dynamics/commit/a9068fbc4ddb3b9bb73b024cdacf875e456292f8) and review-exampleCartesianControlClient (https://github.com/roboticslab-uc3m/kinematics-dynamics/commit/f4b2d7c9fe0e6484e70d55acdc1e91c418f4da65, see below) branches:

$ python exampleCartesianControlClient.py 
yarp: Port /cartesianControlExample/rpc:c active at tcp://192.168.1.46:10015/
yarp: Port /cartesianControlExample/command:o active at tcp://192.168.1.46:10016/
yarp: Sending output from /cartesianControlExample/rpc:c to /teoSim/rightArm/CartesianControl/rpc_transform:s using tcp
yarp: Sending output from /cartesianControlExample/command:o to /teoSim/rightArm/CartesianControl/command:i using udp
[warning] DeviceDriverImpl.cpp:49 open(): FK streaming not supported in --transform mode, using RPC instead.
[success] DeviceDriverImpl.cpp:78 open(): Connected to remote.
[INFO]created device <CartesianControlClient>. See C++ class roboticslab::CartesianControlClient for documentation.
> stat
< ccnc [-0.178209779105, -0.412956590308, 0.462518879179, 0.521838459998, -0.647307259867, -0.555587916527, 135.991909337]
-- movement 1:
> inv [0.45, -0.34, 0.13, 0.0, 1.0, 0.0, 90.0]
< [118.109925822, 178.798304415, -179.545379494, 69.2805537881, -1.28480417886, -97.3857119087]
> movj [0.45, -0.34, 0.13, 0.0, 1.0, 0.0, 90.0]
< [ok]
< [wait...]
-- movement 2:
> inv [0.4, -0.34, 0.13, 0.0, 1.0, 0.0, 90.0]
< [120.213100613, 178.798304415, -179.772585772, 79.2853823126, -1.22302163806, -109.496097994]
> movj [0.4, -0.34, 0.13, 0.0, 1.0, 0.0, 90.0]
< [ok]
< [wait...]
bye!
yarp: Removing output from /cartesianControlExample/rpc:c to /teoSim/rightArm/CartesianControl/rpc_transform:s
yarp: Removing output from /cartesianControlExample/command:o to /teoSim/rightArm/CartesianControl/command:i

Please recompile your bindings and check again. For now, closing as invalid.