[ros-users] multi_dof_joints parameter in arm_navigation stack

[Felix Messmer]

Hello Sachin, hello Gil,


finally I find the time to get back to you!
Work with the multi_dof_joint goes fine so far...

There is just one more thing, that I have problems with:
I implemented a function for visualizing a trajectory (as in 
move_arm_simple_action.cpp) pubishing on the 'display_path' topic.
I query the robot_state to fill the according field 'robot_state' of the 
motion_planning_msgs/DisplayTrajectory.msg and I write the planned 
trajectory for the arm into the 'trajectory' field of it, too. Since I 
do "planning-before-moving", the platform is still in its initial 
position although I set a multi_dof_joint PlatformPose to be used in the 
planning process.
I also write that PlatformPose into the 
'robot_state/multi_dof_joint_state' field of the DisplayTrajectory.msg.
This should place the robot to that new position while visualizing the 
trajectory, right?
Unfortunately it doesn't!

Maybe this is because the multi_dof_joint_state (floating_trans_x/y/z, 
floating_rot_x/y/z/w) is also already included within the 
robot_state/joint_state and this overwrites the additionally given 
robot_state/mutli_dof_joint_state.

Since it is only one position that I want to set and not a 
multi_dof_joint_trajectory, I don't want to insert the PlatformPose in 
the trajectory/multi_dof_joint_trajectory field.
There I only want to have the trajectory for the arm (model_id="arm").

Did you experienced this effect before? Maybe this case just hasn't been 
considered yet.
Using the given robot_state/multi_dof_joint_state over the 
multi_dof_joint_state-values in the robot_state/joint_state should fix 
this problem.


Best regards,
Felix





On 2011-02-08 14:28, Sachin Chitta wrote:
 > Hi Felix,
 >
 > (1) This statement is a warning:
 > [ WARN] [1297153951.492886546, 276.592000000]: State violates goal
 > constraints.
 > You don't usually have to worry about it because it just says that the
 > current state does not match the goal condition.
 >
 > (2) Can you check and make sure that the IK solution is within joint 
limits.
 > The joint limits should come from the urdf. If you have limits that 
you want
 > to specify that are different from the ones in the urdf, you can add them
 > in a file pr2_arm_navigation_config/config/joint_limits.yaml. Could you
 > please check the equivalent file that you have and make sure you have the
 > right limits set.
 >
 > (3) Visualize the solutions - you can look at the pr2 kinematics or the
 > tutorial for motion_planning_visualization to see how you can 
visualize the
 > entire robot state. This will help you debug errors as well.
 >
 > Send more questions if this is still not working.
 >
 > Regards,
 > Sachin
 >
 >
 > On Tue, Feb 8, 2011 at 12:55 AM, Felix Messmer <felix_messmer at web.de> 
wrote:
 >
 > > Dear Gil, dear Sachin,
 > >
 > > thanks a lot for your help and the detailed explanation!
 > > The Error referring to the floating_trans_x joint doesn't appear
 > > anymore...thus, this seems to be fixed! Thanks!
 > > However, I still get an Error message that says that the IK solution
 > > doesn't obey the goal constraints and that joint limits are violated.
 > > Although, the pose goal is right next to the care-o-bot and the (arm!)
 > > IK-solution lies within the joint limits.
 > >
 > >
 > > 
____________________________________________________________________________________________________________________________________________
 > >
 > >
 > > [ INFO] [1297153951.501037132, 276.596000000]: IK returned joint state
 > > for goal that doesn't seem to be valid
 > > [ WARN] [1297153951.501105392, 276.596000000]: IK solution doesn't obey
 > > goal constraints
 > > [ WARN] [1297153951.530798642, 276.603000000]: Joint limits violated.
 > >
 > > [ERROR] [1297153951.541223194, 276.603000000]: Will not plan to
 > > requested joint goal since it violates joint limits constraints
 > >
 > > 
______________________________________________________________________________________________________________________________________________
 > >
 > >
 > > I couldn't look into it more closely, since I'm busy doing something
 > > else first.
 > > But it seems that this is due to the cob's ik_solver. It currently
 > > doesn't support all the services that the pr2_kinematics stack 
provides,
 > > e.g. constraint_aware IK.
 > > Maybe the planner expects some more information within the IK-response
 > > that our ik_solver currently just doesn't provide.
 > >
 > > I'll have a look at that as soon as possible and then get back to you!
 > >
 > >
 > > Best regards,
 > > Felix
 > >
 > >
 > >
 > >
 > >
 > >
 > >
 > >
 > > On 2011-02-04 18:15, Gil Jones wrote:
 > > > Hi Felix,
 > > >
 > > > As Sachin said, the bug you were seeing should be fixed. By way of
 > > > explanation, we were trying to accomplish a few things with the
 > > multi-dof
 > > > transform. The primary purpose is to supply a mechanism for a
 > > world->robot
 > > > transform that supplements the urdf, which doesn't and shouldn't 
really
 > > > supply information about how to transform between a fixed world frame
 > > and a
 > > > frame for the movable base. The world transform should be either be a
 > > 3-dof
 > > > Planar Joint with (x,y,th) or a 6-dof Floating Joint with a full
 > > orientation
 > > > matrix. We found on the PR2 that we had to use a floating joint 
as there
 > > > can occasionally be enough of a pitch and roll in our IMU such 
that they
 > > > need to be accounted for. It's entirely fine to do what you've done
 > > below,
 > > > which is to say that you essentially don't care about the fixed
 > > transform
 > > > and will always be planning based on the current base position of the
 > > > robot. So that should be all you need to do.
 > > >
 > > > Having a fixed reference transform should allow two important 
