forked from RobotLocomotion/drake
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request RobotLocomotion#3631 from sammy-tri/pendulum2_dyna…
…mic_constraint System2 pendulum dynamic constraint
- Loading branch information
Showing
9 changed files
with
420 additions
and
49 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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); | ||
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.