move_group_interface

[Rafael Correia Nascimento]

Hi,

I'm very new whit ROS and moveit, so I'm trying to control my Cyton Alpha with moveit.

I already done the following tutorials: http://moveit.ros.org/wiki/PR2/Setup_Assistant/Quick_Start and http://moveit.ros.org/wiki/PR2/Rviz_Plugin/Quick_Start.

They seen to work correctly since I can move the end effector around. My next step is to choose a (X Y Z) position to plan for. 

Here is the code that I made for it:

  double x = atof(argv[1]);

  double y = atof(argv[2]);

  double z = atof(argv[3]);

  ROS_INFO("Move to : x=%f, y=%f, z=%f",x,y,z);

  geometry_msgs::Pose pos;

  pos.position.x = x;

  pos.position.y = y;

  pos.position.z = z;

  pos.orientation.w = 1;

  group.setPoseReferenceFrame("base_link");

  group.setPoseTarget(pos);

//  group.setPositionTarget(x, y, z);

  group.setRPYTarget(1, 1, 1);

  // plan the motion and then move the group to the sampled target

  group.move();

My problem is that every time I run the program I have a diferent "output" like the endeffector of my robot is moving to other positions instead of the position that I want.

I working in a Ubuntu 12.04 LTS and ROS Hydro.

[Ioan Sucan]

Hello Rafael,

When you call setRPYTarget() only the orientation is take into account, so whatever you have set as position is ignored. You should set the orietation as a quaternion as part of your pose and only call setPoseTarget(). Is that working for you?

Ioan

[Rafael Correia Nascimento]

Hello Ioan,

I have already tried using setPoseTarget() but I can't reache any point with that function. When I use like this:

 double x = atof(argv[1]);

  double y = atof(argv[2]);

  double z = atof(argv[3]);

  ROS_INFO("Move to : x=%f, y=%f, z=%f",x,y,z);

  geometry_msgs::Pose pos;

  pos.position.x = x;

  pos.position.y = y;

  pos.position.z = z;

  pos.orientation.w = 1;

  group.setPoseReferenceFrame("base_link");

  group.setPoseTarget(pos);

  // plan the motion and then move the group to the sampled target

  group.move();

I get an error saying that it couldn't find a solution whatever (x y z) I use.

Is there something wrong with the kinematics? 

Thanks for your response.

[Ioan Sucan]

Do you have IK solvers loaded? Try to use the x y z that correspond to the current position, just to make sure the IK solver is loaded correctly. Do you see any other errors?

Ioan

[Rafael Correia Nascimento]

Let me try to to explain better my problem. So when I use like this:

  double x = atof(argv[1]);

  double y = atof(argv[2]);

  double z = atof(argv[3]);

  ROS_INFO("Move to : x=%f, y=%f, z=%f",x,y,z);

  geometry_msgs::Pose pos;

  pos.position.x = x;

  pos.position.y = y;

  pos.position.z = z;

  pos.orientation.w = 1;

  group.setPoseReferenceFrame("base_link");

  group.setPoseTarget(pos);

  group.move();

I get the following error for any point I set.

rosrun cyton_moveit_tutorials motion_planning_interfactutorial 0 1 0

[ INFO] [1381752868.167913477]: Ready to take MoveGroup commands for group cyton_arm_planning_group.

[ INFO] [1381752869.168459882]: Move to : x=0.000000, y=1.000000, z=0.000000

[ INFO] [1381752874.175520648]: ABORTED: No motion plan found. No execution attempted.

but, if I use like this:

  group.setPoseReferenceFrame("base_link");

  group.setRandomTarget();

  // plan the motion and then move the group to the sampled target

  group.move();

My simulated Cyton Arm can plan and move to random positions.

If I use like this:

  group.setPoseReferenceFrame("base_link");

  group.setRPYTarget(0, 1, 0);

  // plan the motion and then move the group to the sampled target

  group.move();

I have the same output as when I use setRandomTarget(); 

