pal-robotics / play_motion Goto Github PK
View Code? Open in Web Editor NEWA tool to play pre-recorded motions on a robot using ros_control
A tool to play pre-recorded motions on a robot using ros_control
This is what happens when an action goal is interrupted while planning:
To transition to a succeeded state, the goal must be in a preempting or active state, it is currently in state: 8
State 8 is RECALLED
We need motion planning to reach the first point of a motion.
@adolfo-rt I believe that most of the work has already been laid out, right? :)
Something's not getting destructed correctly, or in the right order. This is what play_motion says:
play_motion: /usr/include/boost/thread/pthread/pthread_mutex_scoped_lock.hpp:26: boost::pthread::pthread_mutex_scoped_lock::pthread_mutex_scoped_lock(pthread_mutex_t*): Assertion `!pthread_mutex_lock(m)' failed.
Aborted (core dumped)
And the gdb backtrace
#0 0xb7fdd424 in __kernel_vsyscall ()
#1 0xb7c5fd13 in pthread_cond_timedwait@@GLIBC_2.3.2 () from /lib/i386-linux-gnu/libpthread.so.0
#2 0xb7dd0743 in bool boost::condition_variable::timed_wait<boost::date_time::subsecond_duration<boost::posix_time::time_duration, 1000000ll> >(boost::unique_lock<boost::mutex>&, boost::date_time::subsecond_duration<boost::posix_time::time_duration, 1000000ll> const&) () from /opt/ros/hydro/lib/libroscpp.so
#3 0xb7dcde06 in ros::CallbackQueue::callAvailable(ros::WallDuration) () from /opt/ros/hydro/lib/libroscpp.so
#4 0xb7e1e2e8 in ros::SingleThreadedSpinner::spin(ros::CallbackQueue*) () from /opt/ros/hydro/lib/libroscpp.so
#5 0xb7e02f97 in ros::spin(ros::Spinner&) () from /opt/ros/hydro/lib/libroscpp.so
#6 0xb7e02fc8 in ros::spin() () from /opt/ros/hydro/lib/libroscpp.so
#7 0x0808776f in main (argc=1, argv=0xbfffe704) at /home/adolfo/play_motion_planning/src/play_motion/play_motion/src/play_motion_main.cpp:59
Commit 24bff74 added a 'waitForServer' call to the move_joint_group.cpp script (24bff74#diff-e95a770c00b19a4af5e34d71503596e7943bfdf36f6c3b5c175a24283f6bbe35R53-R59). Since pulling this update I am getting the error from line 57:
[ERROR] [1629790094.641395653, 649.746000000]: Error connecting to the client : arm_left_controller/follow_joint_trajectory !
[ERROR] [1629790095.174947494, 650.253000000]: Error connecting to the client : arm_right_controller/follow_joint_trajectory !
[ERROR] [1629790095.760662505, 650.760000000]: Error connecting to the client : gripper_left_controller/follow_joint_trajectory !
[ERROR] [1629790096.295745091, 651.265000000]: Error connecting to the client : gripper_right_controller/follow_joint_trajectory !
[ERROR] [1629790096.829193277, 651.770000000]: Error connecting to the client : head_controller/follow_joint_trajectory !
[ERROR] [1629790097.386758940, 652.276000000]: Error connecting to the client : torso_controller/follow_joint_trajectory !
The output of rostopic list | grep follow_joint_trajectory
shows that the servers are in fact up and running:
/arm_left_controller/follow_joint_trajectory/cancel
/arm_left_controller/follow_joint_trajectory/feedback
/arm_left_controller/follow_joint_trajectory/goal
/arm_left_controller/follow_joint_trajectory/result
/arm_left_controller/follow_joint_trajectory/status
/arm_right_controller/follow_joint_trajectory/cancel
/arm_right_controller/follow_joint_trajectory/feedback
/arm_right_controller/follow_joint_trajectory/goal
/arm_right_controller/follow_joint_trajectory/result
/arm_right_controller/follow_joint_trajectory/status
/gripper_left_controller/follow_joint_trajectory/cancel
/gripper_left_controller/follow_joint_trajectory/feedback
/gripper_left_controller/follow_joint_trajectory/goal
/gripper_left_controller/follow_joint_trajectory/result
/gripper_left_controller/follow_joint_trajectory/status
/gripper_right_controller/follow_joint_trajectory/cancel
/gripper_right_controller/follow_joint_trajectory/feedback
/gripper_right_controller/follow_joint_trajectory/goal
/gripper_right_controller/follow_joint_trajectory/result
/gripper_right_controller/follow_joint_trajectory/status
/head_controller/follow_joint_trajectory/cancel
/head_controller/follow_joint_trajectory/feedback
/head_controller/follow_joint_trajectory/goal
/head_controller/follow_joint_trajectory/result
/head_controller/follow_joint_trajectory/status
/torso_controller/follow_joint_trajectory/cancel
/torso_controller/follow_joint_trajectory/feedback
/torso_controller/follow_joint_trajectory/goal
/torso_controller/follow_joint_trajectory/result
/torso_controller/follow_joint_trajectory/status
Despite the errors, play_motion still successfully executes the home motion.
Increasing the waitForServer duration from 0.5 to 2.0 seconds gave me the same error but this time the home motion failed.
After switching controllers, play_motion
died with the following error:
play_motion: /usr/include/boost/thread/pthread/pthread_mutex_scoped_lock.hpp:26: boost::pthread::pthread_mutex_scoped_lock::pthread_mutex_scoped_lock(pthread_mutex_t*): Assertion `!pthread_mutex_lock(m)' failed.
Currently, if play_motion is busy executing a motion, and a new request arrives, it will first compute an approach trajectory for the requested motion before checking if it's actually busy or not. Requests should be rejected as early as possible. I'll refactor the run method to exhibit this behavior.
The following post-mortem log data has been reconstructed from a crash triggered by @v-lopez. We don't have ATM a deterministic way to reproduce.
play_motion/play_motion/src/play_motion.cpp:109(controllerCb)
Controller right_arm_controller aborted.
1395915290.107941635 ERROR
actionlib/server/server_goal_handle_imp.h:283(GoalStatus actionlib::ServerGoalHandle<ActionSpec>::getGoalStatus)
Attempt to get goal status on an uninitialized ServerGoalHandle or one that has no ActionServer associated with it.
1395915290.107959489 ERROR
actionlib/server/server_goal_handle_imp.h:283(GoalStatus actionlib::ServerGoalHandle<ActionSpec>::getGoalStatus)
Attempt to get goal status on an uninitialized ServerGoalHandle or one that has no ActionServer associated with it.
1395915290.108058017 ERROR
actionlib/client/simple_action_client.h:338(SimpleClientGoalState actionlib::SimpleActionClient<ActionSpec>::getState)
Trying to getState() when no goal is running. You are incorrectly using SimpleActionClient
1395915290.108064793 ERROR
actionlib/client/simple_action_client.h:338(SimpleClientGoalState actionlib::SimpleActionClient<ActionSpec>::getState)
Trying to getState() when no goal is running. You are incorrectly using SimpleActionClient
1395915303.298149765 ERROR
play_motion/play_motion/src/play_motion.cpp:109(controllerCb)
Controller right_arm_controller aborted.
1395915568.909587136 ERROR
actionlib/server/server_goal_handle_imp.h:116(ServerGoalHandle<ActionSpec>::setCanceled)
To transition to a cancelled state, the goal must be in a pending, recalling, active, or preempting state, it is currently in state: 4 (ABORTED)
1395915568.909595402 ERROR
[11:32:40] Victor Lopez: [roslaunch][ERROR] 2014-03-27 10:19:08,753: [play_motion-10] process has died [pid 5342, exit code -6
This is the current output produced when a motion is canceled (timestamps removed for brevity):
[ WARN] [.]: controller left_arm_controller failed with err 0
[ WARN] [.]: controller head_controller failed with err 0
[ WARN] [.]: controller right_arm_controller failed with err 0
[ WARN] [.]: controller torso_controller failed with err 0
[ INFO] [.]: Motion played successfully.
[ERROR] [.]: You are attempting to call methods on an uninitialized goal handle
Things to look into:
Motion played successfully.
message is not correct. It should say something more like Motion execution was canceled.
. This point might be related to the previous one.Motion execution was canceled.
Motion execution failed. Controller: 'foo' failed with error 0.
Consider adding this feature, in case we want to force a specific planning group.
Given the following parameters in the param server:
play_motion:
motions:
alive_engine:
backwards_1:
joints: []
....
wave:
joints: []
getMotionIds() will return [alive_engine, wave]. But alive_engine is not a motion, but a namespace that contains other motions.
I believe it should:
@v-lopez has observed in a few occassions that even when the joint trajectory controller goals finishes, play_motion
is not notified, and its internal busy state is maintained indefinitely. There is no deterministic way to reproduce, but it is likely that this happened when canceling an active goal, and immediately sending a new one.
A stress test could be setup to repeatedly exercise the above sequence, and see if we can reproduce. We should verify if the problem comes from play_motion
itself, or whether the joint_trajectory_controller
server is somehow not publishing to the result topic.
I provided a wrong motion, with two more trajectory points than joint_names.
joints: [torso_1_joint, torso_2_joint, arm_left_1_joint, arm_left_2_joint, arm_left_3_joint,
arm_left_4_joint, arm_left_5_joint, arm_left_6_joint, arm_left_7_joint, arm_right_1_joint,
arm_right_2_joint, arm_right_3_joint, arm_right_4_joint, arm_right_5_joint, arm_right_6_joint,
arm_right_7_joint, hand_left_thumb_joint, hand_left_index_joint, hand_left_middle_joint]
points:
- positions: [-0.321742561627, 0.237773922422, 0.3, -0.3, 0.696098355901, 0.212187514735,
-0.155362552541, 0.778092302718, 1.74729794359, -0.332998773321, -0.0196655310411,
0.00620328653138, 0.00597665068693, -0.00859981587157, 0.00366430210415, -0.00392862150911,
0.00612770724576, -0.00167366818991, 0.9, 3.2, 3.2]
time_from_start: 1.0
The error message in the play_motion axclient was:
error_code: -8
error_string: Approach motion planning failed
Which is confusing because it happened whether I used planning or not.
This is of low priority, just to keep in mind for future releases.
Hi,
This package seems available in the ROS binary repositories only for indigo.
Does someone knows why it is not in kinetic ?
Thanks
Figuring out the name of a joint is cumbersome, and for users not familiar with a robot it can be hard to remember strings like arm_left_4_joint
. Further, move_joint
can't move every robot joint, but only those with an active joint trajectory controller.
Since play_motion
knows about the available controllers (hence the joints they control) it should be possible to expose a ROS service for querying the joints it can currently act upon.
Tab completion could be built on top of this service.
This issue acts as a tracker for future release 0.6
.
The code of play_motion
needs to be split into 3 distinct entities:
FollowJointTrajectory
action and forward it to control groups.play_motion
currently does. It will be coupled with MoveIt! to provide motion planning.The problem is not in play_motion
but in ros_comm
.
I am opening an issue here as a reminder.
There is a pull request in ros_comm
to address the issue: ros/ros_comm#352
Currently there exists an error code to indicate that a motion is being executed, namely CONTROLLER_BUSY.
The case where play_motion is busy planning is currently not covered. We could add a new error code named PLANNER_BUSY
for this case. The upside is that we would be able to discriminate the reason for play_motion being busy. The downside is that if busy status is all we need, we would have to check against two error codes, maybe through a convenience function.
The alternative proposal would be to have a single error code to represent the busy state, but it implies an API break.
Thoughts and preferences?.
... which should be a valid use case.
To reproduce the issue change any motion the following way:
time_from_start: 1.0 -> time_from_start: 1
Example output upon execution:
[ INFO] [1394210639.559383205, 954.873000000]: sending motion 'arms_t' to controllers
terminate called after throwing an instance of 'XmlRpc::XmlRpcException'
Aborted (core dumped)
We have internal Trajectory
and TrajectoryPoint
structures, which are very close to JointTrajectory
and JointTrajectoryPoint
in trajectory_msgs
.
By using the latter ones, we allow for easier extendability.
The problem was introduced in the refactoring of play_motion.cpp
into play_motion_helpers.cpp
We should get a NodeHandle
namespaced to play_motion/motions
and check nh_motions.hasParam(motion_id)
In another project, we have developed a convenience command-line tool called move_joint
that allows to easily move a joint to a given position, optionally specifying the motion duration. Considering the current state of play_motion
, it should be straightforward to reimplement this tool as a Python script that leverages play_motion
.
The script will need to use functionality from the motion_planning
branch, so it will be implemented there.
Colorize output to better distinguish error/success states.
play_motion
died with the following error after trying to play a motion with a ;
in its name (which was obviously a mistake)
terminate called after throwing an instance of 'ros::InvalidNameException'
what(): Character [;] at element [13] is not valid in Graph Resource Name [motions/hold_;ady/joints]. Valid characters are a-z, A-Z, 0-9, / and _.
[play_motion-9] process has died
It need to catch the ros::InvalidNameException
Apparently, the hands are not part of any planning group.
When triggering a motion that requires the hands to be opened as its first point, the hands being currently closed, the following error is outputted:
[ERROR] [1396352784.141338794, 518.948000000]: Can't construct segment from ROS message: Quintic spline segment can't be constructed: end_time < start_time.
[ERROR] [1396352784.141517573, 518.949000000]: Can't construct segment from ROS message: Quintic spline segment can't be constructed: end_time < start_time.
[ERROR] [1396352784.141643255, 518.949000000]: Can't construct segment from ROS message: Quintic spline segment can't be constructed: end_time < start_time.
[ERROR] [1396352784.141710661, 518.949000000]: Can't construct segment from ROS message: Quintic spline segment can't be constructed: end_time < start_time.
[ERROR] [1396352784.141824146, 518.949000000]: Can't construct segment from ROS message: Quintic spline segment can't be constructed: end_time < start_time.
[ERROR] [1396352784.141900933, 518.949000000]: Can't construct segment from ROS message: Quintic spline segment can't be constructed: end_time < start_time.
[ERROR] [1396352784.142137146, 518.949000000]: Controller right_hand_controller aborted.
Sniffing on /right_hand_controller/follow_joint_trajectory/goal
yields the following goal:
header:
seq: 3
stamp:
secs: 542
nsecs: 831000000
frame_id: ''
goal_id:
stamp:
secs: 542
nsecs: 831000000
id: /play_motion-74-542.831000000
goal:
trajectory:
header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: ''
joint_names: ['hand_right_index_joint', 'hand_right_middle_joint', 'hand_right_thumb_joint']
points:
-
positions: [0.0, 0.0, 0.0]
velocities: [0.0, 0.0, 0.0]
accelerations: []
effort: []
time_from_start:
secs: 6
nsecs: 7342128
-
positions: [0.0, 0.0, 0.0]
velocities: [0.0, 0.0, 0.0]
accelerations: []
effort: []
time_from_start:
secs: 2
nsecs: 0
-
positions: [0.0, 0.0, 0.0]
velocities: [0.0, 0.0, 0.0]
accelerations: []
effort: []
time_from_start:
secs: 3
nsecs: 0
-
positions: [0.0, 0.0, 0.0]
velocities: [0.0, 0.0, 0.0]
accelerations: []
effort: []
time_from_start:
secs: 5
nsecs: 0
path_tolerance: []
goal_tolerance: []
goal_time_tolerance:
secs: 0
nsecs: 0
---
A declarative, efficient, and flexible JavaScript library for building user interfaces.
🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
An Open Source Machine Learning Framework for Everyone
The Web framework for perfectionists with deadlines.
A PHP framework for web artisans
Bring data to life with SVG, Canvas and HTML. 📊📈🎉
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
Some thing interesting about web. New door for the world.
A server is a program made to process requests and deliver data to clients.
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
Some thing interesting about visualization, use data art
Some thing interesting about game, make everyone happy.
We are working to build community through open source technology. NB: members must have two-factor auth.
Open source projects and samples from Microsoft.
Google ❤️ Open Source for everyone.
Alibaba Open Source for everyone
Data-Driven Documents codes.
China tencent open source team.