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

Wrong limits for gripper cause motion control to exit abruptly #165

Closed rsantos88 closed 5 years ago

rsantos88 commented 5 years ago

I've trying to check this app with the real robot, because in the simulator it works pretty well. Placing the robot in a safe position to initiate Cartesian control (elbow near 90º and front shoulder 10º), when I try to move it in Cartesian space using 'a'/'z' or 's'/'x' in the app, the Cartesian Control shows this errors:

[debug] RpcResponder.cpp:50 respond(): Got: [movv] 0.005 0.0 0.0 0.0 0.0 0.0
[warning] RateThreadImpl.cpp:34 checkJointLimits(): Joint q7 out of limits [0.000000,0.000000]: 0.000000.
[error] RateThreadImpl.cpp:55 run(): checkJointLimits failed, stopping control.
[INFO]device active in background...
[debug] RpcResponder.cpp:50 respond(): Got: [stop]
[INFO]device active in background...
[debug] RpcResponder.cpp:50 respond(): Got: [stop]
[debug] RpcResponder.cpp:50 respond(): Got: [movv] -0.005 0.0 0.0 0.0 0.0 0.0
[warning] RateThreadImpl.cpp:34 checkJointLimits(): Joint q7 out of limits [0.000000,0.000000]: 0.000000.
[error] RateThreadImpl.cpp:55 run(): checkJointLimits failed, stopping control.
[INFO]device active in background...
[debug] RpcResponder.cpp:50 respond(): Got: [stop]
[INFO]device active in background...
[debug] RpcResponder.cpp:50 respond(): Got: [movv] 0.0 0.005 0.0 0.0 0.0 0.0
[warning] RateThreadImpl.cpp:34 checkJointLimits(): Joint q7 out of limits [0.000000,0.000000]: 0.000000.
[error] RateThreadImpl.cpp:55 run(): checkJointLimits failed, stopping control.
[debug] RpcResponder.cpp:50 respond(): Got: [stop]
[debug] RpcResponder.cpp:50 respond(): Got: [movv] 0.0 -0.005 0.0 0.0 0.0 0.0
[warning] RateThreadImpl.cpp:34 checkJointLimits(): Joint q7 out of limits [0.000000,0.000000]: 0.000000.
[error] RateThreadImpl.cpp:55 run(): checkJointLimits failed, stopping control.
[INFO]device active in background...
[debug] RpcResponder.cpp:50 respond(): Got: [stop]
[debug] RpcResponder.cpp:50 respond(): Got: [movv] 0.0 0.005 0.0 0.0 0.0 0.0
[warning] RateThreadImpl.cpp:34 checkJointLimits(): Joint q7 out of limits [0.000000,0.000000]: 0.000000.
[error] RateThreadImpl.cpp:55 run(): checkJointLimits failed, stopping control.
[INFO]device active in background...
[debug] RpcResponder.cpp:50 respond(): Got: [stop]
[debug] RpcResponder.cpp:50 respond(): Got: [movv] 0.0 -0.005 0.0 0.0 0.0 0.0
[warning] RateThreadImpl.cpp:34 checkJointLimits(): Joint q7 out of limits [0.000000,0.000000]: 0.000000.
[error] RateThreadImpl.cpp:55 run(): checkJointLimits failed, stopping control.
[debug] RpcResponder.cpp:50 respond(): Got: [stop]
[debug] RpcResponder.cpp:50 respond(): Got: [movv] 0.0 0.005 0.0 0.0 0.0 0.0
[warning] RateThreadImpl.cpp:34 checkJointLimits(): Joint q7 out of limits [0.000000,0.000000]: 0.000000.
[error] RateThreadImpl.cpp:55 run(): checkJointLimits failed, stopping control.
[INFO]device active in background...
[debug] RpcResponder.cpp:50 respond(): Got: [stop]
[INFO]device active in background...
[debug] RpcResponder.cpp:50 respond(): Got: [movv] 0.0 0.0 0.005 0.0 0.0 0.0
[warning] RateThreadImpl.cpp:34 checkJointLimits(): Joint q7 out of limits [0.000000,0.000000]: 0.000000.
[error] RateThreadImpl.cpp:55 run(): checkJointLimits failed, stopping control.
[debug] RpcResponder.cpp:50 respond(): Got: [stop]
[INFO]device active in background...
[debug] RpcResponder.cpp:50 respond(): Got: [movv] 0.0 0.0 -0.005 0.0 0.0 0.0
[warning] RateThreadImpl.cpp:34 checkJointLimits(): Joint q7 out of limits [0.000000,0.000000]: 0.000000.
[error] RateThreadImpl.cpp:55 run(): checkJointLimits failed, stopping control.
[debug] RpcResponder.cpp:50 respond(): Got: [stop]

