Ask Your Question
0

MoveIt and 4 DOF arm [closed]

asked 2020-01-28 09:04:39 -0500

epsilon11101 gravatar image

updated 2020-01-28 10:44:40 -0500

AndyZe gravatar image

UBUNTU : 18.04 ROS VERSION MELODIC

Hi! , im works with and 4 DOF arm , and i have a lot of problems when i was trying to set it at desired position ,

My problems occurss when i try to use the below function :

setPoseTarget (const geometry_msgs::Pose &target, const std::string &end_effector_link="")

setPositionTarget (double x, double y, double z, const std::string &end_effector_link="")

setRPYTarget (double roll, double pitch, double yaw, const std::string &end_effector_link="")

The only function that movesmy arm is :

setApproximateJointValueTarget (const geometry_msgs::Pose &eef_pose, const std::string &end_effector_link="")

i have tried a lot of things to do, i cant found the error, MOVE-IT works fine with its interface, but it give me errors when im traying to move the robot with the C++ API.

I dont know if its about my robot configuration o something like that ,

This is my URDF file : 4 DOF ARM URDF

and the SRDF file : 4 DOF ARM SRDF

and the code :

#include <ros/ros.h>
//MOVE IT

#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

ros::Publisher display_publisher;

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

  ros::init(argc, argv, "custom_planning");
  ros::NodeHandle nh;
  ros::AsyncSpinner spinner(1);
  spinner.start();
  sleep(2.0);



//planning group that we would like to control
  moveit::planning_interface::MoveGroupInterface group("arm");
  moveit::planning_interface::MoveGroupInterface group_g("gripper");
  //we can add or remove collision objects in our virtual world scene
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
  //raw pointers are used to refer to the planning group for improved performance
  const robot_state::JointModelGroup *joint_model_group = group.getCurrentState()->getJointModelGroup("arm");
  group.setEndEffectorLink("link4");
  group.setStartStateToCurrentState();



geometry_msgs::Pose target_pose1;

  // target_pose1.orientation.x =-0.0698152559439;
  // target_pose1.orientation.y =2.08265213726e-06;



 // target_pose1.orientation.z =-1.45756547007e-07;
  // target_pose1.orientation.w = 0.997559938065;
  // target_pose1.position.y = -0.139249505223;
  // target_pose1.position.z =  1.00958973838;

  group.setGoalTolerance(0.0001);
  group.setPositionTarget(0, 1.37, 0.40, "link4");
  //group.setApproximateJointValueTarget(target_pose1,"link4"); only works with this function


  moveit::planning_interface::MoveGroupInterface::Plan my_plan;

  moveit::planning_interface::MoveItErrorCode success = group.plan(my_plan);
  ROS_INFO("Visualizing plan 1 (pose goal) %s", success.val ? "" : "FAILED");

  if (!success)
    throw std::runtime_error("No plan found");
  group.move(); //blocking
  sleep(5.0);

  ros::shutdown();

  return 0;
}

And finally the error :

//NODE ERROR

[ WARN] [1580223145.648796219]: Fail: ABORTED: No motion plan found. No execution attempted.
[ INFO] [1580223145.648850922]: Visualizing plan 1 (pose goal) 
terminate called after throwing an instance of 'std::runtime_error'
  what():  No plan found
Aborted (core dumped)

//MOVE IT ERROR

[ INFO] [1580223110.865485565]: Constructing new MoveGroup connection for group 'arm' in namespace ''
[ INFO] [1580223111.945787906]: Ready to take commands for planning group arm.
[ INFO] [1580223111.945929737]: Looking around: no
[ INFO] [1580223111.946068449]: Replanning: no
[ INFO] [1580223140.518161060]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1580223140.625874939]: Planner configuration 'arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the ...
(more)
edit retag flag offensive reopen merge delete

Closed for the following reason the question is answered, right answer was accepted by epsilon11101
close date 2020-02-20 08:39:12.775891

2 Answers

Sort by ยป oldest newest most voted
1

answered 2020-02-12 12:28:18 -0500

epsilon11101 gravatar image

updated 2020-02-12 12:32:50 -0500

