How to give desired position(x,y,z) to ikfast translation3d solver in cpp?
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.