Feature/ik panda #340
Feature/ik panda #340
Conversation
There was a problem hiding this comment.
Wow, sorry for commenting comment by comment giving you probably a lot of notifications... I just found about the reviewing the changes directly. I will know for the next time!
Also, I have done some more commits that should be addressing the comments. The most important change is the creation of PoseGoals, a container similar to the ContactGoals one., like you asked @gchenfc
There was a problem hiding this comment.
Just the couple minor things.
Also, in the future, reply "Done" on comments you fixed/addressed so that we only have to look at comments still in question. (can you do that on the previous review comments as well?)
I think it's fine to comment on the comments instead of "starting a review".
| ///< Map of time to PoseGoal | ||
| using PoseGoals = std::map<size_t, PoseGoal>; |
There was a problem hiding this comment.
is it possible to have multiple pose goals for a single timestep? In which case maybe std::map<size_t, std::vector<PoseGoal>> would be more appropriate? Or if you don't need it I guess you can leave it.
There was a problem hiding this comment.
Not in the painting case, only one pose per timestep is necessary, I think having multiple poses per timestep on a 7-DoF doesn't make much sense as we would be over constraining the problem, right?
gtdynamics/kinematics/Kinematics.h
Outdated
| @@ -183,7 +183,7 @@ class Kinematics : public Optimizer { | |||
| template <class CONTEXT> | |||
| gtsam::Values initialValues( | |||
| const CONTEXT& context, const Robot& robot, double gaussian_noise = 0.1, | |||
| const gtsam::Values& initial_joints = gtsam::Values()) const; | |||
| const gtsam::Values& initial_joints = gtsam::Values(), bool use_fk = false) const; | |||
There was a problem hiding this comment.
What does this flag do? Please amend doxygen above
| @@ -126,13 +126,20 @@ NonlinearFactorGraph Kinematics::poseGoalObjectives<Slice>( | |||
| const gtsam::Values& goal_poses) const { | |||
| gtsam::NonlinearFactorGraph graph; | |||
|
|
|||
| for (const gtsam::Key& key : goal_poses.keys()) { | |||
There was a problem hiding this comment.
No, that is not how you iterate over a map in c++ :-) So
for (auto&& it : goal_poses) {
const Key& key = it->first;
const gtsam::Pose3& desired_pose = it->second;
}
gtdynamics/kinematics/Kinematics.h
Outdated
| /** | ||
| * @fn Create a pose prior for a given link for each given pose. | ||
| * @param context Slice or Interval instance. | ||
| * @param pose_goals goals for poses |
There was a problem hiding this comment.
You are still not explaining what pose_goals is. size_t can be many things, what does it mean? Does it pertain to all contexts?
| /** | ||
| * @fn Factors that minimize joint angles. | ||
| * @param context Slice or Interval instance. | ||
| * @param robot Robot specification from URDF/SDF. | ||
| * @param joint_priors Values where the mean of the priors is specified. The |
There was a problem hiding this comment.
Same comment. You use Values. Does that mean in your case you specify different priors for every time step? Or, is it simply robot-specific in which case aa std::map with joint indices would be the right thing. I already commented on this but not getting ansers. And, answers should end up in the documentation...
|
@Joobz please finish up this PR before you head out, else it will stay in review purgatory. :) |
This PR changes and adds some functions in kinematics:
poseGoalObjectives(): add prior to link COM poses.jointAngleObjectives(): modified it so we can add any mean to the prior. Previously it was always centered at the origin.jointAngleLimits(): add a jointLimitFactor only to the angles, compared to the one in joints that adds a factor to all dynamics related variables.initialValues(): modified it so we can start from non-random points for arbitrary joints.inverseWithPose(): IK with a given COM pose for the 7th link.There are also some changes to Robot and Joint files that @dellaert changed because we ran into some problems.