ahundt / grl

Robotics tools in C++11. Implements soft real time arm drivers for Kuka LBR iiwa plus V-REP, ROS, Constrained Optimization based planning, Hand Eye Calibration and Inverse Kinematics integration.
https://ahundt.github.io/grl/
BSD 2-Clause "Simplified" License
155 stars 73 forks source link

additional checks in for commandedPosition_goal in JavaDriver? #130

Open tdinesh opened 7 years ago

tdinesh commented 7 years ago

The plugin crashes when I switch to ArmState_MoveArmJointServo. Maybe it is good to have additional check on commandedPosition_goal at https://github.com/ahundt/grl/blob/master/include/grl/kuka/KukaJAVAdriver.hpp#L254

!!decoding error: missing required field!! commandedpos:[] terminate called after throwing an instance of 'boost::exception_detail::clone_impl<boost::exception_detail::error_info_injector<std::runtime_error> >' what(): Error decoding received data

Thoughts?

tdinesh commented 7 years ago

Following are the errors. Joint angles are published on /joint_states. No new FRI data available, is an FRI application running on the Kuka arm? Total sucessful transfers: 6446 Total attempts: 25788 Consecutive Failures: 3 Consecutive Successes: 0

tdinesh commented 7 years ago

Running KukaFRIClientDataDriverTest does not move the arm. Looks like the sessionState never goes to KUKA::FRI::COMMANDING_ACTIVE, but is always MONITORING_READY(2). @ahundt Am I missing something? Also, what version of SunrisOS do you use?

using: 192.170.10.100 30200 192.170.10.2 30200 [2017-02-10 20:56:09.567] [console] [info] position: [1.37219e-05, 0.45248, 0.000143571, 1.62817, -4.24721e-05, 0.0359461, -7.10065e-05] us: 0 connectionQuality: 3 operationMode: 2 sessionState: 2 driveState: 0 ipoJointPosition: [] ipoJointPositionOffsets: []

ahundt commented 7 years ago

Hmm, yes there may be a regression somewhere. I do believe I tested it immediately before the 4.1.0 release, where KukaFRIClientDataTest was able to successfully drive the robot. I also have a video. :-)

That said, FRI commanding has been always been much more flaky than java commanding and receiving sensor updates over FRI.

Are you running FRI test or the GRL_Driver on the kuka pendant? Which C++ app/plugin/test are you running?

tdinesh commented 7 years ago

I am running the GRL_Driver, which one is the FRI test? Also, haven't been able to command positions in JAVA mode due to GRL_driver crashing with empty commandedPositions.

ahundt commented 7 years ago

The plugin crashes when I switch to ArmState_MoveArmJointServo. Maybe it is good to have additional check on commandedPosition_goal at https://github.com/ahundt/grl/blob/master/include/grl/kuka/KukaJAVAdriver.hpp#L254

I think failing when no goal has been set before switching into servo mode is likely correct behavior for safety reasons. Also, you'll need to make the call that sets the goal position before actually moving: https://github.com/ahundt/grl/blob/master/include/grl/kuka/KukaJAVAdriver.hpp#L421

ahundt commented 7 years ago

'boost::exception_detail::clone_impl<boost::exception_detail::error_info_injector >' what(): Error decoding received data

In accordance with my last post, this is actually an exception that's not being caught. Are you using the ROS driver under C++, or directly using the driver API?

Also if you would like you can get my email address here, https://github.com/ahundt, and if you email me your phone number or skype info we could speak on the phone or on skype/facetime if that would help.

ahundt commented 7 years ago

For what you need to run for the FRI only test, see the following comment from the previous issue: https://github.com/ahundt/grl/issues/119#issuecomment-277475156

ahundt commented 7 years ago

The short part is for an FRI only test: C++: KukaFRIClientDataDriverTest.cpp Java: FRIHoldsPosition_Command.java

ahundt commented 7 years ago

and make sure your robot is very clear of any obstacles, make sure it is positioned so the zero position is pointing up (robot mounted vertically), plus be ready with the red button...