RTTConnect Planning Problem
Moises Estrada
I need a hint on how to solve a planning problem. I developed a 5-DOF inverse kinematic solver but when I try to
use the moveit_commander to move left (or right), it will abort:
arm_gp> go left 0.1
[ INFO] [1404434366.877511319]: ABORTED: No motion plan found. No execution attempted.
Failed while moving left
This is the output:
[ INFO] [1404434366.870057856]: Inverse Kinematic found a solution.
[ INFO] [1404434366.870243634]: Inverse Kinematic found a solution.
...
[ INFO] [1404434366.870615957]: Inverse Kinematic found a solution.
[ERROR] [1404434366.875629653]: RRTConnect: Unable to sample any valid states for goal tree
[ INFO] [1404434366.875810193]: RRTConnect: Created 1 states (1 start + 0 goal)
[ INFO] [1404434366.875873469]: No solution found after 0.203555 seconds
[ INFO] [1404434366.875969082]: Unable to solve the planning problem
I have no idea what is the problem, I don't think is the IK solver because it finds the solutions...
Any help will be appreciated,
Thank you.
Adolfo Rodríguez Tsouroukdissian
Any help will be appreciated,
Thank you.
--
Adolfo Rodríguez Tsouroukdissian
Senior robotics engineer
adolfo.r...@pal-robotics.com
http://www.pal-robotics.com
PAL ROBOTICS S.L
c/ Pujades 77-79, 4º4ª
08005 Barcelona, Spain.
Tel. +34.93.414.53.47
Fax.+34.93.209.11.09
Skype: adolfo.pal-robotics
Facebook - Twitter - PAL Robotics YouTube Channel
AVISO DE CONFIDENCIALIDAD: Este mensaje y sus documentos adjuntos, pueden contener información privilegiada y/o confidencial que está dirigida exclusivamente a su destinatario. Si usted recibe este mensaje y no es el destinatario indicado, o el empleado encargado de su entrega a dicha persona, por favor, notifíquelo inmediatamente y remita el mensaje original a la dirección de correo electrónico indicada. Cualquier copia, uso o distribución no autorizados de esta comunicación queda estrictamente prohibida.
CONFIDENTIALITY NOTICE: This e-mail and the accompanying document(s) may contain confidential information which is privileged and intended only for the individual or entity to whom they are addressed. If you are not the intended recipient, you are hereby notified that any disclosure, copying, distribution or use of this e-mail and/or accompanying document(s) is strictly prohibited. If you have received this e-mail in error, please immediately notify the sender at the above e-mail address.
Moises Estrada
Thank you.
Wojtynek, Michael
Could you give us some code snippet with the collision checking? I´m stucking at the same problem right now.
Thank you in advance.
-----Ursprüngliche Nachricht-----
Von: moveit...@googlegroups.com im Auftrag von Moises Estrada
Gesendet: Fr 04.07.2014 18:54
An: moveit...@googlegroups.com
Betreff: Re: RTTConnect Planning Problem
Ah of course, I didn't check for collisions.
Thank you.
On Thursday, July 3, 2014 5:48:53 PM UTC-7, Moises Estrada wrote:
>
> Hey all,
>
> I need a hint on how to solve a planning problem. I developed a 5-DOF
> inverse kinematic solver but when I try to
> use the moveit_commander to move left (or right), it will abort:
>
> arm_gp> go left 0.1
> [ INFO] [1404434366.877511319]: ABORTED: No motion plan found. No
> execution attempted.
> Failed while moving left
>
> This is the output:
>
>
>
>
>
>
>
> *[ INFO] [1404434366.870057856]: Inverse Kinematic found a solution.[
> INFO] [1404434366.870243634]: Inverse Kinematic found a solution....[ INFO]
> [1404434366.870615957]: Inverse Kinematic found a solution.[ERROR]
> [1404434366.875629653]: RRTConnect: Unable to sample any valid states for
> goal tree[ INFO] [1404434366.875810193]: RRTConnect: Created 1 states (1
> start + 0 goal)[ INFO] [1404434366.875873469]: No solution found after
> 0.203555 seconds[ INFO] [1404434366.875969082]: Unable to solve the
> planning problem*
Moises Estrada
All I am using to produce this error is my own kinematics plugin,
the demo.launch file from the moveit setup assistant and the
moveit_commander_cmdline.py. I did however tried a C++ code
to see if I got the same results, which I did. Here is that code:
std::vector<geometry_msgs::Pose> waypoints;
geometry_msgs::Pose start_pose = group.getCurrentPose().pose;
moveit::planning_interface::MoveGroup::Plan plan;
geometry_msgs::Pose target_pose2 = start_pose;
target_pose2.position.y -= 0.2;
waypoints.push_back(target_pose2); // left
moveit_msgs::RobotTrajectory trajectory;
double fraction = group.computeCartesianPath(waypoints,
0.01, // eef_step
0.0, // jump_threshold
trajectory);
ROS_INFO("Visualizing plan 4 (cartesian path) (%.2f%% acheived)",
fraction * 100.0);
/* Sleep to give Rviz time to visualize the plan. */
sleep(5.0);
group.execute(plan);
I hope I answered your question.
Wojtynek, Michael
Hey,
thanks for your code. According to your posting I've added the collision checking and I've followed the IKfast tutorials related to the pr2 project (https://github.com/ros-planning/moveit_pr2/blob/hydro-devel/pr2_moveit_tutorials/kinematics/src/ros_api_tutorial.cpp).
I got an error message (Result: False -31) which means that there was no Ikfast solution found. Another error message, I suppose from moveit, is "Different number of names and positions in JointState message: 8, 6". There seems to be some mesh up with the Joints.
To answer your question, yes I'm using the *.srdf at least at this command line: robot_model_loader::RobotModelLoader robot_model_loader("robot_description").
Below is the Ikfast code, maybe you can find my mistakes:
########################################
#include <ros/ros.h>
// MoveIt!
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>
// Robot state publishing
#include <moveit/robot_state/conversions.h>
#include <moveit_msgs/DisplayRobotState.h>
// Kinematics
#include <moveit_msgs/GetPositionIK.h>
int main(int argc, char **argv)
{
ros::init(argc, argv, "movetest", ros::init_options::AnonymousName);
// start a ROS spinning thread
ros::AsyncSpinner spinner(1);
spinner.start();
// Start a service client
ros::NodeHandle node_handle;
ros::ServiceClient service_client = node_handle.serviceClient<moveit_msgs::GetPositionIK> ("compute_ik");
while(!service_client.exists())
{ ROS_INFO("Waiting for service");
sleep(1.0);
}
moveit_msgs::GetPositionIK::Request service_request;
moveit_msgs::GetPositionIK::Response service_response;
service_request.ik_request.group_name = "ur5_manipulator";
service_request.ik_request.pose_stamped.header.frame_id = "arm_base_link";
service_request.ik_request.pose_stamped.pose.position.x = 0.75;
service_request.ik_request.pose_stamped.pose.position.y = 0.188;
service_request.ik_request.pose_stamped.pose.position.z = 0.0;
service_request.ik_request.pose_stamped.pose.orientation.x = 0.0;
service_request.ik_request.pose_stamped.pose.orientation.y = 0.0;
service_request.ik_request.pose_stamped.pose.orientation.z = 0.0;
service_request.ik_request.pose_stamped.pose.orientation.w = 1.0;
service_client.call(service_request, service_response);
/* Load the robot model */
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
/* Get a shared pointer to the model */
robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();
ROS_INFO("Test");
/* WORKING WITH THE KINEMATIC STATE */
ROS_INFO("Model frame: %s", kinematic_model->getModelFrame().c_str());
robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));
/* Get the configuration for the joints robot arm*/
const robot_state::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("ur5_manipulator");
/* Get the names of the joints in the right_arm*/
service_request.ik_request.robot_state.joint_state.name = joint_model_group->getJointModelNames();
/* Get the joint values and put them into the message, this is where you could put in your own set of values as well.*/
//kinematic_state->setToRandomPositions(joint_model_group);
kinematic_state->copyJointGroupPositions(joint_model_group, service_request.ik_request.robot_state.joint_state.position);
/* Call the service again*/
service_client.call(service_request, service_response);
ROS_INFO_STREAM("Result: " << ((service_response.error_code.val == service_response.error_code.SUCCESS) ? "True " : "False ") << service_response.error_code.val);
/* Check for collisions*/
service_request.ik_request.avoid_collisions = true;
/* Call the service again*/
service_client.call(service_request, service_response);
ROS_INFO_STREAM("Result: " << ((service_response.error_code.val == service_response.error_code.SUCCESS) ? "True " : "False ") << service_response.error_code.val);
/* Visualize the result*/
moveit_msgs::DisplayRobotState msg;
kinematic_state->setVariableValues(service_response.solution.joint_state);
robot_state::robotStateToRobotStateMsg(*kinematic_state, msg.state);
//robot_state_publisher.publish(msg);
//Sleep to let the message go through
ros::Duration(2.0).sleep();
ros::shutdown();
return 0;
}
########################################
-----Original Message-----
From: moveit...@googlegroups.com on behalf of Moises Estrada
Sent: Mon 7/7/2014 7:19 PM
To: moveit...@googlegroups.com
Cc: moi...@gmail.com; Wojtynek, Michael
Subject: Re: RTTConnect Planning Problem
Are you referring to the .srdf file? I did not write any collision code.
All I am using to produce this error is my own kinematics plugin,
the demo.launch file from the moveit setup assistant and the
moveit_commander_cmdline.py. I did however tried a C++ code
to see if I got the same results, which I did. Here is that code:
*std::vector<geometry_msgs::Pose> waypoints; geometry_msgs::Pose
start_pose = group.getCurrentPose().pose;
moveit::planning_interface::MoveGroup::Plan plan; geometry_msgs::Pose
target_pose2 = start_pose; target_pose2.position.y -= 0.2;
waypoints.push_back(target_pose2); // left moveit_msgs::RobotTrajectory
trajectory; double fraction =
group.computeCartesianPath(waypoints,
0.01, // eef_step 0.0, //
jump_threshold trajectory);
ROS_INFO("Visualizing plan 4 (cartesian path) (%.2f%% acheived)",
fraction * 100.0); /* Sleep to give Rviz time to visualize the plan.
*/ sleep(5.0); group.execute(plan); *
I hope I answered your question.
On Sunday, July 6, 2014 6:47:06 AM UTC-7, Wojtynek, Michael wrote:
>
>
> Could you give us some code snippet with the collision checking? I´m
> stucking at the same problem right now.
>
> Thank you in advance.
>
> -----Ursprüngliche Nachricht-----
> Von: moveit...@googlegroups.com <javascript:> im Auftrag von Moises
> Estrada
> Gesendet: Fr 04.07.2014 18:54
> An: moveit...@googlegroups.com <javascript:>
Moises Estrada
[ INFO] [1404951412.378119027]: Loading robot model 'pentaxis'...
[ INFO] [1404951412.591004413]: IK SOLVER STARTED
[ INFO] [1404951412.592370789]: Test
[ INFO] [1404951412.592523602]: Model frame: /base_footprint
[ INFO] [1404951412.602988478]: Result: False -31
[ INFO] [1404951414.701056533]: Result: False -31
The problem is that the kinematics solver is unable to find the solution for the given position+orientation.
In the end I was able to solve the original problem, to do so I added more possible
solutions for the IK solver and increase the solver timeout from 0.5 to 5.
So finally everything is working great!
On Thursday, July 3, 2014 5:48:53 PM UTC-7, Moises Estrada wrote: