How to use kdl ik solver for a 3dof rpp robot

asked 2019-03-23 15:23:07 -0500

Nazar gravatar image

updated 2019-03-24 12:29:57 -0500

Hi, I am working on a RPP robot with and end effector with 1dof. The robot is designed to work in a mirco channel printing station. I run the cpp code given below to move my robot to two 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();

  sleep(2.0);
  moveit::planning_interface::MoveGroupInterface group("arm"); 
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;  
  ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
  moveit_msgs::DisplayTrajectory display_trajectory;

  ROS_INFO("Reference frame: %s", group.getPlanningFrame().c_str());
  ROS_INFO("Reference frame: %s", group.getEndEffectorLink().c_str());

  // PLANNING AND MOVING TO FIRST POINT
  group.setStartStateToCurrentState ();
  group.setPositionTarget(-0.147,-0.244,0.278,"finger");
  group.move();

  //PLANNING AND MOVING TO SECOND POINT
  group.setStartStateToCurrentState ();
  group.setPositionTarget(-0.282,0.009,0.278,"finger");
  group.move();

  ros::waitForShutdown();
  sleep(5.0);
  ros::shutdown();  
  return 0;
}

I am using kdl ik solver. But the result obtained (broken into 3 steps) is given below:-

STEP 1-The robot move to the first point from the zero-position** with proper obstacle avoidance.

Image: point1 correctly reached. https://imgur.com/SDvIvX5

STEP 2-Then the robot comes back to zero-position by crashing into the obstacles. (I don't want this robot to come to zero-position. Bcs the next point is very close to it.)

crashing image: https://imgur.com/3O76uWf

zero postion: https://imgur.com/gCQXnTU

STEP 3-Then robot moves to the second point prescribed very nicely with proper obstacle avoidance.

STEPS 1 and STEP 3 are only desired. I dont want step 2. What is that i should do? I feel i have no more stones left unturned. Kindly help.

How to avoid the robot going to home position in between every two planned points? (While going to the home position, collision is also not checked)

NB: **zero position is not set by me. It is the pose in which the CAD model was created.

I have also tried setApproximateJointValueTarget(). But results are the same as mentioned above.

I am using ros melodic and kinetic (issues exist on both) I am a beginner .strong text

edit retag flag offensive close merge delete

Comments

Could you please attach your images directly to your post? I've given you sufficient karma to be able to do that.

gvdhoorn gravatar image gvdhoorn  ( 2019-03-25 03:36:49 -0500 )edit