How to give desired position(x,y,z) to ikfast translation3d solver in cpp?

asked 2019-03-26 14:59:52 -0500

Nazar gravatar image

Hi, I have been trying so many ways move my robot to a specific point using ikfast translation 3d solver. I am working on a3dof manipulator in ros kinetic. The interactive markers work on Rviz and I am able plan and execute using rviz gui. But i need to plan and move my manipulator using cpp code instead of rviz. Which function should i call in the cpp code??

This is the error i always get:

nazar@nazars-stuff:~$ rosrun move_to_point test_custom_node 
[ INFO] [1553629942.002232999, 7.351000000]: Loading robot model 'rpp'...
[ INFO] [1553629942.002404290, 7.351000000]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ INFO] [1553629942.306021789, 7.543000000]: Loading robot model 'rpp'...
[ INFO] [1553629942.306666036, 7.543000000]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ WARN] [1553629942.325382051, 7.556000000]: The root link base has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.
[ INFO] [1553629955.546756272, 18.482000000]: Ready to take commands for planning group arm.
[ INFO] [1553629955.555035921, 18.482000000]: Reference frame: /base
[ INFO] [1553629955.555090483, 18.482000000]: Reference frame: link3
[ INFO] [1553629955.667542640, 18.563000000]: ABORTED: No motion plan found. No execution attempted.
[ INFO] [1553629955.817610119, 18.673000000]: ABORTED: No motion plan found. No execution attempted.

This is the cpp code I used:

#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();


  /* This sleep is ONLY to allow Rviz to come up */
  sleep(2.0);

  // BEGIN_TUTORIAL
  // 
  // Setup
  // ^^^^^
  // 
  // The :move_group_interface:`MoveGroup` class can be easily 
  // setup using just the name
  // of the group you would like to control and plan for.
  moveit::planning_interface::MoveGroupInterface group("arm");

  // We will use the :planning_scene_interface:`PlanningSceneInterface`
  // class to deal directly with the world.
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;  

  // (Optional) Create a publisher for visualizing plans in Rviz.
  ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
  moveit_msgs::DisplayTrajectory display_trajectory;

  // Getting Basic Information
  // ^^^^^^^^^^^^^^^^^^^^^^^^^
  //
  // We can print the name of the reference frame for this robot.
  ROS_INFO("Reference frame: %s", group.getPlanningFrame().c_str());

  // We can also print the name of the end-effector link for this group.
  ROS_INFO("Reference frame: %s", group.getEndEffectorLink().c_str());

  // Planning to a Pose goal
  // ^^^^^^^^^^^^^^^^^^^^^^^
  // We can plan a motion for this group to a desired pose for the 
  // end-effector.

  // point one
  group.setStartStateToCurrentState ();
  group.setPositionTarget(-0.161, -0.248, 0.274,"link3");
  group.move();

  //point two
  group.setStartStateToCurrentState ();
  group.setPositionTarget(-0.296, 0.014, 0.274,"link3");
  group.move();

  ros::waitForShutdown();
  sleep(5.0);
// END_TUTORIAL

  ros::shutdown();  
  return 0;
}

Kindly help me to move ahead.

edit retag flag offensive close merge delete