Hi, I am using MoveIT with ROS to try to move my robot to a pre defined (hardcoded) position. The problem that I have is that the "easiest" to reach targets cannot be reached, and "harder" targets can be reached. The start state of the robot looks like this:
http://imgur.com/a/1yTAF
I have drawn a red box where I want to be able to reach targets. Now, I am using move_group from MoveIT (C++) like this:
int main(int argc, char **argv)
{
ros::init(argc, argv, "cpp_arm");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
static const string PLANNING_GROUP = "arm";
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
const robot_state::JointModelGroup *joint_model_group = move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
ROS_INFO_NAMED("arm", "Reference frame: %s", move_group.getPlanningFrame().c_str());
ROS_INFO_NAMED("gripper", "End effector link: %s", move_group.getEndEffectorLink().c_str());
geometry_msgs::Pose target_pose1;
target_pose1.orientation.w = 1.0;
target_pose1.position.x = -0.2;
target_pose1.position.y = 0.1;
target_pose1.position.z = 0.60;
move_group.setPoseTarget(target_pose1);
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success = move_group.plan(my_plan);
ROS_INFO_NAMED("arm", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
move_group.move();
}
This results in the robot to move to the specific location:
http://imgur.com/a/NiBKk
When you look at this video, you can see the robot twists around itself and move in a strange way to its location.
(ignore the stuttering, its an camera issue):
https://youtu.be/UfdCbqkBx-4
Now, because of this strange movement ( I think ) the robot cannot reach simple positions in front or behind it like this:
http://imgur.com/a/N2ExD
And this:
http://imgur.com/a/8BOcG
I have used the same positions to move to as where the cubes are located at but it cannot reach them. I am getting the following erros:
Fail: ABORTED: No motion plan found. No execution attempted
And
RRTConnect: Unable to sample any valid states for goal tree
This is my URDF file:
https://pastebin.com/iuE3DG1L
Why is this happening and how can I reach goal states in front of my robot?
Thanks in advance
↧