Skip to content

Commit

Permalink
Merge pull request RobotLocomotion#3631 from sammy-tri/pendulum2_dyna…
Browse files Browse the repository at this point in the history
…mic_constraint

System2 pendulum dynamic constraint
  • Loading branch information
jwnimmer-tri authored Oct 3, 2016
2 parents 3a2cf52 + 0cad8ec commit 132167e
Show file tree
Hide file tree
Showing 9 changed files with 420 additions and 49 deletions.
18 changes: 14 additions & 4 deletions drake/examples/Pendulum/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -11,13 +11,23 @@ if(lcm_FOUND)
REQUIRES
VERSION 0.0.1)

add_library_with_exports(LIB_NAME drakePendulumSystem
SOURCE_FILES pendulum_system.cc)
target_link_libraries(drakePendulumSystem drakeSystemFramework)
pods_install_libraries(drakePendulumSystem)
drake_install_headers(pendulum_system.h)
pods_install_pkg_config_file(drake-pendulum-system
LIBS -ldrakePendulumSystem
REQUIRES
VERSION 0.0.1)

add_executable(pendulum_run_dynamics pendulum_run_dynamics.cc)
target_link_libraries(pendulum_run_dynamics
drakeRBM
drakeLCMSystem
threads)
drakePendulumSystem
drakeRigidBodyPlant
drakeSystemAnalysis)
drake_add_test(NAME pendulum_run_dynamics
COMMAND pendulum_run_dynamics --non-realtime)
COMMAND pendulum_run_dynamics)