The arm doesn't move. I'll copy the initialization of Cartesian Control if it was helpful:

 yarpdev --device BasicCartesianControl --name /teo/rightArm/CartesianControl --from /usr/local/share/teo-configuration-files/contexts/kinematics/rightArmKinematics.ini --angleRepr axisAngle --robot remote_controlboard --local /BasicCartesianControl/teo/rightArm --remote /teo/rightArm
[info] DeviceDriverImpl.cpp:17 open(): Subdevice BasicCartesianControl
[debug] DeviceDriverImpl.cpp:11 open(): BasicCartesianControl config: (H0 (0 -1 0 0 0 0 -1 -0.34692 1 0 0 0.4967 0 0 0 1)) (angleRepr axisAngle) (device BasicCartesianControl) (from "/usr/local/share/teo-configuration-files/contexts/kinematics/rightArmKinematics.ini") (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.202) (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.187496) (alpha -90.0) (mass 0.3) (cog 0 0 0) (inertia 0 0 0)) (local "/BasicCartesianControl/teo/rightArm") (name "/teo/rightArm/CartesianControl") (numLinks 6) (remote "/teo/rightArm") (robot remote_controlboard) (single_threaded 1) (subdevice BasicCartesianControl).
yarp: Port /BasicCartesianControl/teo/rightArm/rpc:o active at tcp://2.2.2.100:10015/
yarp: Port /BasicCartesianControl/teo/rightArm/command:o active at tcp://2.2.2.100:10016/
yarp: Port /BasicCartesianControl/teo/rightArm/stateExt:i active at tcp://2.2.2.100:10017/
yarp: Sending output from /BasicCartesianControl/teo/rightArm/rpc:o to /teo/rightArm/rpc:i using tcp
yarp: Sending output from /BasicCartesianControl/teo/rightArm/command:o to /teo/rightArm/command:i using udp
yarp: Receiving input from /teo/rightArm/stateExt:o to /BasicCartesianControl/teo/rightArm/stateExt:i using udp
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
[info] DeviceDriverImpl.cpp:74 open(): numRobotJoints: 7.
[info] DeviceDriverImpl.cpp:89 open(): Joint 0 limits: [-88.500000,98.100000]
[info] DeviceDriverImpl.cpp:89 open(): Joint 1 limits: [-75.500000,29.600000]
[info] DeviceDriverImpl.cpp:89 open(): Joint 2 limits: [-57.000000,80.100000]
[info] DeviceDriverImpl.cpp:89 open(): Joint 3 limits: [-32.900000,83.500000]
[info] DeviceDriverImpl.cpp:89 open(): Joint 4 limits: [-99.600000,76.800000]
[info] DeviceDriverImpl.cpp:89 open(): Joint 5 limits: [-56.900000,107.500000]
[info] DeviceDriverImpl.cpp:89 open(): Joint 6 limits: [0.000000,0.000000]
[debug] DeviceDriverImpl.cpp:18 open(): config: (H0 (0 -1 0 0 0 0 -1 -0.34692 1 0 0 0.4967 0 0 0 1)) (angleRepr axisAngle) (device KdlSolver) (from "/usr/local/share/teo-configuration-files/contexts/kinematics/rightArmKinematics.ini") (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.202) (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.187496) (alpha -90.0) (mass 0.3) (cog 0 0 0) (inertia 0 0 0)) (local "/BasicCartesianControl/teo/rightArm") (maxs (98.1 29.6 80.1 83.5 76.8 107.5 0.0)) (mins (-88.5 -75.5 -57.0 -32.9 -99.6 -56.9 0.0)) (name "/teo/rightArm/CartesianControl") (numLinks 6) (remote "/teo/rightArm") (robot remote_controlboard) (single_threaded 1) (subdevice BasicCartesianControl).
[info] DeviceDriverImpl.cpp:22 open(): kinematics: none.ini [none.ini]
||| did not find none.ini
yarp: cannot read from 
[debug] DeviceDriverImpl.cpp:33 open(): fullConfig: (H0 (0 -1 0 0 0 0 -1 -0.34692 1 0 0 0.4967 0 0 0 1)) (angleRepr axisAngle) (device KdlSolver) (from "/usr/local/share/teo-configuration-files/contexts/kinematics/rightArmKinematics.ini") (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.202) (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.187496) (alpha -90.0) (mass 0.3) (cog 0 0 0) (inertia 0 0 0)) (local "/BasicCartesianControl/teo/rightArm") (maxs (98.1 29.6 80.1 83.5 76.8 107.5 0.0)) (mins (-88.5 -75.5 -57.0 -32.9 -99.6 -56.9 0.0)) (name "/teo/rightArm/CartesianControl") (numLinks 6) (remote "/teo/rightArm") (robot remote_controlboard) (single_threaded 1) (subdevice BasicCartesianControl).
[info] DeviceDriverImpl.cpp:37 open(): numLinks: 6 [1]
[info] DeviceDriverImpl.cpp:49 open(): gravity: 0.0 0.0 -9.81 [0.0 0.0 -9.81]
[info] DeviceDriverImpl.cpp:65 open(): H0:
 0.000000   -1.000000    0.000000    0.000000
 0.000000    0.000000   -1.000000   -0.346920
 1.000000    0.000000    0.000000    0.496700
 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:92 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:92 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:92 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:92 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:92 open(): Added: link_4 (offset 0.000000) (D 0.202000) (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:92 open(): Added: link_5 (offset 90.000000) (D 0.000000) (A 0.187496) (alpha -90.000000) (mass 0.300000) (cog 0.000000 0.000000 0.000000) (inertia 0.000000 0.000000 0.000000)
[info] DeviceDriverImpl.cpp:163 open(): HN:
 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
[ 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:166 open(): Chain number of segments (post- H0 and HN): 8
[info] DeviceDriverImpl.cpp:167 open(): Chain number of joints (post- H0 and HN): 6
[INFO]created device <KdlSolver>. See C++ class roboticslab::KdlSolver for documentation.
[info] DeviceDriverImpl.cpp:110 open(): numSolverJoints: 6.
[warning] DeviceDriverImpl.cpp:114 open(): numRobotJoints(7) != numSolverJoints(6) !!!
[INFO]created device <BasicCartesianControl>. See C++ class roboticslab::BasicCartesianControl for documentation.
yarp: Port /teo/rightArm/CartesianControl/rpc:s active at tcp://2.2.2.100:10018/
yarp: Port /teo/rightArm/CartesianControl/command:i active at tcp://2.2.2.100:10019/
yarp: Port /teo/rightArm/CartesianControl/state:o active at tcp://2.2.2.100:10020/
yarp: Port /teo/rightArm/CartesianControl/rpc_transform:s active at tcp://2.2.2.100:10021/
[INFO]created device <CartesianControlServer>. See C++ class roboticslab::CartesianControlServer for documentation.
yarp: Port /teo/rightArm/CartesianControl/quit active at tcp://2.2.2.100:10022/
[INFO]device active in background...
[INFO]device active in background...
[INFO]device active in background...
[INFO]device active in background...
[INFO]device active in background...
[INFO]device active in background...
yarp: Receiving input from /KeyboardCartesianControlClient/rpc:c to /teo/rightArm/CartesianControl/rpc:s using tcp
yarp: Receiving input from /KeyboardCartesianControlClient/command:o to /teo/rightArm/CartesianControl/command:i using udp
yarp: Sending output from /teo/rightArm/CartesianControl/state:o to /KeyboardCartesianControlClient/state:i using udp
[debug] RpcResponder.cpp:50 respond(): Got: [get] [cpf]
[debug] RpcResponder.cpp:50 respond(): Got: [stop]
[INFO]device active in background...
[INFO]device active in background...
[INFO]device active in background...
[INFO]device active in background...
PeterBowman commented 5 years ago

Gripper limits as fetched by BCC via remote_controlboard are [0,0]:

[info] DeviceDriverImpl.cpp:89 open(): Joint 0 limits: [-88.500000,98.100000]
[info] DeviceDriverImpl.cpp:89 open(): Joint 1 limits: [-75.500000,29.600000]
[info] DeviceDriverImpl.cpp:89 open(): Joint 2 limits: [-57.000000,80.100000]
[info] DeviceDriverImpl.cpp:89 open(): Joint 3 limits: [-32.900000,83.500000]
[info] DeviceDriverImpl.cpp:89 open(): Joint 4 limits: [-99.600000,76.800000]
[info] DeviceDriverImpl.cpp:89 open(): Joint 5 limits: [-56.900000,107.500000]
[info] DeviceDriverImpl.cpp:89 open(): Joint 6 limits: [0.000000,0.000000]

Per launchManipulation.ini, those should be [-1200,1200].

This is both a regression caused by https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/161 and a silent bug buried somewhere in yarp-devices.

PeterBowman commented 5 years ago

As a quick workaround, you could just set llim via RPC and move away from the [0,0] initial limit setup. I'm submitting a patch right now, though.

PeterBowman commented 5 years ago

@rsantos88 please update yarp-devices onboard (https://github.com/roboticslab-uc3m/yarp-devices/commit/1632d72a7f6a2bf601dac52628226e4a1f319472) and try again.

rsantos88 commented 5 years ago

Thanks @PeterBowman ! Now it works with the real robot :)