Ask Your Question
0

MoveIt and 4 DOF arm

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

epsilon11101 gravatar image

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

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 close merge delete

2 Answers

Sort by ยป oldest newest most voted
0

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

epsilon11101 gravatar image

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

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
0

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

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 imageepsilon11101 ( 2020-01-28 11:36:39 -0600 )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 imageAndyZe ( 2020-01-28 11:57:35 -0600 )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 imageHumpelstilzchen ( 2020-01-28 13:00:23 -0600 )edit

sorry for my question but whats a docker image?

epsilon11101 gravatar imageepsilon11101 ( 2020-01-28 13:04:02 -0600 )edit

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

Humpelstilzchen gravatar imageHumpelstilzchen ( 2020-01-28 13:07:36 -0600 )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 imageepsilon11101 ( 2020-01-28 13:12:13 -0600 )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 imageAndyZe ( 2020-01-28 15:37:38 -0600 )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 imageHumpelstilzchen ( 2020-01-29 00:01:08 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

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

Seen: 51 times

Last updated: Feb 12