I finally found a solution for this problems , well i created the plugin with kinetic , in melodic was impossible for me (install openRave https://fsuarez6.github.io/blog/openr... kinetic) , i followed the next steps : http://docs.ros.org/kinetic/api/movei... , i created a fake JOINT that doesnt afect to my arm on EEF position , i used TranslationZAxisAngle4D .

To generate the IKFast Plugin i created a file at the same path that my URDF file :

<robot file="$NAME_OF_YOUR_COLLADA_FILE">
  <Manipulator name="NAME_OF_THE_ROBOT_IN_URDF">
    <base>base_link</base>
    <effector>tool0</effector>
  </Manipulator>
</robot>

in my case was like this :

<robot file="brazo.dae">
    <Manipulator name="brazo.urdf">
        <base>base_link</base>
        <effector>end_effector_link</effector>
    </Manipulator>
</robot>

and i saved it like robot.xml (credits https://answers.ros.org/question/2639... )

then i used the below command :

openrave0.9.py --database inversekinematics --robot=/ikfast/robot.xml --iktype=translationzaxisangle4d --iktests=1000

i didnt use this

python `openrave-config --python-dir`/openravepy/_openravepy_/ikfast.py --robot="$MYROBOT_NAME".dae --iktype=transform6d --baselink="$BASE_LINK" --eelink="$EEF_LINK" --savefile="$IKFAST_OUTPUT_PATH"

then i created the plugin like page said.

The final step is modify some code line in the plugin packages :

in my case the plugin package its name : brazo_ikfast_arm_plugin and we need to modify the line 546 (in my case ) or found the switch/case : IKP_TranslationXAxisAngle4D

and changed this code :

case IKP_TranslationXAxisAngle4D:
case IKP_TranslationYAxisAngle4D:
case IKP_TranslationZAxisAngle4D:
  ROS_ERROR_NAMED(name_, "IK for this IkParameterizationType not implemented yet.");
  return 0;

for this :

case IKP_TranslationXAxisAngle4D:
      double roll, pitch, yaw;
      pose_frame.M.GetRPY(roll,pitch,yaw);
      ComputeIk(trans, &roll, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
      return IKP_TranslationXAxisAngle4D;

case IKP_TranslationYAxisAngle4D:
       pose_frame.M.GetRPY(roll,pitch,yaw);
      ComputeIk(trans, &pitch, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
      return IKP_TranslationYAxisAngle4D;

case IKP_TranslationZAxisAngle4D:
      pose_frame.M.GetRPY(roll,pitch,yaw);
      ComputeIk(trans, &yaw, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
      return IKP_TranslationZAxisAngle4D;

and that's it , hope that be helpful for someone with the same problem

edit flag offensive delete link more

Comments

You nailed it on the head. Here's a similar answer to convince everyone else (and me in an alternate timeline) to do the first half of this: https://answers.ros.org/question/2639...

himty gravatar image himty  ( 2020-12-07 01:12:20 -0500 )edit
0

answered 2020-01-28 10:43:10 -0500

AndyZe gravatar image

This probably isn't what you want to hear, but I don't think MoveIt will work with a 4-dof arm like this. The reason is, you're specifying a 6-dof target pose but your robot only has 4 joints. In general, you can't regulate 6 dimensions with only 4 joints. That is why MoveIt will almost always fail.

Possibly you could get this working with Constraints (e.g. allowing freedom to rotate in 2 dimensions). I'm not sure. That's mentioned in the tutorial here. https://ros-planning.github.io/moveit...

edit flag offensive delete link more

Comments

thanks for your fast response , could you recommend me something to work with movit? , maybe create my own KDL solver o something like that?

epsilon11101 gravatar image epsilon11101  ( 2020-01-28 11:36:39 -0500 )edit

Possibly you could write a KDL solver (I don't know about that) but you still need to communicate to MoveIt that it can't check all 6 dimensions. Or you might try increasing MoveIt's default tolerances.

Or try planning with constraints, like I mentioned above.

AndyZe gravatar image AndyZe  ( 2020-01-28 11:57:35 -0500 )edit

Moveit works with the 4 dof Open Manipulator

However I recommend to create an IKFast kinematics solver. I also recommend to use a docker image for openrave since installing it is really painful.

Humpelstilzchen gravatar image Humpelstilzchen  ( 2020-01-28 13:00:23 -0500 )edit

sorry for my question but whats a docker image?

epsilon11101 gravatar image epsilon11101  ( 2020-01-28 13:04:02 -0500 )edit

It is basically a kind of light virtual machine. Instructions can be found here

Humpelstilzchen gravatar image Humpelstilzchen  ( 2020-01-28 13:07:36 -0500 )edit

thanks a lot =) , i tried this tutorial , but i had problem in this part

FROM personalrobotics/ros-openrave
RUN apt-get update || true && apt-get install -y --no-install-recommends build-essential python-pip liblapack-dev && apt-get clean && rm -rf /var/lib/apt/lists/*
RUN pip install sympy==0.7.1
EOF

# Replace `$DOCKER_IMAGE` with some descriptive name here
$ docker build -t $DOCKER_IMAGE .
epsilon11101 gravatar image epsilon11101  ( 2020-01-28 13:12:13 -0500 )edit

If you watch this video of the 4-DOF Open Manipulator, you can see that it solves for some poses, but sending it to any random pose from code has a small chance of working. :(

https://www.youtube.com/watch?v=KuE-_...

AndyZe gravatar image AndyZe  ( 2020-01-28 15:37:38 -0500 )edit

epsilon11101: You have not provided an error message and this probably should be a new question since this is not suitable for longer discussions here. Also did you check whether ik-type your manipulator is one of the supported types?

Humpelstilzchen gravatar image Humpelstilzchen  ( 2020-01-29 00:01:08 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2020-01-28 09:04:39 -0500

Seen: 574 times

Last updated: Feb 12 '20