Robotics StackExchange | Archived questions

MoveIt and 4 DOF arm

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 planner is constructed.
[ INFO] [1580223140.635057382]: RRTConnect: Starting planning with 1 states already in datastructure
[ERROR] [1580223145.639938298]: RRTConnect: Unable to sample any valid states for goal tree
[ INFO] [1580223145.640045796]: RRTConnect: Created 1 states (1 start + 0 goal)
[ INFO] [1580223145.640099070]: No solution found after 5.005407 seconds
[ INFO] [1580223145.648314375]: Unable to solve the planning problem

Important : My ARM only can moves in Y and Z axis

i was reading about KDL Kinematic plugin that only works with six or more DOF , and i'm tryng to create my own KDL plugin follow the next STEPS but i cant install openrave .

I dont know what's happen :/ hope somebody here can help me , plz i really need the help :C

Thanks for your time fellas :)

Asked by epsilon11101 on 2020-01-28 10:04:39 UTC

Comments

Answers

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_tutorials/doc/motion_planning_api/motion_planning_api_tutorial.html

Asked by AndyZe on 2020-01-28 11:43:10 UTC

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?

Asked by epsilon11101 on 2020-01-28 12:36:39 UTC

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.

Asked by AndyZe on 2020-01-28 12:57:35 UTC

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.

Asked by Humpelstilzchen on 2020-01-28 14:00:23 UTC

sorry for my question but whats a docker image?

Asked by epsilon11101 on 2020-01-28 14:04:02 UTC

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

Asked by Humpelstilzchen on 2020-01-28 14:07:36 UTC

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 .

Asked by epsilon11101 on 2020-01-28 14:12:13 UTC

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-_DLDpcY

Asked by AndyZe on 2020-01-28 16:37:38 UTC

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?

Asked by Humpelstilzchen on 2020-01-29 01:01:08 UTC

AndyZe: Is this "random" or "random valid" pose in the video? Also the creator of this video has more success then I when I was moving the goal state marker around with the default KDL IK solver.Which is why I switched to ikfast. (Of course ikfast does not make it possible to reach invalid positions)

Asked by Humpelstilzchen on 2020-01-29 01:05:48 UTC

@HUmpelstilzchen i was thinking about Translation3D i dont know much about this , im new in arm bots jeje , which recommended to use?

Asked by epsilon11101 on 2020-01-29 10:19:56 UTC

This depends on your configuration.

Asked by Humpelstilzchen on 2020-01-29 13:59:49 UTC

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/openrave-trusty/ kinetic) , i followed the next steps : http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/ikfast/ikfast_tutorial.html , 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/263925/generating-an-ikfast-solution-for-4-dof-arm/?answer=265625#post-id-265625 )

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

Asked by epsilon11101 on 2020-02-12 13:28:18 UTC

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/263925/generating-an-ikfast-solution-for-4-dof-arm/#265625

Asked by himty on 2020-12-07 02:12:20 UTC