Set a start state for the robot
4,078 views
Skip to first unread message
Joris Beuls
Apr 23, 2015, 5:01:35 AM4/23/15
to moveit...@googlegroups.com
Hello,
I was wondering how i can set the start state for a motion plan to the current state of the robot. Rviz does not seem to update it's start state.
The things i tried.:
// The normal function: http://docs.ros.org/hydro/api/moveit_ros_planning_interface/html/classmoveit_1_1planning__interface_1_1MoveGroup.html
// Doesn't work---------------------------------------------------------------------------------------
// part of the moveit_tutorial: http://docs.ros.org/hydro/api/pr2_moveit_tutorials/html/planning/src/doc/move_group_interface_tutorial.html
// Doesn't work----------------------------------------------------------------------------------------
// Works with group.plan(my_plan) and group.execute(my_plan) but not with group.move()
Anyone has another idea? Preferably that the current position is taken as start state and not one that i have to enter myself.
Kind regards
Joris
I was wondering how i can set the start state for a motion plan to the current state of the robot. Rviz does not seem to update it's start state.
The things i tried.:
// The normal function: http://docs.ros.org/hydro/api/moveit_ros_planning_interface/html/classmoveit_1_1planning__interface_1_1MoveGroup.html
// Doesn't work
group.setStartStateToCurrentState();
// part of the moveit_tutorial: http://docs.ros.org/hydro/api/pr2_moveit_tutorials/html/planning/src/doc/move_group_interface_tutorial.html
// Doesn't work
moveit::planning_interface::MoveGroup group("epson_arm");
geometry_msgs::Pose target_pose1;
target_pose1.orientation.w = 1.0;
target_pose1.position.x = 0.40;
target_pose1.position.y = 0;
target_pose1.position.z = 0.3;
robot_state::RobotState start_state(*group.getCurrentState());
const robot_state::JointModelGroup *joint_model_group =
start_state.getJointModelGroup(group.getName());
start_state.setFromIK(joint_model_group, target_pose1);
group.setStartState(start_state);
// Works with group.plan(my_plan) and group.execute(my_plan) but not with group.move()
moveit::planning_interface::MoveGroup group("epson_arm");
std::vector<double> group_variable_values1;
group.getCurrentState()->copyJointGroupPositions(group.getCurrentState()->getRobotModel()->getJointModelGroup(group.getName()), group_variable_values1);
group_variable_values1[0] = 0;
group_variable_values1[1] = -0.42;
group_variable_values1[2] = -1.36;
group_variable_values1[3] = 3.16;
group_variable_values1[4] = 1.32;
group_variable_values1[5] = 1.63;
robot_state::RobotState start_state(*group.getCurrentState());
const robot_state::JointModelGroup *joint_model_group =
start_state.getJointModelGroup(group.getName());
start_state.setJointGroupPositions(joint_model_group, group_variable_values1);
group.setStartState(start_state);
Anyone has another idea? Preferably that the current position is taken as start state and not one that i have to enter myself.
Kind regards
Joris
Yoan Mollard
Apr 23, 2015, 11:35:41 AM4/23/15
to Joris Beuls, moveit...@googlegroups.com
Hi Joris,
Basically if you don't provide any start state, Moveit will use the current state.
Can you explain more what you mean by "RViz does not update its start state"? Is it just a visualization issue?
The "query start state" option in Moveit plugin for RViz concerns only the *query* made through the GUI (quary made by clicking and moving/rotating the blue ball of the end effector), so it's not updated while you're not querying Moveit through the GUI. It does not draw the start state of the query made through the code, although you can guess it during the animation of the planned path.
Cheers,
Yoan
INRIA Research center, Flowers lab
Basically if you don't provide any start state, Moveit will use the current state.
Can you explain more what you mean by "RViz does not update its start state"? Is it just a visualization issue?
The "query start state" option in Moveit plugin for RViz concerns only the *query* made through the GUI (quary made by clicking and moving/rotating the blue ball of the end effector), so it's not updated while you're not querying Moveit through the GUI. It does not draw the start state of the query made through the code, although you can guess it during the animation of the planned path.
Cheers,
Yoan
Yoan Mollard
Research engineer – 3rd hand project
INRIA Research center, Flowers lab
Joris Beuls
Apr 27, 2015, 10:19:58 AM4/27/15
to moveit...@googlegroups.com, joris...@student.uhasselt.be, yoan.m...@inria.fr
Hello, thank you for the reply. When i dont set a new start state, but do set a second goal state after the first is executed, the simulation in rviz shows the second planned path starting from the first start state. Beneath is the code i used to do this. Is there a way to check if it is just a visualisation issue? The joint state publisher gui does not go back to the start values.
geometry_msgs::Pose target_pose1;
target_pose1.orientation.w = 1.0;
target_pose1.position.x = 0.3;
target_pose1.position.y = 0.3;
target_pose1.position.z = 0.3;
group.setGoalTolerance(0.01);
group.setPoseTarget(target_pose1);
success = group.move();
ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED");
sleep(5.0);
geometry_msgs::Pose target_pose2;
target_pose2.orientation.w = 1.0;
target_pose2.position.x = 0;
target_pose2.position.y = 0.45;
target_pose2.position.z = 0.3;
group.setGoalTolerance(0.01);
group.setPoseTarget(target_pose2);
success = group.move();
ROS_INFO("Visualizing plan 2 (pose goal) %s",success?"":"FAILED");
sleep(5.0);
As for the second part about the query. The planned path in rviz does not start in the last state. As seen in this video: https://drive.google.com/open?id=0Bzkj_-Zf37FObDRzSk9GUEZkcE0&authuser=0
Op donderdag 23 april 2015 17:35:41 UTC+2 schreef Yoan Mollard:
Yoan Mollard
Apr 27, 2015, 11:46:54 AM4/27/15
to Joris Beuls, moveit...@googlegroups.com
Hi Joris,
Your video on Google Drive is private.
It looks like you need to go back to an idle position between each goal. It seems that several targets in MoveIt are considered as way points, so if you need to reach several goals by returning to an idle (=starting) position between them, then you should set your idle position as an intermediary goal.
geometry_msgs::Pose idle_pose;
// Fill idle_pose here with current or whatever pose
group.setPoseTarget(idle_pose); // Go to idle pose
group.setPoseTarget(target_pose1); // Go to goal 1
group.setPoseTarget(idle_pose); // Go to idle pose
group.setPoseTarget(target_pose2); // Got to goal 2
// Add new goals here
success = group.move(); // Plan and move according to the way points defined hereabove
Consider that your robot cannot jumpy from point A to point B without indication about how to go there (could be motion planning, interpolation in joint space, interpolation in task space, ...). If you do jump between A and B and send this direcly to the controllers you will most likely damage your motors that will perform this motion at full speed with maximum energy. Thus even the return trajectory going back in idle position must be planned through a new MoveIt goal.
Your video on Google Drive is private.
It looks like you need to go back to an idle position between each goal. It seems that several targets in MoveIt are considered as way points, so if you need to reach several goals by returning to an idle (=starting) position between them, then you should set your idle position as an intermediary goal.
geometry_msgs::Pose idle_pose;
// Fill idle_pose here with current or whatever pose
group.setPoseTarget(idle_pose); // Go to idle pose
group.setPoseTarget(target_pose1); // Go to goal 1
group.setPoseTarget(idle_pose); // Go to idle pose
group.setPoseTarget(target_pose2); // Got to goal 2
// Add new goals here
success = group.move(); // Plan and move according to the way points defined hereabove
Consider that your robot cannot jumpy from point A to point B without indication about how to go there (could be motion planning, interpolation in joint space, interpolation in task space, ...). If you do jump between A and B and send this direcly to the controllers you will most likely damage your motors that will perform this motion at full speed with maximum energy. Thus even the return trajectory going back in idle position must be planned through a new MoveIt goal.
Jochen Welle
Apr 28, 2015, 4:55:16 AM4/28/15
to moveit...@googlegroups.com
Hi Joris,
i had similar problems when setting the start state to the current state
in the past. I observed that a second motion did not start at the
current position and it was not a visualization issue. Additionally, i
had some object attached to the endeffector, which was not considered in
planning when using group.setStartStateToCurrentState(). My current
workaround is to explicitly request the planning scene using a service
call and then use the current state from that scene. I am using
ROS-Indigo packages in Ubuntu 14.04 (there might be differences to Hydro
...).
// initialization
const std::string PLANNING_SCENE_SERVICE = "get_planning_scene";
planning_scene_monitor::PlanningSceneMonitorPtr
planning_scene_monitor_ =
boost::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description",
tf_listener_);
moveit::planning_interface::MoveGroup group("arm")
// each time the current state is needed
planning_scene_monitor_->requestPlanningSceneState(PLANNING_SCENE_SERVICE);
planning_scene_monitor::LockedPlanningSceneRW ps(planning_scene_monitor_);
ps->getCurrentStateNonConst().update();
robot_state::RobotState current_state = ps->getCurrentState();
group.setStartState(current_state);
I do not recall if without attached objects
group.setStartStateToCurrentState() was sufficient.
I have some more notes. First, the setup consists of two nodes, the
move_group node and a client node which uses an instance of
moveit::planning_interface::MoveGroup to communicate with the move_group
node. Therefore, if we want to access the current state in the client
node, there has to be some communication to retrieve it. I think a
blocking call in the client node blocks the whole move_group node. This
could prevent us from getting the latest state (?). In your example you
had a sleep(5.0), so this should not be an issue.
Second, from looking at the code, i know that
group.setStartStateToCurrentState() circumvents this problem by not
setting any start state in the planning request, resulting in move_group
using its own current state. This should be what we want, but in my case
with attached object, collisions of the object were not avoided, hence
the above workaround. This also implies that
group.setStartStateToCurrentState() and group.setStartState(
*group.getCurrentState()) are not the same.
@Yoan
I do not understand your answer. If you do
success = group.move();
will plan and move only to target_pose2.
regards
Jochen
On 27.04.2015 17:46, Yoan Mollard wrote:
> Hi Joris,
>
> Your video on Google Drive is private.
>
> It looks like you need to go back to an idle position between each goal.
> It seems that several targets in MoveIt are considered as way points, so
> if you need to reach several goals by returning to an idle (=starting)
> position between them, then you should set your idle position as an
> intermediary goal.
>
> geometry_msgs::Pose idle_pose;
> // Fill idle_pose here with current or whatever pose
> group.setPoseTarget(idle_pose); // Go to idle pose
> group.setPoseTarget(target_pose1); // Go to goal 1
> group.setPoseTarget(idle_pose); // Go to idle pose
> group.setPoseTarget(target_pose2); // Got to goal 2
> // Add new goals here
> success = group.move(); // Plan and move according to the way points
> defined hereabove
>
> Consider that your robot cannot jumpy from point A to point B without
> indication about how to go there (could be motion planning,
> interpolation in joint space, interpolation in task space, ...). If you
> do jump between A and B and send this direcly to the controllers you
> will most likely damage your motors that will perform this motion at
> full speed with maximum energy. Thus even the return trajectory going
> back in idle position must be planned through a new MoveIt goal.
>
> *Yoan Mollard*
> INRIA Research center, Flowers lab*
> *http://flowers.inria.fr/
> *
> *http://3rdhandrobot.eu/
> *
>
>
> ------------------------------------------------------------------------
>
> *From: *"Joris Beuls" <joris...@student.uhasselt.be>
> *To: *moveit...@googlegroups.com
> *Cc: *"joris beuls" <joris...@student.uhasselt.be>, "yoan
> mollard" <yoan.m...@inria.fr>
> *Sent: *Monday, 27 April, 2015 4:19:58 PM
> *Subject: *Re: Set a start state for the robot
> *Yoan Mollard*
> INRIA Research center, Flowers lab*
> *http://flowers.inria.fr/
> *
> *http://3rdhandrobot.eu/
> *
>
>
> ------------------------------------------------------------------------
>
> *From: *"Joris Beuls" <joris...@student.uhasselt.be>
> *To: *moveit...@googlegroups.com
> *Sent: *Thursday, 23 April, 2015 11:01:35 AM
> *Subject: *Set a start state for the robot
> moveit::planning_interface::MoveGroupgroup("epson_arm");
i had similar problems when setting the start state to the current state
in the past. I observed that a second motion did not start at the
current position and it was not a visualization issue. Additionally, i
had some object attached to the endeffector, which was not considered in
planning when using group.setStartStateToCurrentState(). My current
workaround is to explicitly request the planning scene using a service
call and then use the current state from that scene. I am using
ROS-Indigo packages in Ubuntu 14.04 (there might be differences to Hydro
...).
// initialization
const std::string PLANNING_SCENE_SERVICE = "get_planning_scene";
planning_scene_monitor::PlanningSceneMonitorPtr
planning_scene_monitor_ =
boost::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description",
tf_listener_);
moveit::planning_interface::MoveGroup group("arm")
// each time the current state is needed
planning_scene_monitor_->requestPlanningSceneState(PLANNING_SCENE_SERVICE);
planning_scene_monitor::LockedPlanningSceneRW ps(planning_scene_monitor_);
ps->getCurrentStateNonConst().update();
robot_state::RobotState current_state = ps->getCurrentState();
group.setStartState(current_state);
I do not recall if without attached objects
group.setStartStateToCurrentState() was sufficient.
I have some more notes. First, the setup consists of two nodes, the
move_group node and a client node which uses an instance of
moveit::planning_interface::MoveGroup to communicate with the move_group
node. Therefore, if we want to access the current state in the client
node, there has to be some communication to retrieve it. I think a
blocking call in the client node blocks the whole move_group node. This
could prevent us from getting the latest state (?). In your example you
had a sleep(5.0), so this should not be an issue.
Second, from looking at the code, i know that
group.setStartStateToCurrentState() circumvents this problem by not
setting any start state in the planning request, resulting in move_group
using its own current state. This should be what we want, but in my case
with attached object, collisions of the object were not avoided, hence
the above workaround. This also implies that
group.setStartStateToCurrentState() and group.setStartState(
*group.getCurrentState()) are not the same.
@Yoan
I do not understand your answer. If you do
> group.setPoseTarget(idle_pose); // Go to idle pose
> group.setPoseTarget(target_pose1); // Go to goal 1
> group.setPoseTarget(idle_pose); // Go to idle pose
> group.setPoseTarget(target_pose2); // Got to goal 2
each call overwrites the previous pose target. And the final call to
> group.setPoseTarget(target_pose1); // Go to goal 1
> group.setPoseTarget(idle_pose); // Go to idle pose
> group.setPoseTarget(target_pose2); // Got to goal 2
success = group.move();
will plan and move only to target_pose2.
regards
Jochen
On 27.04.2015 17:46, Yoan Mollard wrote:
> Hi Joris,
>
> Your video on Google Drive is private.
>
> It looks like you need to go back to an idle position between each goal.
> It seems that several targets in MoveIt are considered as way points, so
> if you need to reach several goals by returning to an idle (=starting)
> position between them, then you should set your idle position as an
> intermediary goal.
>
> geometry_msgs::Pose idle_pose;
> // Fill idle_pose here with current or whatever pose
> group.setPoseTarget(idle_pose); // Go to idle pose
> group.setPoseTarget(target_pose1); // Go to goal 1
> group.setPoseTarget(idle_pose); // Go to idle pose
> group.setPoseTarget(target_pose2); // Got to goal 2
> // Add new goals here
> success = group.move(); // Plan and move according to the way points
> defined hereabove
>
> Consider that your robot cannot jumpy from point A to point B without
> indication about how to go there (could be motion planning,
> interpolation in joint space, interpolation in task space, ...). If you
> do jump between A and B and send this direcly to the controllers you
> will most likely damage your motors that will perform this motion at
> full speed with maximum energy. Thus even the return trajectory going
> back in idle position must be planned through a new MoveIt goal.
>
> Research engineer – 3rd hand project
> *
> INRIA Research center, Flowers lab*
> *http://flowers.inria.fr/
> *
> *http://3rdhandrobot.eu/
> *
>
>
> ------------------------------------------------------------------------
>
> *From: *"Joris Beuls" <joris...@student.uhasselt.be>
> *To: *moveit...@googlegroups.com
> *Cc: *"joris beuls" <joris...@student.uhasselt.be>, "yoan
> mollard" <yoan.m...@inria.fr>
> *Sent: *Monday, 27 April, 2015 4:19:58 PM
> *Subject: *Re: Set a start state for the robot
> Research engineer – 3rd hand project
> *
> INRIA Research center, Flowers lab*
> *http://flowers.inria.fr/
> *
> *http://3rdhandrobot.eu/
> *
>
>
> ------------------------------------------------------------------------
>
> *From: *"Joris Beuls" <joris...@student.uhasselt.be>
> *To: *moveit...@googlegroups.com
> *Sent: *Thursday, 23 April, 2015 11:01:35 AM
> *Subject: *Set a start state for the robot
Yoan Mollard
Apr 28, 2015, 7:00:13 AM4/28/15
to Jochen Welle, moveit...@googlegroups.com
> each call overwrites the previous pose target. And the final call to success = group.move(); will plan and move only to target_pose2.
Mmh, you're right, I had memories about several successives goals with MoveIt, but now I'm reading the doc again, it seems not be achievable with setPoseTargets either that selects any pose from the list.
Despite there might be a bug in setting the start state, I'm not sure why you need to set the start state to "current" while the current state is used by default?
Best,
Yoan
Joris Beuls
Apr 28, 2015, 9:54:45 AM4/28/15
to moveit...@googlegroups.com
If i don"t set a new start state the robot always starts from it's "home" position. Even after executing a move. This is the problem i am having and what i mean by 'rviz does not update its state'.
Best regards,Joris Beuls
May 1, 2015, 9:57:57 AM5/1/15
to moveit...@googlegroups.com
Hello,
Apparently there was a mistake in the URDF file i was using. Now it automatically updates its start state with the current state without adding extra code. So just setting a goal state and use group.move() works.
Thanks allot for all the good feedback.
Beste regards
Joris
Op donderdag 23 april 2015 11:01:35 UTC+2 schreef Joris Beuls:
Apparently there was a mistake in the URDF file i was using. Now it automatically updates its start state with the current state without adding extra code. So just setting a goal state and use group.move() works.
Thanks allot for all the good feedback.
Beste regards
Joris
Op donderdag 23 april 2015 11:01:35 UTC+2 schreef Joris Beuls:
Simon Schmeißer
May 6, 2015, 10:41:02 AM5/6/15
to moveit...@googlegroups.com
Hi,
I have the same problem as Jochen and am also using some workaround (use move once plan has succeded). There seems to be some quite annoying bug around. I suspect it is a timing/synchronization issue?
Joris: Could you explain some more what you did change in your URDF? I would not expect it to have such a huge influence, but then I'm new here as well.
Regards
Simon
I have the same problem as Jochen and am also using some workaround (use move once plan has succeded). There seems to be some quite annoying bug around. I suspect it is a timing/synchronization issue?
Joris: Could you explain some more what you did change in your URDF? I would not expect it to have such a huge influence, but then I'm new here as well.
Regards
Simon
Message has been deleted
Joris Beuls
May 7, 2015, 5:15:17 AM5/7/15
to moveit...@googlegroups.com
A colleague of mine made the actual changes. He changed the URDF so that
all the axes of the links and joints were in the foot of the robot.
This is possible by adjusting the following parameter:
<origin
xyz="0 -0.1 -0.320"
rpy="0 0 0" />
Best regards
Joris
<origin
xyz="0 -0.1 -0.320"
rpy="0 0 0" />
Best regards
Joris
Xiaoxu Cao
Apr 20, 2017, 3:36:34 PM4/20/17
to MoveIt! Users
Hi Joris
I meet the same problem with you. You how you change the URDF ? How does it cause the problem? Can you go to the detail? I am suffering from this tricky problem now.
在 2015年5月7日星期四 UTC-4上午5:15:17，Joris Beuls写道：
在 2015年5月7日星期四 UTC-4上午5:15:17，Joris Beuls写道：
Reply all
Reply to author
Forward
0 new messages