moveit c++ interface- unexpected robot movement
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
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
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
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?
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
It's possible there is a rotation hidden in the URDF, can you add that or a link to where it's stored.
@PeteBlackerThe3rd: I have uploaded files to git and posted the link under Update 2. Can you please check it once ??
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: 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??
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: 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.