Update stomp plugins ik code #46
Conversation
jrgnicho
force-pushed
the
update-stomp-plugins-ik-code
branch
from
0e5c4c0
to
9960a65
Compare
stomp_plugins/example_pages.dox
Outdated
| - cartesian_convergence: A vector of convergence values for each cartesian DOF [vx vy vz wx wy wz]. | ||
| - joint_update_rates: The rates at which to update the joints during numerical ik computations. | ||
| - max_ik_iterations: Limit on the number of iterations allowed for numerical ik computations. | ||
| - class: The class name.S |
|
It's nice to see that stomp can now plan to cartesian goals using this branch. However I still have some issues (I try to configure my stomp to use both cartesian and joint goals, just like ompl can handle it). However it seems that I should use different configs for them Here is my config for joint goals This will give me an error for cartesian goals: If I add goal sampling with the following config: I run into some issues:
and use ik even though we already have a ik solution (the joint goal)
Thanks for your great work, I hope most of this is just misconfiguration on my side. I'm new to stomp but I really enjoy the results so far and I got a general question: Do you intend to later generalize this to use the default ik plugin (usually ikfast in our case) instead of tracIK? |
|
@simonschmeisser Thanks for reviewing and testing this, I'd use the configuration for the 'manipulator_tool' group instead, the one you have tested still uses an old (and possibly invalid) configuration. However, I should update the configuration for the 'manipulator' group to prevent others from running a bad configuration. |
|
@jrgnicho Is this ready to be merged? |
|
@jrgnicho Sorry I somehow messed up and used the wrong git branch, fixed that now. Anyway I updated to the following config: stomp/manipulator:
group_name: manipulator
optimization:
num_timesteps: 100
num_iterations: 100
num_iterations_after_valid: 5
num_rollouts: 20
max_rollouts: 100
initialization_method: 3 #[1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST
control_cost_weight: 1.0
task:
noise_generator:
- class: stomp_moveit/GoalGuidedMultivariateGaussian
stddev: [0.05, 0.4, 1.2, 0.4, 0.4, 0.1]
# #- class: stomp_moveit/NormalDistributionSampling
# #stddev: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
cost_functions:
- class: stomp_moveit/CollisionCheck
collision_penalty: 1.0
cost_weight: 1.0
kernel_window_percentage: 0.2
longest_valid_joint_move: 0.05
# - class: stomp_moveit/ObstacleDistanceGradient
# max_distance: 0.2
# cost_weight: 1.0
# longest_valid_joint_move: 0.05
- class: stomp_moveit/ToolGoalPose
position_cost_weight: 0.5
orientation_cost_weight: 0.5
noisy_filters:
- class: stomp_moveit/JointLimits
lock_start: True
lock_goal: False
- class: stomp_moveit/MultiTrajectoryVisualization
line_width: 0.01
rgb: [0, 255, 0]
marker_array_topic: stomp_trajectories
marker_namespace: noisy
update_filters:
- class: stomp_moveit/ConstrainedCartesianGoal
- class: stomp_moveit/PolynomialSmoother
poly_order: 5
- class: stomp_moveit/TrajectoryVisualization
line_width: 0.01
rgb: [191, 0, 255]
error_rgb: [255, 0, 0]
publish_intermediate: True
marker_topic: stomp_trajectory
marker_namespace: optimized
And now have some issues again:
if (movePlan.m_activeTarget == POSE || movePlan.m_activeTarget == POSITION || movePlan.m_activeTarget == ORIENTATION) {
geometry_msgs::PoseStamped msg;
tf::poseEigenToMsg(movePlan.m_goalPose, msg.pose);
msg.header.stamp = ros::Time(0.0);
msg.header.frame_id = "world_frame";
moveit_msgs::Constraints c = kinematic_constraints::constructGoalConstraints(m_lastLink, msg, goal_position_tolerance_, goal_orientation_tolerance_);
if (movePlan.m_activeTarget == ORIENTATION)
c.position_constraints.clear();
if (movePlan.m_activeTarget == POSITION)
c.orientation_constraints.clear();
request.goal_constraints.resize(1);
request.goal_constraints[0] = kinematic_constraints::mergeConstraints(request.goal_constraints[0], c);stomp tries to plan towards the pose with reference to "base_link"
The displayed coordinate system is "base_link" while "world frame" is rotated by -90° around z, ie x pointing towards the observer. The goal would have been to the right of the right hand box
Another example, here the robot is supposed to move to the slightly transparent position but plans to a 90° rotated goal instead |
|
@simonschmeisser the code may be ignoring the coordinate frame for the goal tool pose, I'll look into this. Is your testing setup available ? |
|
@Levi-Armstrong it's nor ready to be merge yet, I'll address the issues @simonschmeisser has exposed and push the fixes. |
|
@jrgnicho unfortunately it's not available. I can try creating a test case on Tuesday the earliest, sorry |
|
@simonschmeisser that'd be helpful if you can. Thanks |
|
@jrgnicho Sorry for the delay, had some important visitors here
I forked your branch to provide you with a testcase: https://github.com/isys-vision/industrial_moveit/tree/update-stomp-plugins-ik-code |
|
@simonschmeisser, excellent, I'll test my changes with this setup. Thank you |
|
@simonschmeisser I made some changes to map the robot base coordinate system to the target's frame id. Could you test again? |
| * @param constraints A moveit_msgs message that encapsulates the cartesian constraints specifications. | ||
| * @param tool_pose The tool pose as specified in the constraint in the target_frame. | ||
| * @param tolerance The tolerance values on the tool pose as specified in the constraint message. | ||
| * @param target_frame The coordinate frame of the tool pose. If black the model root is used |
There was a problem hiding this comment.
Typo: If black the model root is used -> If blank? or If empty
| * @param constraints A moveit_msgs message that encapsulates the cartesian constraints specifications. | ||
| * @param tool_pose The tool pose as specified in the constraint message. | ||
| * @param tolerance The tolerance values on the tool pose as specified in the constraint message. | ||
| * @param target_frame The coordinate frame of the tool pose. If black the model root is used |
|
@jrgnicho it works a lot better than before and plans to correct positions (both pose and joint goals). Cool! For joint targets I get an error: |
|
@simonschmeisser , I'll take a look and see why it fails with joint goals. How are you sending a motion plan, through a service call or the move group interface? Even though both methods should be equivalent, the constraints created to represent the motion plan tend to differ. |
|
We copied from MoveGroup and are thus using the PlanningPipeline (I hope it's not too much code) moveit_msgs::MotionPlanRequest request;
request.group_name = "manipulator";
request.num_planning_attempts = 4;
request.max_velocity_scaling_factor = 1.0;
request.allowed_planning_time = 20.0;
request.planner_id = "RRTConnectkConfigDefault";
request.workspace_parameters = moveit_msgs::WorkspaceParameters();
const double goal_joint_tolerance_ = 1e-4;
const double goal_position_tolerance_ = 1e-4; // 0.1 mm
const double goal_orientation_tolerance_ = 1e-3; // ~0.1 deg
robot_state::robotStateToRobotStateMsg(*movePlan.m_startState, request.start_state);
if (movePlan.m_activeTarget == JOINT)
{
request.goal_constraints.resize(1);
request.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(*movePlan.m_goalState, m_planningSceneMonitor->getRobotModel()->getJointModelGroup("manipulator"), goal_joint_tolerance_);
} else if (movePlan.m_activeTarget == POSE || movePlan.m_activeTarget == POSITION || movePlan.m_activeTarget == ORIENTATION) {
geometry_msgs::PoseStamped msg;
tf::poseEigenToMsg(movePlan.m_goalPose, msg.pose);
msg.header.stamp = ros::Time(0.0);
msg.header.frame_id = "world_frame";
moveit_msgs::Constraints c = kinematic_constraints::constructGoalConstraints(m_lastLink, msg, goal_position_tolerance_, goal_orientation_tolerance_);
if (movePlan.m_activeTarget == ORIENTATION)
c.position_constraints.clear();
if (movePlan.m_activeTarget == POSITION)
c.orientation_constraints.clear();
request.goal_constraints.resize(1);
request.goal_constraints[0] = kinematic_constraints::mergeConstraints(request.goal_constraints[0], c);
} else {
ROS_ERROR("Unable to construct goal representation");
m_semaphore.release();
return moveit::planning_interface::MoveItErrorCode::INVALID_GOAL_CONSTRAINTS;
}
//if (path_constraints_)
// request.path_constraints = *path_constraints_;
bool success;
planning_interface::MotionPlanResponse res;
if (scene) {
success = planning_pipeline_->generatePlan(scene, request, res);
} else {
planning_scene_monitor::LockedPlanningSceneRO ps(m_planningSceneMonitor);
success = planning_pipeline_->generatePlan(ps, request, res);
}
|
|
OK, this should help, I'll try to work on it tonight. Thanks |
|
@jrgnicho I'll be in vacation for the next four weeks so no testing from my side till september, sry |
|
Sorry @simonschmeisser I've been swamped lately. I think you've given me enough feedback to fix the issues you had described. Thank |
|
hey @jrgnicho any news? |
|
@simonschmeisser I'll resume working on this around mid December. |
|
Big +1 for this, I've done the same myself for my ICRA work (which got rejected) and now working on IROS with the same codebase. I could try to merge this and that cartesian seed code and submit a PR with them. Opinions? |
|
@bmagyar no objections from my side on somebody fixing this ;) I won't be able to find out why Joint goals fail and how to fix it too soon |
|
@raghavendersahdev is trying to migrate more stomp integration into the main moveit and we want to port the stomp_moveit plugin over. i don't want to lose this suggested PR but reading through it, it seems momentum has been lost on getting it finished. any updates on this PR? |
|
@davetcoleman I'm afraid I won't be able to do anything on it too soon. |
jrgnicho
force-pushed
the
update-stomp-plugins-ik-code
branch
from
e935166
to
da81cf9
Compare
|
After merging #77 this PR is ready for review. |
|
I'll first continue doing functional reviews, I saw nothing obviously wrong/bad when looking through the diff but will repeat that once I'm back into the code I'm using a 6-DOF fixed robot as seen in screenshots above. It has a gripper with one DOF and one mimic joint attached which is in a different JointModelGroup (and not actually controlled by MoveIt!). My current configuration is as follows: https://gist.github.com/simonschmeisser/ec19cdabefa2e6e56e3381f278b2ab48 Case 1: noisy_filters: JointLimits/lock_goal : True && JointTarget This case often works but not necessarily. It often invalidates a valid trajectory during "post refinement"/iterations after valid and then starts optimizing anew. I thought you added a step-back to a previous valid solution, that might be missing here? Case 2: noisy_filters: JointLimits/lock_goal : True && PoseTarget Case 3: lock_goal: False && JointTarget Many goals are evaluated (see spheres in screenshot) but none seem to match validity requirements.
https://gist.github.com/simonschmeisser/a011a3a3588cfbc0add4784a80b02ac1 Case 4: lock_goal: False && PoseTarget Visualization looks about the same as case 3, seems to suffer from same goal validity problems https://gist.github.com/simonschmeisser/8e7fdc4fa6875cda9f79ab3a04872b90 I'm not sure which of these are regressions and which bugs existed before, but I think those issues sound fixable and will have a look myself now. |
| else | ||
| { | ||
| // reverting updates as no improvement was made | ||
| parameters_optimized_ -= parameters_updates_; |
There was a problem hiding this comment.
we need to set back parameters_valid_ here as well as the previous solution might have been valid:
// is now valid again
if (parameters_valid_prev_)
parameters_valid_ = true;
or more simply
// set back validity as well, previous might have been valid
parameters_valid_ = parameters_valid_prev_;
this fixes "Case 1:"
There was a problem hiding this comment.
I've rearrange this part so as to set the current trajectory to valid when it was reverted to the previous valid one.
There was a problem hiding this comment.
Case 2 can be fixed here:
if there are no joint constraints, this should succeed as well, I modified it to read as following:
// saving goal state
if(lock_goal_)
{
bool goal_state_saved = false;
bool has_joint_constraints = false;
for(auto& gc: req.goal_constraints)
{
if (gc.joint_constraints.empty())
continue;
else
has_joint_constraints = true;
for(auto& jc : gc.joint_constraints)
{
goal_state_->setVariablePosition(jc.joint_name,jc.position);
goal_state_saved = true;
}
if(!goal_state_->satisfiesBounds(robot_model_->getJointModelGroup(group_name_)))
{
ROS_WARN("%s Requested Goal State is out of bounds",getName().c_str());
}
break;
}
if(has_joint_constraints && !goal_state_saved)
{
ROS_ERROR_STREAM("Failed to save goal state");
return false;
}
}
Not sure if that should end up in a separate PR, I can do so if preferred
| tool_goal_tolerance_ << ptol, ptol, ptol, rtol, rtol, rtol; | ||
| } | ||
|
|
||
| break; |
There was a problem hiding this comment.
What is the intention of that break?
There was a problem hiding this comment.
I think I wanted to stop inspecting the constraints once a valid one was found but I my implementation of this intent might've not quite reflected that.
| } | ||
|
|
||
| break; |
There was a problem hiding this comment.
previously that would break only if found_goal == true
There was a problem hiding this comment.
I modified this to break out of the look when this condition is met.
|
ToolGoalPose seems to generate Goals outside the GoalConstraints, that's why Case 3 and 4 fail: I will have a look at that tomorrow |
| @@ -278,7 +288,7 @@ bool GoalGuidedMultivariateGaussian::generateNoise(const Eigen::MatrixXd& parame | |||
| return true; | |||
| } | |||
|
|
|||
| bool GoalGuidedMultivariateGaussian::generateRandomGoal(const Eigen::VectorXd& seed_joint_pose,Eigen::VectorXd& goal_joint_pose) | |||
| bool GoalGuidedMultivariateGaussian::generateRandomGoal(const Eigen::VectorXd& reference_joint_pose,Eigen::VectorXd& goal_joint_pose) | |||
There was a problem hiding this comment.
In my observations (case 3 and case4) this function always returns exactly the same joint_position, thus goal_joint_noise in line 266 is all zero. This is not the main problem however. In 284 all trajectory points (==cols) get noise applied. That includes the last column, which is the goal. The sampling noise might be bigger than the goal tolerance however. So if I add
noise.rightCols(1) = goal_joint_noise;
before
parameters_noise = parameters + noise;
Case 3 and case 4 work again.
Why the goal noise is all zero remains to be fixed
There was a problem hiding this comment.
In my observations (case 3 and case4) this function always returns exactly the same joint_position, thus goal_joint_noise in line 266 is all zero
The goal noise will be zero whenever the cartesian tolerances are small.
The sampling noise might be bigger than the goal tolerance however.
Yes, but the Constrained cartesian goal filter applied later on should enforce the constraints at the goal
I add
noise.rightCols(1) = goal_joint_noise;
I did make this change, however in my test setup it caused stomp to fail very frequently. My guess is that some noise at the goal (even if it's in violation of the requested constraint) allows exploring more reachable goal configurations.
There was a problem hiding this comment.
Hmm, I have no such thing as a "Constrained cartesian goal filter" in my configuration, should I?
There was a problem hiding this comment.
Hmm, so when I add - class: stomp_moveit/ConstrainedCartesianGoal as you have it in the example file it sometimes works (ca 60%) but there are still some failures whereas when effectively disabling the noise as in a few comments above it always works. (Except for cases where we compare to the "other" representation of the euler angles). The right thing here would probably be to constrain the noise to be within the goal tolerances
There was a problem hiding this comment.
The Constrained cartesian goal filter enforces the goal tolerances. I'm till puzzled by how you are getting better results when the noise is zeroed out, not sure what to make of that.
There was a problem hiding this comment.
I also noticed that your stomp_moveit/ToolGoalPose cost function uses 25.0 for the weights. I've been using 0.5 for both
- class: stomp_moveit/ToolGoalPose
position_cost_weight: 25.0
orientation_cost_weight: 25.0
The plugin assumes that they add up to 1. My fault for not documenting that
There was a problem hiding this comment.
Well I haven't checked that thoroughly this time, but last iteration the noise was too big so it would always be just outside the tolerances
maybe additionally you could add an error/warning if this does not add up to 1 (or normalize it). I will check on Monday if that changes anything
|
@jrgnicho would you like a PR against your branch or do you prefer fixing it yourself? |
|
another issue (but probably another PR ...) we had similar exciting issues with euler angles recently and switched to comparing in angle-axis format. Not sure how that translates to the limits we currently have though |
|
+1 to merging this 2 year old PR!! |
|
For me two open issues remain but we might want to merge this anyway to move ahead
I observed a crash when running stomp from multiple threads but have not yet investigated this |
|
I've addressed @simonschmeisser comments. My test setup consisted of the stomp demo launch |
@simonschmeisser I haven't had it crash on me yet so I'm not sure how to replicate this. |
We plan many possible alternative "routes"/picks from worker threads in our application. So stomp is run simultaneously a couple of times (~5). This worked fine with ompl as there is some sort of thread affine cache but I'm not sure if that is within the planning pipeline or within the plugin. Will check that and also try to pin down the origin of the crash |
|
Ok, looking a bit at the code it is quite obvious that stomp core is not threadsafe. Which is totally ok. So in a follow up PR we would need to implement this in |
|
side note: if you add |
|
@simonschmeisser will you be ok with merging this as is? |
|
Yes totally, we can iron out smaller stuff separately later on. Thanks! |
Is this mode of planning done outside of the move_group node workflow? |
|
yes, we have our own equivalent classes ... it could be implemented as a MoveGroupCapability though |
|
@simonschmeisser thanks for your patience in reviewing this PR. |
Yes I'm not surprised that it would crash in a multithreaded application as the stomp_core class wasn't made to be used in that type of context and so it lacks important thread-safety features. Creating a stomp_core instance per plan request as you mentioned in a previous comment is likely to work OK however you'll have to fare the cost of having multiple (potentially large) Eigen matrices. |
|
Nice! I'll try that approach and measure the costs soonish |
This PR makes some important changes:
I haven't added a formal unit test, however I have tested the planning under various scenarios (joint goal, fully constrained tool and under constrained) with nodes that aren't part of this repo. I can make those into formal unit test if requested although that may take a bit more time.