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