Skip to content

Feature/ik panda #340

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 12 commits into
base: master
Choose a base branch
from
Open

Feature/ik panda #340

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
12 commits
Select commit Hold shift + click to select a range
5b13133
test IK for Panda, initial check-in
dellaert Feb 14, 2022
5197e23
Make fix/unfix methods const
dellaert Feb 14, 2022
ea19ce1
Removed logmap and use gtsam
dellaert Feb 14, 2022
54b26c1
Add constraints for fixed links
dellaert Feb 14, 2022
7e44402
initial values function prototype
Joobz Feb 16, 2022
e534344
Added new functions in kinematics
Joobz Feb 23, 2022
9a55580
joint angle limits
Joobz Mar 7, 2022
9c70904
Adressed comments
Joobz Mar 8, 2022
d2226e4
PoseGoals and addressed more comments
Joobz Mar 9, 2022
fdb0b63
Adressed comments
Joobz Mar 9, 2022
0cf03b5
added gpmp and changed kinematics
May 16, 2022
54c72df
small update
Joobz Jul 6, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions gtdynamics/kinematics/Kinematics.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Copy link
Member

@dellaert dellaert Mar 11, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

What does this flag do? Please amend doxygen above


/**
* @fn Inverse kinematics given a set of contact goals.
Expand All @@ -210,7 +210,7 @@ class Kinematics : public Optimizer {
* @return values with poses and joint angles
*/
template <class CONTEXT>
gtsam::Values inverseWithPose(
gtsam::Values inverse(
const CONTEXT& context, const Robot& robot,
const gtsam::Values& goal_poses,
const gtsam::Values& joint_priors = gtsam::Values()) const;
Expand Down
4 changes: 2 additions & 2 deletions gtdynamics/kinematics/KinematicsInterval.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -98,10 +98,10 @@ NonlinearFactorGraph Kinematics::jointAngleLimits<Interval>(
template <>
Values Kinematics::initialValues<Interval>(
const Interval& interval, const Robot& robot, double gaussian_noise,
const gtsam::Values& joint_priors) const {
const gtsam::Values& joint_priors, bool use_fk) const {
Values values;
for (size_t k = interval.k_start; k <= interval.k_end; k++) {
values.insert(initialValues(Slice(k), robot, gaussian_noise, joint_priors));
values.insert(initialValues(Slice(k), robot, gaussian_noise, joint_priors, use_fk));
}
return values;
}
Expand Down
46 changes: 24 additions & 22 deletions gtdynamics/kinematics/KinematicsSlice.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -73,13 +73,13 @@ EqualityConstraints Kinematics::constraints<Slice>(const Slice& slice,
// Get an expression for the unknown link pose.
Pose3_ bTcom(PoseKey(link->id(), slice.k));

// Kust make sure it does not move from its original rest pose
// Just make sure it does not move from its original rest pose
Pose3_ bMcom(link->bMcom());

// Create expression to calculate the error in tangent space
auto constraint_expr = gtsam::logmap(bTcom, bMcom);

// Add constriant
// Add constraint
constraints.emplace_shared<VectorExpressionEquality<6>>(constraint_expr,
tolerance);
}
Expand Down Expand Up @@ -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()) {
Copy link
Member

@dellaert dellaert Mar 11, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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;
}

const gtsam::Pose3& desired_pose = goal_poses.at<gtsam::Pose3>(key);
// TODO(toni): use poseprior from unstable gtsam slam or create new
// factors, to add pose from link7
graph.addPrior<gtsam::Pose3>(key, desired_pose, p_.p_cost_model);
}

