MoveIt and 4 DOF arm [closed]

edit

UBUNTU : 18.04 ROS VERSION MELODIC

Hi! , im works with and 4 DOF arm , and i have a lot of problems when i was trying to set it at desired position ,

My problems occurss when i try to use the below function :

setPoseTarget (const geometry_msgs::Pose &target, const std::string &end_effector_link="")

setPositionTarget (double x, double y, double z, const std::string &end_effector_link="")

setRPYTarget (double roll, double pitch, double yaw, const std::string &end_effector_link="")

The only function that movesmy arm is :

setApproximateJointValueTarget (const geometry_msgs::Pose &eef_pose, const std::string &end_effector_link="")

i have tried a lot of things to do, i cant found the error, MOVE-IT works fine with its interface, but it give me errors when im traying to move the robot with the C++ API.

I dont know if its about my robot configuration o something like that ,

This is my URDF file : 4 DOF ARM URDF

and the SRDF file : 4 DOF ARM SRDF

and the code :

#include <ros/ros.h>
//MOVE IT

#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

ros::Publisher display_publisher;

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

  ros::init(argc, argv, "custom_planning");
  ros::NodeHandle nh;
  ros::AsyncSpinner spinner(1);
  spinner.start();
  sleep(2.0);



//planning group that we would like to control
  moveit::planning_interface::MoveGroupInterface group("arm");
  moveit::planning_interface::MoveGroupInterface group_g("gripper");
  //we can add or remove collision objects in our virtual world scene
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
  //raw pointers are used to refer to the planning group for improved performance
  const robot_state::JointModelGroup *joint_model_group = group.getCurrentState()->getJointModelGroup("arm");
  group.setEndEffectorLink("link4");
  group.setStartStateToCurrentState();



geometry_msgs::Pose target_pose1;

  // target_pose1.orientation.x =-0.0698152559439;
  // target_pose1.orientation.y =2.08265213726e-06;



 // target_pose1.orientation.z =-1.45756547007e-07;
  // target_pose1.orientation.w = 0.997559938065;
  // target_pose1.position.y = -0.139249505223;
  // target_pose1.position.z =  1.00958973838;

  group.setGoalTolerance(0.0001);
  group.setPositionTarget(0, 1.37, 0.40, "link4");
  //group.setApproximateJointValueTarget(target_pose1,"link4"); only works with this function


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

  moveit::planning_interface::MoveItErrorCode success = group.plan(my_plan);
  ROS_INFO("Visualizing plan 1 (pose goal) %s", success.val ? "" : "FAILED");

  if (!success)
    throw std::runtime_error("No plan found");
  group.move(); //blocking
  sleep(5.0);

  ros::shutdown();

  return 0;
}

And finally the error :

//NODE ERROR

[ WARN] [1580223145.648796219]: Fail: ABORTED: No motion plan found. No execution attempted.
[ INFO] [1580223145.648850922]: Visualizing plan 1 (pose goal) 
terminate called after throwing an instance of 'std::runtime_error'
  what():  No plan found
Aborted (core dumped)

//MOVE IT ERROR

[ INFO] [1580223110.865485565]: Constructing new MoveGroup connection for group 'arm' in namespace ''
[ INFO] [1580223111.945787906]: Ready to take commands for planning group arm.
[ INFO] [1580223111.945929737]: Looking around: no
[ INFO] [1580223111.946068449]: Replanning: no
[ INFO] [1580223140.518161060]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1580223140.625874939]: Planner configuration 'arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the ...