Moveit tutorial cartesian path error, missing valid timestamp data?
Hello,
I am following tutorial for Cartesian path using move_group c++ interface. But I am getting error when I send this trajectory to motoman robot.
[ERROR] [1556871367.163994139]: Validation failed: Missing valid timestamp data for trajectory pt 1
[ERROR] [1556871371.219235895]: Controller is taking too long to execute trajectory (the expected upper bound for the trajectory execution was 4.056057 seconds). Stopping trajectory
source code I am executing is,
static const std::string PLANNING_GROUP = "arm";
// The :move_group_interface:`MoveGroup` class can be easily
// setup using just the name of the planning group you would like to control and plan for.
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
// We will use the :planning_scene_interface:`PlanningSceneInterface`
// class to add and remove collision objects in our "virtual world" scene
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
// Raw pointers are frequently used to refer to the planning group for improved performance.
const robot_state::JointModelGroup* joint_model_group = move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
geometry_msgs::Pose target_pose2 = move_group.getCurrentPose().pose;
std::vector<geometry_msgs::Pose> waypoints;
waypoints.push_back(target_pose2);
//geometry_msgs::Pose target_pose3 = target_pose2;
target_pose2.position.z -= 0.4;
waypoints.push_back(target_pose2);
target_pose2.position.x += 0.4;
waypoints.push_back(target_pose2);
move_group.setMaxVelocityScalingFactor(0.1);
moveit_msgs::RobotTrajectory trajectory;
const double jump_thresold = 0.0;
const double eef_step = 0.01;
move_group.computeCartesianPath(waypoints, eef_step, jump_thresold, trajectory);
moveit::planning_interface::MoveGroupInterface::Plan my_plan1;
my_plan1.trajectory_ = trajectory;
move_group.execute(my_plan1);
How can I solve this?
thanks.
...................................................................................................
[ERROR] [1557146297.748611305] [/motion_streaming_interface] [ros.motoman_driver]: Validation failed: Missing valid timestamp data for trajectory pt 1
[ERROR] [1557146299.550803668] [/move_group] [ros.moveit_ros_planning.trajectory_execution_manager]: Controller is taking too long to execute trajectory (the expected upper bound for the trajectory execution was 1.804171 seconds). Stopping trajectory.
Not an answer but an observation: you may want to change your ROS logging configuration to include the name of the node and/or the logger so that you can more easily determine where certain log lines are coming from.
Example:
The "validation failed" line is most likely coming from
motoman_driver
.what can be possible answer? is there any mistake in code? robot is moving in rviz but not in real.
Please update the error lines with the updated rosconsole format config first.
sorry but I could not figured out how to update that list.
but when I set first target pose1 and then using cartesian path from that pose(target pose1) to new target pose2 then It is working. but directly getting pose from move_group.getCurrentPose().pose is not working on real robot.
add the line I showed you to your
$HOME/.bashrc
(prefix it withexport
), start a new terminal and rerun your code.updated...
Can you read out the trajectory.joint_trajectory.points[point number].time_from_start.toSec(). For every point of the trajectory will be check for a time_from_start that is not 0.ONLY THE FIRST POINT OF THE TRAJECTORY HAS A TIME_FROM_START OF 0. If the second point has a time value of 0, that means the robot does not need to move from points.
I am facing same issue when using KDL with cartesian paths, I dont know how this can be tackled down,
computeCartesianPath()
takesvector<geometryPose>
which indeed lacks theheader
and leading this error