Arm not moving with Cartesian pose

edit

Hello All,

I am having a 3 DOF robotic arm. For this arm, I am using IK-Fast motion planner for its movement.

I am using below code to move the arm to different points:

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>

int main(int argc, char** argv)
{

ros::init(argc, argv, "move_group_interface_tutorial");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();

moveit::planning_interface::MoveGroupInterface move_group("arm");
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
const robot_state::JointModelGroup *joint_model_group = move_group.getCurrentState()->getJointModelGroup("arm");

ROS_INFO_NAMED("tutorial", "Reference frame: %s", move_group.getPlanningFrame().c_str());
ROS_INFO_NAMED("tutorial", "End effector link: %s", move_group.getEndEffectorLink().c_str());

moveit::planning_interface::MoveGroupInterface::Plan my_plan;

/* Move Code: With xyz positions */
geometry_msgs::PoseStamped target_pose1;
target_pose1 = move_group.getCurrentPose(move_group.getEndEffectorLink());
//target_pose1.orientation.w = 1.0;
target_pose1.pose.position.y = 0.01;
//target_pose1.position.y = 0.0;
//target_pose1.position.z = 0.58;
move_group.setPoseTarget(target_pose1);

/* Move Code: With Joint positions 
moveit::core::RobotStatePtr current_state = move_group.getCurrentState();
std::vector<double> joint_group_positions;
current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);
for(std::vector<double>::iterator it = joint_group_positions.begin(); it != joint_group_positions.end(); it++)
    ROS_INFO("Joint position: %lf", *it);
joint_group_positions[0] = -0.5;
move_group.setJointValueTarget(joint_group_positions);
*/
move_group.plan(my_plan);
move_group.move();

return 0;
}

Here my problem is that, I am not able to move the arm with setting the xyz pose. I am able to move it with setting the joints, as shown in commented part of the code. I am also able to move it with RViz goal settings.

And I am getting below errors:

[ WARN] [1528544186.050648987]: Fail: ABORTED: No motion plan found. No execution attempted.
[ INFO] [1528544191.086188354]: ABORTED: No motion plan found. No execution attempted.

And

[ INFO] [1528544186.058533344]: Planner configuration 'arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
Debug:   RRTConnect: Planner range detected to be 0.706318
Info:    RRTConnect: Starting planning with 1 states already in datastructure
Debug:   RRTConnect: Waiting for goal region samples ...
Debug:   Beginning sampling thread computation
Error:   RRTConnect: Unable to sample any valid states for goal tree
         at line 215 in /tmp/binarydeb/ros-kinetic-ompl-1.2.1/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp
Info:    RRTConnect: Created 1 states (1 start + 0 goal)
Info:    No solution found after 5.007490 seconds
Debug:   Attempting to stop goal sampling thread...
Debug:   Stopped goal sampling thread after 0 sampling attempts

I am not able to find the solution for this. Please let me know what should be the problem for this.

Saurabh