Skip to content

Commit

Permalink
got rid of raw pointers to RigidBodySystem
Browse files Browse the repository at this point in the history
  • Loading branch information
@psiorx
psiorx committed Mar 6, 2016
1 parent aa0a6a6 commit 73c4d73
Show file tree
Hide file tree
Showing 12 changed files with 160 additions and 140 deletions.
23 changes: 12 additions & 11 deletions drake/examples/Acrobot/test/urdfDynamicsTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,10 +9,11 @@ using namespace Drake;

int main(int argc, char* argv[]) {
auto r = Acrobot();
auto r_urdf = RigidBodySystem(
getDrakePath() + "/examples/Acrobot/Acrobot.urdf", DrakeJoint::FIXED);
auto r_sdf = RigidBodySystem(getDrakePath() + "/examples/Acrobot/Acrobot.sdf",
DrakeJoint::FIXED);

auto r_urdf = make_shared<RigidBodySystem>();
r_urdf->addRobotFromFile(getDrakePath() + "/examples/Acrobot/Acrobot.urdf", DrakeJoint::FIXED);
auto r_sdf = make_shared<RigidBodySystem>();
r_sdf->addRobotFromFile(getDrakePath() + "/examples/Acrobot/Acrobot.sdf", DrakeJoint::FIXED);

// for debugging:
/*
Expand Down Expand Up @@ -45,13 +46,13 @@ int main(int argc, char* argv[]) {
r_sdf.getRigidBodyTree()->dynamicsBiasTerm(kinsol_sdf,f_ext) << endl;
*/

auto xdot = toEigen(r.dynamics(0.0, x0, u0));
auto xdot_urdf = r_urdf.dynamics(0.0, x0_rb, u0_rb);
// cout << "xdot = " << xdot.transpose() << endl;
// cout << "xdot_rb = " << xdot_rb.transpose() << endl;
valuecheckMatrix(xdot_urdf, xdot, 1e-8);
auto xdot = toEigen(r.dynamics(0.0,x0,u0));
auto xdot_urdf = r_urdf->dynamics(0.0, x0_rb, u0_rb);
// cout << "xdot = " << xdot.transpose() << endl;
// cout << "xdot_rb = " << xdot_rb.transpose() << endl;
valuecheckMatrix(xdot_urdf,xdot,1e-8);

auto xdot_sdf = r_sdf.dynamics(0.0, x0_rb, u0_rb);
valuecheckMatrix(xdot_sdf, xdot, 1e-8);
auto xdot_sdf = r_sdf->dynamics(0.0, x0_rb, u0_rb);
valuecheckMatrix(xdot_sdf,xdot,1e-8);
}
}
11 changes: 6 additions & 5 deletions drake/examples/Cars/simulateLCM.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,11 +78,12 @@ int main(int argc, char* argv[]) {
// be reused
DrakeJoint::FloatingBaseType floating_base_type = DrakeJoint::QUATERNION;

auto rigid_body_sys =
make_shared<RigidBodySystem>(argv[1], floating_base_type);
auto const& tree = rigid_body_sys->getRigidBodyTree();
for (int i = 2; i < argc; i++)
tree->addRobotFromSDF(argv[i], DrakeJoint::FIXED); // add environment

auto rigid_body_sys = make_shared<RigidBodySystem>();
rigid_body_sys->addRobotFromFile(argv[1],floating_base_type);
auto const & tree = rigid_body_sys->getRigidBodyTree();
for (int i=2; i<argc; i++)
tree->addRobotFromSDF(argv[i],DrakeJoint::FIXED); // add environment

if (argc < 3) { // add flat terrain
double box_width = 1000;
Expand Down
22 changes: 10 additions & 12 deletions drake/examples/Pendulum/test/urdfDynamicsTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,13 +7,11 @@ using namespace std;
using namespace Eigen;
using namespace Drake;

int main(int argc, char* argv[]) {
// auto tree =
// make_shared<RigidBodyTree>(getDrakePath()+"/examples/Pendulum/Pendulum.urdf",DrakeJoint::FIXED);
// // Segfaults on MSVC win32
auto tree = shared_ptr<RigidBodyTree>(new RigidBodyTree(
getDrakePath() + "/examples/Pendulum/Pendulum.urdf", DrakeJoint::FIXED));
auto rbsys = RigidBodySystem(tree);

int main(int argc, char* argv[])
{
auto tree = make_shared<RigidBodyTree>(getDrakePath()+"/examples/Pendulum/Pendulum.urdf",DrakeJoint::FIXED);
auto rbsys = make_shared<RigidBodySystem>(tree);
auto p = Pendulum();

for (int i = 0; i < 1000; i++) {
Expand All @@ -23,10 +21,10 @@ int main(int argc, char* argv[]) {
RigidBodySystem::StateVector<double> x0_rb = toEigen(x0);
RigidBodySystem::InputVector<double> u0_rb = toEigen(u0);

auto xdot = toEigen(p.dynamics(0.0, x0, u0));
auto xdot_rb = rbsys.dynamics(0.0, x0_rb, u0_rb);
// cout << "xdot = " << xdot.transpose() << endl;
// cout << "xdot_rb = " << xdot_rb.transpose() << endl;
valuecheckMatrix(xdot_rb, xdot, 1e-8);


auto xdot = toEigen(p.dynamics(0.0,x0,u0));
auto xdot_rb = rbsys->dynamics(0.0,x0_rb,u0_rb);
valuecheckMatrix(xdot_rb,xdot,1e-8);
}
}
4 changes: 3 additions & 1 deletion drake/examples/Quadrotor/QuadrotorControl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,9 @@ class QuadrotorControl {
typedef drake::lcmt_quadrotor_control_t LCMMessageType;
static std::string channel() { return "QUAD_CONTROL"; };

QuadrotorControl(void) : motors(Eigen::Vector4d::Zero()) {}
QuadrotorControl(void) : motors(Eigen::Vector4d::Zero()) {
motors << 1.226250, 1.226250, 1.226250, 1.226250;
}

template <typename Derived>
QuadrotorControl(const Eigen::MatrixBase<Derived>& x) : motors(x) {};
Expand Down
11 changes: 7 additions & 4 deletions drake/examples/Quadrotor/runDynamics.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,19 +18,21 @@ int main(int argc, char* argv[]) {
return 1;

DrakeJoint::FloatingBaseType floating_base_type = DrakeJoint::QUATERNION;
auto rigid_body_sys = make_shared<RigidBodySystem>(getDrakePath()+"/examples/Quadrotor/quadrotor.urdf",floating_base_type);
auto rigid_body_sys = make_shared<RigidBodySystem>();
auto const & tree = rigid_body_sys->getRigidBodyTree();
rigid_body_sys->addRobotFromFile(getDrakePath()+"/examples/Quadrotor/quadrotor.urdf", floating_base_type);

auto sensor_frame = tree->frames[0]; //this is a propellor frame. probably want a body frame for this

auto accelerometer = make_shared<RigidBodyAccelerometer>(rigid_body_sys.get(), "accelerometer", sensor_frame);
auto gyroscope = make_shared<RigidBodyGyroscope>(rigid_body_sys.get(), "gyroscope", sensor_frame);
auto accelerometer = make_shared<RigidBodyAccelerometer>(rigid_body_sys, "accelerometer", sensor_frame);
auto gyroscope = make_shared<RigidBodyGyroscope>(rigid_body_sys, "gyroscope", sensor_frame);
auto noise_model = make_shared<GaussianNoiseModel<double, 3>>(0, 0.01);
accelerometer->setNoiseModel(noise_model);
gyroscope->setNoiseModel(noise_model);
rigid_body_sys->addSensor(accelerometer);
rigid_body_sys->addSensor(gyroscope);


double box_width = 1000;
double box_depth = 10;
DrakeShapes::Box geom(Vector3d(box_width, box_width, box_depth));
Expand All @@ -41,7 +43,8 @@ int main(int argc, char* argv[]) {
world->addVisualElement(DrakeShapes::VisualElement(geom,T_element_to_link,color));
tree->addCollisionElement(RigidBody::CollisionElement(geom,T_element_to_link,world),world,"terrain");
tree->updateStaticCollisionElements();



auto visualizer = make_shared<BotVisualizer<RigidBodySystem::StateVector>>(lcm,tree);

auto quad_control_to_rbsys_input = make_shared<Gain<QuadrotorControl, RigidBodySystem::InputVector>>(Eigen::Matrix4d::Identity());
Expand Down
17 changes: 8 additions & 9 deletions drake/examples/Quadrotor/test/urdfDynamicsTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,10 +7,11 @@ using namespace std;
using namespace Eigen;
using namespace Drake;

int main(int argc, char* argv[]) {
auto rbsys =
RigidBodySystem(getDrakePath() + "/examples/Quadrotor/quadrotor.urdf",
DrakeJoint::ROLLPITCHYAW);
int main(int argc, char* argv[])
{
auto rbsys = make_shared<RigidBodySystem>();
rbsys->addRobotFromFile(getDrakePath()+"/examples/Quadrotor/quadrotor.urdf",DrakeJoint::ROLLPITCHYAW);

auto p = Quadrotor();

for (int i = 0; i < 1000; i++) {
Expand All @@ -20,10 +21,8 @@ int main(int argc, char* argv[]) {
RigidBodySystem::StateVector<double> x0_rb = toEigen(x0);
RigidBodySystem::InputVector<double> u0_rb = toEigen(u0);

auto xdot = toEigen(p.dynamics(0.0, x0, u0));
auto xdot_rb = rbsys.dynamics(0.0, x0_rb, u0_rb);
// cout << "xdot = " << xdot.transpose() << endl;
// cout << "xdot_rb = " << xdot_rb.transpose() << endl;
valuecheckMatrix(xdot_rb, xdot, 1e-8);
auto xdot = toEigen(p.dynamics(0.0,x0,u0));
auto xdot_rb = rbsys->dynamics(0.0,x0_rb,u0_rb);
valuecheckMatrix(xdot_rb,xdot,1e-8);
}
}
66 changes: 35 additions & 31 deletions drake/systems/plants/RigidBodySystem.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -294,13 +294,14 @@ Drake::getInitialState(const RigidBodySystem& sys) {
return x0;
}

RigidBodyPropellor::RigidBodyPropellor(RigidBodySystem* sys, XMLElement* node,
RigidBodyPropellor::RigidBodyPropellor(std::shared_ptr<RigidBodySystem> sys, XMLElement* node,
const std::string& name)
: RigidBodyForceElement(sys, name),
scale_factor_thrust(1.0),
scale_factor_moment(1.0),
lower_limit(-numeric_limits<double>::infinity()),
upper_limit(numeric_limits<double>::infinity()) {

auto tree = sys->getRigidBodyTree();

XMLElement* parent_node = node->FirstChildElement("parent");
Expand Down Expand Up @@ -328,7 +329,7 @@ RigidBodyPropellor::RigidBodyPropellor(RigidBodySystem* sys, XMLElement* node,
// todo: parse visual info?
}

RigidBodySpringDamper::RigidBodySpringDamper(RigidBodySystem* sys,
RigidBodySpringDamper::RigidBodySpringDamper(std::shared_ptr<RigidBodySystem> sys,
XMLElement* node,
const std::string& name)
: RigidBodyForceElement(sys, name),
Expand Down Expand Up @@ -360,17 +361,18 @@ RigidBodySpringDamper::RigidBodySpringDamper(RigidBodySystem* sys,
tree->addFrame(frameB);
}

RigidBodyAccelerometer::RigidBodyAccelerometer(RigidBodySystem *sys,
RigidBodyAccelerometer::RigidBodyAccelerometer(std::shared_ptr<RigidBodySystem> sys,
const std::string& name,
const std::shared_ptr<RigidBodyFrame> frame)
: RigidBodySensor(sys,name), frame(frame) { }


Eigen::VectorXd RigidBodyAccelerometer::output(const double &t,
const KinematicsCache<double> &rigid_body_state,
const RigidBodySystem::InputVector<double>& u) const
{
auto q = rigid_body_state.getQ();
auto v = rigid_body_state.getV();
auto const& q = rigid_body_state.getQ();
auto const& v = rigid_body_state.getV();
VectorXd x(q.size() + v.size());
x << q, v;
auto xdd = sys->dynamics(t, x, u);
Expand All @@ -388,7 +390,7 @@ Eigen::VectorXd RigidBodyAccelerometer::output(const double &t,
return accel_body;
}

RigidBodyGyroscope::RigidBodyGyroscope(RigidBodySystem *sys, const std::string& name, const std::shared_ptr<RigidBodyFrame> frame)
RigidBodyGyroscope::RigidBodyGyroscope(const std::shared_ptr<RigidBodySystem> sys, const std::string& name, const std::shared_ptr<RigidBodyFrame> frame)
: RigidBodySensor(sys,name), frame(frame)
{

Expand All @@ -408,7 +410,7 @@ Eigen::VectorXd RigidBodyGyroscope::output(const double &t,
}

RigidBodyDepthSensor::RigidBodyDepthSensor(
RigidBodySystem* sys, const std::string& name,
std::shared_ptr<RigidBodySystem> sys, const std::string& name,
const std::shared_ptr<RigidBodyFrame> frame, tinyxml2::XMLElement* node)
: RigidBodySensor(sys, name),
frame(frame),
Expand Down Expand Up @@ -484,8 +486,7 @@ RigidBodyDepthSensor::RigidBodyDepthSensor(

Eigen::VectorXd RigidBodyDepthSensor::output(const double &t,
const KinematicsCache<double> &rigid_body_state,
const RigidBodySystem::InputVector<double>& u) const
{
const RigidBodySystem::InputVector<double>& u) const {
VectorXd distances(num_pixel_cols*num_pixel_rows);
Vector3d origin = sys->getRigidBodyTree()->transformPoints(rigid_body_state,
Vector3d::Zero(),
Expand All @@ -498,7 +499,7 @@ Eigen::VectorXd RigidBodyDepthSensor::output(const double &t,
return distances;
}

void parseForceElement(RigidBodySystem* sys, XMLElement* node) {
void parseForceElement(std::shared_ptr<RigidBodySystem> sys, XMLElement* node) {
string name = node->Attribute("name");

if (XMLElement* propellor_node = node->FirstChildElement("propellor")) {
Expand All @@ -513,7 +514,8 @@ void parseForceElement(RigidBodySystem* sys, XMLElement* node) {
}
}

void parseRobot(RigidBodySystem* sys, XMLElement* node) {
void parseRobot(std::shared_ptr<RigidBodySystem> sys, XMLElement* node)
{
if (!node->Attribute("name"))
throw runtime_error("Error: your robot must have a name attribute");
string robotname = node->Attribute("name");
Expand All @@ -524,21 +526,21 @@ void parseRobot(RigidBodySystem* sys, XMLElement* node) {
parseForceElement(sys, force_node);
}

void parseURDF(RigidBodySystem* sys, XMLDocument* xml_doc) {
XMLElement* node = xml_doc->FirstChildElement("robot");
if (!node)
throw std::runtime_error("ERROR: This urdf does not contain a robot tag");

void parseURDF(std::shared_ptr<RigidBodySystem> sys, XMLDocument *xml_doc)
{
XMLElement *node = xml_doc->FirstChildElement("robot");
if (!node) throw std::runtime_error("ERROR: This urdf does not contain a robot tag");
parseRobot(sys, node);
}

void parseSDFJoint(RigidBodySystem* sys, string model_name, XMLElement* node,
PoseMap& pose_map) {
void parseSDFJoint(std::shared_ptr<RigidBodySystem> sys, string model_name, XMLElement* node, PoseMap& pose_map)
{
// todo: parse joint sensors
}

void parseSDFLink(RigidBodySystem* sys, string model_name, XMLElement* node,
PoseMap& pose_map) {
void parseSDFLink(std::shared_ptr<RigidBodySystem> sys, string model_name, XMLElement* node, PoseMap& pose_map)
{
const char* attr = node->Attribute("name");
if (!attr) throw runtime_error("ERROR: link tag is missing name attribute");
string link_name(attr);
Expand Down Expand Up @@ -584,10 +586,10 @@ void parseSDFLink(RigidBodySystem* sys, string model_name, XMLElement* node,
}
}

void parseSDFModel(RigidBodySystem* sys, XMLElement* node) {
PoseMap pose_map; // because sdf specifies almost everything in the global
// (actually model) coordinates instead of relative
// coordinates. sigh...

void parseSDFModel(std::shared_ptr<RigidBodySystem> sys, XMLElement* node)
{
PoseMap pose_map; // because sdf specifies almost everything in the global (actually model) coordinates instead of relative coordinates. sigh...

if (!node->Attribute("name"))
throw runtime_error("Error: your model must have a name attribute");
Expand All @@ -602,11 +604,11 @@ void parseSDFModel(RigidBodySystem* sys, XMLElement* node) {
parseSDFJoint(sys, model_name, elnode, pose_map);
}

void parseSDF(RigidBodySystem* sys, XMLDocument* xml_doc) {
XMLElement* node = xml_doc->FirstChildElement("sdf");
if (!node)
throw std::runtime_error(
"ERROR: This xml file does not contain an sdf tag");

void parseSDF(std::shared_ptr<RigidBodySystem> sys, XMLDocument *xml_doc)
{
XMLElement *node = xml_doc->FirstChildElement("sdf");
if (!node) throw std::runtime_error("ERROR: This xml file does not contain an sdf tag");

for (XMLElement* elnode = node->FirstChildElement("model"); elnode;
elnode = node->NextSiblingElement("model"))
Expand All @@ -623,7 +625,8 @@ void RigidBodySystem::addRobotFromURDFString(
// sensors, etc)
XMLDocument xml_doc;
xml_doc.Parse(xml_string.c_str());
parseURDF(this, &xml_doc);

parseURDF(shared_from_this(), &xml_doc);
}

void RigidBodySystem::addRobotFromURDF(
Expand All @@ -640,7 +643,7 @@ void RigidBodySystem::addRobotFromURDF(
throw std::runtime_error("failed to parse xml in file " + urdf_filename +
"\n" + xml_doc.ErrorName());
}
parseURDF(this, &xml_doc);
parseURDF(shared_from_this(), &xml_doc);
}

void RigidBodySystem::addRobotFromSDF(
Expand All @@ -657,7 +660,7 @@ void RigidBodySystem::addRobotFromSDF(
throw std::runtime_error("failed to parse xml in file " + sdf_filename +
"\n" + xml_doc.ErrorName());
}
parseSDF(this, &xml_doc);
parseSDF(shared_from_this(), &xml_doc);
}

void RigidBodySystem::addRobotFromFile(
Expand All @@ -677,3 +680,4 @@ void RigidBodySystem::addRobotFromFile(
throw runtime_error("unknown file extension: " + ext);
}
}

Loading

0 comments on commit 73c4d73

Please sign in to comment.