ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

moveit c++ interface- unexpected robot movement

asked 2019-02-21 11:40:56 -0500

mvish7 gravatar image

updated 2019-02-22 08:26:22 -0500

Hello,

I’m trying to implement pick and place with moveit and gazebo. i have gazebo simulated robot and i’m sending cartesian pose through moveit’s c++ interface.

at initial stage i want my robot to approach the block, i have fetched the pose of the robot from gazebo by calling the service get_model_state, and i have fed it to the moveit c++ interface program.

i expect robot to move to the object but robot moves in exact opposite direction. can anyone explain this behaviour and what i’m doing wrong?

Below you will find: Image of robot and object that i want to approach

image description

Pose retrieved from gazebo

position_x=0.871467

position_y=-0.076430

position_z=0.295392

orientation_x=0.002768

orientation_y=-0.003604

orientation_z=0.003009

orientation_w=0.999985

below is the code i’m using

int main(int argc, char** argv)
{
  ros::init(argc, argv, "moveit_interface");
  ros::NodeHandle node_handle;
  ros::AsyncSpinner spinner(1);
  spinner.start();

  static const std::string PLANNING_GROUP = "panda_arm";

  moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);

   //setting up movegroup class
   moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

   const robot_state::JointModelGroup* joint_model_group =
   move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

   move_group.setPlanningTime(15);



  // Planning to a Pose goal
  // We can plan a motion for this group to a desired pose for the
  // end-effector.
  geometry_msgs::Pose target_pose1;
  target_pose1.orientation.x = 0.002768;
  target_pose1.orientation.y = -0.003604;
  target_pose1.orientation.z = 0.003009 ;
  target_pose1.orientation.w = 0.999985;
  target_pose1.position.x = 0.871467;
  target_pose1.position.y = -0.076430;
  target_pose1.position.z = 0.295392;
  move_group.setApproximateJointValueTarget(target_pose1, "panda_link7");
   //move_group.setPoseTarget(target_pose1);


   //call the planer to plan the myplan
   moveit::planning_interface::MoveGroupInterface::Plan my_plan;

   //moving to pose goal
   move_group.move();

   ros::shutdown();
 return 0;
}

Update 1

Reference frames of object and robot image description image description

Update2

URDF File for robot can be found here.

Main file is panda_arm_hand.urdf.xacro, File related to arm is panda_arm.xacro, File related to gripper is hand.xacro

Update3

robot position after minimizing x distance image description

edit retag flag offensive close merge delete

Comments

My first guess is that the reference frames of the object position and the pose goal are different. Is there a 90 degree rotation between the gazebo world frame and the robots base link by any chance?

PeteBlackerThe3rd gravatar image PeteBlackerThe3rd  ( 2019-02-21 12:04:15 -0500 )edit

Hi, Please find updated question. i have posted two snapshots showing reference frames of robot and block.I think that there is no rotation between two coordinate frames

mvish7 gravatar image mvish7  ( 2019-02-21 12:18:26 -0500 )edit

It's possible there is a rotation hidden in the URDF, can you add that or a link to where it's stored.

PeteBlackerThe3rd gravatar image PeteBlackerThe3rd  ( 2019-02-22 05:17:17 -0500 )edit

@PeteBlackerThe3rd: I have uploaded files to git and posted the link under Update 2. Can you please check it once ??

mvish7 gravatar image mvish7  ( 2019-02-22 06:11:53 -0500 )edit

If you use setPoseTarget (the line you have commented out) does it say something like "no motion plan found"? Seems out of the joint space, try lowering the x position value (e.g. 0.7), and maybe straightening the orientation (w=0, x=0.9239, y=-0.3827, z=0)

aPonza gravatar image aPonza  ( 2019-02-22 06:29:32 -0500 )edit

@aPonza: when i use setPoseTarget moveit errors out saying"Aborted: no plan found ". Thats why i'm using setApproximateJointTarget.I have fetched this pose from gazebo and i know it falls robot workspace. Still changing the pose values can make me reach my block??

mvish7 gravatar image mvish7  ( 2019-02-22 06:39:49 -0500 )edit
1

The joint space of a Franka Emika Panda is at most 855mm in the front, so I can confirm that is the issue. You can find an image at pag. 42 of the manual or google image searching "Franka Emika Panda joint space". If it's a block you can probably grasp it parallel to the ground, from the front?

aPonza gravatar image aPonza  ( 2019-02-22 08:19:13 -0500 )edit

@aPonza: You are absolutely right. I changed the position of the block to reduce it's x coordinate and retrived it's position from gazebo. But now my robot moves near to the block. I'm giving the position of the block to my moveit code so i expect robot arm to hit the block.

mvish7 gravatar image mvish7  ( 2019-02-22 08:24:11 -0500 )edit

1 Answer

Sort by » oldest newest most voted
0

answered 2019-02-25 02:37:17 -0500

mvish7 gravatar image

Thanks @aPonza for comments. Your comments guided me to solve the problem. The right answer is as follows:

  1. My pose was outside the reach of robot arm i.e x coordinate was not in robot workspace.

  2. i was giving orientation of object to my robot hence robot end effector was achieving the strange pose. Below is the orientation to approach the object from x-direction.

    orientation.setRPY(-M_PI / 2, -M_PI / 4, -M_PI / 2)

which can be assigned to robot as follows:

  target_pose1.orientation = tf2::toMsg(orientation)
edit flag offensive delete link more

Question Tools

3 followers

Stats

Asked: 2019-02-21 11:40:56 -0500

Seen: 703 times

Last updated: Feb 25 '19