pieces of
 > > > functionality - as you say, it should allow you to plan to move the
 > > base,
 > > > and it should also allow you to pose the robot some where else in the
 > > world
 > > > and plan based on that new pose - for instance, you might be
 > > interested to
 > > > know whether an object across the room from the robot's current
 > > location can
 > > > be picked up given a particular approach pose. By specifying a new
 > > > multi-dof transform in the start_state message our system should 
support
 > > > such things, though it is not the case that our current planners 
fully
 > > > support the former use-case.
 > > >
 > > > The other purpose of the multi-dof joints is to equip the system to
 > > use tf
 > > > transforms instead of just joint_state messages to populate the 
current
 > > > position of the robot. The system is set up to first try to set
 > > multi-dof
 > > > joints from individually labeled entries in the joint state and then
 > > to use
 > > > tf to get a pose transform for the entire joint if it needs to. The
 > > message
 > > > you are seeing refers to one of the labeled single dofs -
 > > 'floating_trans_x'
 > > > - of the multi-dof joint 'base_joint'. The bug was that for the
 > > purposes of
 > > > checking joint limits the individually-labeled dofs were not being
 > > mapped to
 > > > the multi-dof joint.
 > > >
 > > > As arm_navigation matures we will try to do a better job of making
 > > migration
 > > > guides that explain new features and inform users what they need 
to do
 > > to
 > > > adopt a new version. At this point we're focused on making relatively
 > > large
 > > > changes that we hope will make things work better and make it easier
 > > to get
 > > > our stacks working on other arms, but we still have a ways to go. 
We're
 > > > targeting e-turtle for a 0.9 or 1.0, at which point things should
 > > stabilize
 > > > a good deal.
 > > >
 > > > Please let us know if the bug fix doesn't work or if you have other
 > > > problems.
 > > >
 > > > -Gil
 > > >
 > > > --
 > > > E. Gil Jones (gjones at willowgarage.com)
 > > > Research Engineer
 > > > Willow Garage, Inc.
 > > > 68 Willow Road
 > > > Menlo Park, CA 94025
 > > > 650.475.9772 <tel:+16504759772>
 > > >
 > > >
 > > > On Fri, Feb 4, 2011 at 1:23 AM, Sachin Chitta
 > > <sachinc at willowgarage.com>wrote:
 > > >
 > > > > Hi Felix,
 > > > >
 > > > > This should be fixed in motion_planning_common 0.3.5 (thanks to 
Gil).
 > > > > Can you try it out and let us know.
 > > > >
 > > > > Thanks,
 > > > > Sachin
 > > > >
 > > > > On Wed, Feb 2, 2011 at 4:09 AM, "Felix Meßmer" 
<Felix_Messmer at web.de>
 > > > > wrote:
 > > > > > Dear community,
 > > > > >
 > > > > > there is another feature in the unstable/diamondback version 
of the
 > > > > arm_navigation stack that I have problems with:
 > > > > > the multi_dof_joints parameter-option!
 > > > > >
 > > > > > When I use our old launch file
 > > > > (cob_arm_navigation/cob3_planning_environment.launch) that 
loads the
 > > > > collision_checks and planning_groups paramters to the parameter
 > > server, I
 > > > > get the following error message from both the trajectory_filter and
 > > the
 > > > > ompl_planning node:
 > > > > >
 > > > > > [ WARN] [1296646033.585385464]: No multi dof joints specified,
 > > including
 > > > > root to world conversion
 > > > > > [ WARN] [1296646033.585623806]: Can't do anything without a root
 > > > > transform
 > > > > >
 > > > > > I've adapted the planning_environment.launch file to also 
load the
 > > > > multi_dof_joints parameter (looking at the according pr2-config
 > > files).
 > > > > > But now planning reqest as in move_arm_simple_pose goal fail due
 > > to the
 > > > > following error message from the environment_server node and the
 > > move_arm
 > > > > action server node:
 > > > > >
 > > > > > [ WARN] [1296647088.916018038, 830.563000000]: No joint with name
 > > > > floating_trans_x
 > > > > > [ WARN] [1296647088.916446633, 830.563000000]: Joint limits
 > > violated.
 > > > > >
 > > > > > [ERROR] [1296647088.925356070, 830.563000000]: Will not plan to
 > > requested
 > > > > joint goal since it violates joint limits constraints
 > > > > >
 > > > > >
 > > > > > The multi_dof_joints.yaml file I use looks like this:
 > > > > >
 > > > > >
 > > > > >
 > > > > >
 > > > > >
 > > > > > I guess the multi_dof_joint parameter is meant to introduce a
 > > joint that
 > > > > stands for the moving platform with 3 DoFs (trans_x, trans_y, 
rot_z)
 > > > > > Does that mean that if the pose goal is specified in a 
world/global
 > > > > coordinate system, the planner also considers and calculates a
 > > possibly
 > > > > necessary platform motion to reach the specied goal?
 > > > > >
 > > > > > Is there a parameter (use_multidof?) for the 
environment_server to
 > > > > deactivate multi_dof_joints?
 > > > > > OR:
 > > > > > What else do I have to add to use that feature?
 > > > > >
 > > > > >
 > > > > > I hope you can give me a short explanation on that. Thanks a lot!
 > > > > >
 > > > > >
 > > > > >
 > > > > > Regards,
 > > > > > Felix
 > > > > > ___________________________________________________________
 > > > > > WEB.DE DSL Doppel-Flat ab 19,99
 > > _______________________________________________
 > > ros-users mailing list
 > > ros-users at code.ros.org
 > > https://code.ros.org/mailman/listinfo/ros-users
 > >
 >
 >
 >
 > --
 > Sachin Chitta
 > Research Scientist
 > Willow Garage
 > _______________________________________________
 > ros-users mailing list
 > ros-users at code.ros.org
 > https://code.ros.org/mailman/listinfo/ros-users
 >