// Add priors on link poses with desired poses from argument
for (auto&& link : robot.links()) {
auto pose_key = PoseKey(link->id(), slice.k);
if (goal_poses.exists(pose_key)) {
const gtsam::Pose3& desired_pose = goal_poses.at<gtsam::Pose3>(pose_key);
// TODO: use poseprior from unstable gtsam slam or create new factors, to
// add pose from link7
// TODO(toni): use poseprior from unstable gtsam slam or create new
// factors, to add pose from link7
graph.addPrior<gtsam::Pose3>(pose_key, desired_pose, p_.p_cost_model);
}
}
Expand All @@ -148,9 +155,8 @@ NonlinearFactorGraph Kinematics::jointAngleObjectives<Slice>(
// Minimize the joint angles.
for (auto&& joint : robot.joints()) {
const gtsam::Key key = JointAngleKey(joint->id(), slice.k);
double joint_mean = 0.0;
if (mean.exists(key)) joint_mean = mean.at<double>(key);
graph.addPrior<double>(key, joint_mean, p_.prior_q_cost_model);
graph.addPrior<double>(key, (mean.exists(key) ? mean.at<double>(key) : 0.0),
p_.prior_q_cost_model);
}

return graph;
Expand All @@ -171,31 +177,27 @@ NonlinearFactorGraph Kinematics::jointAngleLimits<Slice>(
}

template <>
Values Kinematics::initialValues<Slice>(
const Slice& slice, const Robot& robot, double gaussian_noise,
const gtsam::Values& initial_joints) const {
Values Kinematics::initialValues<Slice>(const Slice& slice, const Robot& robot,
double gaussian_noise,
const gtsam::Values& initial_joints,
bool use_fk) const {
Values values;

auto sampler_noise_model =
gtsam::noiseModel::Isotropic::Sigma(6, gaussian_noise);
gtsam::Sampler sampler(sampler_noise_model);

// Initialize all joint angles.
bool any_value = false;
for (auto&& joint : robot.joints()) {
auto key = JointAngleKey(joint->id(), slice.k);
double value;
if (initial_joints.exists(key)) {
value = initial_joints.at<double>(key);
any_value = true;
} else
value = sampler.sample()[0];
InsertJointAngle(&values, joint->id(), slice.k, value);
double angle = initial_joints.exists(key) ? initial_joints.at<double>(key)
: sampler.sample()[0];
InsertJointAngle(&values, joint->id(), slice.k, angle);
}

// Maybe fk takes a long time, so only compute it if there was a given initial
// joint value in this slice
if (any_value) {
if (use_fk) {
// Initialize poses with fk of initialized values
auto fk = robot.forwardKinematics(values, slice.k);
for (auto&& link : robot.links()) {
Expand Down Expand Up @@ -241,12 +243,12 @@ Values Kinematics::inverse<Slice>(const Slice& slice, const Robot& robot,
}

template <>
gtsam::Values Kinematics::inverseWithPose<Slice>(
gtsam::Values Kinematics::inverse<Slice>(
const Slice& slice, const Robot& robot, const gtsam::Values& goal_poses,
const gtsam::Values& joint_priors) const {
auto graph = this->graph(slice, robot);

// Add prior on joint angles to constrain the solution
// Add prior on joint angles to prefer solution close to our initial estimates
graph.add(this->jointAngleObjectives(slice, robot, joint_priors));

// Add priors on link poses with desired poses from argument
Expand All @@ -258,7 +260,7 @@ gtsam::Values Kinematics::inverseWithPose<Slice>(
// Robot kinematics constraints
auto constraints = this->constraints(slice, robot);

auto initial_values = this->initialValues(slice, robot, 0.1, joint_priors);
auto initial_values = this->initialValues(slice, robot, 0.1, joint_priors, true);

return this->optimize(graph, constraints, initial_values);
}
Expand Down
4 changes: 2 additions & 2 deletions gtdynamics/kinematics/KinematicsTrajectory.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -78,11 +78,11 @@ NonlinearFactorGraph Kinematics::jointAngleObjectives<Trajectory>(
template <>
Values Kinematics::initialValues<Trajectory>(
const Trajectory& trajectory, const Robot& robot, double gaussian_noise,
const gtsam::Values& initial_joints) const {
const gtsam::Values& initial_joints, bool use_fk) const {
Values values;
for (auto&& phase : trajectory.phases()) {
values.insert(
initialValues<Interval>(phase, robot, gaussian_noise, initial_joints));
initialValues<Interval>(phase, robot, gaussian_noise, initial_joints, use_fk));
}
return values;
}
Expand Down
16 changes: 8 additions & 8 deletions tests/testKinematicsSlice.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -94,7 +94,7 @@ gtsam::Values jointVectorToValues(const Robot& robot,
return joint_values;
}

TEST(Slice, initial_values) {
TEST(Slice, initialValues) {
// Load robot from urdf file
const Robot panda =
CreateRobotFromFile(kUrdfPath + std::string("panda/panda.urdf"));
Expand All @@ -108,7 +108,7 @@ TEST(Slice, initial_values) {
gtsam::Values initial_joints = jointVectorToValues(robot, initial);

gtsam::Values initial_values =
kinematics.initialValues(kSlice, robot, 0.0, initial_joints);
kinematics.initialValues(kSlice, robot, 0.0, initial_joints, true);

// We should only have 7 values for joints and 8 for link poses
EXPECT_LONGS_EQUAL(15, initial_values.size())
Expand Down Expand Up @@ -147,7 +147,7 @@ TEST(Slice, JointAngleObjectives) {
means_vector << 0, 0, 0, 0, 0, 0, 0;
gtsam::Values expected_means = jointVectorToValues(robot, means_vector);
gtsam::Values initial =
kinematics.initialValues(kSlice, robot, 0.0, expected_means);
kinematics.initialValues(kSlice, robot, 0.0, expected_means, true);
double tol = 1e-5;
EXPECT_DOUBLES_EQUAL(0.0, joint_priors.error(initial), tol)

Expand All @@ -161,13 +161,13 @@ TEST(Slice, JointAngleObjectives) {
EXPECT_LONGS_EQUAL(7, joint_priors.size())

// check that error at 0 is now not 0
initial = kinematics.initialValues(kSlice, robot, 0.0, expected_means);
initial = kinematics.initialValues(kSlice, robot, 0.0, expected_means, true);
EXPECT(tol < joint_priors.error(initial))

// Check that the evaluated error at the expected means is 0
means_vector << 1, 0, 1, 0, 1, 0, 1;
expected_means = jointVectorToValues(robot, means_vector);
initial = kinematics.initialValues(kSlice, robot, 0.0, expected_means);
initial = kinematics.initialValues(kSlice, robot, 0.0, expected_means, true);
EXPECT_DOUBLES_EQUAL(0.0, joint_priors.error(initial), tol)

// Define means of all joints different than 0
Expand All @@ -180,7 +180,7 @@ TEST(Slice, JointAngleObjectives) {
// Check that the evaluated error at the expected means is 0
means_vector << 1, 0.5, 1, -1, 1, 0.5, 1;
expected_means = jointVectorToValues(robot, means_vector);
initial = kinematics.initialValues(kSlice, robot, 0.0, expected_means);
initial = kinematics.initialValues(kSlice, robot, 0.0, expected_means, true);
EXPECT_DOUBLES_EQUAL(0.0, joint_priors.error(initial), tol)
}

Expand Down Expand Up @@ -241,7 +241,7 @@ TEST(Slice, PoseGoalObjectives) {
initial << 0.1, 0.2, 0.3, -0.4, 0.5, 0.6, 0.7;
gtsam::Values initial_joints = jointVectorToValues(robot, initial);
auto initial_values =
kinematics.initialValues(kSlice, robot, 0.0, initial_joints);
kinematics.initialValues(kSlice, robot, 0.0, initial_joints, true);
double tol = 1e-4;
GTSAM_PRINT(initial_values.at<gtsam::Pose3>(PoseKey(7, k)));
EXPECT(assert_equal(sT7, initial_values.at<gtsam::Pose3>(PoseKey(7, k)), tol))
Expand Down Expand Up @@ -299,7 +299,7 @@ TEST(Slice, PandaIK) {

// Call IK solver
auto values =
kinematics.inverseWithPose(kSlice, robot, goal_poses, initial_joints);
kinematics.inverse(kSlice, robot, goal_poses, initial_joints);

// Check that base link did not budge (much)
auto base_link = robot.link("link0");
Expand Down