Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

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

Closed
rsantos88 opened this issue Oct 25, 2018 · 4 comments
Closed

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

rsantos88 opened this issue Oct 25, 2018 · 4 comments
Assignees
@PeterBowman
Labels
bug Something isn't working

Comments

@rsantos88
Copy link
Contributor

rsantos88 commented Oct 25, 2018 •
edited by PeterBowman
Loading

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 PeterBowman added the bug Something isn't working label Oct 25, 2018
@PeterBowman
Copy link
Member

PeterBowman commented Oct 25, 2018 •
edited
Loading

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 #161 and a silent bug buried somewhere in yarp-devices.

@PeterBowman PeterBowman changed the title Trying KeyboardControllerClient app with Teo Wrong limits for gripper cause motion control to exit abruptly Oct 25, 2018
@PeterBowman
Copy link
Member

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 added a commit to roboticslab-uc3m/yarp-devices that referenced this issue Oct 25, 2018
@PeterBowman
Fetch limits from parent device's .ini config
1632d72 
Addresses roboticslab-uc3m/kinematics-dynamics#165.
@PeterBowman PeterBowman self-assigned this Oct 25, 2018
@PeterBowman
Copy link
Member

@rsantos88 please update yarp-devices onboard (roboticslab-uc3m/yarp-devices@1632d72) and try again.

@rsantos88
Copy link
Contributor Author

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

rsantos88 pushed a commit to roboticslab-uc3m/yarp-devices that referenced this issue Jan 29, 2019
@PeterBowman
Fetch limits from parent device's .ini config
5b0a26e 
Addresses roboticslab-uc3m/kinematics-dynamics#165.
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees

@PeterBowman PeterBowman

Labels
bug Something isn't working
Projects
None yet
Milestone
No milestone
Development

No branches or pull requests
2 participants
@PeterBowman @rsantos88