I have no idea what is wrong, because as you can see, with random position kinematics seems to work. I realy apreciated your patience and your help. I need to make it works.

Thanks again.

Rafael

2013/10/13 Ioan Sucan <isu...@gmail.com>

[Ioan Sucan]

Rafael,

Can you tell me what launch files are run in order to start everything? Before you run your code.

The setRandomTarget() sets joint positions, so no IK is needed for planning to work. FK is always available.

For setPoseTarget or setRPYTarget an IK solver needs to be loaded. I suspect you do not have expected params loaded on your param server.

Can you run this:
rosparam list | grep kinematics

Ioan

[Rafael Correia Nascimento]

Hi Ioan,

I'm running roslaunch cyton_arm_moveit_config demo.launch.

The output for rospapram list | grep kinematics :

rosparam list | grep kinematics

/move_group/cyton_arm_planning_group/kinematics_solver

/move_group/cyton_arm_planning_group/kinematics_solver_attempts

/move_group/cyton_arm_planning_group/kinematics_solver_search_resolution

/move_group/cyton_arm_planning_group/kinematics_solver_timeout

/move_group/cyton_gripper_planning_group/kinematics_solver

/move_group/cyton_gripper_planning_group/kinematics_solver_attempts

/move_group/cyton_gripper_planning_group/kinematics_solver_search_resolution

/move_group/cyton_gripper_planning_group/kinematics_solver_timeout

/rviz_rafael_ubuntu_8995_6758606607700323751/cyton_arm_planning_group/kinematics_solver

/rviz_rafael_ubuntu_8995_6758606607700323751/cyton_arm_planning_group/kinematics_solver_attempts

/rviz_rafael_ubuntu_8995_6758606607700323751/cyton_arm_planning_group/kinematics_solver_search_resolution

/rviz_rafael_ubuntu_8995_6758606607700323751/cyton_arm_planning_group/kinematics_solver_timeout

/rviz_rafael_ubuntu_8995_6758606607700323751/cyton_gripper_planning_group/kinematics_solver

/rviz_rafael_ubuntu_8995_6758606607700323751/cyton_gripper_planning_group/kinematics_solver_attempts

/rviz_rafael_ubuntu_8995_6758606607700323751/cyton_gripper_planning_group/kinematics_solver_search_resolution

/rviz_rafael_ubuntu_8995_6758606607700323751/cyton_gripper_planning_group/kinematics_solver_timeout

:D

2013/10/14 Ioan Sucan <isu...@gmail.com>

[Ioan Sucan]

Rafael,

I now suspect your group is not a chain. Which group are you calling setPoseTarget()?

Can you show me the URDF & SRDF? You should use this for the arm, not the gripper, and you should ideally use a chain to define the arm, rather than a group of joints.

Ioan

[Rafael Correia Nascimento]

Ioan,

