[kinetic] Invalid Trajectory: start point deviates from current robot state more than 0.01

[vonshta...@gmail.com]

Hi,

I am using Moveit with ROS kinetic for planning the motions for a UR5 arm. I am using the Track_IK kinematic solver. The problem is , I am speicifying simple goal poses for the gripper in cartesian space, but I am getting the error message in the terminal of moveit_planning execution.launch.

[ERROR] [1511284205.660285187]:
Invalid Trajectory: start point deviates from current robot state more than 0.01
joint 'arm_elbow_joint': expected: 0, current: 1.84494
[ INFO] [1511284205.660356791]: Execution completed: ABORTED
[ INFO] [1511284205.700050212]: Execution request received for ExecuteTrajectory action.
[ WARN] [1511284205.708508088]: Controller failed with error code INVALID_GOAL
[ WARN] [1511284205.708629940]: Controller handle reports status FAILED
[ INFO] [1511284205.708804955]: Execution completed: FAILED
[ INFO] [1511284205.739949934]: Execution request received for ExecuteTrajectory action.

I looked into the issue mentioned here. But I ma using trac_Ik instead of Descartes. So I am not sure how to implement a proper solution.

I also referenced this, but then changing the

/move_group/trajectory_execution/allowed_start_tolerance to 0.0 is giving me another error.

[ WARN] [1511285456.138267775]: Controller failed with error code INVALID_GOAL
[ WARN] [1511285456.138359326]: Controller handle reports status FAILED
[ INFO] [1511285456.138473683]: Execution completed: FAILED
[ INFO] [1511285456.168054862]: Execution request received for ExecuteTrajectory action.

in the terminal where I launch the UR_driver to interface with the real robot I have the error

[ERROR] [1511285455.959112929]: Goal start doesn't match current pose
[ INFO] [1511285455.995846536]: on_goal
[ERROR] [1511285455.996445868]: Goal start doesn't match current pose
[ INFO] [1511285456.036065982]: on_goal

and in the terminal where I launch the node I get

**[ INFO] [1511285455.930223750]: ABORTED: Solution found but controller failed during execution
[ INFO] [1511285455.968737779]: ABORTED: Solution found but controller failed during execution
[ INFO] [1511285456.009758052]: ABORTED: Solution found but controller failed during execution
[ INFO] [1511285456.049777660]: ABORTED: Solution found but controller failed during execution
**

I am also attaching part of the code here to get a better view of the things

` int main(int argc, char **argv)
{
ros::init(argc, argv, "move_group_pick");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
ros::Duration sleep_time(3.0);
group.reset( new moveit::planning_interface::MoveGroupInterface ("arm"));
planning_scene_interface.reset(new moveit::planning_interface::PlanningSceneInterface());
transformListener = boost::shared_ptrtf::TransformListener(new tf::TransformListener(node_handle,
ros::Duration(1.0)));
display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
rviz_updater = node_handle.advertise<std_msgs::Empty>("/rviz/moveit/update_goal_state", 1, true);
timer = node_handle.createTimer(ros::Duration(0.1), timerCallback);
ROS_INFO("Reference frame: %s", group->getPlanningFrame().c_str());
ROS_INFO("Reference frame: %s", group->getEndEffectorLink().c_str());
client = node_handle.serviceClient<ur_msgs::SetIO>("ur_driver/set_io");
srand(time(NULL));
target_pose0 = Pose_pick_up();
if (move2pose(group, target_pose0) == -1) return (-1) ;
sleep_time.sleep();
target_pose0 = Pose_mrkr();
if (move2pose(group, target_pose0) == -1) return (-1) ;
sleep_time.sleep();
/// spawn/attach the box here
ros::Publisher planning_scene_diff_publisher = node_handle.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
while(planning_scene_diff_publisher.getNumSubscribers() < 1)
{
ros::WallDuration sleep_t(0.5);
sleep_t.sleep();
}

moveit_msgs::AttachedCollisionObject attached_object;
attached_object.link_name = "gripper_link";
attached_object.object.header.frame_id = "gripper_link";
attached_object.object.id = "box";
}
///----------.......................................
//.................................`
Any help is much appreciated.
@v4hn , @gavanderhoorn

[G.A. vd. Hoorn - 3ME]

On 21-11-2017 19:32, vonshta...@gmail.com wrote:
> Hi,
>
> I am using Moveit with ROS kinetic for planning the motions for a UR5 arm.
> I am using the Track_IK kinematic solver. The problem is , I am speicifying
> simple goal poses for the gripper in cartesian space, but I am getting the
> error message in the terminal of moveit_planning execution.launch.

[..]

Hi,

could I please ask you to not cross-post questions to both this mailing
list /and/ the moveit issue tracker [1]?

Cross-posting leads to disjointed discussions with duplication of effort
and that wastes everyone's time.


Thanks,

Gijs

[1] https://github.com/ros-planning/moveit/issues/694

[v4hn]

Hopefully you got this fixed by now, but just for the record:

On Tue, Nov 21, 2017 at 10:38:46AM -0800, vonshta...@gmail.com wrote:
> [ERROR] [1511284205.660285187]:
> Invalid Trajectory: start point deviates from current robot state more than
> 0.01
> joint 'arm_elbow_joint': expected: 0, current: 1.84494

This means the trajectory you try to execute starts far away from the current
state of the robot. Likely your trajectory starts at the all-zero configuration
which means whichever node plans the trajectory doesn't know, or less likely
ignores, the current joint configuration. The most common source of error to
trigger this would be that the `move_group` node does not subscribe to the
correct JointState topic.


v4hn


--
Michael Görner, M.Sc. Cognitive Science, PhD Student
Universität Hamburg
Faculty of Mathematics, Informatics and Natural Sciences
Department of Informatics
Group Technical Aspects of Multimodal Systems

Vogt-Kölln-Straße 30
D-22527 Hamburg

Room: F-315
Phone: +49 40 42883-2432
Website: https://tams.informatik.uni-hamburg.de/people/goerner/