add_executable(pendulum_run_energy_shaping pendulum_run_energy_shaping.cc)
target_link_libraries(pendulum_run_energy_shaping
Expand Down
81 changes: 44 additions & 37 deletions drake/examples/Pendulum/pendulum_run_dynamics.cc
Original file line number Diff line number Diff line change
@@ -1,44 +1,51 @@

#include <iostream>

#include "drake/common/drake_path.h"
#include "drake/common/polynomial.h"
#include "drake/examples/Pendulum/Pendulum.h"
#include "drake/systems/Simulation.h"
#include "drake/systems/LCMSystem.h"
#include "drake/systems/cascade_system.h"
#include "drake/systems/plants/BotVisualizer.h"
#include "drake/examples/Pendulum/pendulum_system.h"
#include "drake/systems/analysis/simulator.h"
#include "drake/systems/framework/diagram.h"
#include "drake/systems/framework/diagram_builder.h"
#include "drake/systems/framework/primitives/constant_vector_source.h"
#include "drake/systems/plants/joints/floating_base_types.h"
#include "drake/util/drakeAppUtil.h"
#include "drake/systems/plants/rigid_body_plant/rigid_body_tree_lcm_publisher.h"
#include "drake/systems/plants/RigidBodyTree.h"

namespace drake {
namespace examples {
namespace pendulum {
namespace {

int do_main(int argc, char* argv[]) {
lcm::LCM lcm;
RigidBodyTree tree(GetDrakePath() + "/examples/Pendulum/Pendulum.urdf",
systems::plants::joints::kFixed);
Eigen::VectorXd tau = Eigen::VectorXd::Zero(1);

systems::DiagramBuilder<double> builder;
auto source = builder.AddSystem<systems::ConstantVectorSource>(tau);
auto pendulum = builder.AddSystem<PendulumSystem>();
auto publisher =
builder.AddSystem<systems::RigidBodyTreeLcmPublisher>(tree, &lcm);
builder.Connect(source->get_output_port(), pendulum->get_tau_port());
builder.Connect(pendulum->get_output_port(), publisher->get_input_port(0));
auto diagram = builder.Build();

systems::Simulator<double> simulator(*diagram);
systems::Context<double>* pendulum_context =
diagram->GetMutableSubsystemContext(
simulator.get_mutable_context(), pendulum);
pendulum->set_theta(pendulum_context, 1.);
pendulum->set_thetadot(pendulum_context, 0.);

simulator.Initialize();
simulator.StepTo(10);
return 0;
}

using namespace std;
using namespace drake;
} // namespace
} // namespace pendulum
} // namespace examples
} // namespace drake

int main(int argc, char* argv[]) {
shared_ptr<lcm::LCM> lcm = make_shared<lcm::LCM>();
if (!lcm->good()) return 1;

auto p = make_shared<Pendulum>();
auto v = make_shared<BotVisualizer<PendulumState> >(
lcm, GetDrakePath() + "/examples/Pendulum/Pendulum.urdf",
drake::systems::plants::joints::kFixed);

PendulumState<double> x0;
x0.theta = 1;
x0.thetadot = 0;
// cout << "PendulumState::x0 = " << x0 << endl;

auto sys = cascade(p, v);

cout << "coords:" << getCoordinateName(x0, 0) << ", "
<< getCoordinateName(x0, 1) << endl;

SimulationOptions options;
options.realtime_factor = 1.0;
if (commandLineOptionExists(argv, argv + argc, "--non-realtime")) {
options.warn_real_time_violation = true;
}

// simulate(*sys, 0, 10, x0, options);
runLCM(sys, lcm, 0, 10, x0, options);
return drake::examples::pendulum::do_main(argc, argv);
}
129 changes: 129 additions & 0 deletions drake/examples/Pendulum/pendulum_system.cc
Original file line number Diff line number Diff line change
@@ -0,0 +1,129 @@
#include "drake/examples/Pendulum/pendulum_system.h"

#include "drake/common/eigen_autodiff_types.h"
#include "drake/common/drake_throw.h"

namespace drake {
namespace examples {
namespace pendulum {

namespace {
constexpr int kStateSize = 2; // position, velocity
}

template <typename T>
PendulumStateVector<T>::PendulumStateVector(
const T& initial_theta, const T& initial_thetadot)
: systems::BasicVector<T>(kStateSize) {
set_theta(initial_theta);
set_thetadot(initial_thetadot);
}

template <typename T>
PendulumStateVector<T>::PendulumStateVector()
: PendulumStateVector(0., 0.) {}

template <typename T>
PendulumStateVector<T>::~PendulumStateVector() {}

template <typename T>
T PendulumStateVector<T>::get_theta() const {
return this->GetAtIndex(0);
}

template <typename T>
void PendulumStateVector<T>::set_theta(const T& theta) {
this->SetAtIndex(0, theta);
}

template <typename T>
T PendulumStateVector<T>::get_thetadot() const {
return this->GetAtIndex(1);
}

template <typename T>
void PendulumStateVector<T>::set_thetadot(const T& thetadot) {
this->SetAtIndex(1, thetadot);
}

template <typename T>
PendulumStateVector<T>* PendulumStateVector<T>::DoClone() const {
return new PendulumStateVector<T>(get_theta(), get_thetadot());
}

template class DRAKEPENDULUMSYSTEM_EXPORT PendulumStateVector<double>;
template class DRAKEPENDULUMSYSTEM_EXPORT PendulumStateVector<AutoDiffXd>;

template <typename T>
PendulumSystem<T>::PendulumSystem() {
this->DeclareInputPort(
systems::kVectorValued, 1, systems::kContinuousSampling);
this->DeclareOutputPort(
systems::kVectorValued, kStateSize, systems::kContinuousSampling);
}

template <typename T>
PendulumSystem<T>::~PendulumSystem() {}

template <typename T>
const systems::SystemPortDescriptor<T>&
PendulumSystem<T>::get_tau_port() const {
return this->get_input_port(0);
}

template <typename T>
const systems::SystemPortDescriptor<T>&
PendulumSystem<T>::get_output_port() const {
return systems::System<T>::get_output_port(0);
}

template <typename T>
std::unique_ptr<systems::BasicVector<T>>
PendulumSystem<T>::AllocateOutputVector(
const systems::SystemPortDescriptor<T>& descriptor) const {
DRAKE_THROW_UNLESS(descriptor.get_size() == kStateSize);
return std::make_unique<PendulumStateVector<T>>();
}

template <typename T>
std::unique_ptr<systems::ContinuousState<T>>
PendulumSystem<T>::AllocateContinuousState() const {
return std::make_unique<systems::ContinuousState<T>>(
std::make_unique<PendulumStateVector<T>>(),
1 /* num_q */, 1 /* num_v */, 0 /* num_z */);
static_assert(kStateSize == 1 + 1, "State size has changed");
}

template <typename T>
void PendulumSystem<T>::EvalOutput(const systems::Context<T>& context,
systems::SystemOutput<T>* output) const {
get_mutable_output(output)->set_value(get_state(context).get_value());
}

// Compute the actual physics.
template <typename T>
void PendulumSystem<T>::EvalTimeDerivatives(
const systems::Context<T>& context,
systems::ContinuousState<T>* derivatives) const {
DRAKE_ASSERT_VOID(systems::System<T>::CheckValidContext(context));

const PendulumStateVector<T>& state = get_state(context);
PendulumStateVector<T>* derivative_vector = get_mutable_state(derivatives);

derivative_vector->set_theta(state.get_thetadot());
// Pendulum formula from Section 2.2 of Russ Tedrake. Underactuated
// Robotics: Algorithms for Walking, Running, Swimming, Flying, and
// Manipulation (Course Notes for MIT 6.832). Downloaded on
// 2016-09-30 from
// http://underactuated.csail.mit.edu/underactuated.html?chapter=2
derivative_vector->set_thetadot(
(get_tau(context) - m_ * g_ * lc_ * sin(state.get_theta()) -
b_ * state.get_thetadot()) / I_);
}

template class DRAKEPENDULUMSYSTEM_EXPORT PendulumSystem<double>;
template class DRAKEPENDULUMSYSTEM_EXPORT PendulumSystem<AutoDiffXd>;

} // namespace pendulum
} // namespace examples
} // namespace drake
136 changes: 136 additions & 0 deletions drake/examples/Pendulum/pendulum_system.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,136 @@
#pragma once

#include <memory>

#include "drake/drakePendulumSystem_export.h"
#include "drake/systems/framework/basic_vector.h"
#include "drake/systems/framework/leaf_system.h"

namespace drake {
namespace examples {
namespace pendulum {

/// The state of a one-dimensional pendulum system.
///
/// @tparam T The vector element type, which must be a valid Eigen scalar.
///
/// Instantiated templates for the following kinds of T's are provided:
/// - double
/// - AutoDiffXd
template <typename T>
class PendulumStateVector : public systems::BasicVector<T> {
public:
PendulumStateVector(const T& initial_theta, const T& initial_thetadot);
PendulumStateVector();
~PendulumStateVector() override;

T get_theta() const;
void set_theta(const T& theta);
T get_thetadot() const;
void set_thetadot(const T& thetadot);

explicit PendulumStateVector(const PendulumStateVector& other) = delete;
PendulumStateVector& operator=(const PendulumStateVector& other) = delete;
explicit PendulumStateVector(PendulumStateVector&& other) = delete;
PendulumStateVector& operator=(PendulumStateVector&& other) = delete;

private:
PendulumStateVector<T>* DoClone() const override;
};

/// A model of a one-dimensional pendulum system, similar to the one
/// described in Chapter 2 of Russ Tedrake. Underactuated Robotics:
/// Algorithms for Walking, Running, Swimming, Flying, and
/// Manipulation (Course Notes for MIT 6.832). Downloaded on
/// 2016-09-30 from
/// http://underactuated.csail.mit.edu/underactuated.html?chapter=2
///
/// @tparam T The vector element type, which must be a valid Eigen scalar.
///
/// Instantiated templates for the following kinds of T's are provided:
/// - double
/// - AutoDiffXd
template <typename T>
class PendulumSystem : public systems::LeafSystem<T> {
public:
PendulumSystem();
~PendulumSystem() override;

using MyContext = systems::Context<T>;
using MyContinuousState = systems::ContinuousState<T>;
using MyOutput = systems::SystemOutput<T>;

/// The input force to this system is not direct feedthrough.
bool has_any_direct_feedthrough() const override { return false; }

/// Returns the input port to the externally applied force.
const systems::SystemPortDescriptor<T>& get_tau_port() const;

/// Returns the port to output state.
const systems::SystemPortDescriptor<T>& get_output_port() const;

void set_theta(MyContext* context, const T& theta) const {
get_mutable_state(context)->set_theta(theta);
}

void set_thetadot(MyContext* context, const T& thetadot) const {
get_mutable_state(context)->set_thetadot(thetadot);
}

void EvalOutput(const MyContext& context, MyOutput* output) const override;

void EvalTimeDerivatives(const MyContext& context,
MyContinuousState* derivatives) const override;

explicit PendulumSystem(const PendulumSystem& other) = delete;
PendulumSystem& operator=(const PendulumSystem& other) = delete;
explicit PendulumSystem(PendulumSystem&& other) = delete;
PendulumSystem& operator=(PendulumSystem&& other) = delete;

protected:
// LeafSystem<T> override.
std::unique_ptr<MyContinuousState>
AllocateContinuousState() const override;

std::unique_ptr<systems::BasicVector<T>> AllocateOutputVector(
const systems::SystemPortDescriptor<T>& descriptor) const override;

private:
T get_tau(const MyContext& context) const {
return this->EvalVectorInput(context, 0)->GetAtIndex(0);
}

static const PendulumStateVector<T>& get_state(
const MyContinuousState& cstate) {
return dynamic_cast<const PendulumStateVector<T>&>(cstate.get_state());
}

static PendulumStateVector<T>* get_mutable_state(
MyContinuousState* cstate) {
return dynamic_cast<PendulumStateVector<T>*>(cstate->get_mutable_state());
}

static PendulumStateVector<T>* get_mutable_output(MyOutput* output) {
return dynamic_cast<PendulumStateVector<T>*>(
output->GetMutableVectorData(0));
}

static const PendulumStateVector<T>& get_state(const MyContext& context) {
return get_state(*context.get_continuous_state());
}

static PendulumStateVector<T>* get_mutable_state(MyContext* context) {
return get_mutable_state(context->get_mutable_continuous_state());
}

const double m_{1.0}; // kg
const double l_{.5}; // m
const double b_{0.1}; // kg m^2 /s
const double lc_{.5}; // m
const double I_{.25}; // m*l^2; % kg*m^2
const double g_{9.81}; // m/s^2
};

} // namespace pendulum
} // namespace examples
} // namespace drake
2 changes: 1 addition & 1 deletion drake/examples/Pendulum/test/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@ if(lcm_FOUND)

drake_add_cc_test(pendulum_dynamic_constraint_test)
target_link_libraries(pendulum_dynamic_constraint_test
drakeDynamicConstraint drakeLCMSystem)
drakePendulumSystem drakeDynamicConstraint)

add_executable(pendulum_trajectory_optimization_test
pendulum_trajectory_optimization_test.cc)
Expand Down
Loading

0 comments on commit 132167e

Please sign in to comment.