I have two groups, cyton_arm_planning_group (I'm using this group) and the group chyton_gripper_planning_group.

  move_group_interface::MoveGroup group("cyton_arm_planning_group");

The URDF and SRDF are in attachment.

[Rafael Correia Nascimento]

Yes, I can see and move the arm with the interactive markers. 

2013/10/14 Ioan Sucan <isu...@gmail.com>

Not really to be honest. Do you see interactive markers in rviz when you run your code? Can you move the arm that way?

Ioan

On Mon, Oct 14, 2013 at 7:42 PM, Rafael Correia Nascimento <rafael....@gmail.com> wrote:

Hi Ioan,

Did you have some idea about what is happening with my simulated arm in moveit?

Thanks for your help.

2013/10/14 Rafael Correia Nascimento <rafael....@gmail.com>

Hi Ioan,

I tried it but it not works, I have the same output as before.

2013/10/14 Ioan Sucan <isu...@gmail.com>

Can you try to define the group cyton_arm_planning_group using
<chain base_link="base_link" tip_link="extension" />

And try again?

You can edit the .srdf manually.

[Rafael Correia Nascimento]

Please look at these print screens.

And I have a question, did you have moveit working ? If yes, could you please tell me how exactly you set a (X Y Z) goal for your robot?

Thank you very much :D

[Scott Kiesel]

It has been a few days since your last post. Have you had any luck with this? I think I'm running into a similar issue and am *very* interested in resolving it.

I wrote a small C++ program using the move_group_interface, setRandomTarget works as it does for you (attributed to being a joint configuration goal and only needing FK?), when I switch to something like setPoseTarget or setPositionTarget things stop working.

I think Ioan was suggesting something about the IK solver not being loaded? I do see the error message "unable to construct goal representation" when plan or move are called.

I use the demo.launch file created from the Setup Assistant to get "everything" up and running. I have the same sort of functionality inside of Rviz where I can drag the "goal arm" around and then ask the MoveIt! plugin to plan and excute, which works great. After that's going, I launch my individual node which is really simple at this point:

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

  ros::init(argc, argv, "move_group_interface_demo");

  ros::NodeHandle node;

  ros::AsyncSpinner spinner(1);

  spinner.start();

  move_group_interface::MoveGroup group("arm");

  ros::Rate rate(0.1);

  while (node.ok()) {

    group.setPoseTarget(group.getCurrentPose());

    //group.setRandomTarget(); //doing this instead would allow for planning to succeed

    moveit::planning_interface::MoveGroup::Plan plan;

    group.setStartStateToCurrentState();

    if(group.plan(plan)) {

      ROS_INFO("Planning Successful!");

      group.execute(plan);

    }

    else {

      ROS_WARN("Planned failed! Not moving");

    }

    rate.sleep();

  }

  

  ros::waitForShutdown();

  return 0;

}

I also see MoveIt! print out another error in the terminal running my node after the call to plan reporting "ABORT: Catastrophic Failure".

[Rafael Correia Nascimento]

Hi Scott,

unfortunately I'm stucked in this problem and as you I'm pretty interesting in solving it too. As Ioan said to me , the method setRandomTarget works because it needs only the FK.

The demo.launch brings up the IK solver, but if you want to make sure that it's loaded run you can type in another terminal "rosparam list | grep kinematics' and you will see.

Ioan had said to me that it's better to represent an arm with chains instead of joint or links, you can try it, you only need to modify in the setup assistent.

I tried nodes very similar to yours, but without any luck. 

I don't know about the "ABORT: Catastrophic Failure", but as I said before I'm very interesting in solving this problem too, if you like when can work together to solve this.

if you want I can share with you my nodes and let me know if you make some progress.

PS: Sorry about my bad english.

Rafael

2013/10/18 Scott Kiesel <kiesel...@gmail.com>

[Scott Kiesel]

Yea for sure. I've read through this thread and tried to get everything up to the point where Ioan has suggested things be. The IK solver seems to be running when I grep through the running ROS services. I've been looking through the MoveIt! source to see if I could figure out where/why these errors are being reported. I've found a couple spots that it might be, but I will definitely let you know if I come up with a solution.

-Scott

[Scott Kiesel]

Great news! I just pulled in the latest stuff from ROS and there were some MoveIt! updates it looks like that fixed the issue I was having. You should definitely update your packages, rebuild, and try again to see if things have also been fixed for you.

[Rafael Correia Nascimento]

Hi Scott,

this is really great news! I have downloaded the last version of ROS and Moveit! and tried to put things to work, but it seems that my problem still here.  Could you please tell me how are you using the move group interface?

And thanks for telling me about the update!

2013/10/23 Scott Kiesel <kiesel...@gmail.com>

[Dinesh Madusanke]

Dear Rafael,
I am having the same problem I have updated all the ROS and moveIt packages. Did you solve your problem ? Can you give me any suggestions ?
Thanks.

[Giorgio Toscana]

Hi All,

I'm having the same problem too!!! Someone was able to solve it??Thanks