pal-robotics / play_motion Goto Github PK

A tool to play pre-recorded motions on a robot using ros_control

C++ 86.19% Python 11.45% CMake 2.37%

play_motion's Introduction

play_motion

This repository consists of play_motion, a tool to play and handle pre-recorded motions, and its associated messages in play_motion_msgs

Have a look at the main README file to see how it works.

play_motion's People

Contributors

Stargazers

Watchers

play_motion's Issues

PlayMotion::run should not compute approach trajectories when it's busy

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.

Allow to cancel goals while doing motion planning

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

Error connecting to action client following commit 24bff740e6e79723aedd618428cfda2daf0e58a5

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.

play_motion occasionally not getting notified of controller result

@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.

Confusing error code when joint_name/trajectory has a size mismatch

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.

Big split

This issue acts as a tracker for future release 0.6.

The code of play_motion needs to be split into 3 distinct entities:

a motion dispatcher
this will expose a FollowJointTrajectory action and forward it to control groups.
a pre-recorded motion player
this is what play_motion currently does. It will be coupled with MoveIt! to provide motion planning.
a motion handling library and tools

Implement proper error code when motion is aborted by controller

Allow to specify planning groups in play_motion request

Consider adding this feature, in case we want to force a specific planning group.

Assertion thrown when killing play_motion with motion_planning

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

Move messages to separate package play_motion_msgs

Implement convenience move_joint script.

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.

getParamCached() does not work

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

Availability of this package on the ROS buildfarm

Hi,

This package seems available in the ROS binary repositories only for indigo.
Does someone knows why it is not in kinetic ?

Thanks

Program crashes with empty motion name

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)

Colorize axcli output

Colorize output to better distinguish error/success states.

Unit test code for populating trajectory velocities

Time offsetting after initial interpolation for joints outside of planning groups is wrong

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

---

Investigate play_motion crash

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

Busy error codes

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?.

Integrate with MoveIt!

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? :)

Add tab-completion to move_joint script

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.

Fix behavior when canceling a motion

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:

The last message suggests something fishy in the actionlib logic.
The 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.
When a motion is canceled, we should probably discriminate between the cases of the play_motion goal being canceled, and somebody preempting our controller goals. Ideally, a single log statement could reflect that. Examples:

Motion execution was canceled.
Motion execution failed. Controller: 'foo' failed with error 0.

exception thrown when time_from_start field of motion is integer

... 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)

Use standard messages instead of custom data structures

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.

getMotionIds() doesn't handle motions in nested namespaces

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:

Check that what it has found is a motion (ie it has joints, points, etc..)
If it is not a motion, look for motions nested within it.

play_motion dies on start of a new controller

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.

Dies when a motion name contains invalid characters

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

pal-robotics / play_motion Goto Github PK

play_motion's Introduction

play_motion

play_motion's People

Contributors

Stargazers

Watchers

Forkers

play_motion's Issues

Recommend Projects

Recommend Topics

Recommend Org