Skip to content

Commit

Permalink
Merge pull request #389 from borglab/default-gravity
Browse files Browse the repository at this point in the history
  • Loading branch information
@varunagrawal
varunagrawal authored Oct 27, 2023
2 parents 028ed5a + b141b1f commit 6544947
Show file tree
Hide file tree
Showing 6 changed files with 35 additions and 50 deletions.
11 changes: 4 additions & 7 deletions gtdynamics.i
View file
Original file line number Diff line number Diff line change
Expand Up @@ -390,11 +390,11 @@ enum CollocationScheme { Euler, RungeKutta, Trapezoidal, HermiteSimpson };

class DynamicsGraph {
DynamicsGraph();
DynamicsGraph(const std::optional<gtsam::Vector3> &gravity,
DynamicsGraph(const gtsam::Vector3 &gravity = gtsam::Vector3(0, 0, 9.81),
const std::optional<gtsam::Vector3> &planar_axis);
DynamicsGraph(const gtdynamics::OptimizerSetting &opt);
DynamicsGraph(const gtdynamics::OptimizerSetting &opt,
const std::optional<gtsam::Vector3> &gravity,
const gtsam::Vector3 &gravity = gtsam::Vector3(0, 0, 9.81),
const std::optional<gtsam::Vector3> &planar_axis);

gtsam::GaussianFactorGraph linearDynamicsGraph(
Expand Down Expand Up @@ -745,12 +745,9 @@ gtsam::Vector6 Wrench(const gtsam::Values &values, int i, int j, int t=0);
#include <gtdynamics/dynamics/Simulator.h>

class Simulator {
Simulator(const gtdynamics::Robot &robot, const gtsam::Values &initial_values);
Simulator(const gtdynamics::Robot &robot, const gtsam::Values &initial_values,
const std::optional<gtsam::Vector3> &gravity);
Simulator(const gtdynamics::Robot &robot, const gtsam::Values &initial_values,
const gtsam::Vector3 &gravity,
const gtsam::Vector3 &planar_axis);
const gtsam::Vector3 &gravity = gtsam::Vector3(0, 0, 9.81),
const std::optional<gtsam::Vector3> &planar_axis);

void reset(const double t);
void forwardDynamics(const gtsam::Values &torques);
Expand Down
35 changes: 11 additions & 24 deletions gtdynamics/dynamics/DynamicsGraph.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,13 +69,13 @@ GaussianFactorGraph DynamicsGraph::linearDynamicsGraph(
const Pose3 T_wi = Pose(known_values, i, t);
const Vector6 V_i = Twist(known_values, i, t);
Vector6 rhs = Pose3::adjointMap(V_i).transpose() * G_i * V_i;
if (gravity_) {
Vector gravitational_force =
T_wi.rotation().transpose() * (*gravity_) * m_i;
for (int i = 3; i < 6; ++i) {
rhs[i] += gravitational_force[i - 3];
}

// Compute gravitational forces. If gravity=(0, 0, 0), this will be zeros.
Vector gravitational_force = T_wi.rotation().transpose() * gravity_ * m_i;
for (int i = 3; i < 6; ++i) {
rhs[i] += gravitational_force[i - 3];
}

auto accel_key = TwistAccelKey(i, t);
if (connected_joints.size() == 0) {
graph.add(accel_key, G_i, rhs, all_constrained);
Expand Down Expand Up @@ -207,18 +207,11 @@ gtsam::NonlinearFactorGraph DynamicsGraph::qFactors(
JointAngleKey(joint->id(), k), opt_.p_cost_model, joint));
}

// TODO(frank): whoever write this should clean up this mess.
gtsam::Vector3 gravity;
if (gravity_)
gravity = *gravity_;
else
gravity = gtsam::Vector3(0, 0, -9.8);

// Add contact factors.
if (contact_points) {
for (auto &&cp : *contact_points) {
ContactHeightFactor contact_pose_factor(
PoseKey(cp.link->id(), k), opt_.cp_cost_model, cp.point, gravity);
PoseKey(cp.link->id(), k), opt_.cp_cost_model, cp.point, gravity_);
graph.add(contact_pose_factor);
}
}
Expand Down Expand Up @@ -282,13 +275,6 @@ gtsam::NonlinearFactorGraph DynamicsGraph::dynamicsFactors(
const std::optional<double> &mu) const {
NonlinearFactorGraph graph;

// TODO(frank): whoever write this should clean up this mess.
gtsam::Vector3 gravity;
if (gravity_)
gravity = *gravity_;
else
gravity = gtsam::Vector3(0, 0, -9.8);

double mu_; // Static friction coefficient.
if (mu)
mu_ = *mu;
Expand Down Expand Up @@ -316,7 +302,7 @@ gtsam::NonlinearFactorGraph DynamicsGraph::dynamicsFactors(
// Add contact dynamics constraints.
graph.emplace_shared<ContactDynamicsFrictionConeFactor>(
PoseKey(i, k), wrench_key, opt_.cfriction_cost_model, mu_,
gravity);
gravity_);

graph.emplace_shared<ContactDynamicsMomentFactor>(
wrench_key, opt_.cm_cost_model,
Expand All @@ -326,7 +312,7 @@ gtsam::NonlinearFactorGraph DynamicsGraph::dynamicsFactors(

// add wrench factor for link
graph.add(
WrenchFactor(opt_.fa_cost_model, link, wrench_keys, k, gravity));
WrenchFactor(opt_.fa_cost_model, link, wrench_keys, k, gravity_));
}
}

Expand All @@ -337,9 +323,10 @@ gtsam::NonlinearFactorGraph DynamicsGraph::dynamicsFactors(
auto const_joint = joint;
graph.add(WrenchEquivalenceFactor(opt_.f_cost_model, const_joint, k));
graph.add(TorqueFactor(opt_.t_cost_model, const_joint, k));
if (planar_axis_)
if (planar_axis_) {
graph.add(WrenchPlanarFactor(opt_.planar_cost_model, *planar_axis_,
const_joint, k));
}
}
return graph;
}
Expand Down
7 changes: 4 additions & 3 deletions gtdynamics/dynamics/DynamicsGraph.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,15 +40,16 @@ enum CollocationScheme { Euler, RungeKutta, Trapezoidal, HermiteSimpson };
class DynamicsGraph {
private:
OptimizerSetting opt_;
std::optional<gtsam::Vector3> gravity_, planar_axis_;
const gtsam::Vector3 gravity_;
std::optional<gtsam::Vector3> planar_axis_;

public:
/**
* Constructor
* @param gravity gravity in world frame
* @param planar_axis axis of the plane, used only for planar robot
*/
DynamicsGraph(const std::optional<gtsam::Vector3> &gravity = {},
DynamicsGraph(const gtsam::Vector3 &gravity = gtsam::Vector3(0, 0, -9.81),
const std::optional<gtsam::Vector3> &planar_axis = {})
: opt_(OptimizerSetting()),
gravity_(gravity),
Expand All @@ -61,7 +62,7 @@ class DynamicsGraph {
* @param planar_axis axis of the plane, used only for planar robot
*/
DynamicsGraph(const OptimizerSetting &opt,
const std::optional<gtsam::Vector3> &gravity = {},
const gtsam::Vector3 &gravity = gtsam::Vector3(0, 0, -9.8),
const std::optional<gtsam::Vector3> &planar_axis = {})
: opt_(opt), gravity_(gravity), planar_axis_(planar_axis) {}

Expand Down
4 changes: 1 addition & 3 deletions gtdynamics/dynamics/Simulator.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,8 +33,6 @@ class Simulator {
int t_;
DynamicsGraph graph_builder_;
gtsam::Values initial_values_;
std::optional<gtsam::Vector3> gravity_;
std::optional<gtsam::Vector3> planar_axis_;
gtsam::Values current_values_;
gtsam::Values new_kinematics_;

Expand All @@ -49,7 +47,7 @@ class Simulator {
* @param planar_axis planar axis vector
*/
Simulator(const Robot &robot, const gtsam::Values &initial_values,
const std::optional<gtsam::Vector3> &gravity = {},
const gtsam::Vector3 &gravity = gtsam::Vector3(0, 0, 9.81),
const std::optional<gtsam::Vector3> &planar_axis = {})
: robot_(robot),
t_(0),
Expand Down
2 changes: 1 addition & 1 deletion gtdynamics/statics/Statics.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,7 +62,7 @@ Vector6 ResultantWrench(const std::vector<Vector6> &wrenches, double mass,
// Calculate resultant wrench, fills up H with identity matrices if asked.
const Vector6 external_wrench = ResultantWrench(wrenches, H);

// Potentiall add gravity wrench.
// Potentially add gravity wrench.
if (gravity) {
gtsam::Matrix6 H_wTcom;
auto gravity_wrench =
Expand Down
26 changes: 14 additions & 12 deletions gtdynamics/universal_robot/RobotModels.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,8 +28,8 @@ gtdynamics::Robot getRobot() {
kSdfPath + std::string("test/four_bar_linkage_pure.sdf"));
return four_bar;
}
gtsam::Vector3 gravity = (gtsam::Vector(3) << 0, 0, 0).finished();
gtsam::Vector3 planar_axis = (gtsam::Vector(3) << 1, 0, 0).finished();
gtsam::Vector3 gravity = gtsam::Vector3::Zero();
gtsam::Vector3 planar_axis(1, 0, 0);

} // namespace four_bar_linkage_pure

Expand All @@ -40,8 +40,8 @@ gtdynamics::Robot getRobot() {
robot = robot.fixLink("l1");
return robot;
}
gtsam::Vector3 gravity = (gtsam::Vector(3) << 0, 0, 0).finished();
gtsam::Vector3 planar_axis = (gtsam::Vector(3) << 1, 0, 0).finished();
gtsam::Vector3 gravity = gtsam::Vector3::Zero();
gtsam::Vector3 planar_axis(1, 0, 0);

} // namespace simple_urdf

Expand All @@ -61,8 +61,8 @@ gtdynamics::Robot getRobot() {
robot = robot.fixLink("l1");
return robot;
}
gtsam::Vector3 gravity = (gtsam::Vector(3) << 0, 0, 0).finished();
gtsam::Vector3 planar_axis = (gtsam::Vector(3) << 1, 0, 0).finished();
gtsam::Vector3 gravity = gtsam::Vector3::Zero();
gtsam::Vector3 planar_axis(1, 0, 0);

} // namespace simple_urdf_zero_inertia

Expand All @@ -72,8 +72,8 @@ gtdynamics::Robot getRobot() {
kUrdfPath + std::string("test/simple_urdf_eq_mass.urdf"));
return robot;
}
gtsam::Vector3 gravity = (gtsam::Vector(3) << 0, 0, 0).finished();
gtsam::Vector3 planar_axis = (gtsam::Vector(3) << 1, 0, 0).finished();
gtsam::Vector3 gravity = gtsam::Vector3::Zero();
gtsam::Vector3 planar_axis(1, 0, 0);

} // namespace simple_urdf_eq_mass

Expand All @@ -83,8 +83,9 @@ gtdynamics::Robot getRobot() {
kSdfPath + std::string("test/simple_rr.sdf"), "simple_rr_sdf");
return robot;
}
gtsam::Vector3 gravity = (gtsam::Vector(3) << 0, 0, 0).finished();
gtsam::Vector3 planar_axis = (gtsam::Vector(3) << 1, 0, 0).finished();

gtsam::Vector3 gravity = gtsam::Vector3::Zero();
gtsam::Vector3 planar_axis(1, 0, 0);

} // namespace simple_rr

Expand All @@ -95,7 +96,8 @@ gtdynamics::Robot getRobot() {
jumping_robot = jumping_robot.fixLink("l0");
return jumping_robot;
}
gtsam::Vector3 gravity = (gtsam::Vector(3) << 0, 0, -9.8).finished();
gtsam::Vector3 planar_axis = (gtsam::Vector(3) << 1, 0, 0).finished();

gtsam::Vector3 gravity(0, 0, -9.8);
gtsam::Vector3 planar_axis(1, 0, 0);

} // namespace jumping_robot

0 comments on commit 6544947

Please sign in to comment.