From f7cbe318c278da010a88cf6be130b472a36867c8 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Mon, 15 Jun 2015 19:11:28 -0400 Subject: [PATCH 01/65] created basic compound pendulum tutorial and started domino tutorial --- CMakeLists.txt | 6 + tutorials/CMakeLists.txt | 12 ++ tutorials/tutorialCompoundPendulum.cpp | 236 +++++++++++++++++++++++++ tutorials/tutorialDominoes.cpp | 109 ++++++++++++ 4 files changed, 363 insertions(+) create mode 100644 tutorials/CMakeLists.txt create mode 100644 tutorials/tutorialCompoundPendulum.cpp create mode 100644 tutorials/tutorialDominoes.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index fb04b198709be..30a559b383f0c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -49,6 +49,7 @@ else() option(BUILD_SHARED_LIBS "Build shared libraries" ON) endif() option(DART_BUILD_EXAMPLES "Build examples" ON) +option(DART_BUILD_TUTORIALS "Build tutorials" ON) option(DART_BUILD_UNITTESTS "Build unit tests" ON) #=============================================================================== @@ -452,6 +453,7 @@ message(STATUS "BUILD_SHARED_LIBS: ${BUILD_SHARED_LIBS}") message(STATUS "ENABLE_OPENMP : ${ENABLE_OPENMP}") message(STATUS "Build core only : ${BUILD_CORE_ONLY}") message(STATUS "Build examples : ${DART_BUILD_EXAMPLES}") +message(STATUS "Build tutorials : ${DART_BUILD_TUTORIALS}") message(STATUS "Build unit tests : ${DART_BUILD_UNITTESTS}") message(STATUS "Install path : ${CMAKE_INSTALL_PREFIX}") message(STATUS "CXX_FLAGS : ${CMAKE_CXX_FLAGS}") @@ -525,6 +527,10 @@ if(NOT BUILD_CORE_ONLY) add_subdirectory(apps) endif() + if(DART_BUILD_TUTORIALS) + add_subdirectory(tutorials) + endif() + endif() #=============================================================================== diff --git a/tutorials/CMakeLists.txt b/tutorials/CMakeLists.txt new file mode 100644 index 0000000000000..5162a65829a20 --- /dev/null +++ b/tutorials/CMakeLists.txt @@ -0,0 +1,12 @@ +file(GLOB tutorials_src "*.cpp") +list(SORT tutorials_src) + +message(STATUS "") +message(STATUS "[ Tutorials ]") + +foreach(tutorial ${tutorials_src}) + get_filename_component(tutorial_base ${tutorial} NAME_WE) + message(STATUS "Adding tutorial: ${tutorial_base}") + add_executable(${tutorial_base} ${tutorial}) + target_link_libraries(${tutorial_base} dart) +endforeach(tutorial) diff --git a/tutorials/tutorialCompoundPendulum.cpp b/tutorials/tutorialCompoundPendulum.cpp new file mode 100644 index 0000000000000..7521b6e14c43d --- /dev/null +++ b/tutorials/tutorialCompoundPendulum.cpp @@ -0,0 +1,236 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/dart.h" + +constexpr double default_height = 1.0; // m +constexpr double default_width = 0.2; // m +constexpr double default_depth = 0.2; // m + +constexpr double default_torque = 3.0; // N-m +constexpr int default_countdown = 200; // Number of timesteps for applying force + +using namespace dart::dynamics; +using namespace dart::simulation; + +class MyWindow : public dart::gui::SimWindow +{ +public: + + /// Constructor + MyWindow(WorldPtr world) + : _weldConstraint(nullptr), + _positiveSign(true) + { + setWorld(world); + + // Find the Skeleton named "pendulum" within the World + _pendulum = world->getSkeleton("pendulum"); + + // Make sure that the pendulum was found in the World + assert(_pendulum != nullptr); + + _forceCountDown.resize(_pendulum->getNumDofs(), 0); + } + + /// Handle keyboard input + void keyboard(unsigned char key, int x, int y) override + { + switch(key) + { + case '1': + applyForce(0); + break; + case '2': + applyForce(1); + break; + case '3': + applyForce(2); + break; + case '4': + applyForce(3); + break; + case '5': + applyForce(4); + break; + case '6': + applyForce(5); + break; + case '7': + applyForce(6); + break; + case '8': + applyForce(7); + break; + case '9': + applyForce(8); + break; + case '0': + applyForce(9); + break; + case '-': + _positiveSign = !_positiveSign; + break; + case 'r': + { + if(_weldConstraint) + removeConstraint(); + else + addConstraint(); + break; + } + } + + SimWindow::keyboard(key, x, y); + } + + void timeStepping() override + { + for(size_t i=0; i<_forceCountDown.size(); ++i) + { + const ShapePtr& shape = + _pendulum->getBodyNode(i)->getVisualizationShape(0); + DegreeOfFreedom* dof = _pendulum->getDof(i); + + if(_forceCountDown[i] > 0) + { + dof->setForce(_positiveSign? default_torque : -default_torque); + shape->setColor(dart::Color::Red()); + --_forceCountDown[i]; + } + else + { + dof->setForce(0.0); + shape->setColor(dart::Color::Blue()); + } + } + + SimWindow::timeStepping(); + } + + void applyForce(size_t index) + { + if(index < _forceCountDown.size()) + _forceCountDown[index] = default_countdown; + } + + /// Add a constraint to turn the bottom of the pendulum into a triangle + void addConstraint() + { + BodyNode* tip = _pendulum->getBodyNode(_pendulum->getNumBodyNodes()-1); + + // Weld the last link to the world + _weldConstraint = new dart::constraint::WeldJointConstraint(tip); + mWorld->getConstraintSolver()->addConstraint(_weldConstraint); + } + + /// Remove any existing constraint, allowing the pendulum to flail freely + void removeConstraint() + { + mWorld->getConstraintSolver()->removeConstraint(_weldConstraint); + delete _weldConstraint; + _weldConstraint = nullptr; + } + +protected: + + /// The pendulum that we will be perturbing + SkeletonPtr _pendulum; + + /// Pointer to the weld constraint that we will be turning on and off + dart::constraint::WeldJointConstraint* _weldConstraint; + + /// Number of iterations before clearing a force entry + std::vector _forceCountDown; + + /// Whether a force should be applied in the positive or negative direction + bool _positiveSign; +}; + +BodyNode* addBody(const SkeletonPtr& pendulum, + BodyNode* parent, const std::string& name) +{ + // Set up the properties for the Joint + RevoluteJoint::Properties properties; + properties.mName = name+"_joint"; + properties.mAxis = Eigen::Vector3d::UnitY(); + if(parent) + { + properties.mT_ParentBodyToJoint.translation() = + Eigen::Vector3d(0, 0, default_height); + } + + // Create a new BodyNode, attached to its parent by a RevoluteJoint + BodyNodePtr bn = pendulum->createJointAndBodyNodePair( + parent, properties, BodyNode::Properties(name)).second; + + // Create a BoxShape to be used for both visualization and collision checking + std::shared_ptr box(new BoxShape( + Eigen::Vector3d(default_width, default_depth, default_height))); + box->setColor(dart::Color::Blue()); + Eigen::Isometry3d box_tf(Eigen::Isometry3d::Identity()); + Eigen::Vector3d center = Eigen::Vector3d(0, 0, default_height/2.0); + box_tf.translation() = center; + box->setLocalTransform(box_tf); + + bn->addVisualizationShape(box); + bn->addCollisionShape(box); + + // Move the center of mass to the center of the object + bn->setLocalCOM(center); + + return bn; +} + +int main(int argc, char* argv[]) +{ + SkeletonPtr pendulum = Skeleton::create("pendulum"); + + // Add each body to the pendulum + BodyNode* bn = addBody(pendulum, nullptr, "body1"); + bn = addBody(pendulum, bn, "body2"); + bn = addBody(pendulum, bn, "body3"); + bn = addBody(pendulum, bn, "body4"); + bn = addBody(pendulum, bn, "body5"); + + WorldPtr world(new World); + world->addSkeleton(pendulum); + + MyWindow window(world); + glutInit(&argc, argv); + window.initWindow(640, 480, "Compound Pendulum Tutorial"); + glutMainLoop(); +} diff --git a/tutorials/tutorialDominoes.cpp b/tutorials/tutorialDominoes.cpp new file mode 100644 index 0000000000000..08ef4d7204759 --- /dev/null +++ b/tutorials/tutorialDominoes.cpp @@ -0,0 +1,109 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/dart.h" + +constexpr double default_angle = 20.0*M_PI/180.0; +constexpr double default_distance = 0.01; +constexpr double default_domino_height = 0.07; +constexpr double default_domino_width = 0.03; + +using namespace dart::dynamics; + +class MyWindow : public dart::gui::SimWindow +{ +public: + + MyWindow(dart::simulation::WorldPtr world) + { + + } + + void keyboard(unsigned char key, int x, int y) override + { + if(!_hasEverRun) + { + switch(key) + { + case 'q': + attemptToCreateDomino( default_angle); + case 'w': + attemptToCreateDomino(0.0); + case 'e': + attemptToCreateDomino(-default_angle); + case 'd': + deleteLastDomino(); + case ' ': + _hasEverRun = true; + } + } + + SimWindow::keyboard(key, x, y); + } + + + void attemptToCreateDomino(double angle) + { + + } + + void deleteLastDomino() + { + + } + + +protected: + + /// History of the dominoes that have been created + std::vector _dominoes; + + /// History of the angles that the user has specified + std::vector _angles; + + /// Sum of all angles so far + double _totalAngle; + + /// Set to true the first time spacebar is pressed + bool _hasEverRun; + +}; + + +int main(int argc, char* argv[]) +{ + +} From 7a470d324c96e5031d60301da45f6e2a31acb859 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Mon, 15 Jun 2015 19:33:15 -0400 Subject: [PATCH 02/65] final touches on pendulum tutorial --- ...Pendulum.cpp => tutorialMultiPendulum.cpp} | 21 +++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) rename tutorials/{tutorialCompoundPendulum.cpp => tutorialMultiPendulum.cpp} (89%) diff --git a/tutorials/tutorialCompoundPendulum.cpp b/tutorials/tutorialMultiPendulum.cpp similarity index 89% rename from tutorials/tutorialCompoundPendulum.cpp rename to tutorials/tutorialMultiPendulum.cpp index 7521b6e14c43d..1531a9d65007d 100644 --- a/tutorials/tutorialCompoundPendulum.cpp +++ b/tutorials/tutorialMultiPendulum.cpp @@ -40,7 +40,7 @@ constexpr double default_height = 1.0; // m constexpr double default_width = 0.2; // m constexpr double default_depth = 0.2; // m -constexpr double default_torque = 3.0; // N-m +constexpr double default_torque = 15.0; // N-m constexpr int default_countdown = 200; // Number of timesteps for applying force using namespace dart::dynamics; @@ -150,6 +150,7 @@ class MyWindow : public dart::gui::SimWindow /// Add a constraint to turn the bottom of the pendulum into a triangle void addConstraint() { + // Get the last body in the pendulum BodyNode* tip = _pendulum->getBodyNode(_pendulum->getNumBodyNodes()-1); // Weld the last link to the world @@ -201,6 +202,8 @@ BodyNode* addBody(const SkeletonPtr& pendulum, std::shared_ptr box(new BoxShape( Eigen::Vector3d(default_width, default_depth, default_height))); box->setColor(dart::Color::Blue()); + + // Set the location of the Box Eigen::Isometry3d box_tf(Eigen::Isometry3d::Identity()); Eigen::Vector3d center = Eigen::Vector3d(0, 0, default_height/2.0); box_tf.translation() = center; @@ -217,19 +220,33 @@ BodyNode* addBody(const SkeletonPtr& pendulum, int main(int argc, char* argv[]) { + // Create an empty Skeleton with the name "pendulum" SkeletonPtr pendulum = Skeleton::create("pendulum"); - // Add each body to the pendulum + // Add each body to the last BodyNode in the pendulum BodyNode* bn = addBody(pendulum, nullptr, "body1"); bn = addBody(pendulum, bn, "body2"); bn = addBody(pendulum, bn, "body3"); bn = addBody(pendulum, bn, "body4"); bn = addBody(pendulum, bn, "body5"); + // Set the initial position of the first DegreeOfFreedom so that the pendulum + // starts to swing right away + pendulum->getDof(0)->setPosition(120*M_PI/180.0); + + // Create a world and add the pendulum to the world WorldPtr world(new World); world->addSkeleton(pendulum); + // Create a window for rendering the world and handling user input MyWindow window(world); + + // Print instructions + std::cout << "space bar: simulation on/off" << std::endl; + std::cout << "'1' -> '9': apply torque to a pendulum body" << std::endl; + std::cout << "'r': add/remove constraint on the end of the chain" << std::endl; + + // Initialize glut, initialize the window, and begin the glut event loop glutInit(&argc, argv); window.initWindow(640, 480, "Compound Pendulum Tutorial"); glutMainLoop(); From 027c2a1a12f0fa441b7fc19f178eb223d8c4ced3 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Mon, 15 Jun 2015 22:30:15 -0400 Subject: [PATCH 03/65] adding damping to the tutorial --- tutorials/tutorialMultiPendulum.cpp | 103 +++++++++++++++++++++------- 1 file changed, 79 insertions(+), 24 deletions(-) diff --git a/tutorials/tutorialMultiPendulum.cpp b/tutorials/tutorialMultiPendulum.cpp index 1531a9d65007d..418e282796e84 100644 --- a/tutorials/tutorialMultiPendulum.cpp +++ b/tutorials/tutorialMultiPendulum.cpp @@ -43,6 +43,8 @@ constexpr double default_depth = 0.2; // m constexpr double default_torque = 15.0; // N-m constexpr int default_countdown = 200; // Number of timesteps for applying force +constexpr double default_damping = 0.1; + using namespace dart::dynamics; using namespace dart::simulation; @@ -119,11 +121,20 @@ class MyWindow : public dart::gui::SimWindow void timeStepping() override { - for(size_t i=0; i<_forceCountDown.size(); ++i) + // Reset all the shapes to be Blue + for(size_t i=0; i<_pendulum->getNumBodyNodes(); ++i) { const ShapePtr& shape = _pendulum->getBodyNode(i)->getVisualizationShape(0); + + shape->setColor(dart::Color::Blue()); + } + + // Any DOFs with an active countdown are given a joint force and colored red + for(size_t i=0; i<_forceCountDown.size(); ++i) + { DegreeOfFreedom* dof = _pendulum->getDof(i); + const ShapePtr& shape = dof->getChildBodyNode()->getVisualizationShape(0); if(_forceCountDown[i] > 0) { @@ -131,13 +142,9 @@ class MyWindow : public dart::gui::SimWindow shape->setColor(dart::Color::Red()); --_forceCountDown[i]; } - else - { - dof->setForce(0.0); - shape->setColor(dart::Color::Blue()); - } } + // Step the simulation forward SimWindow::timeStepping(); } @@ -147,6 +154,11 @@ class MyWindow : public dart::gui::SimWindow _forceCountDown[index] = default_countdown; } + void changeDamping(double delta) + { + + } + /// Add a constraint to turn the bottom of the pendulum into a triangle void addConstraint() { @@ -181,23 +193,8 @@ class MyWindow : public dart::gui::SimWindow bool _positiveSign; }; -BodyNode* addBody(const SkeletonPtr& pendulum, - BodyNode* parent, const std::string& name) +void setGeometry(const BodyNodePtr& bn) { - // Set up the properties for the Joint - RevoluteJoint::Properties properties; - properties.mName = name+"_joint"; - properties.mAxis = Eigen::Vector3d::UnitY(); - if(parent) - { - properties.mT_ParentBodyToJoint.translation() = - Eigen::Vector3d(0, 0, default_height); - } - - // Create a new BodyNode, attached to its parent by a RevoluteJoint - BodyNodePtr bn = pendulum->createJointAndBodyNodePair( - parent, properties, BodyNode::Properties(name)).second; - // Create a BoxShape to be used for both visualization and collision checking std::shared_ptr box(new BoxShape( Eigen::Vector3d(default_width, default_depth, default_height))); @@ -209,11 +206,68 @@ BodyNode* addBody(const SkeletonPtr& pendulum, box_tf.translation() = center; box->setLocalTransform(box_tf); + // Add it as a visualization and collision shape bn->addVisualizationShape(box); bn->addCollisionShape(box); // Move the center of mass to the center of the object bn->setLocalCOM(center); +} + +BodyNode* makeRootBody(const SkeletonPtr& pendulum, const std::string& name) +{ + BallJoint::Properties properties; + properties.mName = name+"_joint"; + properties.mDampingCoefficients = Eigen::Vector3d::Constant(default_damping); + + BodyNodePtr bn = pendulum->createJointAndBodyNodePair( + nullptr, properties, BodyNode::Properties(name)).second; + + // Make a shape for the Joint + const double& R = default_width; + std::shared_ptr ball( + new EllipsoidShape(sqrt(2)*Eigen::Vector3d(R, R, R))); + ball->setColor(dart::Color::Blue()); + bn->addVisualizationShape(ball); + + // Set the geometry of the Body + setGeometry(bn); + + return bn; +} + +BodyNode* addBody(const SkeletonPtr& pendulum, + BodyNode* parent, const std::string& name) +{ + // Set up the properties for the Joint + RevoluteJoint::Properties properties; + properties.mName = name+"_joint"; + properties.mAxis = Eigen::Vector3d::UnitY(); + properties.mT_ParentBodyToJoint.translation() = + Eigen::Vector3d(0, 0, default_height); + properties.mDampingCoefficient = default_damping; + + // Create a new BodyNode, attached to its parent by a RevoluteJoint + BodyNodePtr bn = pendulum->createJointAndBodyNodePair( + parent, properties, BodyNode::Properties(name)).second; + + // Make a shape for the Joint + const double R = default_width/2.0; + const double h = default_depth; + std::shared_ptr cyl( + new CylinderShape(R, h)); + cyl->setColor(dart::Color::Blue()); + + // Line up the cylinder with the Joint axis + Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); + tf.linear() = dart::math::eulerXYZToMatrix( + Eigen::Vector3d(90.0*M_PI/180.0, 0, 0)); + cyl->setLocalTransform(tf); + + bn->addVisualizationShape(cyl); + + // Set the geometry of the Body + setGeometry(bn); return bn; } @@ -224,7 +278,8 @@ int main(int argc, char* argv[]) SkeletonPtr pendulum = Skeleton::create("pendulum"); // Add each body to the last BodyNode in the pendulum - BodyNode* bn = addBody(pendulum, nullptr, "body1"); +// BodyNode* bn = addBody(pendulum, nullptr, "body1"); + BodyNode* bn = makeRootBody(pendulum, "body1"); bn = addBody(pendulum, bn, "body2"); bn = addBody(pendulum, bn, "body3"); bn = addBody(pendulum, bn, "body4"); @@ -232,7 +287,7 @@ int main(int argc, char* argv[]) // Set the initial position of the first DegreeOfFreedom so that the pendulum // starts to swing right away - pendulum->getDof(0)->setPosition(120*M_PI/180.0); + pendulum->getDof(1)->setPosition(120*M_PI/180.0); // Create a world and add the pendulum to the world WorldPtr world(new World); From 54e1d376ae0f8963396b34ddec1b77ebaf5c8f07 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Mon, 15 Jun 2015 23:54:35 -0400 Subject: [PATCH 04/65] finished multi pendulum tutorial and added skeleton code --- tutorials/tutorialMultiPendulum-Finished.cpp | 388 +++++++++++++++++++ tutorials/tutorialMultiPendulum.cpp | 145 ++++--- 2 files changed, 472 insertions(+), 61 deletions(-) create mode 100644 tutorials/tutorialMultiPendulum-Finished.cpp diff --git a/tutorials/tutorialMultiPendulum-Finished.cpp b/tutorials/tutorialMultiPendulum-Finished.cpp new file mode 100644 index 0000000000000..a154dcb0e2e55 --- /dev/null +++ b/tutorials/tutorialMultiPendulum-Finished.cpp @@ -0,0 +1,388 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/dart.h" + +const double default_height = 1.0; // m +const double default_width = 0.2; // m +const double default_depth = 0.2; // m + +const double default_torque = 15.0; // N-m +const int default_countdown = 200; // Number of timesteps for applying force + +const double default_rest_position = 0.0; +const double delta_rest_position = 10.0*M_PI/180.0; + +const double default_stiffness = 0.0; +const double delta_stiffness = 10; + +const double default_damping = 5.0; +const double delta_damping = 1.0; + +using namespace dart::dynamics; +using namespace dart::simulation; + +class MyWindow : public dart::gui::SimWindow +{ +public: + + /// Constructor + MyWindow(WorldPtr world) + : _weldConstraint(nullptr), + _positiveSign(true) + { + setWorld(world); + + // Find the Skeleton named "pendulum" within the World + _pendulum = world->getSkeleton("pendulum"); + + // Make sure that the pendulum was found in the World + assert(_pendulum != nullptr); + + _forceCountDown.resize(_pendulum->getNumDofs(), 0); + } + + void applyForce(size_t index) + { + if(index < _forceCountDown.size()) + _forceCountDown[index] = default_countdown; + } + + void changeRestPosition(double delta) + { + for(size_t i=0; i<_pendulum->getNumDofs(); ++i) + { + DegreeOfFreedom* dof = _pendulum->getDof(i); + double q0 = dof->getRestPosition() + delta; + + // The system becomes numerically unstable when the rest position exceeds + // 90 degrees + if(std::abs(q0) > 90.0*M_PI/180.0) + q0 = q0>0? 90.0*M_PI/180.0 : -90.0*M_PI/180.0; + + dof->setRestPosition(q0); + } + + // Only curl up along one axis in the BallJoint + _pendulum->getDof(0)->setRestPosition(0.0); + _pendulum->getDof(2)->setRestPosition(0.0); + } + + void changeStiffness(double delta) + { + for(size_t i=0; i<_pendulum->getNumDofs(); ++i) + { + DegreeOfFreedom* dof = _pendulum->getDof(i); + double stiffness = dof->getSpringStiffness() + delta; + if(stiffness < 0.0) + stiffness = 0.0; + dof->setSpringStiffness(stiffness); + } + } + + void changeDamping(double delta) + { + for(size_t i=0; i<_pendulum->getNumDofs(); ++i) + { + DegreeOfFreedom* dof = _pendulum->getDof(i); + double damping = dof->getDampingCoefficient() + delta; + if(damping < 0.0) + damping = 0.0; + dof->setDampingCoefficient(damping); + } + } + + /// Add a constraint to turn the bottom of the pendulum into a triangle + void addConstraint() + { + // Get the last body in the pendulum + BodyNode* tip = _pendulum->getBodyNode(_pendulum->getNumBodyNodes()-1); + + // Weld the last link to the world + _weldConstraint = new dart::constraint::WeldJointConstraint(tip); + mWorld->getConstraintSolver()->addConstraint(_weldConstraint); + } + + /// Remove any existing constraint, allowing the pendulum to flail freely + void removeConstraint() + { + mWorld->getConstraintSolver()->removeConstraint(_weldConstraint); + delete _weldConstraint; + _weldConstraint = nullptr; + } + + /// Handle keyboard input + void keyboard(unsigned char key, int x, int y) override + { + switch(key) + { + case '1': + applyForce(0); + break; + case '2': + applyForce(1); + break; + case '3': + applyForce(2); + break; + case '4': + applyForce(3); + break; + case '5': + applyForce(4); + break; + case '6': + applyForce(5); + break; + case '7': + applyForce(6); + break; + case '8': + applyForce(7); + break; + case '9': + applyForce(8); + break; + case '0': + applyForce(9); + break; + + case '-': + _positiveSign = !_positiveSign; + break; + + case 'q': + changeRestPosition( delta_rest_position); + break; + case 'a': + changeRestPosition(-delta_rest_position); + break; + + case 'w': + changeStiffness( delta_stiffness); + break; + case 's': + changeStiffness(-delta_stiffness); + break; + + case 'e': + changeDamping( delta_damping); + break; + case 'd': + changeDamping(-delta_damping); + break; + + case 'r': + { + if(_weldConstraint) + removeConstraint(); + else + addConstraint(); + break; + } + + default: + SimWindow::keyboard(key, x, y); + } + } + + void timeStepping() override + { + // Reset all the shapes to be Blue + for(size_t i=0; i<_pendulum->getNumBodyNodes(); ++i) + { + const ShapePtr& shape = + _pendulum->getBodyNode(i)->getVisualizationShape(0); + + shape->setColor(dart::Color::Blue()); + } + + // Any DOFs with an active countdown are given a joint force and colored red + for(size_t i=0; i<_forceCountDown.size(); ++i) + { + DegreeOfFreedom* dof = _pendulum->getDof(i); + const ShapePtr& shape = dof->getChildBodyNode()->getVisualizationShape(0); + + if(_forceCountDown[i] > 0) + { + dof->setForce(_positiveSign? default_torque : -default_torque); + shape->setColor(dart::Color::Red()); + --_forceCountDown[i]; + } + } + + // Step the simulation forward + SimWindow::timeStepping(); + } + +protected: + + /// The pendulum that we will be perturbing + SkeletonPtr _pendulum; + + /// Pointer to the weld constraint that we will be turning on and off + dart::constraint::WeldJointConstraint* _weldConstraint; + + /// Number of iterations before clearing a force entry + std::vector _forceCountDown; + + /// Whether a force should be applied in the positive or negative direction + bool _positiveSign; +}; + +void setGeometry(const BodyNodePtr& bn) +{ + // Create a BoxShape to be used for both visualization and collision checking + std::shared_ptr box(new BoxShape( + Eigen::Vector3d(default_width, default_depth, default_height))); + box->setColor(dart::Color::Blue()); + + // Set the location of the Box + Eigen::Isometry3d box_tf(Eigen::Isometry3d::Identity()); + Eigen::Vector3d center = Eigen::Vector3d(0, 0, default_height/2.0); + box_tf.translation() = center; + box->setLocalTransform(box_tf); + + // Add it as a visualization and collision shape + bn->addVisualizationShape(box); + bn->addCollisionShape(box); + + // Move the center of mass to the center of the object + bn->setLocalCOM(center); +} + +BodyNode* makeRootBody(const SkeletonPtr& pendulum, const std::string& name) +{ + BallJoint::Properties properties; + properties.mName = name+"_joint"; + properties.mRestPositions = Eigen::Vector3d::Constant(default_rest_position); + properties.mSpringStiffnesses = Eigen::Vector3d::Constant(default_stiffness); + properties.mDampingCoefficients = Eigen::Vector3d::Constant(default_damping); + + BodyNodePtr bn = pendulum->createJointAndBodyNodePair( + nullptr, properties, BodyNode::Properties(name)).second; + + // Make a shape for the Joint + const double& R = default_width; + std::shared_ptr ball( + new EllipsoidShape(sqrt(2)*Eigen::Vector3d(R, R, R))); + ball->setColor(dart::Color::Blue()); + bn->addVisualizationShape(ball); + + // Set the geometry of the Body + setGeometry(bn); + + return bn; +} + +BodyNode* addBody(const SkeletonPtr& pendulum, + BodyNode* parent, const std::string& name) +{ + // Set up the properties for the Joint + RevoluteJoint::Properties properties; + properties.mName = name+"_joint"; + properties.mAxis = Eigen::Vector3d::UnitY(); + properties.mT_ParentBodyToJoint.translation() = + Eigen::Vector3d(0, 0, default_height); + properties.mRestPosition = default_rest_position; + properties.mSpringStiffness = default_stiffness; + properties.mDampingCoefficient = default_damping; + + // Create a new BodyNode, attached to its parent by a RevoluteJoint + BodyNodePtr bn = pendulum->createJointAndBodyNodePair( + parent, properties, BodyNode::Properties(name)).second; + + // Make a shape for the Joint + const double R = default_width/2.0; + const double h = default_depth; + std::shared_ptr cyl( + new CylinderShape(R, h)); + cyl->setColor(dart::Color::Blue()); + + // Line up the cylinder with the Joint axis + Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); + tf.linear() = dart::math::eulerXYZToMatrix( + Eigen::Vector3d(90.0*M_PI/180.0, 0, 0)); + cyl->setLocalTransform(tf); + + bn->addVisualizationShape(cyl); + + // Set the geometry of the Body + setGeometry(bn); + + return bn; +} + +int main(int argc, char* argv[]) +{ + // Create an empty Skeleton with the name "pendulum" + SkeletonPtr pendulum = Skeleton::create("pendulum"); + + // Add each body to the last BodyNode in the pendulum + BodyNode* bn = makeRootBody(pendulum, "body1"); + bn = addBody(pendulum, bn, "body2"); + bn = addBody(pendulum, bn, "body3"); + bn = addBody(pendulum, bn, "body4"); + bn = addBody(pendulum, bn, "body5"); + + // Set the initial position of the first DegreeOfFreedom so that the pendulum + // starts to swing right away + pendulum->getDof(1)->setPosition(120*M_PI/180.0); + + // Create a world and add the pendulum to the world + WorldPtr world(new World); + world->addSkeleton(pendulum); + + // Create a window for rendering the world and handling user input + MyWindow window(world); + + // Print instructions + std::cout << "space bar: simulation on/off" << std::endl; + std::cout << "'1' -> '9': apply torque to a pendulum body" << std::endl; + std::cout << "'-': Change sign of applied joint torques" << std::endl; + std::cout << "'q': Increase joint rest positions" << std::endl; + std::cout << "'a': Decrease joint rest positions" << std::endl; + std::cout << "'w': Increase joint spring stiffness" << std::endl; + std::cout << "'s': Decrease joint spring stiffness" << std::endl; + std::cout << "'e': Increase joint damping" << std::endl; + std::cout << "'d': Decrease joint damping" << std::endl; + std::cout << "'r': add/remove constraint on the end of the chain" << std::endl; + + // Initialize glut, initialize the window, and begin the glut event loop + glutInit(&argc, argv); + window.initWindow(640, 480, "Compound Pendulum Tutorial"); + glutMainLoop(); +} diff --git a/tutorials/tutorialMultiPendulum.cpp b/tutorials/tutorialMultiPendulum.cpp index 418e282796e84..8f7e12dcbbd0f 100644 --- a/tutorials/tutorialMultiPendulum.cpp +++ b/tutorials/tutorialMultiPendulum.cpp @@ -1,3 +1,4 @@ + /* * Copyright (c) 2015, Georgia Tech Research Corporation * All rights reserved. @@ -36,14 +37,21 @@ #include "dart/dart.h" -constexpr double default_height = 1.0; // m -constexpr double default_width = 0.2; // m -constexpr double default_depth = 0.2; // m +const double default_height = 1.0; // m +const double default_width = 0.2; // m +const double default_depth = 0.2; // m + +const double default_torque = 15.0; // N-m +const int default_countdown = 200; // Number of timesteps for applying force + +const double default_rest_position = 0.0; +const double delta_rest_position = 10.0*M_PI/180.0; -constexpr double default_torque = 15.0; // N-m -constexpr int default_countdown = 200; // Number of timesteps for applying force +const double default_stiffness = 0.0; +const double delta_stiffness = 10; -constexpr double default_damping = 0.1; +const double default_damping = 5.0; +const double delta_damping = 1.0; using namespace dart::dynamics; using namespace dart::simulation; @@ -68,6 +76,38 @@ class MyWindow : public dart::gui::SimWindow _forceCountDown.resize(_pendulum->getNumDofs(), 0); } + void applyForce(size_t) + { + // Lesson 1 + } + + void changeRestPosition(double) + { + // Lesson 2 + } + + void changeStiffness(double) + { + // Lesson 3 + } + + void changeDamping(double) + { + // Lesson 4 + } + + /// Add a constraint to turn the bottom of the pendulum into a triangle + void addConstraint() + { + // Lesson 5 + } + + /// Remove any existing constraint, allowing the pendulum to flail freely + void removeConstraint() + { + // Lesson 6 + } + /// Handle keyboard input void keyboard(unsigned char key, int x, int y) override { @@ -103,9 +143,32 @@ class MyWindow : public dart::gui::SimWindow case '0': applyForce(9); break; + case '-': _positiveSign = !_positiveSign; break; + + case 'q': + changeRestPosition( delta_rest_position); + break; + case 'a': + changeRestPosition(-delta_rest_position); + break; + + case 'w': + changeStiffness( delta_stiffness); + break; + case 's': + changeStiffness(-delta_stiffness); + break; + + case 'e': + changeDamping( delta_damping); + break; + case 'd': + changeDamping(-delta_damping); + break; + case 'r': { if(_weldConstraint) @@ -114,70 +177,20 @@ class MyWindow : public dart::gui::SimWindow addConstraint(); break; } - } - SimWindow::keyboard(key, x, y); + default: + SimWindow::keyboard(key, x, y); + } } void timeStepping() override { - // Reset all the shapes to be Blue - for(size_t i=0; i<_pendulum->getNumBodyNodes(); ++i) - { - const ShapePtr& shape = - _pendulum->getBodyNode(i)->getVisualizationShape(0); - - shape->setColor(dart::Color::Blue()); - } - - // Any DOFs with an active countdown are given a joint force and colored red - for(size_t i=0; i<_forceCountDown.size(); ++i) - { - DegreeOfFreedom* dof = _pendulum->getDof(i); - const ShapePtr& shape = dof->getChildBodyNode()->getVisualizationShape(0); - - if(_forceCountDown[i] > 0) - { - dof->setForce(_positiveSign? default_torque : -default_torque); - shape->setColor(dart::Color::Red()); - --_forceCountDown[i]; - } - } + // Lesson 1 // Step the simulation forward SimWindow::timeStepping(); } - void applyForce(size_t index) - { - if(index < _forceCountDown.size()) - _forceCountDown[index] = default_countdown; - } - - void changeDamping(double delta) - { - - } - - /// Add a constraint to turn the bottom of the pendulum into a triangle - void addConstraint() - { - // Get the last body in the pendulum - BodyNode* tip = _pendulum->getBodyNode(_pendulum->getNumBodyNodes()-1); - - // Weld the last link to the world - _weldConstraint = new dart::constraint::WeldJointConstraint(tip); - mWorld->getConstraintSolver()->addConstraint(_weldConstraint); - } - - /// Remove any existing constraint, allowing the pendulum to flail freely - void removeConstraint() - { - mWorld->getConstraintSolver()->removeConstraint(_weldConstraint); - delete _weldConstraint; - _weldConstraint = nullptr; - } - protected: /// The pendulum that we will be perturbing @@ -218,6 +231,8 @@ BodyNode* makeRootBody(const SkeletonPtr& pendulum, const std::string& name) { BallJoint::Properties properties; properties.mName = name+"_joint"; + properties.mRestPositions = Eigen::Vector3d::Constant(default_rest_position); + properties.mSpringStiffnesses = Eigen::Vector3d::Constant(default_stiffness); properties.mDampingCoefficients = Eigen::Vector3d::Constant(default_damping); BodyNodePtr bn = pendulum->createJointAndBodyNodePair( @@ -245,6 +260,8 @@ BodyNode* addBody(const SkeletonPtr& pendulum, properties.mAxis = Eigen::Vector3d::UnitY(); properties.mT_ParentBodyToJoint.translation() = Eigen::Vector3d(0, 0, default_height); + properties.mRestPosition = default_rest_position; + properties.mSpringStiffness = default_stiffness; properties.mDampingCoefficient = default_damping; // Create a new BodyNode, attached to its parent by a RevoluteJoint @@ -278,7 +295,6 @@ int main(int argc, char* argv[]) SkeletonPtr pendulum = Skeleton::create("pendulum"); // Add each body to the last BodyNode in the pendulum -// BodyNode* bn = addBody(pendulum, nullptr, "body1"); BodyNode* bn = makeRootBody(pendulum, "body1"); bn = addBody(pendulum, bn, "body2"); bn = addBody(pendulum, bn, "body3"); @@ -299,6 +315,13 @@ int main(int argc, char* argv[]) // Print instructions std::cout << "space bar: simulation on/off" << std::endl; std::cout << "'1' -> '9': apply torque to a pendulum body" << std::endl; + std::cout << "'-': Change sign of applied joint torques" << std::endl; + std::cout << "'q': Increase joint rest positions" << std::endl; + std::cout << "'a': Decrease joint rest positions" << std::endl; + std::cout << "'w': Increase joint spring stiffness" << std::endl; + std::cout << "'s': Decrease joint spring stiffness" << std::endl; + std::cout << "'e': Increase joint damping" << std::endl; + std::cout << "'d': Decrease joint damping" << std::endl; std::cout << "'r': add/remove constraint on the end of the chain" << std::endl; // Initialize glut, initialize the window, and begin the glut event loop From ec8e9e2b2285c3c3384b2f9cc9b4c072fe6375c5 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Tue, 16 Jun 2015 00:30:32 -0400 Subject: [PATCH 05/65] renamed gui window --- tutorials/tutorialMultiPendulum-Finished.cpp | 2 +- tutorials/tutorialMultiPendulum.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/tutorials/tutorialMultiPendulum-Finished.cpp b/tutorials/tutorialMultiPendulum-Finished.cpp index a154dcb0e2e55..196e76af0462c 100644 --- a/tutorials/tutorialMultiPendulum-Finished.cpp +++ b/tutorials/tutorialMultiPendulum-Finished.cpp @@ -383,6 +383,6 @@ int main(int argc, char* argv[]) // Initialize glut, initialize the window, and begin the glut event loop glutInit(&argc, argv); - window.initWindow(640, 480, "Compound Pendulum Tutorial"); + window.initWindow(640, 480, "Multi-Pendulum Tutorial"); glutMainLoop(); } diff --git a/tutorials/tutorialMultiPendulum.cpp b/tutorials/tutorialMultiPendulum.cpp index 8f7e12dcbbd0f..eba4c30613b97 100644 --- a/tutorials/tutorialMultiPendulum.cpp +++ b/tutorials/tutorialMultiPendulum.cpp @@ -326,6 +326,6 @@ int main(int argc, char* argv[]) // Initialize glut, initialize the window, and begin the glut event loop glutInit(&argc, argv); - window.initWindow(640, 480, "Compound Pendulum Tutorial"); + window.initWindow(640, 480, "Multi-Pendulum Tutorial"); glutMainLoop(); } From 76fbf24cfc8da7055c962f869c85b1682b78bc65 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Tue, 16 Jun 2015 02:18:02 -0400 Subject: [PATCH 06/65] finished domino portion of the domino tutorial --- tutorials/tutorialDominoes.cpp | 197 +++++++++++++++++++++++++++++++-- 1 file changed, 185 insertions(+), 12 deletions(-) diff --git a/tutorials/tutorialDominoes.cpp b/tutorials/tutorialDominoes.cpp index 08ef4d7204759..eb2051ac68b8f 100644 --- a/tutorials/tutorialDominoes.cpp +++ b/tutorials/tutorialDominoes.cpp @@ -36,20 +36,101 @@ #include "dart/dart.h" -constexpr double default_angle = 20.0*M_PI/180.0; -constexpr double default_distance = 0.01; -constexpr double default_domino_height = 0.07; -constexpr double default_domino_width = 0.03; +const double default_domino_height = 0.07; +const double default_domino_width = 0.03; +const double default_domino_depth = default_domino_width/5.0; + +const double default_distance = default_domino_height/2.0; +const double default_angle = 20.0*M_PI/180.0; + +const double default_push_force = 8.0; // N +const int default_push_duration = 200; // # iterations using namespace dart::dynamics; +using namespace dart::simulation; class MyWindow : public dart::gui::SimWindow { public: - MyWindow(dart::simulation::WorldPtr world) + MyWindow(const WorldPtr& world) + : _totalAngle(0.0), + _hasEverRun(false), + _pushCountDown(0) + { + setWorld(world); + _firstDomino = world->getSkeleton("domino"); + _floor = world->getSkeleton("floor"); + } + + void attemptToCreateDomino(double angle) { + const SkeletonPtr& lastDomino = _dominoes.size()>0? + _dominoes.back() : _firstDomino; + + // Compute the position for the new domino + Eigen::Vector3d dx = default_distance*Eigen::Vector3d( + cos(_totalAngle), sin(_totalAngle), 0.0); + + Eigen::Vector6d x = lastDomino->getPositions(); + x.tail<3>() += dx; + // Adjust the angle for the new domino + x[2] = _totalAngle + angle; + + SkeletonPtr newDomino = _firstDomino->clone(); + newDomino->setName("domino #" + std::to_string(_dominoes.size()+1)); + newDomino->setPositions(x); + + mWorld->addSkeleton(newDomino); + + // Compute collisions + dart::collision::CollisionDetector* detector = + mWorld->getConstraintSolver()->getCollisionDetector(); + detector->detectCollision(true, true); + + // Look through the collisions to see if any dominoes are penetrating each + // other + bool dominoCollision = false; + size_t collisionCount = detector->getNumContacts(); + for(size_t i=0; i < collisionCount; ++i) + { + // If either of the colliding BodyNodes belongs to the floor, then we know + // that we have colliding dominoes + const dart::collision::Contact& contact = detector->getContact(i); + if( contact.bodyNode1.lock()->getSkeleton() != _floor + && contact.bodyNode2.lock()->getSkeleton() != _floor) + { + dominoCollision = true; + break; + } + } + + if(dominoCollision) + { + // Remove the new domino, because it is penetrating an existing one + mWorld->removeSkeleton(newDomino); + } + else + { + // Record the latest domino addition + _angles.push_back(angle); + _dominoes.push_back(newDomino); + _totalAngle += angle; + } + } + + void deleteLastDomino() + { + if(_dominoes.size() > 0) + { + SkeletonPtr lastDomino = _dominoes.back(); + _dominoes.pop_back(); + mWorld->removeSkeleton(lastDomino); + + _totalAngle -= _angles.back(); + _angles.pop_back(); + } } void keyboard(unsigned char key, int x, int y) override @@ -60,34 +141,56 @@ class MyWindow : public dart::gui::SimWindow { case 'q': attemptToCreateDomino( default_angle); + break; case 'w': attemptToCreateDomino(0.0); + break; case 'e': attemptToCreateDomino(-default_angle); + break; case 'd': deleteLastDomino(); + break; case ' ': _hasEverRun = true; + break; + } + } + else + { + switch(key) + { + case 'f': + _pushCountDown = default_push_duration; + break; } } SimWindow::keyboard(key, x, y); } - - void attemptToCreateDomino(double angle) + void timeStepping() override { + if(_pushCountDown > 0) + { + _firstDomino->getBodyNode(0)->addExtForce( + default_push_force*Eigen::Vector3d::UnitX(), + default_domino_height/2.0*Eigen::Vector3d::UnitZ()); - } - - void deleteLastDomino() - { + --_pushCountDown; + } + SimWindow::timeStepping(); } - protected: + /// Base domino. Used to clone new dominoes. + SkeletonPtr _firstDomino; + + /// Floor of the scene + SkeletonPtr _floor; + /// History of the dominoes that have been created std::vector _dominoes; @@ -100,10 +203,80 @@ class MyWindow : public dart::gui::SimWindow /// Set to true the first time spacebar is pressed bool _hasEverRun; + /// The first domino will be pushed on while the value of this is positive + int _pushCountDown; + }; +SkeletonPtr createDomino() +{ + /// Create a Skeleton with the name "domino" + SkeletonPtr domino = Skeleton::create("domino"); + + /// Create a body for the domino + BodyNodePtr body = + domino->createJointAndBodyNodePair(nullptr).second; + + std::shared_ptr box( + new BoxShape(Eigen::Vector3d(default_domino_depth, + default_domino_width, + default_domino_height))); + body->addVisualizationShape(box); + body->addCollisionShape(box); + + domino->getDof("Joint_pos_z")->setPosition(default_domino_height/2.0); + + return domino; +} + +SkeletonPtr createFloor() +{ + SkeletonPtr floor = Skeleton::create("floor"); + + // Give the floor a body + BodyNodePtr body = + floor->createJointAndBodyNodePair(nullptr).second; + + // Give the body a shape + double floor_width = 10.0; + double floor_height = 0.01; + std::shared_ptr box( + new BoxShape(Eigen::Vector3d(floor_width, floor_width, floor_height))); + box->setColor(dart::Color::Black()); + + body->addVisualizationShape(box); + body->addCollisionShape(box); + + // Put the body into position + Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); + tf.translation() = Eigen::Vector3d(0.0, 0.0, -floor_height/2.0); + body->getParentJoint()->setTransformFromParentBodyNode(tf); + + return floor; +} int main(int argc, char* argv[]) { + SkeletonPtr domino = createDomino(); + SkeletonPtr floor = createFloor(); + + WorldPtr world(new World); + world->addSkeleton(domino); + world->addSkeleton(floor); + + MyWindow window(world); + + std::cout << "Before simulation has started, you can create new dominoes:" << std::endl; + std::cout << "'w': Create new domino angled forward" << std::endl; + std::cout << "'q': Create new domino angled to the left" << std::endl; + std::cout << "'e': Create new domino angled to the right" << std::endl; + std::cout << "'d': Delete the last domino that was created" << std::endl; + std::cout << std::endl; + std::cout << "spacebar: Begin simulation (you can no longer create dominoes)" << std::endl; + std::cout << "'f': Push the first domino so that it falls over" << std::endl; + std::cout << "'v': Turn contact force visualization on/off" << std::endl; + glutInit(&argc, argv); + window.initWindow(640, 480, "Dominoes"); + glutMainLoop(); } From dd21616a8d1cacaabb23fde622086b89454f9607 Mon Sep 17 00:00:00 2001 From: Karen Liu Date: Tue, 16 Jun 2015 10:50:22 -0400 Subject: [PATCH 07/65] minor changes on style --- tutorials/tutorialMultiPendulum-Finished.cpp | 6 +++--- tutorials/tutorialMultiPendulum.cpp | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/tutorials/tutorialMultiPendulum-Finished.cpp b/tutorials/tutorialMultiPendulum-Finished.cpp index 196e76af0462c..ffc8355dc1e16 100644 --- a/tutorials/tutorialMultiPendulum-Finished.cpp +++ b/tutorials/tutorialMultiPendulum-Finished.cpp @@ -185,21 +185,21 @@ class MyWindow : public dart::gui::SimWindow break; case 'q': - changeRestPosition( delta_rest_position); + changeRestPosition(delta_rest_position); break; case 'a': changeRestPosition(-delta_rest_position); break; case 'w': - changeStiffness( delta_stiffness); + changeStiffness(delta_stiffness); break; case 's': changeStiffness(-delta_stiffness); break; case 'e': - changeDamping( delta_damping); + changeDamping(delta_damping); break; case 'd': changeDamping(-delta_damping); diff --git a/tutorials/tutorialMultiPendulum.cpp b/tutorials/tutorialMultiPendulum.cpp index eba4c30613b97..eeca8abea832a 100644 --- a/tutorials/tutorialMultiPendulum.cpp +++ b/tutorials/tutorialMultiPendulum.cpp @@ -149,21 +149,21 @@ class MyWindow : public dart::gui::SimWindow break; case 'q': - changeRestPosition( delta_rest_position); + changeRestPosition(delta_rest_position); break; case 'a': changeRestPosition(-delta_rest_position); break; case 'w': - changeStiffness( delta_stiffness); + changeStiffness(delta_stiffness); break; case 's': changeStiffness(-delta_stiffness); break; case 'e': - changeDamping( delta_damping); + changeDamping(delta_damping); break; case 'd': changeDamping(-delta_damping); From 1491e78b0f81984eed8bb7c40f9baa8e5762e4b4 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Tue, 16 Jun 2015 16:51:02 -0400 Subject: [PATCH 08/65] improvements to multi-pendulum tutorial --- dart/math/Helpers.h | 10 ++ tutorials/tutorialMultiPendulum-Finished.cpp | 143 +++++++++++++++---- tutorials/tutorialMultiPendulum.cpp | 78 ++++++++-- 3 files changed, 190 insertions(+), 41 deletions(-) diff --git a/dart/math/Helpers.h b/dart/math/Helpers.h index 1ef3ac739df00..1d3198e482bd2 100644 --- a/dart/math/Helpers.h +++ b/dart/math/Helpers.h @@ -269,6 +269,16 @@ inline Eigen::Vector3d Red() return Eigen::Vector3d(0.9, 0.1, 0.1); } +inline Eigen::Vector4d Orange(double alpha) +{ + return Eigen::Vector4d(1.0, 0.63, 0.0, alpha); +} + +inline Eigen::Vector3d Orange() +{ + return Eigen::Vector3d(1.0, 0.63, 0.0); +} + inline Eigen::Vector4d Green(double alpha) { return Eigen::Vector4d(0.1, 0.9, 0.1, alpha); diff --git a/tutorials/tutorialMultiPendulum-Finished.cpp b/tutorials/tutorialMultiPendulum-Finished.cpp index 196e76af0462c..6c5cca37230b3 100644 --- a/tutorials/tutorialMultiPendulum-Finished.cpp +++ b/tutorials/tutorialMultiPendulum-Finished.cpp @@ -41,6 +41,7 @@ const double default_width = 0.2; // m const double default_depth = 0.2; // m const double default_torque = 15.0; // N-m +const double default_force = 15.0; // N const int default_countdown = 200; // Number of timesteps for applying force const double default_rest_position = 0.0; @@ -61,8 +62,9 @@ class MyWindow : public dart::gui::SimWindow /// Constructor MyWindow(WorldPtr world) - : _weldConstraint(nullptr), - _positiveSign(true) + : _ballConstraint(nullptr), + _positiveSign(true), + _bodyForce(false) { setWorld(world); @@ -73,6 +75,30 @@ class MyWindow : public dart::gui::SimWindow assert(_pendulum != nullptr); _forceCountDown.resize(_pendulum->getNumDofs(), 0); + + ArrowShape::Properties arrow_properties; + arrow_properties.mRadius = 0.05; + _arrow = std::shared_ptr(new ArrowShape( + Eigen::Vector3d(-default_height, 0.0, default_height/2.0), + Eigen::Vector3d(-default_width/2.0, 0.0, default_height/2.0), + arrow_properties, dart::Color::Orange(1.0))); + } + + void changeDirection() + { + _positiveSign = !_positiveSign; + if(_positiveSign) + { + _arrow->setPositions( + Eigen::Vector3d(-default_height, 0.0, default_height/2.0), + Eigen::Vector3d(-default_width/2.0, 0.0, default_height/2.0)); + } + else + { + _arrow->setPositions( + Eigen::Vector3d( default_height, 0.0, default_height/2.0), + Eigen::Vector3d( default_width/2.0, 0.0, default_height/2.0)); + } } void applyForce(size_t index) @@ -125,23 +151,24 @@ class MyWindow : public dart::gui::SimWindow } } - /// Add a constraint to turn the bottom of the pendulum into a triangle + /// Add a constraint to attach the final link to the world void addConstraint() { // Get the last body in the pendulum BodyNode* tip = _pendulum->getBodyNode(_pendulum->getNumBodyNodes()-1); - // Weld the last link to the world - _weldConstraint = new dart::constraint::WeldJointConstraint(tip); - mWorld->getConstraintSolver()->addConstraint(_weldConstraint); + // Attach the last link to the world + _ballConstraint = new dart::constraint::BallJointConstraint( + tip, tip->getTransform()*Eigen::Vector3d(0.0, 0.0, default_height)); + mWorld->getConstraintSolver()->addConstraint(_ballConstraint); } /// Remove any existing constraint, allowing the pendulum to flail freely void removeConstraint() { - mWorld->getConstraintSolver()->removeConstraint(_weldConstraint); - delete _weldConstraint; - _weldConstraint = nullptr; + mWorld->getConstraintSolver()->removeConstraint(_ballConstraint); + delete _ballConstraint; + _ballConstraint = nullptr; } /// Handle keyboard input @@ -149,6 +176,10 @@ class MyWindow : public dart::gui::SimWindow { switch(key) { + case '-': + changeDirection(); + break; + case '1': applyForce(0); break; @@ -180,10 +211,6 @@ class MyWindow : public dart::gui::SimWindow applyForce(9); break; - case '-': - _positiveSign = !_positiveSign; - break; - case 'q': changeRestPosition( delta_rest_position); break; @@ -207,13 +234,17 @@ class MyWindow : public dart::gui::SimWindow case 'r': { - if(_weldConstraint) + if(_ballConstraint) removeConstraint(); else addConstraint(); break; } + case 'f': + _bodyForce = !_bodyForce; + break; + default: SimWindow::keyboard(key, x, y); } @@ -222,25 +253,72 @@ class MyWindow : public dart::gui::SimWindow void timeStepping() override { // Reset all the shapes to be Blue - for(size_t i=0; i<_pendulum->getNumBodyNodes(); ++i) + for(size_t i = 0; i < _pendulum->getNumBodyNodes(); ++i) { - const ShapePtr& shape = - _pendulum->getBodyNode(i)->getVisualizationShape(0); + BodyNode* bn = _pendulum->getBodyNode(i); + for(size_t j = 0; j < 2; ++j) + { + const ShapePtr& shape = bn->getVisualizationShape(j); + + shape->setColor(dart::Color::Blue()); + } - shape->setColor(dart::Color::Blue()); + // If we have three visualization shapes, that means the arrow is + // attached. We should remove it in case this body is no longer + // experiencing a force + if(bn->getNumVisualizationShapes() == 3) + { + bn->removeVisualizationShape(_arrow); + } } - // Any DOFs with an active countdown are given a joint force and colored red - for(size_t i=0; i<_forceCountDown.size(); ++i) + if(_bodyForce) { - DegreeOfFreedom* dof = _pendulum->getDof(i); - const ShapePtr& shape = dof->getChildBodyNode()->getVisualizationShape(0); - - if(_forceCountDown[i] > 0) + // Apply body forces based on user input, and color the body shape red + for(size_t i = 0; i < _pendulum->getNumBodyNodes(); ++i) { - dof->setForce(_positiveSign? default_torque : -default_torque); - shape->setColor(dart::Color::Red()); - --_forceCountDown[i]; + if(_forceCountDown[i] > 0) + { + BodyNode* bn = _pendulum->getBodyNode(i); + const ShapePtr& shape = bn->getVisualizationShape(1); + shape->setColor(dart::Color::Red()); + + if(_positiveSign) + { + bn->addExtForce( + default_force*Eigen::Vector3d::UnitX(), + Eigen::Vector3d(-default_width/2.0, 0.0, default_height/2.0), + true, true); + } + else + { + bn->addExtForce( + -default_force*Eigen::Vector3d::UnitX(), + Eigen::Vector3d( default_width/2.0, 0.0, default_height/2.0), + true, true); + } + + bn->addVisualizationShape(_arrow); + + --_forceCountDown[i]; + } + } + } + else + { + // Apply joint torques based on user input, and color the Joint shape red + for(size_t i=0; i<_pendulum->getNumDofs(); ++i) + { + if(_forceCountDown[i] > 0) + { + DegreeOfFreedom* dof = _pendulum->getDof(i); + BodyNode* bn = dof->getChildBodyNode(); + const ShapePtr& shape = bn->getVisualizationShape(0); + + dof->setForce(_positiveSign? default_torque : -default_torque); + shape->setColor(dart::Color::Red()); + --_forceCountDown[i]; + } } } @@ -250,17 +328,24 @@ class MyWindow : public dart::gui::SimWindow protected: + /// An arrow shape that we will use to visualize applied forces + std::shared_ptr _arrow; + /// The pendulum that we will be perturbing SkeletonPtr _pendulum; - /// Pointer to the weld constraint that we will be turning on and off - dart::constraint::WeldJointConstraint* _weldConstraint; + /// Pointer to the ball constraint that we will be turning on and off + dart::constraint::BallJointConstraint* _ballConstraint; /// Number of iterations before clearing a force entry std::vector _forceCountDown; /// Whether a force should be applied in the positive or negative direction bool _positiveSign; + + /// True if 1-9 should be used to apply a BodyForce. Otherwise, 1-9 will be + /// used to apply a joint torque. + bool _bodyForce; }; void setGeometry(const BodyNodePtr& bn) diff --git a/tutorials/tutorialMultiPendulum.cpp b/tutorials/tutorialMultiPendulum.cpp index eba4c30613b97..aa644de75688e 100644 --- a/tutorials/tutorialMultiPendulum.cpp +++ b/tutorials/tutorialMultiPendulum.cpp @@ -1,4 +1,3 @@ - /* * Copyright (c) 2015, Georgia Tech Research Corporation * All rights reserved. @@ -42,6 +41,7 @@ const double default_width = 0.2; // m const double default_depth = 0.2; // m const double default_torque = 15.0; // N-m +const double default_force = 15.0; // N const int default_countdown = 200; // Number of timesteps for applying force const double default_rest_position = 0.0; @@ -62,8 +62,9 @@ class MyWindow : public dart::gui::SimWindow /// Constructor MyWindow(WorldPtr world) - : _weldConstraint(nullptr), - _positiveSign(true) + : _ballConstraint(nullptr), + _positiveSign(true), + _bodyForce(false) { setWorld(world); @@ -74,6 +75,30 @@ class MyWindow : public dart::gui::SimWindow assert(_pendulum != nullptr); _forceCountDown.resize(_pendulum->getNumDofs(), 0); + + ArrowShape::Properties arrow_properties; + arrow_properties.mRadius = 0.05; + _arrow = std::shared_ptr(new ArrowShape( + Eigen::Vector3d(-default_height, 0.0, default_height/2.0), + Eigen::Vector3d(-default_width/2.0, 0.0, default_height/2.0), + arrow_properties, dart::Color::Orange(1.0))); + } + + void changeDirection() + { + _positiveSign = !_positiveSign; + if(_positiveSign) + { + _arrow->setPositions( + Eigen::Vector3d(-default_height, 0.0, default_height/2.0), + Eigen::Vector3d(-default_width/2.0, 0.0, default_height/2.0)); + } + else + { + _arrow->setPositions( + Eigen::Vector3d( default_height, 0.0, default_height/2.0), + Eigen::Vector3d( default_width/2.0, 0.0, default_height/2.0)); + } } void applyForce(size_t) @@ -96,7 +121,7 @@ class MyWindow : public dart::gui::SimWindow // Lesson 4 } - /// Add a constraint to turn the bottom of the pendulum into a triangle + /// Add a constraint to attach the final link to the world void addConstraint() { // Lesson 5 @@ -105,7 +130,7 @@ class MyWindow : public dart::gui::SimWindow /// Remove any existing constraint, allowing the pendulum to flail freely void removeConstraint() { - // Lesson 6 + // Lesson 5 } /// Handle keyboard input @@ -113,6 +138,10 @@ class MyWindow : public dart::gui::SimWindow { switch(key) { + case '-': + changeDirection(); + break; + case '1': applyForce(0); break; @@ -144,10 +173,6 @@ class MyWindow : public dart::gui::SimWindow applyForce(9); break; - case '-': - _positiveSign = !_positiveSign; - break; - case 'q': changeRestPosition( delta_rest_position); break; @@ -171,13 +196,17 @@ class MyWindow : public dart::gui::SimWindow case 'r': { - if(_weldConstraint) + if(_ballConstraint) removeConstraint(); else addConstraint(); break; } + case 'f': + _bodyForce = !_bodyForce; + break; + default: SimWindow::keyboard(key, x, y); } @@ -185,25 +214,50 @@ class MyWindow : public dart::gui::SimWindow void timeStepping() override { + // Reset all the shapes to be Blue // Lesson 1 + if(_bodyForce) + { + // Apply body forces based on user input, and color the body shape red + for(size_t i = 0; i < _pendulum->getNumBodyNodes(); ++i) + { + // Lesson 1b + } + } + else + { + // Apply joint torques based on user input, and color the Joint shape red + for(size_t i=0; i<_pendulum->getNumDofs(); ++i) + { + // Lesson 1a + } + } + // Step the simulation forward SimWindow::timeStepping(); } protected: + /// An arrow shape that we will use to visualize applied forces + std::shared_ptr _arrow; + /// The pendulum that we will be perturbing SkeletonPtr _pendulum; - /// Pointer to the weld constraint that we will be turning on and off - dart::constraint::WeldJointConstraint* _weldConstraint; + /// Pointer to the ball constraint that we will be turning on and off + dart::constraint::BallJointConstraint* _ballConstraint; /// Number of iterations before clearing a force entry std::vector _forceCountDown; /// Whether a force should be applied in the positive or negative direction bool _positiveSign; + + /// True if 1-9 should be used to apply a BodyForce. Otherwise, 1-9 will be + /// used to apply a joint torque. + bool _bodyForce; }; void setGeometry(const BodyNodePtr& bn) From 8cf601fe9471093447d815bf39dccd61023c8efa Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Tue, 16 Jun 2015 16:54:55 -0400 Subject: [PATCH 09/65] fixed wording in comment --- tutorials/tutorialMultiPendulum-Finished.cpp | 2 +- tutorials/tutorialMultiPendulum.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/tutorials/tutorialMultiPendulum-Finished.cpp b/tutorials/tutorialMultiPendulum-Finished.cpp index 6c5cca37230b3..2b3cdaa301ba4 100644 --- a/tutorials/tutorialMultiPendulum-Finished.cpp +++ b/tutorials/tutorialMultiPendulum-Finished.cpp @@ -343,7 +343,7 @@ class MyWindow : public dart::gui::SimWindow /// Whether a force should be applied in the positive or negative direction bool _positiveSign; - /// True if 1-9 should be used to apply a BodyForce. Otherwise, 1-9 will be + /// True if 1-9 should be used to apply a body force. Otherwise, 1-9 will be /// used to apply a joint torque. bool _bodyForce; }; diff --git a/tutorials/tutorialMultiPendulum.cpp b/tutorials/tutorialMultiPendulum.cpp index aa644de75688e..1564f07767980 100644 --- a/tutorials/tutorialMultiPendulum.cpp +++ b/tutorials/tutorialMultiPendulum.cpp @@ -255,7 +255,7 @@ class MyWindow : public dart::gui::SimWindow /// Whether a force should be applied in the positive or negative direction bool _positiveSign; - /// True if 1-9 should be used to apply a BodyForce. Otherwise, 1-9 will be + /// True if 1-9 should be used to apply a body force. Otherwise, 1-9 will be /// used to apply a joint torque. bool _bodyForce; }; From 72253b2eee1c4e0096936b222b201f3a600094eb Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Tue, 16 Jun 2015 16:55:37 -0400 Subject: [PATCH 10/65] added a line of instruction --- tutorials/tutorialMultiPendulum.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/tutorials/tutorialMultiPendulum.cpp b/tutorials/tutorialMultiPendulum.cpp index 1564f07767980..0d44d2635415f 100644 --- a/tutorials/tutorialMultiPendulum.cpp +++ b/tutorials/tutorialMultiPendulum.cpp @@ -377,6 +377,7 @@ int main(int argc, char* argv[]) std::cout << "'e': Increase joint damping" << std::endl; std::cout << "'d': Decrease joint damping" << std::endl; std::cout << "'r': add/remove constraint on the end of the chain" << std::endl; + std::cout << "'f': switch between applying joint torques and body forces" << std::endl; // Initialize glut, initialize the window, and begin the glut event loop glutInit(&argc, argv); From 51e414c4abf3c062515544fd4b22cf60720fb26c Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Wed, 17 Jun 2015 09:51:21 -0400 Subject: [PATCH 11/65] added instruction line --- tutorials/tutorialMultiPendulum-Finished.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/tutorials/tutorialMultiPendulum-Finished.cpp b/tutorials/tutorialMultiPendulum-Finished.cpp index 2b3cdaa301ba4..b02cd64d8468c 100644 --- a/tutorials/tutorialMultiPendulum-Finished.cpp +++ b/tutorials/tutorialMultiPendulum-Finished.cpp @@ -465,6 +465,7 @@ int main(int argc, char* argv[]) std::cout << "'e': Increase joint damping" << std::endl; std::cout << "'d': Decrease joint damping" << std::endl; std::cout << "'r': add/remove constraint on the end of the chain" << std::endl; + std::cout << "'f': switch between applying joint torques and body forces" << std::endl; // Initialize glut, initialize the window, and begin the glut event loop glutInit(&argc, argv); From 41b52b9baa37148ab7ba52cd55f0e19f084efff4 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Wed, 17 Jun 2015 12:08:34 -0400 Subject: [PATCH 12/65] adding manipulator to the dominoes tutorial --- tutorials/tutorialDominoes.cpp | 60 ++++++++++++++++++++++++++++++++-- 1 file changed, 57 insertions(+), 3 deletions(-) diff --git a/tutorials/tutorialDominoes.cpp b/tutorials/tutorialDominoes.cpp index eb2051ac68b8f..3c5a1624aae34 100644 --- a/tutorials/tutorialDominoes.cpp +++ b/tutorials/tutorialDominoes.cpp @@ -36,8 +36,8 @@ #include "dart/dart.h" -const double default_domino_height = 0.07; -const double default_domino_width = 0.03; +const double default_domino_height = 0.2; +const double default_domino_width = 0.4*default_domino_height; const double default_domino_depth = default_domino_width/5.0; const double default_distance = default_domino_height/2.0; @@ -49,6 +49,37 @@ const int default_push_duration = 200; // # iterations using namespace dart::dynamics; using namespace dart::simulation; +class Controller +{ +public: + + Controller(const SkeletonPtr& manipulator, const SkeletonPtr& domino) + : _manipulator(manipulator) + { + // Create a transform from the center of the domino to the top of the domino + Eigen::Isometry3d target_offset(Eigen::Isometry3d::Identity()); + target_offset.translation() = + default_domino_height/2.0 * Eigen::Vector3d::UnitZ(); + + // Place the _target SimpleFrame at the top of the domino + _target = std::make_shared( + domino->getBodyNode(0), "target", target_offset); + } + + void computeForces() + { + if(nullptr == _manipulator) + return; + } + +protected: + + SkeletonPtr _manipulator; + + SimpleFramePtr _target; + +}; + class MyWindow : public dart::gui::SimWindow { public: @@ -63,6 +94,8 @@ class MyWindow : public dart::gui::SimWindow _floor = world->getSkeleton("floor"); } + // Attempt to create a new domino. If the new domino would be in collision + // with anything (other than the floor), then discard it. void attemptToCreateDomino(double angle) { const SkeletonPtr& lastDomino = _dominoes.size()>0? @@ -120,6 +153,8 @@ class MyWindow : public dart::gui::SimWindow } } + // Delete the last domino that was added to the scene. (Do not delete the + // original domino) void deleteLastDomino() { if(_dominoes.size() > 0) @@ -171,6 +206,8 @@ class MyWindow : public dart::gui::SimWindow void timeStepping() override { + // If the user has pressed the 'f' key, apply a force to the first domino in + // order to push it over if(_pushCountDown > 0) { _firstDomino->getBodyNode(0)->addExtForce( @@ -255,14 +292,31 @@ SkeletonPtr createFloor() return floor; } +SkeletonPtr createManipulator() +{ + // Load the Skeleton from a file + dart::utils::DartLoader loader; + SkeletonPtr manipulator = + loader.parseSkeleton(DART_DATA_PATH"urdf/KR5/KR5 sixx R650.urdf"); + + // Position its base in a reasonable way + Eigen::Isometry3d tf = Eigen::Isometry3d::Identity(); + tf.translation() = Eigen::Vector3d(-0.75, 0.0, 0.0); + manipulator->getJoint(0)->setTransformFromParentBodyNode(tf); + + return manipulator; +} + int main(int argc, char* argv[]) { SkeletonPtr domino = createDomino(); SkeletonPtr floor = createFloor(); + SkeletonPtr manipulator = createManipulator(); WorldPtr world(new World); world->addSkeleton(domino); world->addSkeleton(floor); + world->addSkeleton(manipulator); MyWindow window(world); @@ -272,7 +326,7 @@ int main(int argc, char* argv[]) std::cout << "'e': Create new domino angled to the right" << std::endl; std::cout << "'d': Delete the last domino that was created" << std::endl; std::cout << std::endl; - std::cout << "spacebar: Begin simulation (you can no longer create dominoes)" << std::endl; + std::cout << "spacebar: Begin simulation (you can no longer create or remove dominoes)" << std::endl; std::cout << "'f': Push the first domino so that it falls over" << std::endl; std::cout << "'v': Turn contact force visualization on/off" << std::endl; From a3f56da0e71d59f9599850a689218ac95cb010a1 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Wed, 17 Jun 2015 22:04:03 -0400 Subject: [PATCH 13/65] more realistic dominoes --- tutorials/tutorialDominoes.cpp | 49 +++++++++++++++++++++++++++++++--- 1 file changed, 45 insertions(+), 4 deletions(-) diff --git a/tutorials/tutorialDominoes.cpp b/tutorials/tutorialDominoes.cpp index 3c5a1624aae34..4695dcbd95eba 100644 --- a/tutorials/tutorialDominoes.cpp +++ b/tutorials/tutorialDominoes.cpp @@ -36,16 +36,25 @@ #include "dart/dart.h" -const double default_domino_height = 0.2; +const double default_domino_height = 0.3; const double default_domino_width = 0.4*default_domino_height; const double default_domino_depth = default_domino_width/5.0; const double default_distance = default_domino_height/2.0; const double default_angle = 20.0*M_PI/180.0; +const double default_domino_density = 2.6e3; // kg/m^3 +const double default_domino_mass = + default_domino_density + * default_domino_height + * default_domino_width + * default_domino_depth; + const double default_push_force = 8.0; // N const int default_push_duration = 200; // # iterations +const double default_endeffector_offset = 0.05; + using namespace dart::dynamics; using namespace dart::simulation; @@ -64,6 +73,11 @@ class Controller // Place the _target SimpleFrame at the top of the domino _target = std::make_shared( domino->getBodyNode(0), "target", target_offset); + + // Grab the last body in the manipulator, and use it as an end effector + _endEffector = _manipulator->getBodyNode(_manipulator->getNumBodyNodes()-1); + + _offset = default_endeffector_offset * Eigen::Vector3d::UnitX(); } void computeForces() @@ -74,10 +88,27 @@ class Controller protected: + /// The manipulator Skeleton that we will be controlling SkeletonPtr _manipulator; + /// The target pose for the controller SimpleFramePtr _target; + /// End effector for the manipulator + BodyNodePtr _endEffector; + + /// The offset of the end effector from the body origin of the last BodyNode + /// in the manipulator + Eigen::Vector3d _offset; + + /// Control gains for the proportional error terms + Eigen::Matrix3d _Kp; + + /// Control gains for the derivative error terms + Eigen::MatrixXd _Kd; + + /// Joint forces for the manipulator (output of the Controller) + Eigen::VectorXd _forces; }; class MyWindow : public dart::gui::SimWindow @@ -247,13 +278,14 @@ class MyWindow : public dart::gui::SimWindow SkeletonPtr createDomino() { - /// Create a Skeleton with the name "domino" + // Create a Skeleton with the name "domino" SkeletonPtr domino = Skeleton::create("domino"); - /// Create a body for the domino + // Create a body for the domino BodyNodePtr body = domino->createJointAndBodyNodePair(nullptr).second; + // Create a shape for the domino std::shared_ptr box( new BoxShape(Eigen::Vector3d(default_domino_depth, default_domino_width, @@ -261,6 +293,12 @@ SkeletonPtr createDomino() body->addVisualizationShape(box); body->addCollisionShape(box); + // Set up inertia for the domino + Inertia inertia; + inertia.setMass(default_domino_mass); + inertia.setMoment(box->computeInertia(default_domino_mass)); + body->setInertia(inertia); + domino->getDof("Joint_pos_z")->setPosition(default_domino_height/2.0); return domino; @@ -301,9 +339,12 @@ SkeletonPtr createManipulator() // Position its base in a reasonable way Eigen::Isometry3d tf = Eigen::Isometry3d::Identity(); - tf.translation() = Eigen::Vector3d(-0.75, 0.0, 0.0); + tf.translation() = Eigen::Vector3d(-0.65, 0.0, 0.0); manipulator->getJoint(0)->setTransformFromParentBodyNode(tf); + manipulator->getDof(1)->setPosition(140.0*M_PI/180.0); + manipulator->getDof(2)->setPosition(-140.0*M_PI/180.0); + return manipulator; } From c558097d3edf387b9d37332672b32a021bbb8fbb Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Thu, 18 Jun 2015 16:47:15 -0400 Subject: [PATCH 14/65] implemented controllers for manipulator --- tutorials/tutorialDominoes.cpp | 140 ++++++++++++++++++++++++++++++--- 1 file changed, 127 insertions(+), 13 deletions(-) diff --git a/tutorials/tutorialDominoes.cpp b/tutorials/tutorialDominoes.cpp index 4695dcbd95eba..03f0e73d64960 100644 --- a/tutorials/tutorialDominoes.cpp +++ b/tutorials/tutorialDominoes.cpp @@ -50,13 +50,15 @@ const double default_domino_mass = * default_domino_width * default_domino_depth; -const double default_push_force = 8.0; // N -const int default_push_duration = 200; // # iterations +const double default_push_force = 8.0; // N +const int default_force_duration = 200; // # iterations +const int default_push_duration = 1000; // # iterations const double default_endeffector_offset = 0.05; using namespace dart::dynamics; using namespace dart::simulation; +using namespace dart::math; class Controller { @@ -71,19 +73,92 @@ class Controller default_domino_height/2.0 * Eigen::Vector3d::UnitZ(); // Place the _target SimpleFrame at the top of the domino - _target = std::make_shared( - domino->getBodyNode(0), "target", target_offset); + _target = std::make_shared(Frame::World(), "target"); + _target->setTransform(target_offset, domino->getBodyNode(0)); // Grab the last body in the manipulator, and use it as an end effector _endEffector = _manipulator->getBodyNode(_manipulator->getNumBodyNodes()-1); + // Compute the body frame offset for the end effector _offset = default_endeffector_offset * Eigen::Vector3d::UnitX(); + + // Grab the current joint angles to use them as desired angles + _qDesired = _manipulator->getPositions(); + + // Set control gains + _Kp_PD = 200.0; + _Kd_PD = 20.0; + + _Kp_OS = 5.0; + _Kd_OS = 1.0; + } + + /// Compute a stable PD controller that also compensates for gravity and + /// Coriolis forces + void setPDForces() + { + if(nullptr == _manipulator) + return; + + // Compute the joint forces needed to compensate for Coriolis forces and + // gravity + const Eigen::VectorXd& Cg = _manipulator->getCoriolisAndGravityForces(); + + // Compute the joint position error + Eigen::VectorXd q = _manipulator->getPositions(); + Eigen::VectorXd dq = _manipulator->getVelocities(); + q += dq * _manipulator->getTimeStep(); + + Eigen::VectorXd q_err = _qDesired - q; + + // Compute the joint velocity error + Eigen::VectorXd dq_err = -dq; + + // Compute the desired joint forces + const Eigen::MatrixXd& M = _manipulator->getMassMatrix(); + _forces = M*(_Kp_PD * q_err + _Kd_PD * dq_err) + Cg; + + _manipulator->setForces(_forces); } - void computeForces() + /// Compute an operational space controller to push on the first domino + void setOperationalSpaceForces() { if(nullptr == _manipulator) return; + + const Eigen::MatrixXd& M = _manipulator->getMassMatrix(); + + // Compute the Jacobian + LinearJacobian J = _endEffector->getLinearJacobian(_offset); + // Compute the pseudo-inverse of the Jacobian + Eigen::MatrixXd pinv_J = J.transpose() * ( J * J.transpose() + + 0.0025*Eigen::Matrix3d::Identity() ).inverse(); + + // Compute the Jacobian time derivative + LinearJacobian dJ = _endEffector->getLinearJacobianDeriv(_offset); + // Comptue the pseudo-inverse of the Jacobian time derivative + Eigen::MatrixXd pinv_dJ = dJ.transpose() * ( dJ * dJ.transpose() + + 0.0025*Eigen::Matrix3d::Identity() ).inverse(); + + // Compute the error + Eigen::Vector3d e = _target->getWorldTransform().translation() + - _endEffector->getWorldTransform()*_offset; + + // Compute the time derivative of the error + Eigen::Vector3d de = - _endEffector->getLinearVelocity(_offset); + + // Compute the forces needed to compensate for Coriolis forces and gravity + const Eigen::VectorXd& Cg = _manipulator->getCoriolisAndGravityForces(); + + // Turn the control gains into matrix form + Eigen::Matrix3d Kp = _Kp_OS * Eigen::Matrix3d::Identity(); + size_t dofs = _manipulator->getNumDofs(); + Eigen::MatrixXd Kd = _Kd_OS * Eigen::MatrixXd::Identity(dofs, dofs); + + _forces = M * (pinv_J*Kp*de + pinv_dJ*_Kp_OS*e) + Cg + Kd*pinv_J*_Kp_OS*e; + + _manipulator->setForces(_forces); } protected: @@ -97,15 +172,26 @@ class Controller /// End effector for the manipulator BodyNodePtr _endEffector; + /// Desired joint positions when not applying the operational space controller + Eigen::VectorXd _qDesired; + /// The offset of the end effector from the body origin of the last BodyNode /// in the manipulator Eigen::Vector3d _offset; - /// Control gains for the proportional error terms - Eigen::Matrix3d _Kp; + /// Control gains for the proportional error terms in the PD controller + double _Kp_PD; + + /// Control gains for the derivative error terms in the PD controller + double _Kd_PD; - /// Control gains for the derivative error terms - Eigen::MatrixXd _Kd; + /// Control gains for the proportional error terms in the operational space + /// controller + double _Kp_OS; + + /// Control gains for the derivative error terms in the operational space + /// controller + double _Kd_OS; /// Joint forces for the manipulator (output of the Controller) Eigen::VectorXd _forces; @@ -118,11 +204,15 @@ class MyWindow : public dart::gui::SimWindow MyWindow(const WorldPtr& world) : _totalAngle(0.0), _hasEverRun(false), + _forceCountDown(0), _pushCountDown(0) { setWorld(world); _firstDomino = world->getSkeleton("domino"); _floor = world->getSkeleton("floor"); + + _controller = std::unique_ptr( + new Controller(world->getSkeleton("manipulator"), _firstDomino)); } // Attempt to create a new domino. If the new domino would be in collision @@ -227,6 +317,10 @@ class MyWindow : public dart::gui::SimWindow switch(key) { case 'f': + _forceCountDown = default_force_duration; + break; + + case 'r': _pushCountDown = default_push_duration; break; } @@ -239,14 +333,25 @@ class MyWindow : public dart::gui::SimWindow { // If the user has pressed the 'f' key, apply a force to the first domino in // order to push it over - if(_pushCountDown > 0) + if(_forceCountDown > 0) { _firstDomino->getBodyNode(0)->addExtForce( default_push_force*Eigen::Vector3d::UnitX(), default_domino_height/2.0*Eigen::Vector3d::UnitZ()); + --_forceCountDown; + } + + if(_pushCountDown > 0) + { + _controller->setOperationalSpaceForces(); + --_pushCountDown; } + else + { + _controller->setPDForces(); + } SimWindow::timeStepping(); } @@ -271,9 +376,16 @@ class MyWindow : public dart::gui::SimWindow /// Set to true the first time spacebar is pressed bool _hasEverRun; - /// The first domino will be pushed on while the value of this is positive + /// The first domino will be pushed by a disembodied force while the value of + /// this is greater than zero + int _forceCountDown; + + /// The manipulator will attempt to push on the first domino while the value + /// of this is greater than zero int _pushCountDown; + std::unique_ptr _controller; + }; SkeletonPtr createDomino() @@ -294,7 +406,7 @@ SkeletonPtr createDomino() body->addCollisionShape(box); // Set up inertia for the domino - Inertia inertia; + dart::dynamics::Inertia inertia; inertia.setMass(default_domino_mass); inertia.setMoment(box->computeInertia(default_domino_mass)); body->setInertia(inertia); @@ -336,6 +448,7 @@ SkeletonPtr createManipulator() dart::utils::DartLoader loader; SkeletonPtr manipulator = loader.parseSkeleton(DART_DATA_PATH"urdf/KR5/KR5 sixx R650.urdf"); + manipulator->setName("manipulator"); // Position its base in a reasonable way Eigen::Isometry3d tf = Eigen::Isometry3d::Identity(); @@ -368,7 +481,8 @@ int main(int argc, char* argv[]) std::cout << "'d': Delete the last domino that was created" << std::endl; std::cout << std::endl; std::cout << "spacebar: Begin simulation (you can no longer create or remove dominoes)" << std::endl; - std::cout << "'f': Push the first domino so that it falls over" << std::endl; + std::cout << "'f': Push the first domino with a disembodies force so that it falls over" << std::endl; + std::cout << "'r': Push the first domino with the manipulator so that it falls over" << std::endl; std::cout << "'v': Turn contact force visualization on/off" << std::endl; glutInit(&argc, argv); From 00a6369cc348ade37acea715ed896f93092cc3af Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Thu, 18 Jun 2015 17:28:21 -0400 Subject: [PATCH 15/65] improved operational space controller --- tutorials/tutorialDominoes.cpp | 63 ++++++++++++++++++++++++---------- 1 file changed, 45 insertions(+), 18 deletions(-) diff --git a/tutorials/tutorialDominoes.cpp b/tutorials/tutorialDominoes.cpp index 03f0e73d64960..c40854eb6669b 100644 --- a/tutorials/tutorialDominoes.cpp +++ b/tutorials/tutorialDominoes.cpp @@ -67,18 +67,23 @@ class Controller Controller(const SkeletonPtr& manipulator, const SkeletonPtr& domino) : _manipulator(manipulator) { + // Grab the last body in the manipulator, and use it as an end effector + _endEffector = _manipulator->getBodyNode(_manipulator->getNumBodyNodes()-1); + // Create a transform from the center of the domino to the top of the domino Eigen::Isometry3d target_offset(Eigen::Isometry3d::Identity()); target_offset.translation() = default_domino_height/2.0 * Eigen::Vector3d::UnitZ(); + // Rotate the transform so that it matches the orientation of the end + // effector + target_offset.linear() = + _endEffector->getTransform(domino->getBodyNode(0)).linear(); + // Place the _target SimpleFrame at the top of the domino _target = std::make_shared(Frame::World(), "target"); _target->setTransform(target_offset, domino->getBodyNode(0)); - // Grab the last body in the manipulator, and use it as an end effector - _endEffector = _manipulator->getBodyNode(_manipulator->getNumBodyNodes()-1); - // Compute the body frame offset for the end effector _offset = default_endeffector_offset * Eigen::Vector3d::UnitX(); @@ -89,8 +94,9 @@ class Controller _Kp_PD = 200.0; _Kd_PD = 20.0; - _Kp_OS = 5.0; - _Kd_OS = 1.0; + _Kp_OS_linear = 5.0; + _Kp_OS_angular = 5.0; + _Kd_OS = 0.01; } /// Compute a stable PD controller that also compensates for gravity and @@ -130,33 +136,50 @@ class Controller const Eigen::MatrixXd& M = _manipulator->getMassMatrix(); // Compute the Jacobian - LinearJacobian J = _endEffector->getLinearJacobian(_offset); + Jacobian J = _endEffector->getWorldJacobian(_offset); // Compute the pseudo-inverse of the Jacobian Eigen::MatrixXd pinv_J = J.transpose() * ( J * J.transpose() - + 0.0025*Eigen::Matrix3d::Identity() ).inverse(); + + 0.0025*Eigen::Matrix6d::Identity() ).inverse(); // Compute the Jacobian time derivative - LinearJacobian dJ = _endEffector->getLinearJacobianDeriv(_offset); + Jacobian dJ = _endEffector->getJacobianClassicDeriv(_offset); // Comptue the pseudo-inverse of the Jacobian time derivative Eigen::MatrixXd pinv_dJ = dJ.transpose() * ( dJ * dJ.transpose() - + 0.0025*Eigen::Matrix3d::Identity() ).inverse(); + + 0.0025*Eigen::Matrix6d::Identity() ).inverse(); - // Compute the error - Eigen::Vector3d e = _target->getWorldTransform().translation() - - _endEffector->getWorldTransform()*_offset; + // Compute the linear error + Eigen::Vector6d e; + e.tail<3>() = _target->getWorldTransform().translation() + - _endEffector->getWorldTransform()*_offset; + + // Compute the angular error + Eigen::AngleAxisd aa(_target->getTransform(_endEffector).linear()); + e.head<3>() = aa.angle() * aa.axis(); // Compute the time derivative of the error - Eigen::Vector3d de = - _endEffector->getLinearVelocity(_offset); + Eigen::Vector6d de = - _endEffector->getSpatialVelocity( + _offset, _target.get(), Frame::World()); // Compute the forces needed to compensate for Coriolis forces and gravity const Eigen::VectorXd& Cg = _manipulator->getCoriolisAndGravityForces(); // Turn the control gains into matrix form - Eigen::Matrix3d Kp = _Kp_OS * Eigen::Matrix3d::Identity(); + Eigen::Matrix6d Kp = Eigen::Matrix6d::Identity(); + Kp.diagonal().head<3>() = _Kp_OS_angular*Eigen::Vector3d::Ones(); + Kp.diagonal().tail<3>() = _Kp_OS_linear*Eigen::Vector3d::Ones(); + size_t dofs = _manipulator->getNumDofs(); Eigen::MatrixXd Kd = _Kd_OS * Eigen::MatrixXd::Identity(dofs, dofs); - _forces = M * (pinv_J*Kp*de + pinv_dJ*_Kp_OS*e) + Cg + Kd*pinv_J*_Kp_OS*e; + // Compute the joint forces needed to exert the desired workspace force + Eigen::Vector6d fDesired = Eigen::Vector6d::Zero(); + fDesired[3] = default_push_force; + Eigen::VectorXd f = J.transpose() * fDesired; + + // Compute the control forces + Eigen::VectorXd dq = _manipulator->getVelocities(); + _forces = M * (pinv_J*Kp*de + pinv_dJ*Kp*e) + - Kd*dq + Kd*pinv_J*Kp*e + Cg + f; _manipulator->setForces(_forces); } @@ -185,9 +208,13 @@ class Controller /// Control gains for the derivative error terms in the PD controller double _Kd_PD; - /// Control gains for the proportional error terms in the operational space - /// controller - double _Kp_OS; + /// Control gains for the proportional linear error terms in the operational + /// space controller + double _Kp_OS_linear; + + /// Control gains for the proportional angular error terms in the operational + /// space controller + double _Kp_OS_angular; /// Control gains for the derivative error terms in the operational space /// controller From 3d834243daee1b584f6a8da21cd838d227d9036d Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Thu, 18 Jun 2015 18:11:17 -0400 Subject: [PATCH 16/65] finished domino tutorial --- tutorials/tutorialDominoes-Finished.cpp | 514 ++++++++++++++++++++++++ tutorials/tutorialDominoes.cpp | 195 ++------- 2 files changed, 544 insertions(+), 165 deletions(-) create mode 100644 tutorials/tutorialDominoes-Finished.cpp diff --git a/tutorials/tutorialDominoes-Finished.cpp b/tutorials/tutorialDominoes-Finished.cpp new file mode 100644 index 0000000000000..b6e2b07519ddb --- /dev/null +++ b/tutorials/tutorialDominoes-Finished.cpp @@ -0,0 +1,514 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/dart.h" + +const double default_domino_height = 0.3; +const double default_domino_width = 0.4*default_domino_height; +const double default_domino_depth = default_domino_width/5.0; + +const double default_distance = default_domino_height/2.0; +const double default_angle = 20.0*M_PI/180.0; + +const double default_domino_density = 2.6e3; // kg/m^3 +const double default_domino_mass = + default_domino_density + * default_domino_height + * default_domino_width + * default_domino_depth; + +const double default_push_force = 8.0; // N +const int default_force_duration = 200; // # iterations +const int default_push_duration = 1000; // # iterations + +const double default_endeffector_offset = 0.05; + +using namespace dart::dynamics; +using namespace dart::simulation; +using namespace dart::math; + +class Controller +{ +public: + + Controller(const SkeletonPtr& manipulator, const SkeletonPtr& domino) + : _manipulator(manipulator) + { + // Grab the current joint angles to use them as desired angles + _qDesired = _manipulator->getPositions(); + + // Grab the last body in the manipulator, and use it as an end effector + _endEffector = _manipulator->getBodyNode(_manipulator->getNumBodyNodes()-1); + + // Compute the body frame offset for the end effector + _offset = default_endeffector_offset * Eigen::Vector3d::UnitX(); + + // Create a transform from the center of the domino to the top of the domino + Eigen::Isometry3d target_offset(Eigen::Isometry3d::Identity()); + target_offset.translation() = + default_domino_height/2.0 * Eigen::Vector3d::UnitZ(); + + // Rotate the transform so that it matches the orientation of the end + // effector + target_offset.linear() = + _endEffector->getTransform(domino->getBodyNode(0)).linear(); + + // Place the _target SimpleFrame at the top of the domino + _target = std::make_shared(Frame::World(), "target"); + _target->setTransform(target_offset, domino->getBodyNode(0)); + + // Set PD control gains + _Kp_PD = 200.0; + _Kd_PD = 20.0; + + // Set operational space control gains + _Kp_OS = 5.0; + _Kd_OS = 0.01; + } + + /// Compute a stable PD controller that also compensates for gravity and + /// Coriolis forces + void setPDForces() + { + if(nullptr == _manipulator) + return; + + // Compute the joint forces needed to compensate for Coriolis forces and + // gravity + const Eigen::VectorXd& Cg = _manipulator->getCoriolisAndGravityForces(); + + // Compute the joint position error + Eigen::VectorXd q = _manipulator->getPositions(); + Eigen::VectorXd dq = _manipulator->getVelocities(); + q += dq * _manipulator->getTimeStep(); + + Eigen::VectorXd q_err = _qDesired - q; + + // Compute the joint velocity error + Eigen::VectorXd dq_err = -dq; + + // Compute the desired joint forces + const Eigen::MatrixXd& M = _manipulator->getMassMatrix(); + _forces = M*(_Kp_PD * q_err + _Kd_PD * dq_err) + Cg; + + _manipulator->setForces(_forces); + } + + /// Compute an operational space controller to push on the first domino + void setOperationalSpaceForces() + { + if(nullptr == _manipulator) + return; + + const Eigen::MatrixXd& M = _manipulator->getMassMatrix(); + + // Compute the Jacobian + Jacobian J = _endEffector->getWorldJacobian(_offset); + // Compute the pseudo-inverse of the Jacobian + Eigen::MatrixXd pinv_J = J.transpose() * ( J * J.transpose() + + 0.0025*Eigen::Matrix6d::Identity() ).inverse(); + + // Compute the Jacobian time derivative + Jacobian dJ = _endEffector->getJacobianClassicDeriv(_offset); + // Comptue the pseudo-inverse of the Jacobian time derivative + Eigen::MatrixXd pinv_dJ = dJ.transpose() * ( dJ * dJ.transpose() + + 0.0025*Eigen::Matrix6d::Identity() ).inverse(); + + // Compute the linear error + Eigen::Vector6d e; + e.tail<3>() = _target->getWorldTransform().translation() + - _endEffector->getWorldTransform()*_offset; + + // Compute the angular error + Eigen::AngleAxisd aa(_target->getTransform(_endEffector).linear()); + e.head<3>() = aa.angle() * aa.axis(); + + // Compute the time derivative of the error + Eigen::Vector6d de = - _endEffector->getSpatialVelocity( + _offset, _target.get(), Frame::World()); + + // Compute the forces needed to compensate for Coriolis forces and gravity + const Eigen::VectorXd& Cg = _manipulator->getCoriolisAndGravityForces(); + + // Turn the control gains into matrix form + Eigen::Matrix6d Kp = _Kp_OS*Eigen::Matrix6d::Identity(); + + size_t dofs = _manipulator->getNumDofs(); + Eigen::MatrixXd Kd = _Kd_OS * Eigen::MatrixXd::Identity(dofs, dofs); + + // Compute the joint forces needed to exert the desired workspace force + Eigen::Vector6d fDesired = Eigen::Vector6d::Zero(); + fDesired[3] = default_push_force; + Eigen::VectorXd f = J.transpose() * fDesired; + + // Compute the control forces + Eigen::VectorXd dq = _manipulator->getVelocities(); + _forces = M * (pinv_J*Kp*de + pinv_dJ*Kp*e) + - Kd*dq + Kd*pinv_J*Kp*e + Cg + f; + + _manipulator->setForces(_forces); + } + +protected: + + /// The manipulator Skeleton that we will be controlling + SkeletonPtr _manipulator; + + /// The target pose for the controller + SimpleFramePtr _target; + + /// End effector for the manipulator + BodyNodePtr _endEffector; + + /// Desired joint positions when not applying the operational space controller + Eigen::VectorXd _qDesired; + + /// The offset of the end effector from the body origin of the last BodyNode + /// in the manipulator + Eigen::Vector3d _offset; + + /// Control gains for the proportional error terms in the PD controller + double _Kp_PD; + + /// Control gains for the derivative error terms in the PD controller + double _Kd_PD; + + /// Control gains for the proportional error terms in the operational + /// space controller + double _Kp_OS; + + /// Control gains for the derivative error terms in the operational space + /// controller + double _Kd_OS; + + /// Joint forces for the manipulator (output of the Controller) + Eigen::VectorXd _forces; +}; + +class MyWindow : public dart::gui::SimWindow +{ +public: + + MyWindow(const WorldPtr& world) + : _totalAngle(0.0), + _hasEverRun(false), + _forceCountDown(0), + _pushCountDown(0) + { + setWorld(world); + _firstDomino = world->getSkeleton("domino"); + _floor = world->getSkeleton("floor"); + + _controller = std::unique_ptr( + new Controller(world->getSkeleton("manipulator"), _firstDomino)); + } + + // Attempt to create a new domino. If the new domino would be in collision + // with anything (other than the floor), then discard it. + void attemptToCreateDomino(double angle) + { + const SkeletonPtr& lastDomino = _dominoes.size()>0? + _dominoes.back() : _firstDomino; + + // Compute the position for the new domino + Eigen::Vector3d dx = default_distance*Eigen::Vector3d( + cos(_totalAngle), sin(_totalAngle), 0.0); + + Eigen::Vector6d x = lastDomino->getPositions(); + x.tail<3>() += dx; + + // Adjust the angle for the new domino + x[2] = _totalAngle + angle; + + // Create the new domino + SkeletonPtr newDomino = _firstDomino->clone(); + newDomino->setName("domino #" + std::to_string(_dominoes.size()+1)); + newDomino->setPositions(x); + + mWorld->addSkeleton(newDomino); + + // Compute collisions + dart::collision::CollisionDetector* detector = + mWorld->getConstraintSolver()->getCollisionDetector(); + detector->detectCollision(true, true); + + // Look through the collisions to see if any dominoes are penetrating + // something + bool dominoCollision = false; + size_t collisionCount = detector->getNumContacts(); + for(size_t i=0; i < collisionCount; ++i) + { + // If neither of the colliding BodyNodes belongs to the floor, then we + // know the new domino is in contact with something it shouldn't be + const dart::collision::Contact& contact = detector->getContact(i); + if( contact.bodyNode1.lock()->getSkeleton() != _floor + && contact.bodyNode2.lock()->getSkeleton() != _floor) + { + dominoCollision = true; + break; + } + } + + if(dominoCollision) + { + // Remove the new domino, because it is penetrating an existing one + mWorld->removeSkeleton(newDomino); + } + else + { + // Record the latest domino addition + _angles.push_back(angle); + _dominoes.push_back(newDomino); + _totalAngle += angle; + } + } + + // Delete the last domino that was added to the scene. (Do not delete the + // original domino) + void deleteLastDomino() + { + if(_dominoes.size() > 0) + { + SkeletonPtr lastDomino = _dominoes.back(); + _dominoes.pop_back(); + mWorld->removeSkeleton(lastDomino); + + _totalAngle -= _angles.back(); + _angles.pop_back(); + } + } + + void keyboard(unsigned char key, int x, int y) override + { + if(!_hasEverRun) + { + switch(key) + { + case 'q': + attemptToCreateDomino( default_angle); + break; + case 'w': + attemptToCreateDomino(0.0); + break; + case 'e': + attemptToCreateDomino(-default_angle); + break; + case 'd': + deleteLastDomino(); + break; + case ' ': + _hasEverRun = true; + break; + } + } + else + { + switch(key) + { + case 'f': + _forceCountDown = default_force_duration; + break; + + case 'r': + _pushCountDown = default_push_duration; + break; + } + } + + SimWindow::keyboard(key, x, y); + } + + void timeStepping() override + { + // If the user has pressed the 'f' key, apply a force to the first domino in + // order to push it over + if(_forceCountDown > 0) + { + _firstDomino->getBodyNode(0)->addExtForce( + default_push_force*Eigen::Vector3d::UnitX(), + default_domino_height/2.0*Eigen::Vector3d::UnitZ()); + + --_forceCountDown; + } + + // Run the controller for the manipulator + if(_pushCountDown > 0) + { + _controller->setOperationalSpaceForces(); + --_pushCountDown; + } + else + { + _controller->setPDForces(); + } + + SimWindow::timeStepping(); + } + +protected: + + /// Base domino. Used to clone new dominoes. + SkeletonPtr _firstDomino; + + /// Floor of the scene + SkeletonPtr _floor; + + /// History of the dominoes that have been created + std::vector _dominoes; + + /// History of the angles that the user has specified + std::vector _angles; + + /// Sum of all angles so far + double _totalAngle; + + /// Set to true the first time spacebar is pressed + bool _hasEverRun; + + /// The first domino will be pushed by a disembodied force while the value of + /// this is greater than zero + int _forceCountDown; + + /// The manipulator will attempt to push on the first domino while the value + /// of this is greater than zero + int _pushCountDown; + + std::unique_ptr _controller; + +}; + +SkeletonPtr createDomino() +{ + // Create a Skeleton with the name "domino" + SkeletonPtr domino = Skeleton::create("domino"); + + // Create a body for the domino + BodyNodePtr body = + domino->createJointAndBodyNodePair(nullptr).second; + + // Create a shape for the domino + std::shared_ptr box( + new BoxShape(Eigen::Vector3d(default_domino_depth, + default_domino_width, + default_domino_height))); + body->addVisualizationShape(box); + body->addCollisionShape(box); + + // Set up inertia for the domino + dart::dynamics::Inertia inertia; + inertia.setMass(default_domino_mass); + inertia.setMoment(box->computeInertia(default_domino_mass)); + body->setInertia(inertia); + + domino->getDof("Joint_pos_z")->setPosition(default_domino_height/2.0); + + return domino; +} + +SkeletonPtr createFloor() +{ + SkeletonPtr floor = Skeleton::create("floor"); + + // Give the floor a body + BodyNodePtr body = + floor->createJointAndBodyNodePair(nullptr).second; + + // Give the body a shape + double floor_width = 10.0; + double floor_height = 0.01; + std::shared_ptr box( + new BoxShape(Eigen::Vector3d(floor_width, floor_width, floor_height))); + box->setColor(dart::Color::Black()); + + body->addVisualizationShape(box); + body->addCollisionShape(box); + + // Put the body into position + Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); + tf.translation() = Eigen::Vector3d(0.0, 0.0, -floor_height/2.0); + body->getParentJoint()->setTransformFromParentBodyNode(tf); + + return floor; +} + +SkeletonPtr createManipulator() +{ + // Load the Skeleton from a file + dart::utils::DartLoader loader; + SkeletonPtr manipulator = + loader.parseSkeleton(DART_DATA_PATH"urdf/KR5/KR5 sixx R650.urdf"); + manipulator->setName("manipulator"); + + // Position its base in a reasonable way + Eigen::Isometry3d tf = Eigen::Isometry3d::Identity(); + tf.translation() = Eigen::Vector3d(-0.65, 0.0, 0.0); + manipulator->getJoint(0)->setTransformFromParentBodyNode(tf); + + // Get it into a useful configuration + manipulator->getDof(1)->setPosition( 140.0*M_PI/180.0); + manipulator->getDof(2)->setPosition(-140.0*M_PI/180.0); + + return manipulator; +} + +int main(int argc, char* argv[]) +{ + SkeletonPtr domino = createDomino(); + SkeletonPtr floor = createFloor(); + SkeletonPtr manipulator = createManipulator(); + + WorldPtr world = std::make_shared(); + world->addSkeleton(domino); + world->addSkeleton(floor); + world->addSkeleton(manipulator); + + MyWindow window(world); + + std::cout << "Before simulation has started, you can create new dominoes:" << std::endl; + std::cout << "'w': Create new domino angled forward" << std::endl; + std::cout << "'q': Create new domino angled to the left" << std::endl; + std::cout << "'e': Create new domino angled to the right" << std::endl; + std::cout << "'d': Delete the last domino that was created" << std::endl; + std::cout << std::endl; + std::cout << "spacebar: Begin simulation (you can no longer create or remove dominoes)" << std::endl; + std::cout << "'f': Push the first domino with a disembodies force so that it falls over" << std::endl; + std::cout << "'r': Push the first domino with the manipulator so that it falls over" << std::endl; + std::cout << "'v': Turn contact force visualization on/off" << std::endl; + + glutInit(&argc, argv); + window.initWindow(640, 480, "Dominoes"); + glutMainLoop(); +} diff --git a/tutorials/tutorialDominoes.cpp b/tutorials/tutorialDominoes.cpp index c40854eb6669b..d866d5a41080e 100644 --- a/tutorials/tutorialDominoes.cpp +++ b/tutorials/tutorialDominoes.cpp @@ -67,35 +67,31 @@ class Controller Controller(const SkeletonPtr& manipulator, const SkeletonPtr& domino) : _manipulator(manipulator) { + // Grab the current joint angles to use them as desired angles + // Lesson 2b + // Grab the last body in the manipulator, and use it as an end effector - _endEffector = _manipulator->getBodyNode(_manipulator->getNumBodyNodes()-1); + // Lesson 3a + + // Compute the body frame offset for the end effector + // Lesson 3b // Create a transform from the center of the domino to the top of the domino - Eigen::Isometry3d target_offset(Eigen::Isometry3d::Identity()); - target_offset.translation() = - default_domino_height/2.0 * Eigen::Vector3d::UnitZ(); + // Lesson 3c // Rotate the transform so that it matches the orientation of the end // effector - target_offset.linear() = - _endEffector->getTransform(domino->getBodyNode(0)).linear(); + // Lesson 3d // Place the _target SimpleFrame at the top of the domino - _target = std::make_shared(Frame::World(), "target"); - _target->setTransform(target_offset, domino->getBodyNode(0)); + // Lesson 3e - // Compute the body frame offset for the end effector - _offset = default_endeffector_offset * Eigen::Vector3d::UnitX(); - - // Grab the current joint angles to use them as desired angles - _qDesired = _manipulator->getPositions(); - - // Set control gains + // Set PD control gains _Kp_PD = 200.0; _Kd_PD = 20.0; - _Kp_OS_linear = 5.0; - _Kp_OS_angular = 5.0; + // Set operational space control gains + _Kp_OS = 5.0; _Kd_OS = 0.01; } @@ -103,85 +99,15 @@ class Controller /// Coriolis forces void setPDForces() { - if(nullptr == _manipulator) - return; - - // Compute the joint forces needed to compensate for Coriolis forces and - // gravity - const Eigen::VectorXd& Cg = _manipulator->getCoriolisAndGravityForces(); + // Lesson 2a - // Compute the joint position error - Eigen::VectorXd q = _manipulator->getPositions(); - Eigen::VectorXd dq = _manipulator->getVelocities(); - q += dq * _manipulator->getTimeStep(); - - Eigen::VectorXd q_err = _qDesired - q; - - // Compute the joint velocity error - Eigen::VectorXd dq_err = -dq; - - // Compute the desired joint forces - const Eigen::MatrixXd& M = _manipulator->getMassMatrix(); - _forces = M*(_Kp_PD * q_err + _Kd_PD * dq_err) + Cg; - - _manipulator->setForces(_forces); + // Lesson 2c } /// Compute an operational space controller to push on the first domino void setOperationalSpaceForces() { - if(nullptr == _manipulator) - return; - - const Eigen::MatrixXd& M = _manipulator->getMassMatrix(); - - // Compute the Jacobian - Jacobian J = _endEffector->getWorldJacobian(_offset); - // Compute the pseudo-inverse of the Jacobian - Eigen::MatrixXd pinv_J = J.transpose() * ( J * J.transpose() - + 0.0025*Eigen::Matrix6d::Identity() ).inverse(); - - // Compute the Jacobian time derivative - Jacobian dJ = _endEffector->getJacobianClassicDeriv(_offset); - // Comptue the pseudo-inverse of the Jacobian time derivative - Eigen::MatrixXd pinv_dJ = dJ.transpose() * ( dJ * dJ.transpose() - + 0.0025*Eigen::Matrix6d::Identity() ).inverse(); - - // Compute the linear error - Eigen::Vector6d e; - e.tail<3>() = _target->getWorldTransform().translation() - - _endEffector->getWorldTransform()*_offset; - - // Compute the angular error - Eigen::AngleAxisd aa(_target->getTransform(_endEffector).linear()); - e.head<3>() = aa.angle() * aa.axis(); - - // Compute the time derivative of the error - Eigen::Vector6d de = - _endEffector->getSpatialVelocity( - _offset, _target.get(), Frame::World()); - - // Compute the forces needed to compensate for Coriolis forces and gravity - const Eigen::VectorXd& Cg = _manipulator->getCoriolisAndGravityForces(); - - // Turn the control gains into matrix form - Eigen::Matrix6d Kp = Eigen::Matrix6d::Identity(); - Kp.diagonal().head<3>() = _Kp_OS_angular*Eigen::Vector3d::Ones(); - Kp.diagonal().tail<3>() = _Kp_OS_linear*Eigen::Vector3d::Ones(); - - size_t dofs = _manipulator->getNumDofs(); - Eigen::MatrixXd Kd = _Kd_OS * Eigen::MatrixXd::Identity(dofs, dofs); - - // Compute the joint forces needed to exert the desired workspace force - Eigen::Vector6d fDesired = Eigen::Vector6d::Zero(); - fDesired[3] = default_push_force; - Eigen::VectorXd f = J.transpose() * fDesired; - - // Compute the control forces - Eigen::VectorXd dq = _manipulator->getVelocities(); - _forces = M * (pinv_J*Kp*de + pinv_dJ*Kp*e) - - Kd*dq + Kd*pinv_J*Kp*e + Cg + f; - - _manipulator->setForces(_forces); + // Lesson 3f } protected: @@ -208,13 +134,9 @@ class Controller /// Control gains for the derivative error terms in the PD controller double _Kd_PD; - /// Control gains for the proportional linear error terms in the operational + /// Control gains for the proportional error terms in the operational /// space controller - double _Kp_OS_linear; - - /// Control gains for the proportional angular error terms in the operational - /// space controller - double _Kp_OS_angular; + double _Kp_OS; /// Control gains for the derivative error terms in the operational space /// controller @@ -244,76 +166,21 @@ class MyWindow : public dart::gui::SimWindow // Attempt to create a new domino. If the new domino would be in collision // with anything (other than the floor), then discard it. - void attemptToCreateDomino(double angle) + void attemptToCreateDomino(double) { - const SkeletonPtr& lastDomino = _dominoes.size()>0? - _dominoes.back() : _firstDomino; - - // Compute the position for the new domino - Eigen::Vector3d dx = default_distance*Eigen::Vector3d( - cos(_totalAngle), sin(_totalAngle), 0.0); - - Eigen::Vector6d x = lastDomino->getPositions(); - x.tail<3>() += dx; - - // Adjust the angle for the new domino - x[2] = _totalAngle + angle; + // Create the new domino + // Lesson 1a - SkeletonPtr newDomino = _firstDomino->clone(); - newDomino->setName("domino #" + std::to_string(_dominoes.size()+1)); - newDomino->setPositions(x); - - mWorld->addSkeleton(newDomino); - - // Compute collisions - dart::collision::CollisionDetector* detector = - mWorld->getConstraintSolver()->getCollisionDetector(); - detector->detectCollision(true, true); - - // Look through the collisions to see if any dominoes are penetrating each - // other - bool dominoCollision = false; - size_t collisionCount = detector->getNumContacts(); - for(size_t i=0; i < collisionCount; ++i) - { - // If either of the colliding BodyNodes belongs to the floor, then we know - // that we have colliding dominoes - const dart::collision::Contact& contact = detector->getContact(i); - if( contact.bodyNode1.lock()->getSkeleton() != _floor - && contact.bodyNode2.lock()->getSkeleton() != _floor) - { - dominoCollision = true; - break; - } - } - - if(dominoCollision) - { - // Remove the new domino, because it is penetrating an existing one - mWorld->removeSkeleton(newDomino); - } - else - { - // Record the latest domino addition - _angles.push_back(angle); - _dominoes.push_back(newDomino); - _totalAngle += angle; - } + // Look through the collisions to see if any dominoes are penetrating + // something + // Lesson 1b } // Delete the last domino that was added to the scene. (Do not delete the // original domino) void deleteLastDomino() { - if(_dominoes.size() > 0) - { - SkeletonPtr lastDomino = _dominoes.back(); - _dominoes.pop_back(); - mWorld->removeSkeleton(lastDomino); - - _totalAngle -= _angles.back(); - _angles.pop_back(); - } + // Lesson 1c } void keyboard(unsigned char key, int x, int y) override @@ -362,17 +229,14 @@ class MyWindow : public dart::gui::SimWindow // order to push it over if(_forceCountDown > 0) { - _firstDomino->getBodyNode(0)->addExtForce( - default_push_force*Eigen::Vector3d::UnitX(), - default_domino_height/2.0*Eigen::Vector3d::UnitZ()); - + // Lesson 1d --_forceCountDown; } + // Run the controller for the manipulator if(_pushCountDown > 0) { _controller->setOperationalSpaceForces(); - --_pushCountDown; } else @@ -482,7 +346,8 @@ SkeletonPtr createManipulator() tf.translation() = Eigen::Vector3d(-0.65, 0.0, 0.0); manipulator->getJoint(0)->setTransformFromParentBodyNode(tf); - manipulator->getDof(1)->setPosition(140.0*M_PI/180.0); + // Get it into a useful configuration + manipulator->getDof(1)->setPosition( 140.0*M_PI/180.0); manipulator->getDof(2)->setPosition(-140.0*M_PI/180.0); return manipulator; @@ -494,7 +359,7 @@ int main(int argc, char* argv[]) SkeletonPtr floor = createFloor(); SkeletonPtr manipulator = createManipulator(); - WorldPtr world(new World); + WorldPtr world = std::make_shared(); world->addSkeleton(domino); world->addSkeleton(floor); world->addSkeleton(manipulator); From f9819fd3c9c5426db340571b00930feb8e0bb15d Mon Sep 17 00:00:00 2001 From: Karen Liu Date: Sat, 20 Jun 2015 21:00:20 -0400 Subject: [PATCH 17/65] modified style --- tutorials/tutorialDominoes-Finished.cpp | 56 ++++++++++++------------- 1 file changed, 28 insertions(+), 28 deletions(-) diff --git a/tutorials/tutorialDominoes-Finished.cpp b/tutorials/tutorialDominoes-Finished.cpp index b6e2b07519ddb..9878a3cb23c3d 100644 --- a/tutorials/tutorialDominoes-Finished.cpp +++ b/tutorials/tutorialDominoes-Finished.cpp @@ -37,11 +37,11 @@ #include "dart/dart.h" const double default_domino_height = 0.3; -const double default_domino_width = 0.4*default_domino_height; -const double default_domino_depth = default_domino_width/5.0; +const double default_domino_width = 0.4 * default_domino_height; +const double default_domino_depth = default_domino_width / 5.0; -const double default_distance = default_domino_height/2.0; -const double default_angle = 20.0*M_PI/180.0; +const double default_distance = default_domino_height / 2.0; +const double default_angle = 20.0 * M_PI / 180.0; const double default_domino_density = 2.6e3; // kg/m^3 const double default_domino_mass = @@ -79,7 +79,7 @@ class Controller // Create a transform from the center of the domino to the top of the domino Eigen::Isometry3d target_offset(Eigen::Isometry3d::Identity()); target_offset.translation() = - default_domino_height/2.0 * Eigen::Vector3d::UnitZ(); + default_domino_height / 2.0 * Eigen::Vector3d::UnitZ(); // Rotate the transform so that it matches the orientation of the end // effector @@ -92,7 +92,7 @@ class Controller // Set PD control gains _Kp_PD = 200.0; - _Kd_PD = 20.0; + _Kd_PD = 20.0; // Set operational space control gains _Kp_OS = 5.0; @@ -122,7 +122,7 @@ class Controller // Compute the desired joint forces const Eigen::MatrixXd& M = _manipulator->getMassMatrix(); - _forces = M*(_Kp_PD * q_err + _Kd_PD * dq_err) + Cg; + _forces = M * (_Kp_PD * q_err + _Kd_PD * dq_err) + Cg; _manipulator->setForces(_forces); } @@ -138,33 +138,33 @@ class Controller // Compute the Jacobian Jacobian J = _endEffector->getWorldJacobian(_offset); // Compute the pseudo-inverse of the Jacobian - Eigen::MatrixXd pinv_J = J.transpose() * ( J * J.transpose() - + 0.0025*Eigen::Matrix6d::Identity() ).inverse(); + Eigen::MatrixXd pinv_J = J.transpose() * (J * J.transpose() + + 0.0025 * Eigen::Matrix6d::Identity()).inverse(); // Compute the Jacobian time derivative Jacobian dJ = _endEffector->getJacobianClassicDeriv(_offset); // Comptue the pseudo-inverse of the Jacobian time derivative - Eigen::MatrixXd pinv_dJ = dJ.transpose() * ( dJ * dJ.transpose() - + 0.0025*Eigen::Matrix6d::Identity() ).inverse(); + Eigen::MatrixXd pinv_dJ = dJ.transpose() * (dJ * dJ.transpose() + + 0.0025 * Eigen::Matrix6d::Identity()).inverse(); // Compute the linear error Eigen::Vector6d e; e.tail<3>() = _target->getWorldTransform().translation() - - _endEffector->getWorldTransform()*_offset; + - _endEffector->getWorldTransform() * _offset; // Compute the angular error Eigen::AngleAxisd aa(_target->getTransform(_endEffector).linear()); e.head<3>() = aa.angle() * aa.axis(); // Compute the time derivative of the error - Eigen::Vector6d de = - _endEffector->getSpatialVelocity( + Eigen::Vector6d de = -_endEffector->getSpatialVelocity( _offset, _target.get(), Frame::World()); // Compute the forces needed to compensate for Coriolis forces and gravity const Eigen::VectorXd& Cg = _manipulator->getCoriolisAndGravityForces(); // Turn the control gains into matrix form - Eigen::Matrix6d Kp = _Kp_OS*Eigen::Matrix6d::Identity(); + Eigen::Matrix6d Kp = _Kp_OS * Eigen::Matrix6d::Identity(); size_t dofs = _manipulator->getNumDofs(); Eigen::MatrixXd Kd = _Kd_OS * Eigen::MatrixXd::Identity(dofs, dofs); @@ -176,8 +176,8 @@ class Controller // Compute the control forces Eigen::VectorXd dq = _manipulator->getVelocities(); - _forces = M * (pinv_J*Kp*de + pinv_dJ*Kp*e) - - Kd*dq + Kd*pinv_J*Kp*e + Cg + f; + _forces = M * (pinv_J * Kp * de + pinv_dJ * Kp * e) + - Kd * dq + Kd * pinv_J * Kp * e + Cg + f; _manipulator->setForces(_forces); } @@ -240,11 +240,11 @@ class MyWindow : public dart::gui::SimWindow // with anything (other than the floor), then discard it. void attemptToCreateDomino(double angle) { - const SkeletonPtr& lastDomino = _dominoes.size()>0? + const SkeletonPtr& lastDomino = _dominoes.size() > 0 ? _dominoes.back() : _firstDomino; // Compute the position for the new domino - Eigen::Vector3d dx = default_distance*Eigen::Vector3d( + Eigen::Vector3d dx = default_distance * Eigen::Vector3d( cos(_totalAngle), sin(_totalAngle), 0.0); Eigen::Vector6d x = lastDomino->getPositions(); @@ -255,7 +255,7 @@ class MyWindow : public dart::gui::SimWindow // Create the new domino SkeletonPtr newDomino = _firstDomino->clone(); - newDomino->setName("domino #" + std::to_string(_dominoes.size()+1)); + newDomino->setName("domino #" + std::to_string(_dominoes.size() + 1)); newDomino->setPositions(x); mWorld->addSkeleton(newDomino); @@ -269,12 +269,12 @@ class MyWindow : public dart::gui::SimWindow // something bool dominoCollision = false; size_t collisionCount = detector->getNumContacts(); - for(size_t i=0; i < collisionCount; ++i) + for(size_t i = 0; i < collisionCount; ++i) { // If neither of the colliding BodyNodes belongs to the floor, then we // know the new domino is in contact with something it shouldn't be const dart::collision::Contact& contact = detector->getContact(i); - if( contact.bodyNode1.lock()->getSkeleton() != _floor + if(contact.bodyNode1.lock()->getSkeleton() != _floor && contact.bodyNode2.lock()->getSkeleton() != _floor) { dominoCollision = true; @@ -318,7 +318,7 @@ class MyWindow : public dart::gui::SimWindow switch(key) { case 'q': - attemptToCreateDomino( default_angle); + attemptToCreateDomino(default_angle); break; case 'w': attemptToCreateDomino(0.0); @@ -358,8 +358,8 @@ class MyWindow : public dart::gui::SimWindow if(_forceCountDown > 0) { _firstDomino->getBodyNode(0)->addExtForce( - default_push_force*Eigen::Vector3d::UnitX(), - default_domino_height/2.0*Eigen::Vector3d::UnitZ()); + default_push_force * Eigen::Vector3d::UnitX(), + default_domino_height / 2.0 * Eigen::Vector3d::UnitZ()); --_forceCountDown; } @@ -433,7 +433,7 @@ SkeletonPtr createDomino() inertia.setMoment(box->computeInertia(default_domino_mass)); body->setInertia(inertia); - domino->getDof("Joint_pos_z")->setPosition(default_domino_height/2.0); + domino->getDof("Joint_pos_z")->setPosition(default_domino_height / 2.0); return domino; } @@ -458,7 +458,7 @@ SkeletonPtr createFloor() // Put the body into position Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); - tf.translation() = Eigen::Vector3d(0.0, 0.0, -floor_height/2.0); + tf.translation() = Eigen::Vector3d(0.0, 0.0, -floor_height / 2.0); body->getParentJoint()->setTransformFromParentBodyNode(tf); return floor; @@ -478,8 +478,8 @@ SkeletonPtr createManipulator() manipulator->getJoint(0)->setTransformFromParentBodyNode(tf); // Get it into a useful configuration - manipulator->getDof(1)->setPosition( 140.0*M_PI/180.0); - manipulator->getDof(2)->setPosition(-140.0*M_PI/180.0); + manipulator->getDof(1)->setPosition(140.0 * M_PI / 180.0); + manipulator->getDof(2)->setPosition(-140.0 * M_PI / 180.0); return manipulator; } From 3e3367535ebb36f4378f5e5f179c8050e303b831 Mon Sep 17 00:00:00 2001 From: Karen Liu Date: Sun, 21 Jun 2015 15:47:37 -0400 Subject: [PATCH 18/65] modified style for domino and pendulum tutorials --- tutorials/tutorialDominoes-Finished.cpp | 180 +++++++++---------- tutorials/tutorialDominoes.cpp | 100 +++++------ tutorials/tutorialMultiPendulum-Finished.cpp | 136 +++++++------- tutorials/tutorialMultiPendulum.cpp | 72 ++++---- 4 files changed, 244 insertions(+), 244 deletions(-) diff --git a/tutorials/tutorialDominoes-Finished.cpp b/tutorials/tutorialDominoes-Finished.cpp index 9878a3cb23c3d..5df60b644eab7 100644 --- a/tutorials/tutorialDominoes-Finished.cpp +++ b/tutorials/tutorialDominoes-Finished.cpp @@ -65,16 +65,16 @@ class Controller public: Controller(const SkeletonPtr& manipulator, const SkeletonPtr& domino) - : _manipulator(manipulator) + : mManipulator(manipulator) { // Grab the current joint angles to use them as desired angles - _qDesired = _manipulator->getPositions(); + mQDesired = mManipulator->getPositions(); // Grab the last body in the manipulator, and use it as an end effector - _endEffector = _manipulator->getBodyNode(_manipulator->getNumBodyNodes()-1); + mEndEffector = mManipulator->getBodyNode(mManipulator->getNumBodyNodes() - 1); // Compute the body frame offset for the end effector - _offset = default_endeffector_offset * Eigen::Vector3d::UnitX(); + mOffset = default_endeffector_offset * Eigen::Vector3d::UnitX(); // Create a transform from the center of the domino to the top of the domino Eigen::Isometry3d target_offset(Eigen::Isometry3d::Identity()); @@ -84,90 +84,90 @@ class Controller // Rotate the transform so that it matches the orientation of the end // effector target_offset.linear() = - _endEffector->getTransform(domino->getBodyNode(0)).linear(); + mEndEffector->getTransform(domino->getBodyNode(0)).linear(); // Place the _target SimpleFrame at the top of the domino - _target = std::make_shared(Frame::World(), "target"); - _target->setTransform(target_offset, domino->getBodyNode(0)); + mTarget = std::make_shared(Frame::World(), "target"); + mTarget->setTransform(target_offset, domino->getBodyNode(0)); // Set PD control gains - _Kp_PD = 200.0; - _Kd_PD = 20.0; + mKpPD = 200.0; + mKdPD = 20.0; // Set operational space control gains - _Kp_OS = 5.0; - _Kd_OS = 0.01; + mKpOS = 5.0; + mKdOS = 0.01; } /// Compute a stable PD controller that also compensates for gravity and /// Coriolis forces void setPDForces() { - if(nullptr == _manipulator) + if(nullptr == mManipulator) return; // Compute the joint forces needed to compensate for Coriolis forces and // gravity - const Eigen::VectorXd& Cg = _manipulator->getCoriolisAndGravityForces(); + const Eigen::VectorXd& Cg = mManipulator->getCoriolisAndGravityForces(); // Compute the joint position error - Eigen::VectorXd q = _manipulator->getPositions(); - Eigen::VectorXd dq = _manipulator->getVelocities(); - q += dq * _manipulator->getTimeStep(); + Eigen::VectorXd q = mManipulator->getPositions(); + Eigen::VectorXd dq = mManipulator->getVelocities(); + q += dq * mManipulator->getTimeStep(); - Eigen::VectorXd q_err = _qDesired - q; + Eigen::VectorXd q_err = mQDesired - q; // Compute the joint velocity error Eigen::VectorXd dq_err = -dq; // Compute the desired joint forces - const Eigen::MatrixXd& M = _manipulator->getMassMatrix(); - _forces = M * (_Kp_PD * q_err + _Kd_PD * dq_err) + Cg; + const Eigen::MatrixXd& M = mManipulator->getMassMatrix(); + mForces = M * (mKpPD * q_err + mKdPD * dq_err) + Cg; - _manipulator->setForces(_forces); + mManipulator->setForces(mForces); } /// Compute an operational space controller to push on the first domino void setOperationalSpaceForces() { - if(nullptr == _manipulator) + if(nullptr == mManipulator) return; - const Eigen::MatrixXd& M = _manipulator->getMassMatrix(); + const Eigen::MatrixXd& M = mManipulator->getMassMatrix(); // Compute the Jacobian - Jacobian J = _endEffector->getWorldJacobian(_offset); + Jacobian J = mEndEffector->getWorldJacobian(mOffset); // Compute the pseudo-inverse of the Jacobian Eigen::MatrixXd pinv_J = J.transpose() * (J * J.transpose() + 0.0025 * Eigen::Matrix6d::Identity()).inverse(); // Compute the Jacobian time derivative - Jacobian dJ = _endEffector->getJacobianClassicDeriv(_offset); + Jacobian dJ = mEndEffector->getJacobianClassicDeriv(mOffset); // Comptue the pseudo-inverse of the Jacobian time derivative Eigen::MatrixXd pinv_dJ = dJ.transpose() * (dJ * dJ.transpose() + 0.0025 * Eigen::Matrix6d::Identity()).inverse(); // Compute the linear error Eigen::Vector6d e; - e.tail<3>() = _target->getWorldTransform().translation() - - _endEffector->getWorldTransform() * _offset; + e.tail<3>() = mTarget->getWorldTransform().translation() + - mEndEffector->getWorldTransform() * mOffset; // Compute the angular error - Eigen::AngleAxisd aa(_target->getTransform(_endEffector).linear()); + Eigen::AngleAxisd aa(mTarget->getTransform(mEndEffector).linear()); e.head<3>() = aa.angle() * aa.axis(); // Compute the time derivative of the error - Eigen::Vector6d de = -_endEffector->getSpatialVelocity( - _offset, _target.get(), Frame::World()); + Eigen::Vector6d de = -mEndEffector->getSpatialVelocity( + mOffset, mTarget.get(), Frame::World()); // Compute the forces needed to compensate for Coriolis forces and gravity - const Eigen::VectorXd& Cg = _manipulator->getCoriolisAndGravityForces(); + const Eigen::VectorXd& Cg = mManipulator->getCoriolisAndGravityForces(); // Turn the control gains into matrix form - Eigen::Matrix6d Kp = _Kp_OS * Eigen::Matrix6d::Identity(); + Eigen::Matrix6d Kp = mKpOS * Eigen::Matrix6d::Identity(); - size_t dofs = _manipulator->getNumDofs(); - Eigen::MatrixXd Kd = _Kd_OS * Eigen::MatrixXd::Identity(dofs, dofs); + size_t dofs = mManipulator->getNumDofs(); + Eigen::MatrixXd Kd = mKdOS * Eigen::MatrixXd::Identity(dofs, dofs); // Compute the joint forces needed to exert the desired workspace force Eigen::Vector6d fDesired = Eigen::Vector6d::Zero(); @@ -175,47 +175,47 @@ class Controller Eigen::VectorXd f = J.transpose() * fDesired; // Compute the control forces - Eigen::VectorXd dq = _manipulator->getVelocities(); - _forces = M * (pinv_J * Kp * de + pinv_dJ * Kp * e) + Eigen::VectorXd dq = mManipulator->getVelocities(); + mForces = M * (pinv_J * Kp * de + pinv_dJ * Kp * e) - Kd * dq + Kd * pinv_J * Kp * e + Cg + f; - _manipulator->setForces(_forces); + mManipulator->setForces(mForces); } protected: /// The manipulator Skeleton that we will be controlling - SkeletonPtr _manipulator; + SkeletonPtr mManipulator; /// The target pose for the controller - SimpleFramePtr _target; + SimpleFramePtr mTarget; /// End effector for the manipulator - BodyNodePtr _endEffector; + BodyNodePtr mEndEffector; /// Desired joint positions when not applying the operational space controller - Eigen::VectorXd _qDesired; + Eigen::VectorXd mQDesired; /// The offset of the end effector from the body origin of the last BodyNode /// in the manipulator - Eigen::Vector3d _offset; + Eigen::Vector3d mOffset; /// Control gains for the proportional error terms in the PD controller - double _Kp_PD; + double mKpPD; /// Control gains for the derivative error terms in the PD controller - double _Kd_PD; + double mKdPD; /// Control gains for the proportional error terms in the operational /// space controller - double _Kp_OS; + double mKpOS; /// Control gains for the derivative error terms in the operational space /// controller - double _Kd_OS; + double mKdOS; /// Joint forces for the manipulator (output of the Controller) - Eigen::VectorXd _forces; + Eigen::VectorXd mForces; }; class MyWindow : public dart::gui::SimWindow @@ -223,39 +223,39 @@ class MyWindow : public dart::gui::SimWindow public: MyWindow(const WorldPtr& world) - : _totalAngle(0.0), - _hasEverRun(false), - _forceCountDown(0), - _pushCountDown(0) + : mTotalAngle(0.0), + mHasEverRun(false), + mForceCountDown(0), + mPushCountDown(0) { setWorld(world); - _firstDomino = world->getSkeleton("domino"); - _floor = world->getSkeleton("floor"); + mFirstDomino = world->getSkeleton("domino"); + mFloor = world->getSkeleton("floor"); - _controller = std::unique_ptr( - new Controller(world->getSkeleton("manipulator"), _firstDomino)); + mController = std::unique_ptr( + new Controller(world->getSkeleton("manipulator"), mFirstDomino)); } // Attempt to create a new domino. If the new domino would be in collision // with anything (other than the floor), then discard it. void attemptToCreateDomino(double angle) { - const SkeletonPtr& lastDomino = _dominoes.size() > 0 ? - _dominoes.back() : _firstDomino; + const SkeletonPtr& lastDomino = mDominoes.size() > 0 ? + mDominoes.back() : mFirstDomino; // Compute the position for the new domino Eigen::Vector3d dx = default_distance * Eigen::Vector3d( - cos(_totalAngle), sin(_totalAngle), 0.0); + cos(mTotalAngle), sin(mTotalAngle), 0.0); Eigen::Vector6d x = lastDomino->getPositions(); x.tail<3>() += dx; // Adjust the angle for the new domino - x[2] = _totalAngle + angle; + x[2] = mTotalAngle + angle; // Create the new domino - SkeletonPtr newDomino = _firstDomino->clone(); - newDomino->setName("domino #" + std::to_string(_dominoes.size() + 1)); + SkeletonPtr newDomino = mFirstDomino->clone(); + newDomino->setName("domino #" + std::to_string(mDominoes.size() + 1)); newDomino->setPositions(x); mWorld->addSkeleton(newDomino); @@ -274,8 +274,8 @@ class MyWindow : public dart::gui::SimWindow // If neither of the colliding BodyNodes belongs to the floor, then we // know the new domino is in contact with something it shouldn't be const dart::collision::Contact& contact = detector->getContact(i); - if(contact.bodyNode1.lock()->getSkeleton() != _floor - && contact.bodyNode2.lock()->getSkeleton() != _floor) + if(contact.bodyNode1.lock()->getSkeleton() != mFloor + && contact.bodyNode2.lock()->getSkeleton() != mFloor) { dominoCollision = true; break; @@ -290,9 +290,9 @@ class MyWindow : public dart::gui::SimWindow else { // Record the latest domino addition - _angles.push_back(angle); - _dominoes.push_back(newDomino); - _totalAngle += angle; + mAngles.push_back(angle); + mDominoes.push_back(newDomino); + mTotalAngle += angle; } } @@ -300,20 +300,20 @@ class MyWindow : public dart::gui::SimWindow // original domino) void deleteLastDomino() { - if(_dominoes.size() > 0) + if(mDominoes.size() > 0) { - SkeletonPtr lastDomino = _dominoes.back(); - _dominoes.pop_back(); + SkeletonPtr lastDomino = mDominoes.back(); + mDominoes.pop_back(); mWorld->removeSkeleton(lastDomino); - _totalAngle -= _angles.back(); - _angles.pop_back(); + mTotalAngle -= mAngles.back(); + mAngles.pop_back(); } } void keyboard(unsigned char key, int x, int y) override { - if(!_hasEverRun) + if(!mHasEverRun) { switch(key) { @@ -330,7 +330,7 @@ class MyWindow : public dart::gui::SimWindow deleteLastDomino(); break; case ' ': - _hasEverRun = true; + mHasEverRun = true; break; } } @@ -339,11 +339,11 @@ class MyWindow : public dart::gui::SimWindow switch(key) { case 'f': - _forceCountDown = default_force_duration; + mForceCountDown = default_force_duration; break; case 'r': - _pushCountDown = default_push_duration; + mPushCountDown = default_push_duration; break; } } @@ -355,24 +355,24 @@ class MyWindow : public dart::gui::SimWindow { // If the user has pressed the 'f' key, apply a force to the first domino in // order to push it over - if(_forceCountDown > 0) + if(mForceCountDown > 0) { - _firstDomino->getBodyNode(0)->addExtForce( + mFirstDomino->getBodyNode(0)->addExtForce( default_push_force * Eigen::Vector3d::UnitX(), default_domino_height / 2.0 * Eigen::Vector3d::UnitZ()); - --_forceCountDown; + --mForceCountDown; } // Run the controller for the manipulator - if(_pushCountDown > 0) + if(mPushCountDown > 0) { - _controller->setOperationalSpaceForces(); - --_pushCountDown; + mController->setOperationalSpaceForces(); + --mPushCountDown; } else { - _controller->setPDForces(); + mController->setPDForces(); } SimWindow::timeStepping(); @@ -381,32 +381,32 @@ class MyWindow : public dart::gui::SimWindow protected: /// Base domino. Used to clone new dominoes. - SkeletonPtr _firstDomino; + SkeletonPtr mFirstDomino; /// Floor of the scene - SkeletonPtr _floor; + SkeletonPtr mFloor; /// History of the dominoes that have been created - std::vector _dominoes; + std::vector mDominoes; /// History of the angles that the user has specified - std::vector _angles; + std::vector mAngles; /// Sum of all angles so far - double _totalAngle; + double mTotalAngle; /// Set to true the first time spacebar is pressed - bool _hasEverRun; + bool mHasEverRun; /// The first domino will be pushed by a disembodied force while the value of /// this is greater than zero - int _forceCountDown; + int mForceCountDown; /// The manipulator will attempt to push on the first domino while the value /// of this is greater than zero - int _pushCountDown; + int mPushCountDown; - std::unique_ptr _controller; + std::unique_ptr mController; }; diff --git a/tutorials/tutorialDominoes.cpp b/tutorials/tutorialDominoes.cpp index d866d5a41080e..d2bd4d085dcc8 100644 --- a/tutorials/tutorialDominoes.cpp +++ b/tutorials/tutorialDominoes.cpp @@ -37,11 +37,11 @@ #include "dart/dart.h" const double default_domino_height = 0.3; -const double default_domino_width = 0.4*default_domino_height; -const double default_domino_depth = default_domino_width/5.0; +const double default_domino_width = 0.4 * default_domino_height; +const double default_domino_depth = default_domino_width / 5.0; -const double default_distance = default_domino_height/2.0; -const double default_angle = 20.0*M_PI/180.0; +const double default_distance = default_domino_height / 2.0; +const double default_angle = 20.0 * M_PI / 180.0; const double default_domino_density = 2.6e3; // kg/m^3 const double default_domino_mass = @@ -54,7 +54,7 @@ const double default_push_force = 8.0; // N const int default_force_duration = 200; // # iterations const int default_push_duration = 1000; // # iterations -const double default_endeffector_offset = 0.05; +const double defaultmEndEffectormOffset = 0.05; using namespace dart::dynamics; using namespace dart::simulation; @@ -65,7 +65,7 @@ class Controller public: Controller(const SkeletonPtr& manipulator, const SkeletonPtr& domino) - : _manipulator(manipulator) + : mManipulator(manipulator) { // Grab the current joint angles to use them as desired angles // Lesson 2b @@ -83,16 +83,16 @@ class Controller // effector // Lesson 3d - // Place the _target SimpleFrame at the top of the domino + // Place the mTarget SimpleFrame at the top of the domino // Lesson 3e // Set PD control gains - _Kp_PD = 200.0; - _Kd_PD = 20.0; + mKpPD = 200.0; + mKdPD = 20.0; // Set operational space control gains - _Kp_OS = 5.0; - _Kd_OS = 0.01; + mKpOS = 5.0; + mKdOS = 0.01; } /// Compute a stable PD controller that also compensates for gravity and @@ -113,37 +113,37 @@ class Controller protected: /// The manipulator Skeleton that we will be controlling - SkeletonPtr _manipulator; + SkeletonPtr mManipulator; /// The target pose for the controller - SimpleFramePtr _target; + SimpleFramePtr mTarget; /// End effector for the manipulator - BodyNodePtr _endEffector; + BodyNodePtr mEndEffector; /// Desired joint positions when not applying the operational space controller - Eigen::VectorXd _qDesired; + Eigen::VectorXd mQDesired; /// The offset of the end effector from the body origin of the last BodyNode /// in the manipulator - Eigen::Vector3d _offset; + Eigen::Vector3d mOffset; /// Control gains for the proportional error terms in the PD controller - double _Kp_PD; + double mKpPD; /// Control gains for the derivative error terms in the PD controller - double _Kd_PD; + double mKdPD; /// Control gains for the proportional error terms in the operational /// space controller - double _Kp_OS; + double mKpOS; /// Control gains for the derivative error terms in the operational space /// controller - double _Kd_OS; + double mKdOS; /// Joint forces for the manipulator (output of the Controller) - Eigen::VectorXd _forces; + Eigen::VectorXd mForces; }; class MyWindow : public dart::gui::SimWindow @@ -151,17 +151,17 @@ class MyWindow : public dart::gui::SimWindow public: MyWindow(const WorldPtr& world) - : _totalAngle(0.0), - _hasEverRun(false), - _forceCountDown(0), - _pushCountDown(0) + : mTotalAngle(0.0), + mHasEverRun(false), + mForceCountDown(0), + mPushCountDown(0) { setWorld(world); - _firstDomino = world->getSkeleton("domino"); - _floor = world->getSkeleton("floor"); + mFirstDomino = world->getSkeleton("domino"); + mFloor = world->getSkeleton("floor"); - _controller = std::unique_ptr( - new Controller(world->getSkeleton("manipulator"), _firstDomino)); + mController = std::unique_ptr( + new Controller(world->getSkeleton("manipulator"), mFirstDomino)); } // Attempt to create a new domino. If the new domino would be in collision @@ -185,7 +185,7 @@ class MyWindow : public dart::gui::SimWindow void keyboard(unsigned char key, int x, int y) override { - if(!_hasEverRun) + if(!mHasEverRun) { switch(key) { @@ -202,7 +202,7 @@ class MyWindow : public dart::gui::SimWindow deleteLastDomino(); break; case ' ': - _hasEverRun = true; + mHasEverRun = true; break; } } @@ -211,11 +211,11 @@ class MyWindow : public dart::gui::SimWindow switch(key) { case 'f': - _forceCountDown = default_force_duration; + mForceCountDown = default_force_duration; break; case 'r': - _pushCountDown = default_push_duration; + mPushCountDown = default_push_duration; break; } } @@ -227,21 +227,21 @@ class MyWindow : public dart::gui::SimWindow { // If the user has pressed the 'f' key, apply a force to the first domino in // order to push it over - if(_forceCountDown > 0) + if(mForceCountDown > 0) { // Lesson 1d - --_forceCountDown; + --mForceCountDown; } // Run the controller for the manipulator - if(_pushCountDown > 0) + if(mPushCountDown > 0) { - _controller->setOperationalSpaceForces(); - --_pushCountDown; + mController->setOperationalSpaceForces(); + --mPushCountDown; } else { - _controller->setPDForces(); + mController->setPDForces(); } SimWindow::timeStepping(); @@ -250,32 +250,32 @@ class MyWindow : public dart::gui::SimWindow protected: /// Base domino. Used to clone new dominoes. - SkeletonPtr _firstDomino; + SkeletonPtr mFirstDomino; /// Floor of the scene - SkeletonPtr _floor; + SkeletonPtr mFloor; /// History of the dominoes that have been created - std::vector _dominoes; + std::vector mDominoes; /// History of the angles that the user has specified - std::vector _angles; + std::vector mAngles; /// Sum of all angles so far - double _totalAngle; + double mTotalAngle; /// Set to true the first time spacebar is pressed - bool _hasEverRun; + bool mHasEverRun; /// The first domino will be pushed by a disembodied force while the value of /// this is greater than zero - int _forceCountDown; + int mForceCountDown; /// The manipulator will attempt to push on the first domino while the value /// of this is greater than zero - int _pushCountDown; + int mPushCountDown; - std::unique_ptr _controller; + std::unique_ptr mController; }; @@ -302,7 +302,7 @@ SkeletonPtr createDomino() inertia.setMoment(box->computeInertia(default_domino_mass)); body->setInertia(inertia); - domino->getDof("Joint_pos_z")->setPosition(default_domino_height/2.0); + domino->getDof("Joint_pos_z")->setPosition(default_domino_height / 2.0); return domino; } @@ -327,7 +327,7 @@ SkeletonPtr createFloor() // Put the body into position Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); - tf.translation() = Eigen::Vector3d(0.0, 0.0, -floor_height/2.0); + tf.translation() = Eigen::Vector3d(0.0, 0.0, -floor_height / 2.0); body->getParentJoint()->setTransformFromParentBodyNode(tf); return floor; diff --git a/tutorials/tutorialMultiPendulum-Finished.cpp b/tutorials/tutorialMultiPendulum-Finished.cpp index 53f93837a0870..09ea766fe5dd1 100644 --- a/tutorials/tutorialMultiPendulum-Finished.cpp +++ b/tutorials/tutorialMultiPendulum-Finished.cpp @@ -45,7 +45,7 @@ const double default_force = 15.0; // N const int default_countdown = 200; // Number of timesteps for applying force const double default_rest_position = 0.0; -const double delta_rest_position = 10.0*M_PI/180.0; +const double delta_rest_position = 10.0 * M_PI / 180.0; const double default_stiffness = 0.0; const double delta_stiffness = 10; @@ -62,76 +62,76 @@ class MyWindow : public dart::gui::SimWindow /// Constructor MyWindow(WorldPtr world) - : _ballConstraint(nullptr), - _positiveSign(true), - _bodyForce(false) + : mBallConstraint(nullptr), + mPositiveSign(true), + mBodyForce(false) { setWorld(world); // Find the Skeleton named "pendulum" within the World - _pendulum = world->getSkeleton("pendulum"); + mPendulum = world->getSkeleton("pendulum"); // Make sure that the pendulum was found in the World - assert(_pendulum != nullptr); + assert(mPendulum != nullptr); - _forceCountDown.resize(_pendulum->getNumDofs(), 0); + mForceCountDown.resize(mPendulum->getNumDofs(), 0); ArrowShape::Properties arrow_properties; arrow_properties.mRadius = 0.05; - _arrow = std::shared_ptr(new ArrowShape( - Eigen::Vector3d(-default_height, 0.0, default_height/2.0), - Eigen::Vector3d(-default_width/2.0, 0.0, default_height/2.0), + mArrow = std::shared_ptr(new ArrowShape( + Eigen::Vector3d(-default_height, 0.0, default_height / 2.0), + Eigen::Vector3d(-default_width / 2.0, 0.0, default_height / 2.0), arrow_properties, dart::Color::Orange(1.0))); } void changeDirection() { - _positiveSign = !_positiveSign; - if(_positiveSign) + mPositiveSign = !mPositiveSign; + if(mPositiveSign) { - _arrow->setPositions( - Eigen::Vector3d(-default_height, 0.0, default_height/2.0), - Eigen::Vector3d(-default_width/2.0, 0.0, default_height/2.0)); + mArrow->setPositions( + Eigen::Vector3d(-default_height, 0.0, default_height / 2.0), + Eigen::Vector3d(-default_width / 2.0, 0.0, default_height / 2.0)); } else { - _arrow->setPositions( - Eigen::Vector3d( default_height, 0.0, default_height/2.0), - Eigen::Vector3d( default_width/2.0, 0.0, default_height/2.0)); + mArrow->setPositions( + Eigen::Vector3d(default_height, 0.0, default_height / 2.0), + Eigen::Vector3d(default_width / 2.0, 0.0, default_height / 2.0)); } } void applyForce(size_t index) { - if(index < _forceCountDown.size()) - _forceCountDown[index] = default_countdown; + if(index < mForceCountDown.size()) + mForceCountDown[index] = default_countdown; } void changeRestPosition(double delta) { - for(size_t i=0; i<_pendulum->getNumDofs(); ++i) + for(size_t i = 0; i < mPendulum->getNumDofs(); ++i) { - DegreeOfFreedom* dof = _pendulum->getDof(i); + DegreeOfFreedom* dof = mPendulum->getDof(i); double q0 = dof->getRestPosition() + delta; // The system becomes numerically unstable when the rest position exceeds // 90 degrees - if(std::abs(q0) > 90.0*M_PI/180.0) - q0 = q0>0? 90.0*M_PI/180.0 : -90.0*M_PI/180.0; + if(std::abs(q0) > 90.0 * M_PI / 180.0) + q0 = q0 > 0 ? 90.0 * M_PI / 180.0 : -90.0 * M_PI / 180.0; dof->setRestPosition(q0); } // Only curl up along one axis in the BallJoint - _pendulum->getDof(0)->setRestPosition(0.0); - _pendulum->getDof(2)->setRestPosition(0.0); + mPendulum->getDof(0)->setRestPosition(0.0); + mPendulum->getDof(2)->setRestPosition(0.0); } void changeStiffness(double delta) { - for(size_t i=0; i<_pendulum->getNumDofs(); ++i) + for(size_t i = 0; i < mPendulum->getNumDofs(); ++i) { - DegreeOfFreedom* dof = _pendulum->getDof(i); + DegreeOfFreedom* dof = mPendulum->getDof(i); double stiffness = dof->getSpringStiffness() + delta; if(stiffness < 0.0) stiffness = 0.0; @@ -141,9 +141,9 @@ class MyWindow : public dart::gui::SimWindow void changeDamping(double delta) { - for(size_t i=0; i<_pendulum->getNumDofs(); ++i) + for(size_t i = 0; i < mPendulum->getNumDofs(); ++i) { - DegreeOfFreedom* dof = _pendulum->getDof(i); + DegreeOfFreedom* dof = mPendulum->getDof(i); double damping = dof->getDampingCoefficient() + delta; if(damping < 0.0) damping = 0.0; @@ -155,20 +155,20 @@ class MyWindow : public dart::gui::SimWindow void addConstraint() { // Get the last body in the pendulum - BodyNode* tip = _pendulum->getBodyNode(_pendulum->getNumBodyNodes()-1); + BodyNode* tip = mPendulum->getBodyNode(mPendulum->getNumBodyNodes() - 1); // Attach the last link to the world - _ballConstraint = new dart::constraint::BallJointConstraint( - tip, tip->getTransform()*Eigen::Vector3d(0.0, 0.0, default_height)); - mWorld->getConstraintSolver()->addConstraint(_ballConstraint); + mBallConstraint = new dart::constraint::BallJointConstraint( + tip, tip->getTransform() * Eigen::Vector3d(0.0, 0.0, default_height)); + mWorld->getConstraintSolver()->addConstraint(mBallConstraint); } /// Remove any existing constraint, allowing the pendulum to flail freely void removeConstraint() { - mWorld->getConstraintSolver()->removeConstraint(_ballConstraint); - delete _ballConstraint; - _ballConstraint = nullptr; + mWorld->getConstraintSolver()->removeConstraint(mBallConstraint); + delete mBallConstraint; + mBallConstraint = nullptr; } /// Handle keyboard input @@ -234,7 +234,7 @@ class MyWindow : public dart::gui::SimWindow case 'r': { - if(_ballConstraint) + if(mBallConstraint) removeConstraint(); else addConstraint(); @@ -242,7 +242,7 @@ class MyWindow : public dart::gui::SimWindow } case 'f': - _bodyForce = !_bodyForce; + mBodyForce = !mBodyForce; break; default: @@ -253,9 +253,9 @@ class MyWindow : public dart::gui::SimWindow void timeStepping() override { // Reset all the shapes to be Blue - for(size_t i = 0; i < _pendulum->getNumBodyNodes(); ++i) + for(size_t i = 0; i < mPendulum->getNumBodyNodes(); ++i) { - BodyNode* bn = _pendulum->getBodyNode(i); + BodyNode* bn = mPendulum->getBodyNode(i); for(size_t j = 0; j < 2; ++j) { const ShapePtr& shape = bn->getVisualizationShape(j); @@ -268,56 +268,56 @@ class MyWindow : public dart::gui::SimWindow // experiencing a force if(bn->getNumVisualizationShapes() == 3) { - bn->removeVisualizationShape(_arrow); + bn->removeVisualizationShape(mArrow); } } - if(_bodyForce) + if(mBodyForce) { // Apply body forces based on user input, and color the body shape red - for(size_t i = 0; i < _pendulum->getNumBodyNodes(); ++i) + for(size_t i = 0; i < mPendulum->getNumBodyNodes(); ++i) { - if(_forceCountDown[i] > 0) + if(mForceCountDown[i] > 0) { - BodyNode* bn = _pendulum->getBodyNode(i); + BodyNode* bn = mPendulum->getBodyNode(i); const ShapePtr& shape = bn->getVisualizationShape(1); shape->setColor(dart::Color::Red()); - if(_positiveSign) + if(mPositiveSign) { bn->addExtForce( default_force*Eigen::Vector3d::UnitX(), - Eigen::Vector3d(-default_width/2.0, 0.0, default_height/2.0), + Eigen::Vector3d(-default_width / 2.0, 0.0, default_height / 2.0), true, true); } else { bn->addExtForce( -default_force*Eigen::Vector3d::UnitX(), - Eigen::Vector3d( default_width/2.0, 0.0, default_height/2.0), + Eigen::Vector3d(default_width / 2.0, 0.0, default_height / 2.0), true, true); } - bn->addVisualizationShape(_arrow); + bn->addVisualizationShape(mArrow); - --_forceCountDown[i]; + --mForceCountDown[i]; } } } else { // Apply joint torques based on user input, and color the Joint shape red - for(size_t i=0; i<_pendulum->getNumDofs(); ++i) + for(size_t i = 0; i < mPendulum->getNumDofs(); ++i) { - if(_forceCountDown[i] > 0) + if(mForceCountDown[i] > 0) { - DegreeOfFreedom* dof = _pendulum->getDof(i); + DegreeOfFreedom* dof = mPendulum->getDof(i); BodyNode* bn = dof->getChildBodyNode(); const ShapePtr& shape = bn->getVisualizationShape(0); - dof->setForce(_positiveSign? default_torque : -default_torque); + dof->setForce(mPositiveSign ? default_torque : -default_torque); shape->setColor(dart::Color::Red()); - --_forceCountDown[i]; + --mForceCountDown[i]; } } } @@ -329,23 +329,23 @@ class MyWindow : public dart::gui::SimWindow protected: /// An arrow shape that we will use to visualize applied forces - std::shared_ptr _arrow; + std::shared_ptr mArrow; /// The pendulum that we will be perturbing - SkeletonPtr _pendulum; + SkeletonPtr mPendulum; /// Pointer to the ball constraint that we will be turning on and off - dart::constraint::BallJointConstraint* _ballConstraint; + dart::constraint::BallJointConstraint* mBallConstraint; /// Number of iterations before clearing a force entry - std::vector _forceCountDown; + std::vector mForceCountDown; /// Whether a force should be applied in the positive or negative direction - bool _positiveSign; + bool mPositiveSign; /// True if 1-9 should be used to apply a body force. Otherwise, 1-9 will be /// used to apply a joint torque. - bool _bodyForce; + bool mBodyForce; }; void setGeometry(const BodyNodePtr& bn) @@ -357,7 +357,7 @@ void setGeometry(const BodyNodePtr& bn) // Set the location of the Box Eigen::Isometry3d box_tf(Eigen::Isometry3d::Identity()); - Eigen::Vector3d center = Eigen::Vector3d(0, 0, default_height/2.0); + Eigen::Vector3d center = Eigen::Vector3d(0, 0, default_height / 2.0); box_tf.translation() = center; box->setLocalTransform(box_tf); @@ -372,7 +372,7 @@ void setGeometry(const BodyNodePtr& bn) BodyNode* makeRootBody(const SkeletonPtr& pendulum, const std::string& name) { BallJoint::Properties properties; - properties.mName = name+"_joint"; + properties.mName = name + "_joint"; properties.mRestPositions = Eigen::Vector3d::Constant(default_rest_position); properties.mSpringStiffnesses = Eigen::Vector3d::Constant(default_stiffness); properties.mDampingCoefficients = Eigen::Vector3d::Constant(default_damping); @@ -398,7 +398,7 @@ BodyNode* addBody(const SkeletonPtr& pendulum, { // Set up the properties for the Joint RevoluteJoint::Properties properties; - properties.mName = name+"_joint"; + properties.mName = name + "_joint"; properties.mAxis = Eigen::Vector3d::UnitY(); properties.mT_ParentBodyToJoint.translation() = Eigen::Vector3d(0, 0, default_height); @@ -411,7 +411,7 @@ BodyNode* addBody(const SkeletonPtr& pendulum, parent, properties, BodyNode::Properties(name)).second; // Make a shape for the Joint - const double R = default_width/2.0; + const double R = default_width / 2.0; const double h = default_depth; std::shared_ptr cyl( new CylinderShape(R, h)); @@ -420,7 +420,7 @@ BodyNode* addBody(const SkeletonPtr& pendulum, // Line up the cylinder with the Joint axis Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); tf.linear() = dart::math::eulerXYZToMatrix( - Eigen::Vector3d(90.0*M_PI/180.0, 0, 0)); + Eigen::Vector3d(90.0 * M_PI / 180.0, 0, 0)); cyl->setLocalTransform(tf); bn->addVisualizationShape(cyl); @@ -445,7 +445,7 @@ int main(int argc, char* argv[]) // Set the initial position of the first DegreeOfFreedom so that the pendulum // starts to swing right away - pendulum->getDof(1)->setPosition(120*M_PI/180.0); + pendulum->getDof(1)->setPosition(120 * M_PI / 180.0); // Create a world and add the pendulum to the world WorldPtr world(new World); diff --git a/tutorials/tutorialMultiPendulum.cpp b/tutorials/tutorialMultiPendulum.cpp index 7af7ed30bf957..afe1b018ace56 100644 --- a/tutorials/tutorialMultiPendulum.cpp +++ b/tutorials/tutorialMultiPendulum.cpp @@ -45,7 +45,7 @@ const double default_force = 15.0; // N const int default_countdown = 200; // Number of timesteps for applying force const double default_rest_position = 0.0; -const double delta_rest_position = 10.0*M_PI/180.0; +const double delta_rest_position = 10.0 * M_PI / 180.0; const double default_stiffness = 0.0; const double delta_stiffness = 10; @@ -62,42 +62,42 @@ class MyWindow : public dart::gui::SimWindow /// Constructor MyWindow(WorldPtr world) - : _ballConstraint(nullptr), - _positiveSign(true), - _bodyForce(false) + : mBallConstraint(nullptr), + mPositiveSign(true), + mBodyForce(false) { setWorld(world); // Find the Skeleton named "pendulum" within the World - _pendulum = world->getSkeleton("pendulum"); + mPendulum = world->getSkeleton("pendulum"); // Make sure that the pendulum was found in the World - assert(_pendulum != nullptr); + assert(mPendulum != nullptr); - _forceCountDown.resize(_pendulum->getNumDofs(), 0); + mForceCountDown.resize(mPendulum->getNumDofs(), 0); ArrowShape::Properties arrow_properties; arrow_properties.mRadius = 0.05; - _arrow = std::shared_ptr(new ArrowShape( - Eigen::Vector3d(-default_height, 0.0, default_height/2.0), - Eigen::Vector3d(-default_width/2.0, 0.0, default_height/2.0), + mArrow = std::shared_ptr(new ArrowShape( + Eigen::Vector3d(-default_height, 0.0, default_height / 2.0), + Eigen::Vector3d(-default_width / 2.0, 0.0, default_height / 2.0), arrow_properties, dart::Color::Orange(1.0))); } void changeDirection() { - _positiveSign = !_positiveSign; - if(_positiveSign) + mPositiveSign = !mPositiveSign; + if(mPositiveSign) { - _arrow->setPositions( - Eigen::Vector3d(-default_height, 0.0, default_height/2.0), - Eigen::Vector3d(-default_width/2.0, 0.0, default_height/2.0)); + mArrow->setPositions( + Eigen::Vector3d(-default_height, 0.0, default_height / 2.0), + Eigen::Vector3d(-default_width / 2.0, 0.0, default_height / 2.0)); } else { - _arrow->setPositions( - Eigen::Vector3d( default_height, 0.0, default_height/2.0), - Eigen::Vector3d( default_width/2.0, 0.0, default_height/2.0)); + mArrow->setPositions( + Eigen::Vector3d(default_height, 0.0, default_height / 2.0), + Eigen::Vector3d(default_width / 2.0, 0.0, default_height / 2.0)); } } @@ -196,7 +196,7 @@ class MyWindow : public dart::gui::SimWindow case 'r': { - if(_ballConstraint) + if(mBallConstraint) removeConstraint(); else addConstraint(); @@ -204,7 +204,7 @@ class MyWindow : public dart::gui::SimWindow } case 'f': - _bodyForce = !_bodyForce; + mBodyForce = !mBodyForce; break; default: @@ -217,10 +217,10 @@ class MyWindow : public dart::gui::SimWindow // Reset all the shapes to be Blue // Lesson 1 - if(_bodyForce) + if(mBodyForce) { // Apply body forces based on user input, and color the body shape red - for(size_t i = 0; i < _pendulum->getNumBodyNodes(); ++i) + for(size_t i = 0; i < mPendulum->getNumBodyNodes(); ++i) { // Lesson 1b } @@ -228,7 +228,7 @@ class MyWindow : public dart::gui::SimWindow else { // Apply joint torques based on user input, and color the Joint shape red - for(size_t i=0; i<_pendulum->getNumDofs(); ++i) + for(size_t i = 0; i < mPendulum->getNumDofs(); ++i) { // Lesson 1a } @@ -241,23 +241,23 @@ class MyWindow : public dart::gui::SimWindow protected: /// An arrow shape that we will use to visualize applied forces - std::shared_ptr _arrow; + std::shared_ptr mArrow; /// The pendulum that we will be perturbing - SkeletonPtr _pendulum; + SkeletonPtr mPendulum; /// Pointer to the ball constraint that we will be turning on and off - dart::constraint::BallJointConstraint* _ballConstraint; + dart::constraint::BallJointConstraint* mBallConstraint; /// Number of iterations before clearing a force entry - std::vector _forceCountDown; + std::vector mForceCountDown; /// Whether a force should be applied in the positive or negative direction - bool _positiveSign; + bool mPositiveSign; /// True if 1-9 should be used to apply a body force. Otherwise, 1-9 will be /// used to apply a joint torque. - bool _bodyForce; + bool mBodyForce; }; void setGeometry(const BodyNodePtr& bn) @@ -269,7 +269,7 @@ void setGeometry(const BodyNodePtr& bn) // Set the location of the Box Eigen::Isometry3d box_tf(Eigen::Isometry3d::Identity()); - Eigen::Vector3d center = Eigen::Vector3d(0, 0, default_height/2.0); + Eigen::Vector3d center = Eigen::Vector3d(0, 0, default_height / 2.0); box_tf.translation() = center; box->setLocalTransform(box_tf); @@ -284,7 +284,7 @@ void setGeometry(const BodyNodePtr& bn) BodyNode* makeRootBody(const SkeletonPtr& pendulum, const std::string& name) { BallJoint::Properties properties; - properties.mName = name+"_joint"; + properties.mName = name + "_joint"; properties.mRestPositions = Eigen::Vector3d::Constant(default_rest_position); properties.mSpringStiffnesses = Eigen::Vector3d::Constant(default_stiffness); properties.mDampingCoefficients = Eigen::Vector3d::Constant(default_damping); @@ -295,7 +295,7 @@ BodyNode* makeRootBody(const SkeletonPtr& pendulum, const std::string& name) // Make a shape for the Joint const double& R = default_width; std::shared_ptr ball( - new EllipsoidShape(sqrt(2)*Eigen::Vector3d(R, R, R))); + new EllipsoidShape(sqrt(2) * Eigen::Vector3d(R, R, R))); ball->setColor(dart::Color::Blue()); bn->addVisualizationShape(ball); @@ -310,7 +310,7 @@ BodyNode* addBody(const SkeletonPtr& pendulum, { // Set up the properties for the Joint RevoluteJoint::Properties properties; - properties.mName = name+"_joint"; + properties.mName = name + "_joint"; properties.mAxis = Eigen::Vector3d::UnitY(); properties.mT_ParentBodyToJoint.translation() = Eigen::Vector3d(0, 0, default_height); @@ -323,7 +323,7 @@ BodyNode* addBody(const SkeletonPtr& pendulum, parent, properties, BodyNode::Properties(name)).second; // Make a shape for the Joint - const double R = default_width/2.0; + const double R = default_width / 2.0; const double h = default_depth; std::shared_ptr cyl( new CylinderShape(R, h)); @@ -332,7 +332,7 @@ BodyNode* addBody(const SkeletonPtr& pendulum, // Line up the cylinder with the Joint axis Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); tf.linear() = dart::math::eulerXYZToMatrix( - Eigen::Vector3d(90.0*M_PI/180.0, 0, 0)); + Eigen::Vector3d(90.0 * M_PI / 180.0, 0, 0)); cyl->setLocalTransform(tf); bn->addVisualizationShape(cyl); @@ -357,7 +357,7 @@ int main(int argc, char* argv[]) // Set the initial position of the first DegreeOfFreedom so that the pendulum // starts to swing right away - pendulum->getDof(1)->setPosition(120*M_PI/180.0); + pendulum->getDof(1)->setPosition(120 * M_PI / 180.0); // Create a world and add the pendulum to the world WorldPtr world(new World); From ed7f141fb7721ebf0e1ef1903de50c4d9f14d209 Mon Sep 17 00:00:00 2001 From: Karen Liu Date: Mon, 22 Jun 2015 14:08:12 -0400 Subject: [PATCH 19/65] added biped tutorial; modified styles --- tutorials/tutorialBiped.cpp | 264 +++++++++++++++++++ tutorials/tutorialDominoes-Finished.cpp | 1 + tutorials/tutorialDominoes.cpp | 1 + tutorials/tutorialMultiPendulum-Finished.cpp | 5 +- tutorials/tutorialMultiPendulum.cpp | 1 + 5 files changed, 270 insertions(+), 2 deletions(-) create mode 100644 tutorials/tutorialBiped.cpp diff --git a/tutorials/tutorialBiped.cpp b/tutorials/tutorialBiped.cpp new file mode 100644 index 0000000000000..6541599f8f118 --- /dev/null +++ b/tutorials/tutorialBiped.cpp @@ -0,0 +1,264 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Karen Liu + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/dart.h" + +const double default_force = 50.0; // N +const int default_countdown = 100; // Number of timesteps for applying force + +using namespace dart::dynamics; +using namespace dart::simulation; +using namespace dart::gui; +using namespace dart::utils; + +class Controller +{ +public: + /// Constructor + Controller(const SkeletonPtr& biped) + : mBiped(biped) + { + int nDofs = mBiped->getNumDofs(); + mKp = Eigen::MatrixXd::Identity(nDofs, nDofs); + mKd = Eigen::MatrixXd::Identity(nDofs, nDofs); + + for(size_t i = 0; i < 6; ++i) + { + mKp(i, i) = 0.0; + mKd(i, i) = 0.0; + } + + for(size_t i = 6; i < biped->getNumDofs(); ++i) + { + mKp(i, i) = 400; + mKd(i, i) = 40; + } + + resetTargetPositions(); + } + + /// Reset the desired dof position to the current position + void resetTargetPositions() + { + mTargetPositions = mBiped->getPositions(); + } + + /// Compute commanding force using PD controllers ( Lesson 3 Answer) + void setPDForces() + { + mForces.setZero(); + Eigen::VectorXd q = mBiped->getPositions(); + Eigen::VectorXd dq = mBiped->getVelocities(); + + Eigen::VectorXd p = -mKp * (q - mTargetPositions); + Eigen::VectorXd d = -mKd * dq; + + mForces = p + d; + mBiped->setForces(mForces); + } + + /// Compute commanind forces using Stable-PD controllers (Lesson 3 Answer) + void setSPDForces() + { + mForces.setZero(); + Eigen::VectorXd q = mBiped->getPositions(); + Eigen::VectorXd dq = mBiped->getVelocities(); + + Eigen::MatrixXd invM = (mBiped->getMassMatrix() + mKd * mBiped->getTimeStep()).inverse(); + Eigen::VectorXd p = -mKp * (q + dq * mBiped->getTimeStep() - mTargetPositions); + Eigen::VectorXd d = -mKd * dq; + Eigen::VectorXd qddot = + invM * (-mBiped->getCoriolisAndGravityForces() + p + d + mBiped->getConstraintForces()); + + mForces = p + d - mKd * qddot * mBiped->getTimeStep(); + mBiped->setForces(mForces); + } + + /// Compute commanding forces using ankle strategy (Lesson 4 Answer) + void setAnkleStrategyForces() + { + // Lesson 4 + } + + /// Compute commadning forces due to gravity compensation (Lesson 7 Answer) + void setGravityCompensation() + { + // Lesson 7 + } + + + +protected: + /// The biped Skeleton that we will be controlling + SkeletonPtr mBiped; + + /// Joint forces for the biped (output of the Controller) + Eigen::VectorXd mForces; + + /// Control gains for the proportional error terms in the PD controller + Eigen::MatrixXd mKp; + + /// Control gains for the derivative error terms in the PD controller + Eigen::MatrixXd mKd; + + /// Target positions for the PD controllers + Eigen::VectorXd mTargetPositions; +}; + +class MyWindow : public SimWindow +{ +public: + + /// Constructor + MyWindow(const WorldPtr& world) + : mForceCountDown(0), + mPositiveSign(true) + { + setWorld(world); + + mController = std::unique_ptr(new Controller(mWorld->getSkeleton("biped"))); + } + + /// Handle keyboard input + void keyboard(unsigned char key, int x, int y) override + { + switch(key) + { + case 'q': + mForceCountDown = default_countdown; + mPositiveSign = false; + break; + case 'w': + mForceCountDown = default_countdown; + mPositiveSign = true; + break; + default: + SimWindow::keyboard(key, x, y); + } + } + + void timeStepping() override + { + mController->setSPDForces(); + + // Apply body forces based on user input, and color the body shape red + if(mForceCountDown > 0) + { + BodyNode* bn = mWorld->getSkeleton("biped")->getBodyNode("h_abdomen"); + const ShapePtr& shape = bn->getVisualizationShape(0); + shape->setColor(dart::Color::Red()); + + if(mPositiveSign) + bn->addExtForce(default_force * Eigen::Vector3d::UnitX(), + bn->getCOM(), false, false); + else + bn->addExtForce(-default_force*Eigen::Vector3d::UnitX(), + bn->getCOM(), false, false); + + --mForceCountDown; + } + + // Step the simulation forward + SimWindow::timeStepping(); + } + +protected: + std::unique_ptr mController; + + /// Number of iterations before clearing a force entry + int mForceCountDown; + + /// Whether a force should be applied in the positive or negative direction + bool mPositiveSign; + +}; + +// Lesson 1 Answer +void initializeBiped(SkeletonPtr biped) +{ + // Set joint limits on knees + for(size_t i = 0; i < biped->getNumJoints(); ++i) + biped->getJoint(i)->setPositionLimited(true); + + // Enable self collision check but ignore adjacent bodies + biped->enableSelfCollision(); + + // Set initial configuration + biped->setPosition(biped->getDof("j_thigh_left_z")->getIndexInSkeleton(), 0.15); + biped->setPosition(biped->getDof("j_thigh_right_z")->getIndexInSkeleton(), 0.15); + biped->setPosition(biped->getDof("j_shin_left")->getIndexInSkeleton(), -0.4); + biped->setPosition(biped->getDof("j_shin_right")->getIndexInSkeleton(), -0.4); + biped->setPosition(biped->getDof("j_heel_left_1")->getIndexInSkeleton(), 0.25); + biped->setPosition(biped->getDof("j_heel_right_1")->getIndexInSkeleton(), 0.25); +} + +// Lesson 6 Answer +void setVelocityAccuators(SkeletonPtr biped) +{ + Joint* joint0 = biped->getJoint(0); + joint0->setActuatorType(Joint::PASSIVE); + for (size_t i = 1; i < biped->getNumBodyNodes(); ++i) + { + Joint* joint = biped->getJoint(i); + joint->setActuatorType(Joint::VELOCITY); + } +} + + +int main(int argc, char* argv[]) +{ + // Create the world with a skeleton + WorldPtr world + = SkelParser::readWorld( + DART_DATA_PATH"skel/biped.skel"); + assert(world != nullptr); + + initializeBiped(world->getSkeleton("biped")); + + // Create a window for rendering the world and handling user input + MyWindow window(world); + + // Print instructions + std::cout << "space bar: simulation on/off" << std::endl; + std::cout << "'p': replay simulation" << std::endl; + std::cout << "'v': Turn contact force visualization on/off" << std::endl; + std::cout << "'[' and ']': replay one frame backward and forward" << std::endl; + + // Initialize glut, initialize the window, and begin the glut event loop + glutInit(&argc, argv); + window.initWindow(640, 480, "Multi-Pendulum Tutorial"); + glutMainLoop(); +} diff --git a/tutorials/tutorialDominoes-Finished.cpp b/tutorials/tutorialDominoes-Finished.cpp index 5df60b644eab7..b3fc81cba53dd 100644 --- a/tutorials/tutorialDominoes-Finished.cpp +++ b/tutorials/tutorialDominoes-Finished.cpp @@ -504,6 +504,7 @@ int main(int argc, char* argv[]) std::cout << "'d': Delete the last domino that was created" << std::endl; std::cout << std::endl; std::cout << "spacebar: Begin simulation (you can no longer create or remove dominoes)" << std::endl; + std::cout << "'p': replay simulation" << std::endl; std::cout << "'f': Push the first domino with a disembodies force so that it falls over" << std::endl; std::cout << "'r': Push the first domino with the manipulator so that it falls over" << std::endl; std::cout << "'v': Turn contact force visualization on/off" << std::endl; diff --git a/tutorials/tutorialDominoes.cpp b/tutorials/tutorialDominoes.cpp index d2bd4d085dcc8..e98360e0cdd3c 100644 --- a/tutorials/tutorialDominoes.cpp +++ b/tutorials/tutorialDominoes.cpp @@ -373,6 +373,7 @@ int main(int argc, char* argv[]) std::cout << "'d': Delete the last domino that was created" << std::endl; std::cout << std::endl; std::cout << "spacebar: Begin simulation (you can no longer create or remove dominoes)" << std::endl; + std::cout << "'p': replay simulation" << std::endl; std::cout << "'f': Push the first domino with a disembodies force so that it falls over" << std::endl; std::cout << "'r': Push the first domino with the manipulator so that it falls over" << std::endl; std::cout << "'v': Turn contact force visualization on/off" << std::endl; diff --git a/tutorials/tutorialMultiPendulum-Finished.cpp b/tutorials/tutorialMultiPendulum-Finished.cpp index 09ea766fe5dd1..eac2f8a3667bf 100644 --- a/tutorials/tutorialMultiPendulum-Finished.cpp +++ b/tutorials/tutorialMultiPendulum-Finished.cpp @@ -286,14 +286,14 @@ class MyWindow : public dart::gui::SimWindow if(mPositiveSign) { bn->addExtForce( - default_force*Eigen::Vector3d::UnitX(), + default_force * Eigen::Vector3d::UnitX(), Eigen::Vector3d(-default_width / 2.0, 0.0, default_height / 2.0), true, true); } else { bn->addExtForce( - -default_force*Eigen::Vector3d::UnitX(), + -default_force * Eigen::Vector3d::UnitX(), Eigen::Vector3d(default_width / 2.0, 0.0, default_height / 2.0), true, true); } @@ -456,6 +456,7 @@ int main(int argc, char* argv[]) // Print instructions std::cout << "space bar: simulation on/off" << std::endl; + std::cout << "'p': replay simulation" << std::endl; std::cout << "'1' -> '9': apply torque to a pendulum body" << std::endl; std::cout << "'-': Change sign of applied joint torques" << std::endl; std::cout << "'q': Increase joint rest positions" << std::endl; diff --git a/tutorials/tutorialMultiPendulum.cpp b/tutorials/tutorialMultiPendulum.cpp index afe1b018ace56..f39c3e7164f4e 100644 --- a/tutorials/tutorialMultiPendulum.cpp +++ b/tutorials/tutorialMultiPendulum.cpp @@ -368,6 +368,7 @@ int main(int argc, char* argv[]) // Print instructions std::cout << "space bar: simulation on/off" << std::endl; + std::cout << "'p': replay simulation" << std::endl; std::cout << "'1' -> '9': apply torque to a pendulum body" << std::endl; std::cout << "'-': Change sign of applied joint torques" << std::endl; std::cout << "'q': Increase joint rest positions" << std::endl; From 0c743f5ceb3ef1cadaa26656b7208c0024371b74 Mon Sep 17 00:00:00 2001 From: Karen Liu Date: Wed, 24 Jun 2015 14:27:48 -0400 Subject: [PATCH 20/65] added Biped tutorial and associated files --- apps/vehicle/Main.cpp | 10 + apps/vehicle/MyWindow.cpp | 30 +- dart/collision/CollisionDetector.cpp | 3 + data/skel/biped.skel | 850 ++++++++++++++++++++++++ data/skel/skateboard.skel | 212 ++++++ data/skel/vehicle.skel | 2 +- tutorials/tutorialBiped-Finished.cpp | 482 ++++++++++++++ tutorials/tutorialBiped.cpp | 216 ++++-- tutorials/tutorialDominoes-Finished.cpp | 6 +- 9 files changed, 1723 insertions(+), 88 deletions(-) create mode 100644 data/skel/biped.skel create mode 100644 data/skel/skateboard.skel create mode 100644 tutorials/tutorialBiped-Finished.cpp diff --git a/apps/vehicle/Main.cpp b/apps/vehicle/Main.cpp index c5c688bb1fc74..b091aa0af3c33 100644 --- a/apps/vehicle/Main.cpp +++ b/apps/vehicle/Main.cpp @@ -53,6 +53,16 @@ int main(int argc, char* argv[]) Eigen::Vector3d gravity(0.0, -9.81, 0.0); myWorld->setGravity(gravity); + SkeletonPtr skel = myWorld->getSkeleton("car_skeleton"); + dart::dynamics::Joint* joint0 = skel->getJoint(0); + joint0->setActuatorType(dart::dynamics::Joint::PASSIVE); + for (size_t i = 1; i < skel->getNumBodyNodes(); ++i) + { + dart::dynamics::Joint* joint = skel->getJoint(i); + joint->setActuatorType(dart::dynamics::Joint::VELOCITY); + } + + // create a window and link it to the world MyWindow window; window.setWorld(myWorld); diff --git a/apps/vehicle/MyWindow.cpp b/apps/vehicle/MyWindow.cpp index c47674333abc4..357e1dc8587e6 100644 --- a/apps/vehicle/MyWindow.cpp +++ b/apps/vehicle/MyWindow.cpp @@ -53,18 +53,24 @@ void MyWindow::timeStepping() { size_t dof = vehicle->getNumDofs(); - Eigen::VectorXd q = vehicle->getPositions(); - Eigen::VectorXd dq = vehicle->getVelocities(); - Eigen::VectorXd tau = Eigen::VectorXd::Zero(dof); - - tau[ 6] = -mK * (q[6] - mSteeringWheelAngle) - mD * dq[6]; - tau[ 8] = -mK * (q[8] - mSteeringWheelAngle) - mD * dq[8]; - tau[ 7] = -mD * (dq[ 7] - mBackWheelVelocity); - tau[ 9] = -mD * (dq[ 9] - mBackWheelVelocity); - tau[10] = -mD * (dq[10] - mBackWheelVelocity); - tau[11] = -mD * (dq[11] - mBackWheelVelocity); - - vehicle->setForces(tau); + vehicle->setCommand(7, 10); + vehicle->setCommand(9, 10); + vehicle->setCommand(10, 10); + vehicle->setCommand(11, 10); + + +// Eigen::VectorXd q = vehicle->getPositions(); +// Eigen::VectorXd dq = vehicle->getVelocities(); +// Eigen::VectorXd tau = Eigen::VectorXd::Zero(dof); +// +// tau[ 6] = -mK * (q[6] - mSteeringWheelAngle) - mD * dq[6]; +// tau[ 8] = -mK * (q[8] - mSteeringWheelAngle) - mD * dq[8]; +// tau[ 7] = -mD * (dq[ 7] - mBackWheelVelocity); +// tau[ 9] = -mD * (dq[ 9] - mBackWheelVelocity); +// tau[10] = -mD * (dq[10] - mBackWheelVelocity); +// tau[11] = -mD * (dq[11] - mBackWheelVelocity); +// +// vehicle->setForces(tau); mWorld->step(); } diff --git a/dart/collision/CollisionDetector.cpp b/dart/collision/CollisionDetector.cpp index 2d31168631139..d370bac67bb4f 100644 --- a/dart/collision/CollisionDetector.cpp +++ b/dart/collision/CollisionDetector.cpp @@ -290,6 +290,9 @@ bool CollisionDetector::getPairCollidable(const CollisionNode* _node1, || index2 > mCollidablePairs.size() - 1) return true; + if (index1 == index2) + return false; + return mCollidablePairs[index1][index2]; } diff --git a/data/skel/biped.skel b/data/skel/biped.skel new file mode 100644 index 0000000000000..6b8d7d1d32f1c --- /dev/null +++ b/data/skel/biped.skel @@ -0,0 +1,850 @@ + + + + + 0.001 + 0 -9.81 0 + bullet + + + + 0 0 0 0 0 0 + + 0 0 0 0 0 0 + + 5.6154 + 0.0 0 0.0 + + + 0.0 0 0.0 0 0 0 + + + + 0.1088 0.1088 0.23936 + + + + + 0.0 0 0.0 0 0 0 + + + + 0.1088 0.1088 0.23936 + + + + + + + + 0.01649408 -0.05774016 -0.09072832 0.0 0.0 0.0 + + 6.5709 + 0.0 -0.18605 0.0 + + + 0.0 -0.18605 0.0 0.0 0.0 0.0 + + + + 0.093025 0.3721 0.093025 + + + + + 0.0 -0.18605 0.0 0.0 0.0 0.0 + + + + 0.093025 0.3721 0.093025 + + + + + + + 0.01649408 -0.42984016 -0.09072832 0.0 0.0 0.0 + + 1.7738 + 0.0 -0.18685 0.0 + + + 0.0 -0.18685 0.0 0.0 0.0 0.0 + + + + 0.07474 0.3737 0.07474 + + + + + 0.0 -0.18685 0.0 0.0 0.0 0.0 + + + + 0.07474 0.3737 0.07474 + + + + + + + 0.01649408 -0.80354016 -0.09072832 0.0 0.0 0.0 + + 0.5831 + 0.0216 -0.0216 0.0 + + + 0.0216 -0.0216 0.0 0.0 0.0 0.0 + + + + 0.1080 0.0540 0.1080 + + + 1.0 0.5 0.0 + + + 0.0216 -0.0216 0.0 0.0 0.0 0.0 + + + + 0.1080 0.0540 0.1080 + + + + + + + 0.12649408 -0.80354016 -0.09072832 0.0 0.0 0.0 + + 0.50 + 0.02 -0.02 0.0 + + + 0.02 -0.02 0.0 0.0 0.0 0.0 + + + + 0.1000 0.0500 0.0800 + + + 1.0 0.5 0.0 + + + 0.02 -0.02 0.0 0.0 0.0 0.0 + + + + 0.1000 0.0500 0.0800 + + + + + + + + 0.01649408 -0.05774016 0.09072832 0.0 0.0 0.0 + + 6.5709 + 0.0 -0.18605 0.0 + + + 0.0 -0.18605 0.0 0.0 0.0 0.0 + + + + 0.093025 0.3721 0.093025 + + + + + 0.0 -0.18605 0.0 0.0 0.0 0.0 + + + + 0.093025 0.3721 0.093025 + + + + + + + 0.01649408 -0.42984016 0.09072832 0.0 0.0 0.0 + + 1.7738 + 0.0 -0.18685 0.0 + + + 0.0 -0.18685 0.0 0.0 0.0 0.0 + + + + 0.07474 0.3737 0.07474 + + + + + 0.0 -0.18685 0.0 0.0 0.0 0.0 + + + + 0.07474 0.3737 0.07474 + + + + + + + 0.01649408 -0.80354016 0.09072832 0.0 0.0 0.0 + + 0.5831 + 0.0216 -0.0216 0.0 + + + 0.0216 -0.0216 0.0 0.0 0.0 0.0 + + + + 0.1080 0.0540 0.1080 + + + 1.0 0.5 0.0 + + + 0.0216 -0.0216 0.0 0.0 0.0 0.0 + + + + 0.1080 0.0540 0.1080 + + + + + + + 0.12649408 -0.80354016 0.09072832 0.0 0.0 0.0 + + 0.50 + 0.02 -0.02 0.0 + + + 0.02 -0.02 0.0 0.0 0.0 0.0 + + + + 0.1000 0.0500 0.0800 + + + 1.0 0.5 0.0 + + + 0.02 -0.02 0.0 0.0 0.0 0.0 + + + + 0.1000 0.0500 0.0800 + + + + + + + + 0.0 0.0562 0.0 0.0 0.0 0.0 + + 4.1846 + 0.0 0.12535 0.0 + + + 0.0 0.12535 0.0 0.0 0.0 0.0 + + + + 0.12535 0.2507 0.17549 + + + + + 0.0 0.12535 0.0 0.0 0.0 0.0 + + + + 0.12535 0.2507 0.17549 + + + + + + + 0.0 0.3069 0.0 0.0 0.0 0.0 + + 4.8608 + 0.0 0.0908 0.0 + + + 0.0 0.0908 0.0 0.0 0.0 0.0 + + + + 0.0908 0.1816 0.0908 + + + + + 0.0 0.0908 0.0 0.0 0.0 0.0 + + + + 0.0908 0.1816 0.0908 + + + + + + + 0.0 0.4885 0.0 0.0 0.0 0.0 + + 3.92 + 0.0 0.07825 0.0 + + + 0.0 0.07825 0.0 0.0 0.0 0.0 + + + + 0.101725 0.1565 0.1252 + + + 1.0 0.5 0.0 + + + 0.0 0.07825 0.0 0.0 0.0 0.0 + + + + 0.101725 0.1565 0.1252 + + + + + + + + 0.0 0.3069 0.0 -0.9423 0.0 0.0 + + 2.6264 + 0.0 0.09615 0.0 + + + 0.0 0.09615 0.0 0.0 0.0 0.0 + + + + 0.09615 0.1923 0.09615 + + + + + 0.0 0.09615 0.0 0.0 0.0 0.0 + + + + 0.09615 0.0423 0.09615 + + + + + + + 0.0 0.397146 -0.169809 0.0 0.0 0.0 + + 1.6219 + 0.0 -0.1308 0.0 + + + 0.0 -0.1308 0.0 0.0 0.0 0.0 + + + + 0.07848 0.2616 0.07848 + + + + + 0.0 -0.1308 0.0 0.0 0.0 0.0 + + + + 0.07848 0.2616 0.07848 + + + + + + + 0.0 0.135546 -0.169809 0.0 0.0 0.0 + + 0.6909 + 0.0 -0.1012 0.0 + + + 0.0 -0.1012 0.0 0.0 0.0 0.0 + + + + 0.0506 0.2024 0.0506 + + + + + 0.0 -0.1012 0.0 0.0 0.0 0.0 + + + + 0.0506 0.2024 0.0506 + + + + + + + 0.0 -0.066854 -0.169809 0.0 0.0 0.0 + + 0.2940 + 0.0 -0.06595 0.0 + + + 0.0 -0.06595 0.0 0.0 0.0 0.0 + + + + 0.032975 0.1319 0.032975 + + + 1.0 0.5 0.0 + + + 0.0 -0.06595 0.0 0.0 0.0 0.0 + + + + 0.032975 0.1319 0.032975 + + + + + + + + 0.0 0.3069 0.0 0.9423 0.0 0.0 + + 2.6264 + 0.0 0.09615 0.0 + + + 0.0 0.09615 0.0 0.0 0.0 0.0 + + + + 0.09615 0.1923 0.09615 + + + + + 0.0 0.09615 0.0 0.0 0.0 0.0 + + + + 0.09615 0.0423 0.09615 + + + + + + + 0.0 0.397146 0.169809 0.0 0.0 0.0 + + 1.6219 + 0.0 -0.1308 0.0 + + + 0.0 -0.1308 0.0 0.0 0.0 0.0 + + + + 0.07848 0.2616 0.07848 + + + + + 0.0 -0.1308 0.0 0.0 0.0 0.0 + + + + 0.07848 0.2616 0.07848 + + + + + + + 0.0 0.135546 0.169809 0.0 0.0 0.0 + + 0.6909 + 0.0 -0.1012 0.0 + + + 0.0 -0.1012 0.0 0.0 0.0 0.0 + + + + 0.0506 0.2024 0.0506 + + + + + 0.0 -0.1012 0.0 0.0 0.0 0.0 + + + + 0.0506 0.2024 0.0506 + + + + + + + 0.0 -0.066854 0.169809 0.0 0.0 0.0 + + 0.2940 + 0.0 -0.06595 0.0 + + + 0.0 -0.06595 0.0 0.0 0.0 0.0 + + + + 0.032975 0.1319 0.032975 + + + 1.0 0.5 0.0 + + + 0.0 -0.06595 0.0 0.0 0.0 0.0 + + + + 0.032975 0.1319 0.032975 + + + + + + + + world + h_pelvis + 0 0 0 0 0 0 + 0 0 0 0 0 0 + + + + + 0.0 0.0 0.0 0.0 0.0 0.0 + h_pelvis + h_thigh_left + zyx + 0 0 0 + 0 0 0 + + + + 0.0 0.0 0.0 0.0 0.0 0.0 + h_thigh_left + h_shin_left + + 0.0 0.0 1.0 + + -3.14 + 0.0 + + + -0.17 + 0 + + + + 0.0 0.0 0.0 0.0 0.0 0.0 + h_shin_left + h_heel_left + + 0.0 0.0 1.0 + + -1.57 + 3.14 + + + + 1.0 0.0 0.0 + + -1.57 + 1.57 + + + 0 0 + 0 0 + + + + 0.0 0.0 0.0 0.0 0.0 0.0 + h_heel_left + h_toe_left + + 0.0 0.0 1.0 + + -1.57 + 1.57 + + + 0 + 0 + + + + + 0.0 0.0 0.0 0.0 0.0 0.0 + h_pelvis + h_thigh_right + zyx + 0 0 0 + 0 0 0 + + + + 0.0 0.0 0.0 0.0 0.0 0.0 + h_thigh_right + h_shin_right + + 0.0 0.0 1.0 + + -3.14 + 0.0 + + + -0.17 + 0 + + + + 0.0 0.0 0.0 0.0 0.0 0.0 + h_shin_right + h_heel_right + + 0.0 0.0 1.0 + + -1.57 + 3.14 + + + + 1.0 0.0 0.0 + + -1.57 + 1.57 + + + 0 0 + 0 0 + + + + 0.0 0.0 0.0 0.0 0.0 0.0 + h_heel_right + h_toe_right + + 0.0 0.0 1.0 + + -1.57 + 1.57 + + + 0 + 0 + + + + + 0.0 0.0 0.0 0.0 0.0 0.0 + h_pelvis + h_abdomen + + 1.0 0.0 0.0 + + -1.57 + 1.57 + + + + 0.0 0.0 1.0 + + -1.57 + 1.57 + + + 0 0 + 0 0 + + + + 0.0 0.0 0.0 0.0 0.0 0.0 + h_abdomen + h_spine + + 0.0 1.0 0.0 + + -3.14 + 3.14 + + + 0 + 0 + + + + 0.0 0.0 0.0 0.0 0.0 0.0 + h_spine + h_head + + 0.0 0.0 1.0 + + -1.57 + 1.57 + + + + 1.0 0.0 0.0 + + -1.57 + 1.57 + + + 0 0 + 0 0 + + + + + 0.0 0.0 0.0 0.0 0.0 0.0 + h_spine + h_scapula_left + + 1.0 0.0 0.0 + + -1.57 + 1.57 + + + 0.0 + 0 + + + + 0.0 0.0 0.0 -1.2423 0.0 0.0 + h_scapula_left + h_bicep_left + zyx + 0 0 0 + 0 0 0 + + + + 0.0 0.0 0.0 0.0 0.0 0.0 + h_bicep_left + h_forearm_left + + 0.0 0.0 1.0 + + 0.0 + 3.14 + + + 0 + 0 + + + + 0.0 0.0 0.0 0.0 0.0 0.0 + h_forearm_left + h_hand_left + + 1.0 0.0 0.0 + + -1.57 + 1.57 + + + 0 + 0 + + + + + 0.0 0.0 0.0 0.0 0.0 0.0 + h_spine + h_scapula_right + + 1.0 0.0 0.0 + + -1.57 + 1.57 + + + 0 + 0 + + + + 0.0 0.0 0.0 1.2423 0.0 0.0 + h_scapula_right + h_bicep_right + zyx + 0 0 0 + 0 0 0 + + + + 0.0 0.0 0.0 0.0 0.0 0.0 + h_bicep_right + h_forearm_right + + 0.0 0.0 1.0 + + 0.0 + 3.14 + + + 0 + 0 + + + + 0.0 0.0 0.0 0.0 0.0 0.0 + h_forearm_right + h_hand_right + + 1.0 0.0 0.0 + + -1.57 + 1.57 + + + 0 + 0 + + + + + + + diff --git a/data/skel/skateboard.skel b/data/skel/skateboard.skel new file mode 100644 index 0000000000000..5af7b6d59c6f1 --- /dev/null +++ b/data/skel/skateboard.skel @@ -0,0 +1,212 @@ + + + + + 0.001 + 0 -9.81 0 + bullet + + + + 0 -0.3 0 0 0 0 + + 1 + 0 0 0 0 0 0 + + 1.5 + 0 0 0 + + + 0 0 0 0 0 0 + + + 0.4 0.06 0.18 + + + 0.8 0.3 0.3 + + + 0 0 0 0 0 0 + + + 0.4 0.06 0.18 + + + + + + + 1 + 0.15 -0.02 0.12 0 0 0 + + 0.1 + 0 0 0 + + + 0 0 0 0 0 0 + + + 0.05 + 0.03 + + + 0.2 0.3 0.8 + + + 0 0 0 0 0 0 + + + 0.08 0.08 0.08 + + + + + + + + 1 + 0.15 -0.02 -0.12 0 0 0 + + 0.1 + 0 0 0 + + + 0 0 0 0 0 0 + + + 0.05 + 0.03 + + + 0.2 0.3 0.8 + + + 0 0 0 0 0 0 + + + 0.08 0.08 0.08 + + + + + + + + 1 + -0.15 -0.02 0.12 0 0 0 + + 0.1 + 0 0 0 + + + 0 0 0 0 0 0 + + + 0.05 + 0.03 + + + 0.2 0.3 0.2 + + + 0 0 0 0 0 0 + + + 0.08 0.08 0.08 + + + + + + + + 1 + -0.15 -0.02 -0.12 0 0 0 + + 0.1 + 0 0 0 + + + 0 0 0 0 0 0 + + + 0.05 + 0.03 + + + 0.2 0.3 0.2 + + + 0 0 0 0 0 0 + + + 0.08 0.08 0.08 + + + + + + + + world + main_body + + + main_body + wheel_front_left + 0 0 0 0 0 0 + + 0 1 0 + 0.000 + + + 0 0 1 + 0.000 + + + + main_body + wheel_front_right + 0 0 0 0 0 0 + + 0 1 0 + 0.000 + + + 0 0 1 + 0.000 + + + + main_body + wheel_back_left + 0 0 0 0 0 0 + + 0 0 1 + 0.00 + + + + main_body + wheel_back_right + 0 0 0 0 0 0 + + 0 0 1 + 0.00 + + + + + diff --git a/data/skel/vehicle.skel b/data/skel/vehicle.skel index 88d484220c64a..7c4035f2c2680 100644 --- a/data/skel/vehicle.skel +++ b/data/skel/vehicle.skel @@ -7,7 +7,7 @@ bullet - + 0 -0.375 0 0 0 0 diff --git a/tutorials/tutorialBiped-Finished.cpp b/tutorials/tutorialBiped-Finished.cpp new file mode 100644 index 0000000000000..73d313dafcb4d --- /dev/null +++ b/tutorials/tutorialBiped-Finished.cpp @@ -0,0 +1,482 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Karen Liu + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + + +const double default_speed_increment = 0.5; + +const int default_ik_iterations = 4500; + +const double default_force = 50.0; // N +const int default_countdown = 100; // Number of timesteps for applying force + +#include "dart/dart.h" +using namespace dart::dynamics; +using namespace dart::simulation; +using namespace dart::gui; +using namespace dart::utils; +using namespace dart::math; + + +class Controller +{ +public: + /// Constructor + Controller(const SkeletonPtr& biped) + : mBiped(biped), + mPreOffset(0.0), + mSpeed(0.0) + { + int nDofs = mBiped->getNumDofs(); + + mForces = Eigen::VectorXd::Zero(nDofs); + + mKp = Eigen::MatrixXd::Identity(nDofs, nDofs); + mKd = Eigen::MatrixXd::Identity(nDofs, nDofs); + + for(size_t i = 0; i < 6; ++i) + { + mKp(i, i) = 0.0; + mKd(i, i) = 0.0; + } + + for(size_t i = 6; i < mBiped->getNumDofs(); ++i) + { + mKp(i, i) = 1000; + mKd(i, i) = 50; + } + + setTargetPositions(mBiped->getPositions()); + } + + /// Reset the desired dof position to the current position + void setTargetPositions(const Eigen::VectorXd& pose) + { + mTargetPositions = pose; + } + + /// Clear commanding forces + void clearForces() + { + mForces.setZero(); + } + + /// Add commanding forces from PD controllers (Lesson 2 Answer) + void addPDForces() + { + mForces.setZero(); + Eigen::VectorXd q = mBiped->getPositions(); + Eigen::VectorXd dq = mBiped->getVelocities(); + + Eigen::VectorXd p = -mKp * (q - mTargetPositions); + Eigen::VectorXd d = -mKd * dq; + + mForces += p + d; + mBiped->setForces(mForces); + } + + /// Add commanind forces from Stable-PD controllers (Lesson 3 Answer) + void addSPDForces() + { + Eigen::VectorXd q = mBiped->getPositions(); + Eigen::VectorXd dq = mBiped->getVelocities(); + + Eigen::MatrixXd invM = (mBiped->getMassMatrix() + + mKd * mBiped->getTimeStep()).inverse(); + Eigen::VectorXd p = + -mKp * (q + dq * mBiped->getTimeStep() - mTargetPositions); + Eigen::VectorXd d = -mKd * dq; + Eigen::VectorXd qddot = + invM * (-mBiped->getCoriolisAndGravityForces() + + p + d + mBiped->getConstraintForces()); + + mForces += p + d - mKd * qddot * mBiped->getTimeStep(); + mBiped->setForces(mForces); + } + + /// add commanding forces from ankle strategy (Lesson 4 Answer) + void addAnkleStrategyForces() + { + Eigen::Vector3d COM = mBiped->getCOM(); + Eigen::Vector3d approximatedCOP = mBiped->getBodyNode("h_heel_left")-> + getTransform()* Eigen::Vector3d(0.05, 0, 0); + double offset = COM[0] - approximatedCOP[0]; + int lHeelIndex = mBiped->getDof("j_heel_left_1")->getIndexInSkeleton(); + int rHeelIndex = mBiped->getDof("j_heel_right_1")->getIndexInSkeleton(); + int lToeIndex = mBiped->getDof("j_toe_left")->getIndexInSkeleton(); + int rToeIndex = mBiped->getDof("j_toe_right")->getIndexInSkeleton(); + if (offset < 0.1 && offset > 0.0) { + double k1 = 200.0; + double k2 = 100.0; + double kd = 10.0; + mForces[lHeelIndex] += -k1 * offset + kd * (mPreOffset - offset); + mForces[lToeIndex] += -k2 * offset + kd * (mPreOffset - offset); + mForces[rHeelIndex] += -k1 * offset + kd * (mPreOffset - offset); + mForces[rToeIndex] += -k2 * offset + kd * (mPreOffset - offset); + } else if (offset > -0.2 && offset < -0.05) { + double k1 = 2000.0; + double k2 = 100.0; + double kd = 100.0; + mForces[lHeelIndex] += -k1 * offset + kd * (mPreOffset - offset); + mForces[lToeIndex] += -k2 * offset + kd * (mPreOffset - offset); + mForces[rHeelIndex] += -k1 * offset + kd * (mPreOffset - offset); + mForces[rToeIndex] += -k2 * offset + kd * (mPreOffset - offset); + } + mPreOffset = offset; + mBiped->setForces(mForces); + } + + // Send velocity commands on wheel actuators (Lesson 6 Answer) + void setWheelCommands() + { + int wheelFirstIndex = + mBiped->getDof("joint_front_left_1")->getIndexInSkeleton(); + for (size_t i = wheelFirstIndex; i < mBiped->getNumDofs(); ++i){ + mKp(i, i) = 0.0; + mKd(i, i) = 0.0; + } + + int index1 = mBiped->getDof("joint_front_left_2")->getIndexInSkeleton(); + int index2 = mBiped->getDof("joint_front_right_2")->getIndexInSkeleton(); + int index3 = mBiped->getDof("joint_back_left")->getIndexInSkeleton(); + int index4 = mBiped->getDof("joint_back_right")->getIndexInSkeleton(); + mBiped->setCommand(index1, mSpeed); + mBiped->setCommand(index2, mSpeed); + mBiped->setCommand(index3, mSpeed); + mBiped->setCommand(index4, mSpeed); + } + + void changeWheelSpeed(double increment) + { + mSpeed += increment; + std::cout << "wheel speed = " << mSpeed << std::endl; + } + +protected: + /// The biped Skeleton that we will be controlling + SkeletonPtr mBiped; + + /// Joint forces for the biped (output of the Controller) + Eigen::VectorXd mForces; + + /// Control gains for the proportional error terms in the PD controller + Eigen::MatrixXd mKp; + + /// Control gains for the derivative error terms in the PD controller + Eigen::MatrixXd mKd; + + /// Target positions for the PD controllers + Eigen::VectorXd mTargetPositions; + + /// For ankle strategy: Error in the previous timestep + double mPreOffset; + + /// For velocity actuator: Current speed of the skateboard + double mSpeed; +}; + +class MyWindow : public SimWindow +{ +public: + /// Constructor + MyWindow(const WorldPtr& world) + : mForceCountDown(0), + mPositiveSign(true) + { + setWorld(world); + + mController = std::unique_ptr + (new Controller(mWorld->getSkeleton("biped"))); + } + + /// Handle keyboard input + void keyboard(unsigned char key, int x, int y) override + { + switch(key) + { + case ',': + mForceCountDown = default_countdown; + mPositiveSign = false; + break; + case '.': + mForceCountDown = default_countdown; + mPositiveSign = true; + break; + case 'a': + mController->changeWheelSpeed(default_speed_increment); + break; + case 's': + mController->changeWheelSpeed(-default_speed_increment); + break; + default: + SimWindow::keyboard(key, x, y); + } + } + + void timeStepping() override + { + mController->clearForces(); + + // Lesson 3 + mController->addSPDForces(); + + // Lesson 4 + mController->addAnkleStrategyForces(); + + // Lesson 6 + mController->setWheelCommands(); + + // Apply body forces based on user input, and color the body shape red + if(mForceCountDown > 0) + { + BodyNode* bn = mWorld->getSkeleton("biped")->getBodyNode("h_abdomen"); + const ShapePtr& shape = bn->getVisualizationShape(0); + shape->setColor(dart::Color::Red()); + + if(mPositiveSign) + bn->addExtForce(default_force * Eigen::Vector3d::UnitX(), + bn->getCOM(), false, false); + else + bn->addExtForce(-default_force*Eigen::Vector3d::UnitX(), + bn->getCOM(), false, false); + + --mForceCountDown; + } + + // Step the simulation forward + SimWindow::timeStepping(); + } + +protected: + std::unique_ptr mController; + + /// Number of iterations before clearing a force entry + int mForceCountDown; + + /// Whether a force should be applied in the positive or negative direction + bool mPositiveSign; + +}; + +// Load a biped model and enable joint limits and self-collision +// (Lesson 1 Answer) +SkeletonPtr loadBiped() +{ + // Create the world with a skeleton + WorldPtr world + = SkelParser::readWorld( + DART_DATA_PATH"skel/biped.skel"); + assert(world != nullptr); + + SkeletonPtr biped = world->getSkeleton("biped"); + + // Set joint limits on knees + for(size_t i = 0; i < biped->getNumJoints(); ++i) + biped->getJoint(i)->setPositionLimited(true); + + // Enable self collision check but ignore adjacent bodies + biped->enableSelfCollision(); + + // Set initial configuration + biped->setPosition(biped->getDof("j_thigh_left_z")-> + getIndexInSkeleton(), 0.15); + biped->setPosition(biped->getDof("j_thigh_right_z")-> + getIndexInSkeleton(), 0.15); + biped->setPosition(biped->getDof("j_shin_left")-> + getIndexInSkeleton(), -0.4); + biped->setPosition(biped->getDof("j_shin_right")-> + getIndexInSkeleton(), -1.4); + biped->setPosition(biped->getDof("j_heel_left_1")-> + getIndexInSkeleton(), 0.25); + biped->setPosition(biped->getDof("j_heel_right_1")-> + getIndexInSkeleton(), 0.25); + biped->setPosition(biped->getDof("j_bicep_left_x")-> + getIndexInSkeleton(), 0.8); + biped->setPosition(biped->getDof("j_bicep_right_x")-> + getIndexInSkeleton(), -0.8); + return biped; +} + +// Load a skateboard model and connect it to the biped model via an Euler joint +// (Lesson 5 Answer) +void modifyBipedWithSkateboard(SkeletonPtr biped) +{ + // Load the Skeleton from a file + WorldPtr world = SkelParser::readWorld(DART_DATA_PATH"skel/skateboard.skel"); + + SkeletonPtr skateboard = world->getSkeleton(0); + + EulerJoint::Properties properties = EulerJoint::Properties(); + properties.mT_ChildBodyToJoint.translation() = Eigen::Vector3d(0, 0.1, 0); + + skateboard->getRootBodyNode()->moveTo + (biped->getBodyNode("h_heel_left"), properties); +} + +// Set the actuator type for four wheel joints to "VELOCITY" (Lesson 6 Answer) +void setVelocityAccuators(SkeletonPtr biped) +{ + + Joint* wheel1 = biped->getJoint("joint_front_left"); + Joint* wheel2 = biped->getJoint("joint_front_right"); + Joint* wheel3 = biped->getJoint("joint_back_left"); + Joint* wheel4 = biped->getJoint("joint_back_right"); + wheel1->setActuatorType(Joint::VELOCITY); + wheel2->setActuatorType(Joint::VELOCITY); + wheel3->setActuatorType(Joint::VELOCITY); + wheel4->setActuatorType(Joint::VELOCITY); +} + +// Solve for a balanced pose using IK (Lesson 7 Answer) +Eigen::VectorXd solveIK(SkeletonPtr biped) +{ + Eigen::VectorXd newPose = biped->getPositions(); + BodyNodePtr leftHeel = biped->getBodyNode("h_heel_left"); + BodyNodePtr leftToe = biped->getBodyNode("h_toe_left"); + double initialHeight = -0.8; + + for(size_t i = 0; i < default_ik_iterations; ++i) + { + Eigen::Vector3d deviation = biped->getCOM() - leftHeel->getCOM(); + LinearJacobian jacobian = biped->getCOMLinearJacobian() - + biped->getLinearJacobian(leftHeel, leftHeel->getCOM(leftHeel)); + + // sagittal deviation + double error = deviation[0]; + Eigen::VectorXd gradient = jacobian.row(0); + Eigen::VectorXd newDirection = -0.2 * error * gradient; + + // lateral deviation + error = deviation[2]; + gradient = jacobian.row(2); + newDirection += -0.2 * error * gradient; + + // position constraint on four (approximated) corners of the left foot + Eigen::Vector3d offset(0.0, -0.04, -0.03); + error = (leftHeel->getTransform() * offset)[1] - initialHeight; + gradient = biped->getLinearJacobian(leftHeel, offset).row(1); + newDirection += -0.2 * error * gradient; + + offset[2] = 0.03; + error = (leftHeel->getTransform() * offset)[1] - initialHeight; + gradient = biped->getLinearJacobian(leftHeel, offset).row(1); + newDirection += -0.2 * error * gradient; + + offset[0] = 0.04; + error = (leftToe->getTransform() * offset)[1] - initialHeight; + gradient = biped->getLinearJacobian(leftToe, offset).row(1); + newDirection += -0.2 * error * gradient; + + offset[2] = -0.03; + error = (leftToe->getTransform() * offset)[1] - initialHeight; + gradient = biped->getLinearJacobian(leftToe, offset).row(1); + newDirection += -0.2 * error * gradient; + + newPose += newDirection; + biped->setPositions(newPose); + biped->computeForwardKinematics(true, false, false); + } + return newPose; +} + +SkeletonPtr createFloor() +{ + SkeletonPtr floor = Skeleton::create("floor"); + + // Give the floor a body + BodyNodePtr body = + floor->createJointAndBodyNodePair(nullptr).second; + + // Give the body a shape + double floor_width = 10.0; + double floor_height = 0.01; + std::shared_ptr box( + new BoxShape(Eigen::Vector3d(floor_width, floor_height, floor_width))); + box->setColor(dart::Color::Black()); + + body->addVisualizationShape(box); + body->addCollisionShape(box); + + // Put the body into position + Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); + tf.translation() = Eigen::Vector3d(0.0, -1.0, 0.0); + body->getParentJoint()->setTransformFromParentBodyNode(tf); + + return floor; +} + +int main(int argc, char* argv[]) +{ + SkeletonPtr floor = createFloor(); + + // Lesson 1 + SkeletonPtr biped = loadBiped(); + + // Lesson 5 + modifyBipedWithSkateboard(biped); + + // Lesson 6 + setVelocityAccuators(biped); + + // Lesson 7 + Eigen::VectorXd balancedPose = solveIK(biped); + biped->setPositions(balancedPose); + + WorldPtr world = std::make_shared(); + world->getConstraintSolver()->setCollisionDetector( + new dart::collision::BulletCollisionDetector()); + world->setGravity(Eigen::Vector3d(0.0, -9.81, 0.0)); + + world->addSkeleton(floor); + world->addSkeleton(biped); + + // Create a window for rendering the world and handling user input + MyWindow window(world); + + // Print instructions + std::cout << "'.': forward push" << std::endl; + std::cout << "',': backward push" << std::endl; + std::cout << "'s': increase skateboard forward speed" << std::endl; + std::cout << "'a': increase skateboard backward speed" << std::endl; + std::cout << "space bar: simulation on/off" << std::endl; + std::cout << "'p': replay simulation" << std::endl; + std::cout << "'v': Turn contact force visualization on/off" << std::endl; + std::cout << "'[' and ']': replay one frame backward and forward" << std::endl; + + // Initialize glut, initialize the window, and begin the glut event loop + glutInit(&argc, argv); + window.initWindow(640, 480, "Multi-Pendulum Tutorial"); + glutMainLoop(); +} diff --git a/tutorials/tutorialBiped.cpp b/tutorials/tutorialBiped.cpp index 6541599f8f118..1574909b099a1 100644 --- a/tutorials/tutorialBiped.cpp +++ b/tutorials/tutorialBiped.cpp @@ -34,27 +34,38 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "dart/dart.h" + +const double default_speed_increment = 0.5; + +const int default_ik_iterations = 4500; const double default_force = 50.0; // N const int default_countdown = 100; // Number of timesteps for applying force +#include "dart/dart.h" using namespace dart::dynamics; using namespace dart::simulation; using namespace dart::gui; using namespace dart::utils; +using namespace dart::math; + class Controller { public: /// Constructor Controller(const SkeletonPtr& biped) - : mBiped(biped) + : mBiped(biped), + mPreOffset(0.0), + mSpeed(0.0) { int nDofs = mBiped->getNumDofs(); + + mForces = Eigen::VectorXd::Zero(nDofs); + mKp = Eigen::MatrixXd::Identity(nDofs, nDofs); mKd = Eigen::MatrixXd::Identity(nDofs, nDofs); - + for(size_t i = 0; i < 6; ++i) { mKp(i, i) = 0.0; @@ -63,64 +74,55 @@ class Controller for(size_t i = 6; i < biped->getNumDofs(); ++i) { - mKp(i, i) = 400; - mKd(i, i) = 40; + mKp(i, i) = 1000; + mKd(i, i) = 50; } - resetTargetPositions(); + setTargetPositions(mBiped->getPositions()); } /// Reset the desired dof position to the current position - void resetTargetPositions() + void setTargetPositions(const Eigen::VectorXd& pose) { - mTargetPositions = mBiped->getPositions(); + mTargetPositions = pose; } - /// Compute commanding force using PD controllers ( Lesson 3 Answer) - void setPDForces() + /// Clear commanding forces + void clearForces() { mForces.setZero(); - Eigen::VectorXd q = mBiped->getPositions(); - Eigen::VectorXd dq = mBiped->getVelocities(); - - Eigen::VectorXd p = -mKp * (q - mTargetPositions); - Eigen::VectorXd d = -mKd * dq; - - mForces = p + d; - mBiped->setForces(mForces); } - - /// Compute commanind forces using Stable-PD controllers (Lesson 3 Answer) - void setSPDForces() + + /// Add commanding forces from PD controllers + void addPDForces() { - mForces.setZero(); - Eigen::VectorXd q = mBiped->getPositions(); - Eigen::VectorXd dq = mBiped->getVelocities(); + // Lesson 2 + } - Eigen::MatrixXd invM = (mBiped->getMassMatrix() + mKd * mBiped->getTimeStep()).inverse(); - Eigen::VectorXd p = -mKp * (q + dq * mBiped->getTimeStep() - mTargetPositions); - Eigen::VectorXd d = -mKd * dq; - Eigen::VectorXd qddot = - invM * (-mBiped->getCoriolisAndGravityForces() + p + d + mBiped->getConstraintForces()); - - mForces = p + d - mKd * qddot * mBiped->getTimeStep(); - mBiped->setForces(mForces); + /// Add commanind forces from Stable-PD controllers + void addSPDForces() + { + // Lesson 3 } - /// Compute commanding forces using ankle strategy (Lesson 4 Answer) - void setAnkleStrategyForces() + /// add commanding forces from ankle strategy + void addAnkleStrategyForces() { // Lesson 4 } - /// Compute commadning forces due to gravity compensation (Lesson 7 Answer) - void setGravityCompensation() + // Send velocity commands on wheel actuators + void setWheelCommands() { - // Lesson 7 + // Lesson 6 } + void changeWheelSpeed(double increment) + { + mSpeed += increment; + std::cout << "wheel speed = " << mSpeed << std::endl; + } - protected: /// The biped Skeleton that we will be controlling SkeletonPtr mBiped; @@ -136,12 +138,17 @@ class Controller /// Target positions for the PD controllers Eigen::VectorXd mTargetPositions; + + /// For ankle strategy: Error in the previous timestep + double mPreOffset; + + /// For velocity actuator: Current speed of the skateboard + double mSpeed; }; class MyWindow : public SimWindow { public: - /// Constructor MyWindow(const WorldPtr& world) : mForceCountDown(0), @@ -149,7 +156,8 @@ class MyWindow : public SimWindow { setWorld(world); - mController = std::unique_ptr(new Controller(mWorld->getSkeleton("biped"))); + mController = std::unique_ptr + (new Controller(mWorld->getSkeleton("biped"))); } /// Handle keyboard input @@ -157,14 +165,20 @@ class MyWindow : public SimWindow { switch(key) { - case 'q': + case ',': mForceCountDown = default_countdown; mPositiveSign = false; break; - case 'w': + case '.': mForceCountDown = default_countdown; mPositiveSign = true; break; + case 'a': + mController->changeWheelSpeed(default_speed_increment); + break; + case 's': + mController->changeWheelSpeed(-default_speed_increment); + break; default: SimWindow::keyboard(key, x, y); } @@ -172,7 +186,16 @@ class MyWindow : public SimWindow void timeStepping() override { - mController->setSPDForces(); + mController->clearForces(); + + // Lesson 3 + mController->addSPDForces(); + + // Lesson 4 + mController->addAnkleStrategyForces(); + + // Lesson 6 + mController->setWheelCommands(); // Apply body forces based on user input, and color the body shape red if(mForceCountDown > 0) @@ -206,52 +229,101 @@ class MyWindow : public SimWindow }; -// Lesson 1 Answer -void initializeBiped(SkeletonPtr biped) +// Load a biped model and enable joint limits and self-collision +SkeletonPtr loadBiped() { - // Set joint limits on knees - for(size_t i = 0; i < biped->getNumJoints(); ++i) - biped->getJoint(i)->setPositionLimited(true); + // Lesson 1 - // Enable self collision check but ignore adjacent bodies - biped->enableSelfCollision(); - - // Set initial configuration - biped->setPosition(biped->getDof("j_thigh_left_z")->getIndexInSkeleton(), 0.15); - biped->setPosition(biped->getDof("j_thigh_right_z")->getIndexInSkeleton(), 0.15); - biped->setPosition(biped->getDof("j_shin_left")->getIndexInSkeleton(), -0.4); - biped->setPosition(biped->getDof("j_shin_right")->getIndexInSkeleton(), -0.4); - biped->setPosition(biped->getDof("j_heel_left_1")->getIndexInSkeleton(), 0.25); - biped->setPosition(biped->getDof("j_heel_right_1")->getIndexInSkeleton(), 0.25); + // Create the world with a skeleton + WorldPtr world + = SkelParser::readWorld( + DART_DATA_PATH"skel/biped.skel"); + assert(world != nullptr); + + SkeletonPtr biped = world->getSkeleton("biped"); + + return biped; } -// Lesson 6 Answer +// Load a skateboard model and connect it to the biped model via an Euler joint +void modifyBipedWithSkateboard(SkeletonPtr biped) +{ + // Lesson 5 +} + +// Set the actuator type for four wheel joints to "VELOCITY" void setVelocityAccuators(SkeletonPtr biped) { - Joint* joint0 = biped->getJoint(0); - joint0->setActuatorType(Joint::PASSIVE); - for (size_t i = 1; i < biped->getNumBodyNodes(); ++i) - { - Joint* joint = biped->getJoint(i); - joint->setActuatorType(Joint::VELOCITY); - } + // Lesson 6 } +// Solve for a balanced pose using IK +Eigen::VectorXd solveIK(SkeletonPtr biped) +{ + // Lesson 7 + Eigen::VectorXd newPose = biped->getPositions(); + return newPose; +} + +SkeletonPtr createFloor() +{ + SkeletonPtr floor = Skeleton::create("floor"); + + // Give the floor a body + BodyNodePtr body = + floor->createJointAndBodyNodePair(nullptr).second; + + // Give the body a shape + double floor_width = 10.0; + double floor_height = 0.01; + std::shared_ptr box( + new BoxShape(Eigen::Vector3d(floor_width, floor_height, floor_width))); + box->setColor(dart::Color::Black()); + + body->addVisualizationShape(box); + body->addCollisionShape(box); + + // Put the body into position + Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); + tf.translation() = Eigen::Vector3d(0.0, -1.0, 0.0); + body->getParentJoint()->setTransformFromParentBodyNode(tf); + + return floor; +} int main(int argc, char* argv[]) { - // Create the world with a skeleton - WorldPtr world - = SkelParser::readWorld( - DART_DATA_PATH"skel/biped.skel"); - assert(world != nullptr); + SkeletonPtr floor = createFloor(); + + // Lesson 1 + SkeletonPtr biped = loadBiped(); + + // Lesson 5 + modifyBipedWithSkateboard(biped); - initializeBiped(world->getSkeleton("biped")); + // Lesson 6 + setVelocityAccuators(biped); + + // Lesson 7 + Eigen::VectorXd balancedPose = solveIK(biped); + biped->setPositions(balancedPose); + + WorldPtr world = std::make_shared(); + world->getConstraintSolver()->setCollisionDetector( + new dart::collision::BulletCollisionDetector()); + world->setGravity(Eigen::Vector3d(0.0, -9.81, 0.0)); + + world->addSkeleton(floor); + world->addSkeleton(biped); // Create a window for rendering the world and handling user input MyWindow window(world); // Print instructions + std::cout << "'.': forward push" << std::endl; + std::cout << "',': backward push" << std::endl; + std::cout << "'s': increase skateboard forward speed" << std::endl; + std::cout << "'a': increase skateboard backward speed" << std::endl; std::cout << "space bar: simulation on/off" << std::endl; std::cout << "'p': replay simulation" << std::endl; std::cout << "'v': Turn contact force visualization on/off" << std::endl; diff --git a/tutorials/tutorialDominoes-Finished.cpp b/tutorials/tutorialDominoes-Finished.cpp index b3fc81cba53dd..a6d858e30e53b 100644 --- a/tutorials/tutorialDominoes-Finished.cpp +++ b/tutorials/tutorialDominoes-Finished.cpp @@ -139,13 +139,13 @@ class Controller Jacobian J = mEndEffector->getWorldJacobian(mOffset); // Compute the pseudo-inverse of the Jacobian Eigen::MatrixXd pinv_J = J.transpose() * (J * J.transpose() - + 0.0025 * Eigen::Matrix6d::Identity()).inverse(); + + 0.0025 * Eigen::Matrix6d::Identity()).inverse(); // Compute the Jacobian time derivative Jacobian dJ = mEndEffector->getJacobianClassicDeriv(mOffset); // Comptue the pseudo-inverse of the Jacobian time derivative Eigen::MatrixXd pinv_dJ = dJ.transpose() * (dJ * dJ.transpose() - + 0.0025 * Eigen::Matrix6d::Identity()).inverse(); + + 0.0025 * Eigen::Matrix6d::Identity()).inverse(); // Compute the linear error Eigen::Vector6d e; @@ -233,7 +233,7 @@ class MyWindow : public dart::gui::SimWindow mFloor = world->getSkeleton("floor"); mController = std::unique_ptr( - new Controller(world->getSkeleton("manipulator"), mFirstDomino)); + new Controller(world->getSkeleton("manipulator"), mFirstDomino)); } // Attempt to create a new domino. If the new domino would be in collision From b710756c31ae29fe137b612df16e918f6f636b3d Mon Sep 17 00:00:00 2001 From: Karen Liu Date: Wed, 24 Jun 2015 14:34:03 -0400 Subject: [PATCH 21/65] reverted vehicle app --- apps/vehicle/Main.cpp | 10 ---------- apps/vehicle/MyWindow.cpp | 32 +++++++++++++------------------- 2 files changed, 13 insertions(+), 29 deletions(-) diff --git a/apps/vehicle/Main.cpp b/apps/vehicle/Main.cpp index b091aa0af3c33..f5c7573333a1c 100644 --- a/apps/vehicle/Main.cpp +++ b/apps/vehicle/Main.cpp @@ -52,16 +52,6 @@ int main(int argc, char* argv[]) assert(myWorld != nullptr); Eigen::Vector3d gravity(0.0, -9.81, 0.0); myWorld->setGravity(gravity); - - SkeletonPtr skel = myWorld->getSkeleton("car_skeleton"); - dart::dynamics::Joint* joint0 = skel->getJoint(0); - joint0->setActuatorType(dart::dynamics::Joint::PASSIVE); - for (size_t i = 1; i < skel->getNumBodyNodes(); ++i) - { - dart::dynamics::Joint* joint = skel->getJoint(i); - joint->setActuatorType(dart::dynamics::Joint::VELOCITY); - } - // create a window and link it to the world MyWindow window; diff --git a/apps/vehicle/MyWindow.cpp b/apps/vehicle/MyWindow.cpp index 357e1dc8587e6..f4b8490699d3c 100644 --- a/apps/vehicle/MyWindow.cpp +++ b/apps/vehicle/MyWindow.cpp @@ -51,26 +51,20 @@ void MyWindow::timeStepping() { dart::dynamics::SkeletonPtr vehicle = mWorld->getSkeleton("car_skeleton"); assert(vehicle != 0); - size_t dof = vehicle->getNumDofs(); - - vehicle->setCommand(7, 10); - vehicle->setCommand(9, 10); - vehicle->setCommand(10, 10); - vehicle->setCommand(11, 10); - + size_t dof = vehicle->getNumDofs(); -// Eigen::VectorXd q = vehicle->getPositions(); -// Eigen::VectorXd dq = vehicle->getVelocities(); -// Eigen::VectorXd tau = Eigen::VectorXd::Zero(dof); -// -// tau[ 6] = -mK * (q[6] - mSteeringWheelAngle) - mD * dq[6]; -// tau[ 8] = -mK * (q[8] - mSteeringWheelAngle) - mD * dq[8]; -// tau[ 7] = -mD * (dq[ 7] - mBackWheelVelocity); -// tau[ 9] = -mD * (dq[ 9] - mBackWheelVelocity); -// tau[10] = -mD * (dq[10] - mBackWheelVelocity); -// tau[11] = -mD * (dq[11] - mBackWheelVelocity); -// -// vehicle->setForces(tau); + Eigen::VectorXd q = vehicle->getPositions(); + Eigen::VectorXd dq = vehicle->getVelocities(); + Eigen::VectorXd tau = Eigen::VectorXd::Zero(dof); + + tau[ 6] = -mK * (q[6] - mSteeringWheelAngle) - mD * dq[6]; + tau[ 8] = -mK * (q[8] - mSteeringWheelAngle) - mD * dq[8]; + tau[ 7] = -mD * (dq[ 7] - mBackWheelVelocity); + tau[ 9] = -mD * (dq[ 9] - mBackWheelVelocity); + tau[10] = -mD * (dq[10] - mBackWheelVelocity); + tau[11] = -mD * (dq[11] - mBackWheelVelocity); + + vehicle->setForces(tau); mWorld->step(); } From 71763eb2b05898ea07e10544fc17489601f01187 Mon Sep 17 00:00:00 2001 From: Karen Liu Date: Thu, 25 Jun 2015 16:07:52 -0400 Subject: [PATCH 22/65] minor style changes on tutorialBiped-Finished.cpp --- tutorials/tutorialBiped-Finished.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/tutorials/tutorialBiped-Finished.cpp b/tutorials/tutorialBiped-Finished.cpp index 73d313dafcb4d..14141c60940eb 100644 --- a/tutorials/tutorialBiped-Finished.cpp +++ b/tutorials/tutorialBiped-Finished.cpp @@ -473,7 +473,8 @@ int main(int argc, char* argv[]) std::cout << "space bar: simulation on/off" << std::endl; std::cout << "'p': replay simulation" << std::endl; std::cout << "'v': Turn contact force visualization on/off" << std::endl; - std::cout << "'[' and ']': replay one frame backward and forward" << std::endl; + std::cout << + "'[' and ']': replay one frame backward and forward" << std::endl; // Initialize glut, initialize the window, and begin the glut event loop glutInit(&argc, argv); From beef607fa8bd320e5e8eae904a8de3a059df698a Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 26 Jun 2015 07:24:57 +0900 Subject: [PATCH 23/65] Add initial work for readthedocs --- .gitignore | 2 +- docs/readthedocs/index.md | 7 +++++++ docs/readthedocs/logo.png | Bin 0 -> 15232 bytes docs/readthedocs/tutorials/biped.md | 13 +++++++++++++ docs/readthedocs/tutorials/dominoes.md | 13 +++++++++++++ docs/readthedocs/tutorials/multi-pendulum.md | 13 +++++++++++++ mkdocs.yml | 13 +++++++++++++ 7 files changed, 60 insertions(+), 1 deletion(-) create mode 100644 docs/readthedocs/index.md create mode 100644 docs/readthedocs/logo.png create mode 100644 docs/readthedocs/tutorials/biped.md create mode 100644 docs/readthedocs/tutorials/dominoes.md create mode 100644 docs/readthedocs/tutorials/multi-pendulum.md create mode 100644 mkdocs.yml diff --git a/.gitignore b/.gitignore index ffde43bf85ede..b8c4ec59c0735 100644 --- a/.gitignore +++ b/.gitignore @@ -5,7 +5,6 @@ syntax: glob CPack* CTest* Testing/ -docs/ TAGS .DS_Store .cproject @@ -23,3 +22,4 @@ TAGS *~ /nbproject/ /doxygen/html/ +docs/readthedocs/site \ No newline at end of file diff --git a/docs/readthedocs/index.md b/docs/readthedocs/index.md new file mode 100644 index 0000000000000..7d4bd55eb1a01 --- /dev/null +++ b/docs/readthedocs/index.md @@ -0,0 +1,7 @@ +# Welcome to DART + + + +DART (Dynamic Animation and Robotics Toolkit) is a collaborative, cross-platform, open source library created by the Georgia Tech Graphics Lab and Humanoid Robotics Lab. The library provides data structures and algorithms for kinematic and dynamic applications in robotics and computer animation. DART is distinguished by its accuracy and stability due to its use of generalized coordinates to represent articulated rigid body systems and computation of Lagrange's equations derived from D.Alembert's principle to describe the dynamics of motion. For developers, in contrast to many popular physics engines which view the simulator as a black box, DART gives full access to internal kinematic and dynamic quantities, such as the mass matrix, Coriolis and centrifugal forces, transformation matrices and their derivatives. DART also provides efficient computation of Jacobian matrices for arbitrary body points and coordinate frames. Contact and collision are handled using an implicit time-stepping, velocity-based LCP (linear-complementarity problem) to guarantee non-penetration, directional friction, and approximated Coulomb friction cone conditions. For collision detection, DART uses FCL developed by Willow Garage and the UNC Gamma Lab. DART has applications in robotics and computer animation because it features a multibody dynamic simulator and tools for control and motion planning. Multibody dynamic simulation in DART is an extension of RTQL8, an open source software created by the Georgia Tech Graphics Lab. For ease of use, we separately provide the open source GRIP environment library which simplifies user interface, world loading, visualization and video recording. + +DART is fully integrated with Gazebo 2.0, a multi-robot simulator for outdoor environments. DART supports most features provided in Gazebo, including the link structure, sensor simulation, and all the joint types. DART also supports models in URDF, SDF, and VSK file formats. diff --git a/docs/readthedocs/logo.png b/docs/readthedocs/logo.png new file mode 100644 index 0000000000000000000000000000000000000000..6c593af571b07702ee6fd0ac9694bcbb9e1c8f52 GIT binary patch literal 15232 zcmZvDby$>7^zTwqN(e07N{4`SH%K>9OGvlCE*(-!i-0IdcY~xXB`ruJ-JMIbGY+-AyZEf++$NiVJ z7yuypOj%A^*L&_D!_Q53I%7Mi!bSXJ7>&6Vb3@*fz8Z+7@oE3CHxVYJhW+L)GV<)c;jl<^9;h zME@)M#(?wOTxIqCqk@95AphN`n=8vE`X2(RIzQPa{Jeqxp2)$L%3UhUG_vRZZ#~it z^IwJFaESmOS}gHj1)YIr_?C%&L6K`{h)FZI!J8CQ$>W)d|^quQ#gIm0k4Je)B zj8j&tUw64jS}#u($J@#2n}zhw9}OlaN4x6l&v!mWWHsjRipsQDouK`bW(@wt4!_Qg z)Us8>pWMV>cs5bu?)lh))7vc9r{TqomGNyeqpz3(4@lRj34Q-j(9Ka39+Dcij}&u@n!0pYKg-M6w3Jo%E)S$?+x%U#i&Y3HeJ!BM3e5{}DmKqVj&`VX*VJ+wbR+ zB`ztA?sk{HSL}zIm)#0dQ0`_b*_3tRwc-0O{TtY%&U&8h>d zUdZjOWKC?VPpo;soJbB7X4W3I)zl8yl5Rrf1k7YSKX_`q0Qvo!ET{*uBpEnQ-Nhh3=KMcz1C4uO*3~q>>FTmcsVpwY z*kAbVMWGHm704w2(PK0Edb;(FnSNoNp+4vKAB9xiJpBR@4ef3NKzk4b8z*%S-$h3P}a*-x9ZA$@ee2#=zaO~e-hIM6<2 z(4OY&@(eB)9RA7=DjbadK&c ze+n9G$8FG*fU}zEsa4*nT+=3DZaox}WRTzPCuw5yPinS2rtA@MjxfJel@H?Tvjp*a z*q7%eTGrtgXRyO0{kWS?fwUn<8%e(DO@3la6CuZ2R8ak>3TvQ<0YZq+_I3>P z>fD}f=4&h=NYmn8b(Z@nAO5;uiBVN; zA-Z4X(hf)tdr?lC2#!QqD3_^2=~(28R9hpI`E5B>27bMcvK)_>h&y{ShWBQSe!`)v z$-^*SIEt>l1O=`W%Fqu5G&C7F4FEs1f-1WOg;RcwJ`-eT&rVsbPDCY0dHV4VLB6{&NzX$wkc-`D4p-Cf$S^mJ&Sh#Lfbj=} z$_b6UJGy=yB=;di*0Z0-hCm*LZZ5-WXPCd0TYJjYG~J+uN}lPwqmQb6gmi!3KBZR6 zitV|9OcV3U5gwHTsBg1SuVuIr{B59_#><;yo@bKE?W0;QDAwE`h2Rj+eQ$dtXk%f& zeUb~bihm)r$JFJ*1Dfm_$ixRbl7+O3x;a=D9PB$?2^mC^E~?SZg-m!3oM7DA1LR;t z-)1CLCu}wBQ>*%m+NE+R^=AT2<9|KG^(riug`3(4sGQ)rV;nGfI8=QxjI=J0QT_Fhw?UL?hN1s-_u`nbZa@$NAk~Bh1#|NW@=Lq= zKiYGK`wj2Wz;8blsFk+NdRG6K9h~xd!#O;*)=A>wpjM=l9_~gf4^>*}$R!|ha=)YV zws6LD=}gxl#CvYYkgQ%*|4Y@wG~gRCjoG`DUdm+^NZ>;maV03XD*Jv2tH4bCx#_p& zEd;xR1MeyoZDrY+i4}?EH{e#0y?Y?jS^bW~YZJv)C2{%BbuGk&{iMhfj(2Z2o&u); zd#95OSCj`FpUqO~Sj4Cu=ADs}8>on;2_iE%U*#Fwil8pFrGFykA;Q!2nKRe4yUKxb zh<6(scj{c!b?hi2vToRF$kdeh`Yy}bs(Jd4?cLPd@*_K!%WvkkH4+f(PlZM5V+ZHg zpFW7!+P(djcRd^Pn$Kqs--atgIPl?~dO5URv>ID3sGs|3vp>TOx#4`se8)Xr@)TI) z-3JX?k|@sC=`GrBq#85@>m!n75T;q{Em`rp^MuED%MF*-w?dcA(nBaS>e%71v|KoLNn-=hIm$N1I1tKkp8RhfG9dcPBrHPp79DiJWcC zW@Q9D9)jejF$v%e>&*L}n0q8F!5V6Dxn#itxu(!_uEghS^VkD7RgJzs0B@VxFJfr- zKUd%~I4eCMsvuFxzwh7AMlmWUdNa(?Jt`Vks%904!a*i+4 zbhey(8dM+3d4TJYN(Fn+Tg5XVH^&#IMbP0wByWOc>(E{XV7DD+6T~YdSdfwlA73Or zmcN=Nyzk;3eMajq_2&@Z?JaJNgwb$huPZFHM>NKp6a2aMQ5e?UB>m3*%v09;n$wby zuOvWGJ{gm{S|zq9gveLhT=nYwgeqny#2tg!BGe9oO}Q_u45eAutBj%{a#t!>EYRG> zF6U=?DSC5POI!kyaXxCFADWL>uE{Ll=;9@|Oo+A5)NF0CY$R(mZ1OH~=b1a7UYqAO zHvtG#XUftuARE%=3N1K!@F4_E? z(%k1h-NB&m`93ARC>L1Sm1LK`d&_dYueLy;rLT;Gg;Kp%A9M5V+Dn%`2wp_t;v1kGM7WWVIC*F$Ed20&i-Tlu>NmFL7FHZtaW0Z}C`XoU@d|ys3#h z#p%OgW%NSZ+gvcoSA{@3_t|qgdM=N8qNm)xUOqXZVclwNIo|djGz}#2^#M8t z1|8y+6(RbV;W1sj?y$ID!j-b?ZGVXJdnBtBdMLH-3BHfnuS$sWyC4c?UGF96r*1O{ z4xX$^us-MRgeM_9+kOLWN(z}|R+VJsS&2*iH7@Q%NhQPe%X+%g-j$^*1&j}ze^G*o zU9nK3Jol*qh%s6=+`5)4L%VT@nU$869K?%RHS6sV#{CM+;kRR}^e0}u@q;zXH&!ym z2x|Sj$_i60CCG}@Y(fQ2{k*%Bq#K0>k z(*7wTs;LbS?S1;jy?p)pr4Tbfmuv2x>iZaL;j&N6($&MnA1p`%z==>MLsp1w{wWeE zRV#_vJG&3C{p>W}U7^lQ$mwwqtx-SSVB@r+6nn9Rc~8}>*39RHo=@I62VXw1meW~{ zuTIDo8{;xHzmcaPk(5xxBHt)K`CytE3{drcQDdN7c+w@z6 z(LLmn9>mz~`&+sFf(?o4l*F1U=+o~O^^9k~hX4A3pIhMT@M(yX4&jigCrPV9^MZcf zf@gv582+Ubh>%W3KaTy^Zw+Oy`+x#oNPDx|-z~}W?%Foy9=H8;nGT5unk5qm;L5v4 z@RyMA&M3maTvqT4@IB+vkCOmho1yr!fHWdU{t&vIc{@u~PvM25QT zTvv?7xH$K*W_NPkBt3R)f6!k4d3bYaJUvEI_!OyL_lpLf9^g1W*!&T;V%(~A2}^H|og?iqrRWn?JZis6D`ncGsmuUY>6kzH+3yW4yNF(- zCo3;SbmPMmcHXdBffQ8cvvkr3gKFL2v*m*31q!Q{l~ z0MOmA;h@_6(#rzVFC{Vb$nO!dLaMQi`SJ3;9Bg;ifbjd1-UURSLu|{KZ(*Qgnar-i z&eT!1d-?_C<)*WE2I({vq=k&&O8nqa|6?2*b|+FkLk1uve+<0_c&pT< zqu;%dYq5F}z_l>DoWU7;a8y}vk=tN}9Zul@93>85Ubt0i7Zm47Agvg8QSFQ})+ zDM!y{w!n&kr4ejS|u1{ThGtnNOf<5h#|Qt8(apA-d{WST^| zmOQlRz$L#G*F&*FDFK#gn|);xV{B*QPA8CJS#wM+Vic@cJ8p)xTRF=VPg-lv3WAgA zaq?HI!w>M9esTUFinbq17q` z9bPxo`5uSq{;8Qg>d}_~SBR0>;b=q07%#QnvQCns4MrY0?4)jcH&}6mUmE zNulZ2Z>zaUV$cbw@?DG70|p%RjZ!2Qx=0>|?0q*Wu~sBKg}!k{IRK2JFHsqmyhkU} z0MS<$(!_9j`l-~rz!-ufcB-ho&Q$V9`ej&R%;#qs!g#V<*A|jn}^1Q~n?tDV<6Xye5Ccjox z^=xjF-~dJbM*ir}3(*Xk@8f$8)+0aqi(@sPjKL*6eQ=T1Sqtl24D{-^w8pdj_H*CG zF2~}nDdT%X#q=;Y{DbZeO8MF4))TA4lxrAn$S}~aVd1P-hrru&j(Z%9&`Kqsq$znLB?}Pga!H*Q16LQ<6o{xXA4$-r(>t;YBIxm3AFS@Yidu93=x=CUW_^&i>Wk z)05Zk%eaV=Rz$URxnlGOt_iyCF`&Xm_N#^Q+q5FHpEa9{WQFc0cVY$G+6F;*+*RR9 zm>3p>EMY9L{&RLU32U9;0AP5_PL=+4v;!Ot(kf~ zZQy7Og9OCEAwjE2WzaXb6D1Pk0Svrb6G2*{R@DG$f|RX7>SGt;zHJ zDfz+#bLtCTr>4Eh7up|uW-L2 zo3}A}V5BW3%MiFbTvT!*d8xB#OH-ifQM->Re(&J<`GFe{^td4Gfj2= zVaS_sb5e3`zm+A2Zl`Hbc}#aeR##ht!)I~BS)^$`5hi(zQu=t1H+h%3yeF73<=erz z+O|7il+i4vn7S*&i8S2$Mk|A`wYq$?H!01WH)6xq)?#uT?f-nv(=KL~OG~zFm7O@S zDZ%??nwc3$BGZ1VxY7qYuMBN3w!!g%XQ5R># zf>NU0)P1&REbM@ES$82NmNe~0qyCR;hx$kw_eg&obmU+y2_?U!L&Jhy`LMi=k#iXMUni)VDLINqmS7OMqQR*J47KA#5(>Y5ghee{};a?^HR={N*OHfJf|TTpInyE zL{W9M_3-(8DSpvUBpE`mB?aj=SRy&eV|XbS9YxF^eHGC>N?tbYvo>FJ2!vP6e8lnZ zXNg`h-H_&M#i=!qv9m?a6{UQkUIa0EHW{UpZ!J2~i{QfWFoS?!?Wbm%DbKfq`2tQu zY#i*%Sy>l?PHyTlW<%^SJC=)y)2sxHsfeNnpHG1`V@-2gnKhqgn^Y72kzD{=aR#(S z>ZojOPb=E;b>5Lyjf_f0SLsYl_c}f%xwgs&HEs&rnAa`E&ns%70i^0g!xn*xKsmO& z{`1px+$*+F)DPkjmH>johb<8m!5DY-pc^|sX~M_8bd4F-1r&gUT*Rc#4^{z-?H)sp z7~44&hs6NHFVAwlP|&F62q{yLo(BL6IR#f1H0WVd<(w2YGBFwy$o+-@pox_tWBEI0 z$h(W+P>NBYfK0Duyt?D@kPBC>BC3q9a`=_%w_t?UDfSLHEj0D?_JlF9)>f-u=SKQE0mFoIBIA~V>?ec|CG^$ix=u^(tsv2(^ zkLw1jwKgFKa^wWaYX6DboR9c}leI`<75uaoBJD2TH3!$2X^k_rusk-!Bsip_b?MWI z&*?`lV{R?Z7m)$J+|<3!D<8~BWm+*SUjcHdiRFx17f;6)LY@_O6nWZz!cVzZDflRM z@ZDOw-tMSd0%y6#Xp$9Zg>0JqLSdGAFJ8Trp+{>Hxw~p5e+%k}eE5iORZA+vL10){ z22PC1nZJ%d{LYO67{fbdWc+n5#IkVREPg5hxWchNnPgCabx!c=8DmAFz1M((ACtNj ztdK2ru@Ptl2OZlxmgG9*d8P%Dw`TI`m!8nsrBL{`s1d)NF_OKvbW=<>>bn@V)CGo0 z%%6C7@mz^w$%eC=7LsgMwTNi3{6+P(wh?>HZ5~faQ0K#8D9aOM-{nw5)7AG+-)k({ zLnIru%`gG&6GZRE@K&K1jBa6SGEI4|C{VDua#KPyanjm7HrvBqhTyxyl*j4eto4yR z;*jMrbF{+5{14l<%kyS+=+<>IGuLU-PO2E{&#~vwifl&4I|~@=ggh9|0z~-z4%rF= z<(a}$7Zeh=L|Jroj)YH!(IQn}*CS{$^W#GMp|lGi?NnP<@qo=e1i7>}Wsat)&~1H* z#2E>Is5#7zQfFiO+0SB{h6d!?ByZ65jV^xL*ksx$Pd?3gWJ)lCA*h4IX{={DkE$Nq zLF&8VC7*I77!P_LM>4Mo<&EHl&DOMqX>!WFd>bMcBK*(KA*?TLPt{b zoExK3d)M5wDJGoo6K&QVCH7h_ICkmH3ptqYOp#uxJoO(eq3p!f z)XVJT4aFddHa^A<73@p@YVRzXfQZWDZ8d$Ll}y7dS`yrnnpzADza{Aaa3)YgWBe|$ z(f%X@mqjVt7p;(RI!~>C`ZB}r$y}Yh`3Qq<-_KT3Ol}nDCGXdO?IZiGjarhZiqem=SWC3}fSP=dr+kMh0mM}Yn-qf#wj1`K<*6Ebrq!v`--AxH@o zMzr_Vp5h3b_YVa%J4r2;?X*?H_hgv`IOdoZlHP%4tYGB5KUI)Q{=wXKu*oX4yHmIK z!w4%sej~1^vYOd2)T<%3HhkfMGT&~s`^WD9DRXpDjWm%SO?4KQP>DDoC!NYudGmC) zgrvEy0i2(3Z{WE=)yJe@&|jGxURasENX1cmT)?3Ql$=QiG~3*#WiZ*~?xPi{HC zWrmfhxz5n^84L?WfIRiPH+E6ILZdZp|!tWt<`94dP1o& z@Xm#04rs=6w$WKL@#nfCbZ^_xxj&LrtRa1cRe%{OZ z*8fM1mVf=twO<=0$^PD4) z`;XtN`2`C>WC-X1o=IC>uq?tYGvnGi?ScPFayfdnBk4oN`~rJ^mAsFlNCr&^0s70G zU|1MmKU9rJt#+E#^ag+xZ-=-s=N$JKUtUb&F$p)^MoGl%9V%8|`DA!RXfJmr&^OiE zlzSSLh`BJ{!QZWk9*+&lxSyY1UwMg)P>Emtd0QyNa{6QJ=O*LBoKf=;HKK zc?>>1_iRjghQ$DV@X=T#*-|{tUs9wdBDuEb)Nelf09>(_wmP4jjwqaO~sCVst!uq?+(UYs#EZ>(TwW=awhlNNV~#&VQa3osB}M1R;to ze=afi+}OPDAYt;?$(W0;7CU`>MP)}nZq>r0g5$Mh-W=6J+Mror{4~*bdB=3^TGmX3 zVVRxZPP7#ZZKui2)}lpa;$U^!6epYgHbdG{!y=+X3EOebhExcZyqms z`}PRGlr~hjf6^)1*EB11h_AZKKP!^R(h{yTh$(ug+NPII2E#Y`T+_ur! zEB_NZ$H&XDRM5@E80JD7H>}sm-#dH}&eOAEaD=qMn5mGf>;0PBW_8-RNi^5v%F}{v zoyx|Ho65@|mxrlYzt^Na4?D#V5g4F^H|(M?qQtq0NN(shq8wDoA_?@EC$Y6sO}2PV zw>|(oDR&RYU3jreE*G+W&9ytc?n;>U%&QsXE-+Or1gLuMPb9vzWqk;;K{*syWKUlp zK6ruoC8x;Dgl{dm9Y$~<<{tMcrby*G$nP*i4USJ%&riHt{75^R-VvePpiyJ|lt{-v zsLB}At3L>nAxbDgOj6q55sW;)AApC;0q3x)rTWwR$_S4&c^O10lU~B_s*9QBfElhJ z2zh5>&kdMlDzaHL-E{e*5d<8p8%5_n~yWaleD>ATQQ9gVo+cro5WWno^fVD#!tB zPe@+Hj0CXUv}X{*Ic6lisyXcivIxGWZ2oqok7D?(-p{rcIR2hMthyx@Ko1L?A(yiE z%q!46Zp+OjL7FhG8#?FhGO$#yT z5PW1iDhKw%Y{@EZ%b6d~47Dw4!VD|E{Qb_mp)<@NznkF__LM^{>;0R^<@RJF%C05$ z;A@OGahZjm*Ig$V1-S}D)Az*@SS5oouMu;^L$q0yPS$h8TP!@fuu}IJg8fXYbAlpM zN&&l#=Shx~3x$I0!@U6#wRIjpzPlaieT(bLHGxt2_jdH!b89o1ELGVpyrw;y_^AS& zz#B7DYC%v-)EsA6*>Z6`F3yfKyEeQI#Z9ePvL86{Ik%0WtvH_Ijfyxv-Q2?awhwbwE*p$|ms9u>CSzb8m0 zu)nbzIq>}|lR;)wjc8~YK`+j)bbmhjWLG3gG?ag(Af{5XPDO?Re^0D+FVsJ4u8inB ztS2`h8)(~#y{4CWDhJaRqB2ocRQb+Cv_v#cJL8(05kSxUx_h4qkN-&<5YZAU>Ld$L4$c-c2qfQghSQO}zt%C)-3g!Ys z*MG!x#|1pwC7dgBFf8YO=qme+AoBycZhNF<0vyNJ1e7oV{>n#XF5Qp9Kp!@wD>vRE%2hlh8sv++SS11 z&$aM8zb{9W;Lu?&i}O&!?px2HkT-U&I21W2HZE7jV$3s-y9R6fb)3_;ktc)aT@yNH zWfEpi;!I(_2}+E1L)ilj-BrrBVr%TBp&xN&4(zBiYw&sIQ?Ma{j%8<**DvOt{L2l< zS8_i&d^3&+4tK=3jj7UeGUe_rBeiW|SeU)(p{JT9c@n7}>@fe_wJ_G#hEH2S0EV}U zrHGuEoD_3sgI1}wZ2Ki6Y)o$HI!wcs+ICTh<|Rb3yu$pk2qX*+0K%)(hjvWVT7;_y z%Xp?k%y1LCs>Tdg+LaWFNn($G2$*&*d1+J?}Lm3C}4(_ZogW3 z+C5m#?C8~nf~5@Z7yVfFkKn%0e$}U!7!dh$m86N%==6$Q_E}eIdMarQXJ+O}SiWAs zq~ZwXSmhJyr+`#L`neEk&uTZe3P#AvTp>G{#<|Dr2H+c!~5={P0ZU&YQrQxE+-zVkO>pqH4d7*P!J>4g zue+MAe^g9e+nnPGcJb~B>&uaowwGS#M=s8d>)I_)3qi62r+;azENF&v)N&EfSS|2` ziPy=ZBQh~)oyqTR)(6dk_`|;>+H8Mwdq^S@-gKd6$~(@c>x&&s49BDd(oZRl{u2zq zhV~gudc7rkOO*VK@GaPXT2n5gptXXRUGla}xTAnT(;<-K3sn%v zC-hH{ZTp1~L+8JiQgyd_EoX^c?rdvEnZ6e+1sZMZ4s9GawX`2C#iP>xB~MnkMzYmJPtmVn~4eb4Li5ORV!FO+In>Jy>{7`iuvom-lXc#_Sah38JD(6^=s2reI4eMf40fufd7wF zp*Y=ss#u63?JAm9&H$U-z5N>e+ie$zzi0imBHk()azXv$CMQ~K)>J#F8niPrkA~YW zI;*++Qw8$+&jhop$v{zNyw;R!09gU-dmX^6Kj)-#RF(#q9?S?9hC8@Z0GfS4ts*^OwVQ`X1vX zeH*FFy_~Gt&QVWQgvHzcO^WlCGFpMvZ@e^-(1{DSAHw|rDD!q0I*kurYb zJ4MNH2-sO(osb`wNNng_)8;3783=BUZ%o^}$X?ExEqqT|ZN7SO=kdhTX)w-J6zz0t z@XUEuCII*bO`^(7<-(@t6aE-paO3K)Sj?7y_aBVI8CbJCMFLd)eN6I6<>naMAxU0^ zSuoS$!M@GfQ$9(qL4X&oC*`uhz=<<+FKgXq_B7>!72p`y_s&xP*Sl{kGaxvjWV3M7 zt7>>6T4`CoLD3K2cetuB>3&y_V;}r$^Knpt$Kr?RK7mj7+4IL|3S$OLa;6fy6Wa(` z97x0$A#>vtCv> zx{(lJn+tv68I0%^=(=i{r(2p0f0wS&Cos}zt;L zqACiIj;Tk5u56#t752v3{x?C{A2qxrK%yu23cH^?F8CNK*gu0Z!!`qAvnWaJAw2DW zP!Y7R#OEc}?|Tr35!AOoZmWZw{7U?Y&uWsj(7_ay>7TEc4J+(Yb6?szbL=tbIA<6_ zKd*RyO&{z2BC1$=8D#~7* zXEIAw|K1hWD|oWWUI?nHuZ2e^U%*X7Ykf1`oUWJ~bNPK6n6UBiD>O0IoY6{H7*V}2 zcpqQGy1XUUm|EIsLQAp?XXm9oLygc=O1tL#ov%Lo9P4XRZ)w;l*Wi2uX zHBMLt7#K67^*(CpZaJzQR*u>lko+>vscZNq@gmEjrT?Orvti`>ey$v4NpbAy|XCbHR-bzn3jn(&Ed?31{2e8W^VmullbMDvpz{6(qwT`95;k z-qc7BWkEpsmeR{c$AqTsrxW!Q=E_;(4a{ZuqcDHyZxejE4X$LJEn4=0AhSSYEv=!C0 zfJn4~^YSmfwBzj{rN0HY2;;9S+xaw14+a@&5aikraxYk=)Z$jznHP9E9D`m5OukvN zej+wa*2sd|hnwsr5n^3tH^L|IY{mqFCJ*Gf&OFJ^IP42)Fu4@6ridIr-9||wFx}3# zt7@Rx7Z4c(vJT%JGG16aa)K#&)uKvow4O77i4F#4< zi^hEp6C+xlLk(S%I9pbo+LBpg%;=+-GF!Q>9JHcYt*QFmB(a8XM8@6rYs`;mj(VR{pSV9FBgPTh`gPGaicw%~E%MDDM@_#R z91B1P9Os2nfq@+sV-g=n$SkZDpKTECJE|B~PS1YDq?~^*zpl%Zr(R%XES$TT<4aY% zb)+v3yG8()d!OmHG=;Fp$ec#nao3~?;&U9zGi_<>?<@j=PklJS@~5q}vGSoQw?|y= z2`9YFU7yx!KlmOFZx1K;S-v*UsT{0K;+2OnTAP)d5*vq!aCqwLX@1s2Bs_O6$mL~Y z@*dnFUGD)-ZpLd_PY#xfT}$psEj}Zrq{M{|WPrDR_BNP|30e0Di>**|1?-ec}tG(-H8HwXW#-0)xg-WS2Jr1+L^RR4~ zyWPh1vBzV!^$OK$+m^+--4_)A1T6j?v39f2O&r z(C^U8YPAFwY5h3^moQWi<$q{fjEF1g)hR$U5ylheQU|Xb$A+UEeVm9q=*PkkiT7X+h3OH>Wv8Z?6mr*zZdoO^4JjWgg zB^G^{nq_hWGJbygKJ_8I^Yfki6%W!Av%ta}i7iY(i(_s|&Ybp*Y@WHx&zY0q_+B4K zxocXsXA~;+ejo7l4N&e7g`H8b|E>0rE5__cx1f+lbdkx7%flk#?@mX8QEOmfb&s9S ziZp~Y$)-JChVV*+J4P-w;Bhp=_W5hP-&+PP`wPzxwUKW2r^EiVnb6c>A#Mw`uI zUX4@9NEf4Z%UVW+aw=zFV~l`1Of0B_v&N8%=r7Zit`wTY^$;EbKS<)$W$7OLi@}@* zdFTBX4nOsg4Z?h5et0O`;Mo_8)s^$LCrGBW7ADuU>?>Q}`(Be%B;Y{C`Fp*wu(Ir~ zyio?f##8Y|v52C3{+K>4i;1lSXGVb` zd?aMJBDcrjQ7@jydD`#y6m3D|{V)EA=v!CEIAQ`K44`7lWXddm1ZN?=$MbfOe!=K; z>yTNkk7`c)Ie+^6DQEn~(Nyzd1V2ljth}5KSAsgWa9(|YUP$2fo5ju9b3diVODlws z-pcDa7wTK{Ok0qx1x;GQv!NP+||;Q%7R~mw1=(wSn?dg=EF*2F@O8eO`PltEZu%7`PR5a=+Q7uO)2#| zApJXBIlR)sfabt%kR(p$^ebeW6?8VD-nv?o_q6frfGgdISZz(mqt1^Qt!sf!?~17B zxWB&x`?u~WU1P(-HZp_`dy`IoGZ&MH+rX4s=5v`{Kf7bSsUd6L%KM2A^ogijL{!mn zT?dC-wVh>n`&9o%OKJE_wEm@1+%11jgnTsn_fU}Gh`=BvaU|~Da#S&z zKp$^aQ*7}Eve|+)o%EZk0^d>waD7TwR%r!HhB+759C3KqSJ`0lOGth?OD>HM)%hjm za1q@;bFRK|!gXfrHzKX+P5bpleRa9|_e`jI;8QpFHH1N36tMIH5$)zo!;WR_0O#Ih z&tzVzpwz!~W>R_6xJrK71De9IclYmp_sl+Z#JtXkn%ySn*E)sH-o-&CgTAk%dAy_3 zLKv(lnP1gt9&dA(cFSP5U@x`mj6{c$-xX`CJ)o!eW0p&bR=x8h#=sF%&6GL4fA7yg z+Xk8{k81cO*_=+>v(=U?^K{w6qP+%_RUv+hn@2D0YW$D%#MKj4p7qvt-j}2}$vN)7 z1M)l4rS&-P<=fL-~sqeWcYL68igb^+6!`FFp$)VLIARXay&2 zeLIUvWPVCrP@FC46xoqqQ9WOl7s}=oH=fK}j%a<>C@3G14J0{ypBwnC5V1KWen56?EGC#OE>zhY9oz9|7_eUQK zC#%K7#F4Z7RS{vqdc|n!OdV&M1Bb!Qc%Ap_yY~@(>GQKmr+hB;wWR~21Ofeb2Y&Ce zeDeEIjQ%L_-bUMp2y0q^f_|PKAd|k7YsCO`QvE;Qm09HaQWgBQadvQ#m>QjEUEwRK z=D&Y;A-6{<#*LFfVcK>pHnZ!$=2x~D0}Cnum40%TnyHb#uZeYAIr*RMHiicnMMDD9 zc>0I@4@zWzje@@>gdrV~yTU4V^4~fOkzB$*TNEJWQB8=%`G2qf$8wO^dc=JN_!v!a Ue1d Date: Fri, 26 Jun 2015 07:29:53 +0900 Subject: [PATCH 24/65] Add readthedocs badge to README.md --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index 9f6ff5515b71c..41d425ac326d4 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,3 @@ [![Build Status](https://travis-ci.org/dartsim/dart.png?branch=master)](https://travis-ci.org/dartsim/dart) [![Build status](https://ci.appveyor.com/api/projects/status/6rta8olo95bpu84r?svg=true)](https://ci.appveyor.com/project/jslee02/dart) +[![Documentation Status](https://readthedocs.org/projects/dart/badge/?version=latest)](https://readthedocs.org/projects/dart/?badge=latest) From 966af760e8b2a65756bb13921edca82f88c59e42 Mon Sep 17 00:00:00 2001 From: Karen Liu Date: Thu, 25 Jun 2015 21:53:56 -0400 Subject: [PATCH 25/65] added tutorials/introduction.md; modified mkdocs.yml --- docs/readthedocs/tutorials/introduction.md | 7 +++++++ mkdocs.yml | 7 +++++++ 2 files changed, 14 insertions(+) create mode 100644 docs/readthedocs/tutorials/introduction.md diff --git a/docs/readthedocs/tutorials/introduction.md b/docs/readthedocs/tutorials/introduction.md new file mode 100644 index 0000000000000..2bd6b871b0828 --- /dev/null +++ b/docs/readthedocs/tutorials/introduction.md @@ -0,0 +1,7 @@ +The purpose of this tutorial is to provide a quick introduction to +using DART. We designed many hands-on exercises to make the learning +effective and fun. To follow along with this tutorial, first locate +the tutorial code in the directory: dart/tutorials. For each of the +four tutorials, we provide the skeleton code as the starting point +(e.g. tutorialMultiPendulum.cpp) and +the final code as the answer to the tutorial (e.g. tutorialMultiPendulum-Finished.cpp). The examples are based on the APIs of DART 5.0. diff --git a/mkdocs.yml b/mkdocs.yml index ccfd4468d0bb4..e41608ab478cc 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -11,3 +11,10 @@ theme: readthedocs docs_dir: docs/readthedocs site_dir: docs/readthedocs/site +pages: +- Home: 'index.md' +- Tutorials: + - 'Introduction': 'tutorials/introduction.md' + - 'Pendulum': 'tutorials/multi-pendulum.md' + - 'Manipulator': 'tutorials/dominoes.md' + - 'Biped': 'tutorials/biped.md' \ No newline at end of file From 5e33c29d3647f89537dddacb9cbfb127c742c104 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Sun, 28 Jun 2015 18:49:36 -0400 Subject: [PATCH 26/65] avoiding bullet if it is not available --- CMakeLists.txt | 1 + tutorials/tutorialBiped-Finished.cpp | 5 ++++- tutorials/tutorialBiped.cpp | 5 ++++- 3 files changed, 9 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b3998b4acb824..def8cd64d92e4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -296,6 +296,7 @@ endif() if(BULLET_FOUND) message(STATUS "Looking for BulletCollision - ${BULLET_VERSION} found") set(HAVE_BULLET_COLLISION TRUE) + add_definitions(-DHAVE_BULLET_COLLISION) else() message(STATUS "Looking for BulletCollision - NOT found, please install libbullet-dev") set(HAVE_BULLET_COLLISION FALSE) diff --git a/tutorials/tutorialBiped-Finished.cpp b/tutorials/tutorialBiped-Finished.cpp index 14141c60940eb..02d39ac22cb42 100644 --- a/tutorials/tutorialBiped-Finished.cpp +++ b/tutorials/tutorialBiped-Finished.cpp @@ -455,9 +455,12 @@ int main(int argc, char* argv[]) biped->setPositions(balancedPose); WorldPtr world = std::make_shared(); + world->setGravity(Eigen::Vector3d(0.0, -9.81, 0.0)); + +#ifdef HAVE_BULLET_COLLISION world->getConstraintSolver()->setCollisionDetector( new dart::collision::BulletCollisionDetector()); - world->setGravity(Eigen::Vector3d(0.0, -9.81, 0.0)); +#endif world->addSkeleton(floor); world->addSkeleton(biped); diff --git a/tutorials/tutorialBiped.cpp b/tutorials/tutorialBiped.cpp index 1574909b099a1..8b89bf5fe3499 100644 --- a/tutorials/tutorialBiped.cpp +++ b/tutorials/tutorialBiped.cpp @@ -309,9 +309,12 @@ int main(int argc, char* argv[]) biped->setPositions(balancedPose); WorldPtr world = std::make_shared(); + world->setGravity(Eigen::Vector3d(0.0, -9.81, 0.0)); + +#ifdef HAVE_BULLET_COLLISION world->getConstraintSolver()->setCollisionDetector( new dart::collision::BulletCollisionDetector()); - world->setGravity(Eigen::Vector3d(0.0, -9.81, 0.0)); +#endif world->addSkeleton(floor); world->addSkeleton(biped); From c09c97c339e42934d389138a64f93b4db411acab Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Sun, 28 Jun 2015 19:03:08 -0400 Subject: [PATCH 27/65] added manipulator loading as a lesson --- tutorials/tutorialDominoes.cpp | 26 ++++++-------------------- 1 file changed, 6 insertions(+), 20 deletions(-) diff --git a/tutorials/tutorialDominoes.cpp b/tutorials/tutorialDominoes.cpp index e98360e0cdd3c..03922b25620b6 100644 --- a/tutorials/tutorialDominoes.cpp +++ b/tutorials/tutorialDominoes.cpp @@ -64,11 +64,11 @@ class Controller { public: - Controller(const SkeletonPtr& manipulator, const SkeletonPtr& domino) + Controller(const SkeletonPtr& manipulator, const SkeletonPtr& /*domino*/) : mManipulator(manipulator) { // Grab the current joint angles to use them as desired angles - // Lesson 2b + // Lesson 2c // Grab the last body in the manipulator, and use it as an end effector // Lesson 3a @@ -99,9 +99,9 @@ class Controller /// Coriolis forces void setPDForces() { - // Lesson 2a + // Lesson 2b - // Lesson 2c + // Lesson 2d } /// Compute an operational space controller to push on the first domino @@ -335,22 +335,8 @@ SkeletonPtr createFloor() SkeletonPtr createManipulator() { - // Load the Skeleton from a file - dart::utils::DartLoader loader; - SkeletonPtr manipulator = - loader.parseSkeleton(DART_DATA_PATH"urdf/KR5/KR5 sixx R650.urdf"); - manipulator->setName("manipulator"); - - // Position its base in a reasonable way - Eigen::Isometry3d tf = Eigen::Isometry3d::Identity(); - tf.translation() = Eigen::Vector3d(-0.65, 0.0, 0.0); - manipulator->getJoint(0)->setTransformFromParentBodyNode(tf); - - // Get it into a useful configuration - manipulator->getDof(1)->setPosition( 140.0*M_PI/180.0); - manipulator->getDof(2)->setPosition(-140.0*M_PI/180.0); - - return manipulator; + // Lesson 2a + return Skeleton::create("manipulator"); } int main(int argc, char* argv[]) From 5dc233e906d7df48406e4a09688cb5af7189eaa0 Mon Sep 17 00:00:00 2001 From: Karen Liu Date: Mon, 29 Jun 2015 13:55:56 -0400 Subject: [PATCH 28/65] edited biped tutorial; small changes in tutorialBiped-Finished.cpp --- docs/readthedocs/tutorials/biped.md | 208 ++++++++++++++++++++++++++- tutorials/tutorialBiped-Finished.cpp | 36 +++-- 2 files changed, 225 insertions(+), 19 deletions(-) diff --git a/docs/readthedocs/tutorials/biped.md b/docs/readthedocs/tutorials/biped.md index 0063e634a11e1..c2a1e1ae027b8 100644 --- a/docs/readthedocs/tutorials/biped.md +++ b/docs/readthedocs/tutorials/biped.md @@ -1,13 +1,209 @@ # Overview +This tutorial demonstrates the dynamic features in DART to facilitate +the development of controllers for bipedal and wheel-based robots. -# Section 1 +The tutorial consists of seven Lessons covering the following +topics: -## Subsection 1 +- SKEL file format. +- Joint limits and self-collision. +- Actuators types and management. +- APIs for Jacobian matrices and other kinematic quantities. +- APIs for dynamic quantities. +- Skeleton editing. -## Subsection 2 +# Lesson 1: Joint limits and self-collision -# Section 2 +Let's start by opening the skeleton code +[tutorialBiped.cpp](http://). After creating a floor in +[main()](http://), [loadBiped()](http://) is called and a bipedal figure described +in SKEL format is loaded. Each SKEL file describes a +[World](http://dartsim.github.io/dart/d7/d41/classdart_1_1simulation_1_1World.html). Here +we load in a World from biped.skel and assign the bipedal figure to a +[Skeleton](http://dartsim.github.io/dart/d3/d19/classdart_1_1dynamics_1_1Skeleton.html) +pointer called *biped*. -## Subsection 1 -## Subsection 2 + WorldPtr world = SkelParser::readWorld(DART_DATA_PATH"skel/biped.skel"); + assert(world != nullptr); + + SkeletonPtr biped = world->getSkeleton("biped"); + +Running the skeleton code without any modification, you should see a +human-like character dropping on the ground and collapsing into +itself. Therefore, the first task in this Lesson is to enforce more realistic +joint limits. + +DART allows the user to set upper and lower bounds on each degree of +freedom in the SKEL file or using provided APIs. In biped.skel, you +should see the description of the right knee joint in this format: + + + ... + + 0.0 0.0 1.0 + + -3.14 + 0.0 + + + ... + + +The <upper> and <lower> tags make sure that the knee can only flex but +not extend in one direction. Alternatively, you can directly specify the joint limits +in the code using +[setPositionUpperLimit](http://dartsim.github.io/dart/d6/d5b/classdart_1_1dynamics_1_1Joint.html#aa0635643a0a8c1f22edb8243e86ea801) +and [setPositionLowerLimit](http://dartsim.github.io/dart/d6/d5b/classdart_1_1dynamics_1_1Joint.html#adadee231309b62cd3e3d904f75f2a969). + +In either case, the joint limits on the biped will not be activated until you call [setPositionLimited](http://dartsim.github.io/dart/d6/d5b/classdart_1_1dynamics_1_1Joint.html#a3212ca5f7893cfd9a5422ab17df4038b) + + for(size_t i = 0; i < biped->getNumJoints(); ++i) + biped->getJoint(i)->setPositionLimited(true); + +Once the joint limits are set, the next task is to enforce +self-collision. By default, DART does not check self-collision within +a skeleton. You can enable self-collision checking on the biped by + + biped->enableSelfCollision(); + +This function will enable self-collision on every non-adjacent pair of +body nodes. If you wish to also enable self-collision on adjacent body +nodes, set the optional parameter to true: + + biped->enableSelfCollision(true); + +Running the program again, you should see that the character is still +floppy like a ragdoll, but now the joints do not bend backward and the +body nodes do not penetrate each other anymore. + +# Lesson 2: Proportional-derivative (PD) control + +To actively control its own motion, the biped must exert internal +forces using actuators. In this Lesson, we will design one of the +simplest controllers to produce internal forces that make the biped +hold a target pose. The proportional-derivative (PD) control has +the form of Τ = -kp (θ - +θtarget) - kd θ̇, where θ +and θ̇ are the current position and velocity of a degree of +freedom, θtarget is the target position set by the +controller, and kp and kd are the stiffness and +damping coefficients. The detailed description of a PD controller can +be found [here](https://en.wikipedia.org/wiki/PID_controller). + +The first task is to set the biped to a desired pose in +[setInitialPose()](htt[:\\). To set +each degree of freedom individually, you can use [setPosition](http://dartsim.github.io/dart/d3/d19/classdart_1_1dynamics_1_1Skeleton.html#ac2036ea4998f688173d19ace0edab841): + + biped->setPosition(biped->getDof("j_thigh_left_z")->getIndexInSkeleton(), 0.15); + +Here the degree of freedom named "j_thigh_left_z" is set to 0.15 +radian. Note that each degree of freedom in a skeleton has a numerical +index which can be obtained by +[getIndexInSkeleton](http://dartsim.github.io/dart/de/db7/classdart_1_1dynamics_1_1DegreeOfFreedom.html#add2ec1d2f979e9056b466b1be5ee1a86). You +can also set the entire configuration using a vector that holds the +positions of all the degreed of freedoms using +[setPositions](http://dartsim.github.io/dart/d3/d19/classdart_1_1dynamics_1_1Skeleton.html#aee6d1a2be46c277602fae2f1d47762ef). + +Let's set more degrees of freedoms in the lower +body to create a roughly stable standing pose. + + biped->setPosition(biped->getDof("j_thigh_left_z")->getIndexInSkeleton(), 0.15); + biped->setPosition(biped->getDof("j_thigh_right_z")->getIndexInSkeleton(), 0.15); + biped->setPosition(biped->getDof("j_shin_left")->getIndexInSkeleton(), -0.4); + biped->setPosition(biped->getDof("j_shin_right")->getIndexInSkeleton(), -0.4); + biped->setPosition(biped->getDof("j_heel_left_1")->getIndexInSkeleton(), 0.25); + biped->setPosition(biped->getDof("j_heel_right_1")->getIndexInSkeleton(), 0.25); + +Now let's take a look at the constructor of our Controller in the +skeleton code: + + ... + for(size_t i = 0; i < 6; ++i) + { + mKp(i, i) = 0.0; + mKd(i, i) = 0.0; + } + + for(size_t i = 6; i < mBiped->getNumDofs(); ++i) + { + mKp(i, i) = 1000; + mKd(i, i) = 50; + } + ... + +Here we arbitrarily define the stiffness and damping coefficients to +1000 and 50, except for the first six degrees of freedom. Because the +global translation and rotation of the biped are not actuated, the +first six degrees of freedom at the root should not exert any internal +force. Therefore, we set the stiffness and damping coefficients to +zero. + +At the end of the constructor, we set the target position of the PD +controller to the current configuration of the biped: + + setTargetPositions(mBiped->getPositions()); + +[Controller::addForces()](http://) computes the forces generated by the PD +controller and adds these forces to the internal forces of biped using [setForces](http://dartsim.github.io/dart/d3/d19/classdart_1_1dynamics_1_1Skeleton.html#a9a6a9b792fa39639d3af613907d2d8ed): + + Eigen::VectorXd q = mBiped->getPositions(); + Eigen::VectorXd dq = mBiped->getVelocities(); + + Eigen::VectorXd p = -mKp * (q - mTargetPositions); + Eigen::VectorXd d = -mKd * dq; + + mForces += p + d; + mBiped->setForces(mForces); + +Note that the PD control force is *added* to the current internal force +stored in mForces instead of overriding it. + +Now try to run the program and see what happens. The skeleton +disappears almost immediately as soon as you hit the space bar! This +is because our stiffness and damping coefficients are set way too +high. As soon as the biped deviates from the target position, huge +internal forces are generated to cause the numerical simulation to +blow up. + +So let's lower those coefficients a bit. It turns out that each of +the degrees of freedom needs to be individually tuned depending on the +many factors such as the inertial properties of the body nodes, the +type of joints, and the current configuration of the system. Figuring +out an appropriate set of coefficients can be a tedious process +difficult to generalize across new tasks or different skeletons. In +the next Lesson, we will introduce a much more efficient way to +stabilize the PD controllers without endless tuning and +trial-and-errors. + +# Lesson 3: Stable proportional-derivative (SPD) control + +SPD is a variation of PD control proposed by +[Jie Tan](http://www.cc.gatech.edu/~jtan34/project/spd.html). The +basic idea of SPD is to compute control force using the predicted +state at the next time step, instead of the current state. This Lesson +will only demonstrates the implementation of SPD using DART. + +The implementation of SPD involves accessing dynamic quantities in Lagrange's equations of motion. Fortunately, these quantities are readily available via DART APIs. [Controller::addSPDForces()](\http://) shows the full implementation of SPD: + + Eigen::VectorXd q = mBiped->getPositions(); + Eigen::VectorXd dq = mBiped->getVelocities(); + + Eigen::MatrixXd invM = (mBiped->getMassMatrix() + mKd * mBiped->getTimeStep()).inverse(); + Eigen::VectorXd p = -mKp * (q + dq * mBiped->getTimeStep() - mTargetPositions); + Eigen::VectorXd d = -mKd * dq; + Eigen::VectorXd qddot = invM * (-mBiped->getCoriolisAndGravityForces() + p + d + mBiped->getConstraintForces()); + + mForces += p + d - mKd * qddot * mBiped->getTimeStep(); + mBiped->setForces(mForces); + +You can get mass matrix, Coriolis force, gravitational force, and +constraint force projected onto generalized coordinates using function +calls [getMassMatrix](http://dartsim.github.io/dart/d3/d19/classdart_1_1dynamics_1_1Skeleton.html#a1998cb27dd892d259da109509f313830), +[getCoriolisForces](http://dartsim.github.io/dart/d3/d19/classdart_1_1dynamics_1_1Skeleton.html#aeffe03aff506e206f79c5074b3886f08), +[getGravityForces](http://dartsim.github.io/dart/d3/d19/classdart_1_1dynamics_1_1Skeleton.html#a0d278dc0365a99729fdbbee7acf0bcd3), +and +[getConstraintForces](http://dartsim.github.io/dart/d3/d19/classdart_1_1dynamics_1_1Skeleton.html#a4b46912a4f3966efb2e54f1f5a29a77b), +respectively. Constraint forces include forces due to contacts, joint +limits, and other joint constraints set by the user (e.g. the weld +joint constraint in the multi-pendulum tutorial). diff --git a/tutorials/tutorialBiped-Finished.cpp b/tutorials/tutorialBiped-Finished.cpp index 14141c60940eb..b50037a42bb9f 100644 --- a/tutorials/tutorialBiped-Finished.cpp +++ b/tutorials/tutorialBiped-Finished.cpp @@ -96,7 +96,6 @@ class Controller /// Add commanding forces from PD controllers (Lesson 2 Answer) void addPDForces() { - mForces.setZero(); Eigen::VectorXd q = mBiped->getPositions(); Eigen::VectorXd dq = mBiped->getVelocities(); @@ -302,14 +301,19 @@ SkeletonPtr loadBiped() SkeletonPtr biped = world->getSkeleton("biped"); - // Set joint limits on knees + // Set joint limits for(size_t i = 0; i < biped->getNumJoints(); ++i) biped->getJoint(i)->setPositionLimited(true); // Enable self collision check but ignore adjacent bodies biped->enableSelfCollision(); - - // Set initial configuration + + return biped; +} + +// Set initial configuration (Lesson 2 Answer) +void setInitialPose(SkeletonPtr biped) +{ biped->setPosition(biped->getDof("j_thigh_left_z")-> getIndexInSkeleton(), 0.15); biped->setPosition(biped->getDof("j_thigh_right_z")-> @@ -317,16 +321,11 @@ SkeletonPtr loadBiped() biped->setPosition(biped->getDof("j_shin_left")-> getIndexInSkeleton(), -0.4); biped->setPosition(biped->getDof("j_shin_right")-> - getIndexInSkeleton(), -1.4); + getIndexInSkeleton(), -0.4); biped->setPosition(biped->getDof("j_heel_left_1")-> getIndexInSkeleton(), 0.25); biped->setPosition(biped->getDof("j_heel_right_1")-> getIndexInSkeleton(), 0.25); - biped->setPosition(biped->getDof("j_bicep_left_x")-> - getIndexInSkeleton(), 0.8); - biped->setPosition(biped->getDof("j_bicep_right_x")-> - getIndexInSkeleton(), -0.8); - return biped; } // Load a skateboard model and connect it to the biped model via an Euler joint @@ -362,6 +361,14 @@ void setVelocityAccuators(SkeletonPtr biped) // Solve for a balanced pose using IK (Lesson 7 Answer) Eigen::VectorXd solveIK(SkeletonPtr biped) { + // Modify the intial pose to stand on one foot + biped->setPosition(biped->getDof("j_shin_right")-> + getIndexInSkeleton(), -1.4); + biped->setPosition(biped->getDof("j_bicep_left_x")-> + getIndexInSkeleton(), 0.8); + biped->setPosition(biped->getDof("j_bicep_right_x")-> + getIndexInSkeleton(), -0.8); + Eigen::VectorXd newPose = biped->getPositions(); BodyNodePtr leftHeel = biped->getBodyNode("h_heel_left"); BodyNodePtr leftToe = biped->getBodyNode("h_toe_left"); @@ -373,17 +380,17 @@ Eigen::VectorXd solveIK(SkeletonPtr biped) LinearJacobian jacobian = biped->getCOMLinearJacobian() - biped->getLinearJacobian(leftHeel, leftHeel->getCOM(leftHeel)); - // sagittal deviation + // Sagittal deviation double error = deviation[0]; Eigen::VectorXd gradient = jacobian.row(0); Eigen::VectorXd newDirection = -0.2 * error * gradient; - // lateral deviation + // Lateral deviation error = deviation[2]; gradient = jacobian.row(2); newDirection += -0.2 * error * gradient; - // position constraint on four (approximated) corners of the left foot + // Position constraint on four (approximated) corners of the left foot Eigen::Vector3d offset(0.0, -0.04, -0.03); error = (leftHeel->getTransform() * offset)[1] - initialHeight; gradient = biped->getLinearJacobian(leftHeel, offset).row(1); @@ -443,6 +450,9 @@ int main(int argc, char* argv[]) // Lesson 1 SkeletonPtr biped = loadBiped(); + + // Lesson 2 + setInitialPose(biped); // Lesson 5 modifyBipedWithSkateboard(biped); From 94ec483cf35ae3ac3fcd7da3fe406e1e0facaa13 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Mon, 29 Jun 2015 22:29:34 -0400 Subject: [PATCH 29/65] spacing fix --- tutorials/tutorialMultiPendulum-Finished.cpp | 4 ++-- tutorials/tutorialMultiPendulum.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/tutorials/tutorialMultiPendulum-Finished.cpp b/tutorials/tutorialMultiPendulum-Finished.cpp index eac2f8a3667bf..e81e90e63a250 100644 --- a/tutorials/tutorialMultiPendulum-Finished.cpp +++ b/tutorials/tutorialMultiPendulum-Finished.cpp @@ -393,8 +393,8 @@ BodyNode* makeRootBody(const SkeletonPtr& pendulum, const std::string& name) return bn; } -BodyNode* addBody(const SkeletonPtr& pendulum, - BodyNode* parent, const std::string& name) +BodyNode* addBody(const SkeletonPtr& pendulum, BodyNode* parent, + const std::string& name) { // Set up the properties for the Joint RevoluteJoint::Properties properties; diff --git a/tutorials/tutorialMultiPendulum.cpp b/tutorials/tutorialMultiPendulum.cpp index f39c3e7164f4e..156568942eb80 100644 --- a/tutorials/tutorialMultiPendulum.cpp +++ b/tutorials/tutorialMultiPendulum.cpp @@ -305,8 +305,8 @@ BodyNode* makeRootBody(const SkeletonPtr& pendulum, const std::string& name) return bn; } -BodyNode* addBody(const SkeletonPtr& pendulum, - BodyNode* parent, const std::string& name) +BodyNode* addBody(const SkeletonPtr& pendulum, BodyNode* parent, + const std::string& name) { // Set up the properties for the Joint RevoluteJoint::Properties properties; From c6b1e69298476542673fb3777c98d1b6b45c1907 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Mon, 29 Jun 2015 22:29:53 -0400 Subject: [PATCH 30/65] adding collisions tutorial --- tutorials/tutorialCollisions.cpp | 423 +++++++++++++++++++++++++++++++ 1 file changed, 423 insertions(+) create mode 100644 tutorials/tutorialCollisions.cpp diff --git a/tutorials/tutorialCollisions.cpp b/tutorials/tutorialCollisions.cpp new file mode 100644 index 0000000000000..ea3916859d68c --- /dev/null +++ b/tutorials/tutorialCollisions.cpp @@ -0,0 +1,423 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/dart.h" + +const double default_shape_density = 1000; // kg/m^3 +const double default_shape_height = 0.1; // m +const double default_shape_width = 0.03; // m + +const double default_start_height = 0.4; // m + +const double minimum_start_v = 1.0; // m/s +const double maximum_start_v = 4.0; // m/s +const double default_start_v = 2; // m/s + +const double minimum_launch_angle = 20.0*M_PI/180.0; // rad +const double maximum_launch_angle = 60.0*M_PI/180.0; // rad +const double default_launch_angle = 45.0*M_PI/180.0; // rad + +const double maximum_start_w = 10*M_PI; // rad/s +const double default_start_w = 3*M_PI; // rad/s + +const double default_spring_stiffness = 100; +const double default_damping_coefficient = 10; + +const double default_ground_width = 2; +const double default_wall_thickness = 0.01; +const double default_wall_height = 1; +const double default_spawn_range = 0.9*default_ground_width/2; + +using namespace dart::dynamics; +using namespace dart::simulation; + +class MyWindow : public dart::gui::SimWindow +{ +public: + + MyWindow(const WorldPtr& world, const SkeletonPtr& rigidChain, + const SkeletonPtr& softChain, const SkeletonPtr& hybridChain) + : mRandomize(true), + mRD(), + mMT(mRD()), + mDistribution(-1.0, std::nextafter(1.0, 2.0)), + mOriginalRigidChain(rigidChain), + mOriginalSoftChain(softChain), + mOriginalHybridChain(hybridChain) + { + setWorld(world); + } + + void keyboard(unsigned char key, int x, int y) override + { + switch(key) + { + case '1': + addRing(mOriginalRigidChain->clone()); + break; + + case '2': + addRing(mOriginalSoftChain->clone()); + break; + + case 'r': + mRandomize = !mRandomize; + std::cout << "Randomization: " << (mRandomize? "on" : "off") + << std::endl; + break; + + default: + SimWindow::keyboard(key, x, y); + } + } + +protected: + + void addRing(const SkeletonPtr& chain) + { + // Set the starting position for the chain + Eigen::Vector6d positions(Eigen::Vector6d::Zero()); + + if(mRandomize) + positions[4] = default_spawn_range*mDistribution(mMT); + + positions[5] = default_start_height; + chain->getJoint(0)->setPositions(positions); + + // Wrap up the revolute joints of the chain + for(size_t i=6; i < chain->getNumDofs(); ++i) + { + DegreeOfFreedom* dof = chain->getDof(i); + dof->setPosition(dof->getRestPosition()); + } + + // Add the chain to the world + chain->setName(chain->getName()+std::to_string(mWorld->getNumSkeletons())); + mWorld->addSkeleton(chain); + + // Compute collisions + dart::collision::CollisionDetector* detector = + mWorld->getConstraintSolver()->getCollisionDetector(); + detector->detectCollision(true, true); + + // Look through the collisions to see if the new ring would start in + // collision with something + bool collision = false; + size_t collisionCount = detector->getNumContacts(); + for(size_t i = 0; i < collisionCount; ++i) + { + const dart::collision::Contact& contact = detector->getContact(i); + if(contact.bodyNode1.lock()->getSkeleton() == chain + || contact.bodyNode2.lock()->getSkeleton() == chain) + { + collision = true; + break; + } + } + + // Refuse to add the chain if it is in collision + if(collision) + { + mWorld->removeSkeleton(chain); + std::cout << "The new chain spawned in a collision. " + << "It will not be added to the world." << std::endl; + return; + } + + // Create reference frames for setting the initial velocity + Eigen::Isometry3d centerTf(Eigen::Isometry3d::Identity()); + centerTf.translation() = chain->getCOM(); + SimpleFrame center(Frame::World(), "center", centerTf); + + SimpleFrame ref(¢er, "root"); + ref.setRelativeTransform(chain->getBodyNode(0)->getTransform(¢er)); + + // Set the velocities of the reference frames so that we can easily give the + // Skeleton the linear and angular velocities that we want + double angle = default_launch_angle; + double speed = default_start_v; + double angular_speed = default_start_w; + if(mRandomize) + { + angle = (mDistribution(mMT) + 1.0)/2.0 * + (maximum_launch_angle - minimum_launch_angle) + minimum_launch_angle; + + speed = (mDistribution(mMT) + 1.0)/2.0 * + (maximum_start_v - minimum_start_v) + minimum_start_v; + + angular_speed = mDistribution(mMT) * maximum_start_w; + } + +// Eigen::Vector3d v = speed * Eigen::Vector3d(cos(angle), 0.0, sin(angle)); +// Eigen::Vector3d w = angular_speed * Eigen::Vector3d::UnitY(); +// center.setClassicDerivatives(v, w); + + // Use the reference frames to set the velocity of the Skeleton's root + chain->getJoint(0)->setVelocities(ref.getSpatialVelocity()); + + // Create a closed loop to turn the chain into a ring + BodyNode* head = chain->getBodyNode(0); + BodyNode* tail = chain->getBodyNode(chain->getNumBodyNodes()-1); + + Eigen::Vector3d offset = Eigen::Vector3d(0, 0, -default_shape_height / 2.0); + auto constraint = new dart::constraint::BallJointConstraint( + head, tail, head->getWorldTransform() * offset); + + mWorld->getConstraintSolver()->addConstraint(constraint); + } + + // std library objects that allow us to generate high-quality random numbers + bool mRandomize; + std::random_device mRD; + std::mt19937 mMT; + std::uniform_real_distribution mDistribution; + + SkeletonPtr mOriginalRigidChain; + + SkeletonPtr mOriginalSoftChain; + + SkeletonPtr mOriginalHybridChain; + +}; + +template +BodyNode* addRigidShape(const SkeletonPtr& chain, const std::string& name, + Shape::ShapeType type, BodyNode* parent = nullptr) +{ + Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); + Eigen::Vector3d center = Eigen::Vector3d(0, 0, default_shape_height / 2.0); + tf.translation() = center; + + typename JointType::Properties properties; + properties.mName = name+"_joint"; + if(parent) + { + properties.mT_ParentBodyToJoint = tf; + properties.mT_ChildBodyToJoint = tf.inverse(); + } + + BodyNode* bn = chain->createJointAndBodyNodePair( + parent, properties, BodyNode::Properties(name)).second; + + std::shared_ptr shape; + if(Shape::BOX == type) + { + shape = std::make_shared(Eigen::Vector3d( + default_shape_width, + default_shape_width, + default_shape_height)); + } + else if(Shape::CYLINDER == type) + { + shape = std::make_shared(default_shape_width/2, + default_shape_height); + } + + bn->addVisualizationShape(shape); + bn->addCollisionShape(shape); + + Inertia box_inertia; + double mass = default_shape_density * shape->getVolume(); + box_inertia.setMass(mass); + box_inertia.setMoment(shape->computeInertia(mass)); + bn->setInertia(box_inertia); + + return bn; +} + +enum SoftShapeType { + SOFT_BOX = 0, + SOFT_CYLINDER +}; + +template +BodyNode* addSoftShape(const SkeletonPtr& chain, const std::string& name, + SoftShapeType type, BodyNode* parent = nullptr) +{ + Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); + Eigen::Vector3d center = Eigen::Vector3d(0, 0, default_shape_height / 2.0); + tf.translation() = center; + + typename JointType::Properties joint_properties; + joint_properties.mName = name+"_joint"; + if(parent) + { + joint_properties.mT_ParentBodyToJoint = tf; + joint_properties.mT_ChildBodyToJoint = tf.inverse(); + } + + SoftBodyNode::UniqueProperties soft_properties; + if(SOFT_BOX == type) + { + soft_properties = SoftBodyNodeHelper::makeBoxProperties(Eigen::Vector3d( + default_shape_width, default_shape_width, default_shape_height), + Eigen::Isometry3d::Identity(), Eigen::Vector3i(4,4,4), + default_shape_density*default_shape_height*pow(default_shape_width,2)); + } + else if(SOFT_CYLINDER == type) + { + soft_properties = SoftBodyNodeHelper::makeCylinderProperties( + default_shape_width/2.0, default_shape_height, 8, 6, 3, + default_shape_density * default_shape_height + * pow(M_PI*default_shape_width/2.0, 2)); + } + + SoftBodyNode* bn = chain->createJointAndBodyNodePair( + parent, joint_properties, SoftBodyNode::Properties( + BodyNode::Properties(), soft_properties)).second; + + bn->getVisualizationShape(0)->setColor(dart::Color::Blue()); + + return bn; +} + +void setupChain(const SkeletonPtr& chain) +{ + for(size_t i=6; i < chain->getNumDofs(); ++i) + { + DegreeOfFreedom* dof = chain->getDof(i); + dof->setSpringStiffness(default_spring_stiffness); + dof->setDampingCoefficient(default_damping_coefficient); + } + + size_t numEdges = chain->getNumBodyNodes(); + double angle = 2*M_PI/numEdges; + + for(size_t i=1; i < chain->getNumJoints(); ++i) + { + Joint* joint = chain->getJoint(i); + Eigen::Vector3d restPos = BallJoint::convertToPositions(Eigen::Matrix3d( + Eigen::AngleAxisd(angle, Eigen::Vector3d(0, 1, 0)))); + + for(size_t j=0; j<3; ++j) + joint->setRestPosition(j, restPos[j]); + } +} + +SkeletonPtr createRigidChain() +{ + SkeletonPtr chain = Skeleton::create("rigid_chain"); + + BodyNode* bn = addRigidShape(chain, "rigid box 1", Shape::BOX); + bn = addRigidShape(chain, "rigid cyl 2", Shape::CYLINDER, bn); + bn = addRigidShape(chain, "rigid box 3", Shape::BOX, bn); + bn = addRigidShape(chain, "rigid cyl 4", Shape::CYLINDER, bn); + bn = addRigidShape(chain, "rigid box 5", Shape::BOX, bn); + bn = addRigidShape(chain, "rigid cyl 6", Shape::CYLINDER, bn); + + setupChain(chain); + + return chain; +} + +SkeletonPtr createSoftChain() +{ + SkeletonPtr chain = Skeleton::create("soft_chain"); + + BodyNode* bn = addSoftShape(chain, "soft box 1", SOFT_BOX); + bn = addSoftShape(chain, "soft cyl 2", SOFT_CYLINDER, bn); + bn = addSoftShape(chain, "soft box 3", SOFT_BOX, bn); + bn = addSoftShape(chain, "soft cyl 4", SOFT_CYLINDER, bn); + bn = addSoftShape(chain, "soft box 5", SOFT_BOX, bn); + bn = addSoftShape(chain, "soft cyl 6", SOFT_CYLINDER, bn); + + setupChain(chain); + + return chain; +} + +SkeletonPtr createHybridChain() +{ + return Skeleton::create("hybrid_chain"); +} + +SkeletonPtr createGround() +{ + SkeletonPtr ground = Skeleton::create("ground"); + + BodyNode* bn = ground->createJointAndBodyNodePair().second; + + std::shared_ptr shape = std::make_shared( + Eigen::Vector3d(default_ground_width, default_ground_width, + default_wall_thickness)); + shape->setColor(Eigen::Vector3d(0,0,0)); + + bn->addCollisionShape(shape); + bn->addVisualizationShape(shape); + + return ground; +} + +SkeletonPtr createWall() +{ + SkeletonPtr wall = Skeleton::create("wall"); + + BodyNode* bn = wall->createJointAndBodyNodePair().second; + + std::shared_ptr shape = std::make_shared( + Eigen::Vector3d(default_wall_thickness, default_ground_width, + default_wall_height)); + shape->setColor(Eigen::Vector3d(0,0,0)); + + bn->addCollisionShape(shape); + bn->addVisualizationShape(shape); + + Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); + tf.translation() = Eigen::Vector3d(default_ground_width/2.0, 0, + default_wall_height/2.0); + bn->getParentJoint()->setTransformFromParentBodyNode(tf); + + return wall; +} + +int main(int argc, char* argv[]) +{ + SkeletonPtr chain = createRigidChain(); + Eigen::Vector6d positions(Eigen::Vector6d::Zero()); + positions.tail<3>() = Eigen::Vector3d(0,0,3); + chain->getJoint(0)->setPositions(positions); + + WorldPtr world = std::make_shared(); + world->addSkeleton(createGround()); + world->addSkeleton(createWall()); + + MyWindow window(world, createRigidChain(), createSoftChain(), + createHybridChain()); + + glutInit(&argc, argv); + window.initWindow(640, 480, "Collisions"); + glutMainLoop(); +} From 50e717b640603869b9990ab62b2a5a57be6d2624 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Tue, 30 Jun 2015 00:45:50 -0400 Subject: [PATCH 31/65] filling in faces of SoftBodyNodes --- dart/dynamics/SoftBodyNode.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/dart/dynamics/SoftBodyNode.cpp b/dart/dynamics/SoftBodyNode.cpp index 4297d9a83d95e..0d151f2357ea4 100644 --- a/dart/dynamics/SoftBodyNode.cpp +++ b/dart/dynamics/SoftBodyNode.cpp @@ -1175,6 +1175,10 @@ void SoftBodyNode::draw(renderer::RenderInterface* _ri, // _ri->setPenColor(fleshColor); // if (_showMeshs) { + _ri->setPenColor(mSoftShape->getRGBA()); + glEnable(GL_LIGHTING); + glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); + Eigen::Vector3d pos; Eigen::Vector3d pos_normalized; for (size_t i = 0; i < mSoftP.mFaces.size(); ++i) From 14bec82cc9e9fea70690a39784f1fb9d4879bcce Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Tue, 30 Jun 2015 00:46:22 -0400 Subject: [PATCH 32/65] tossing rings and soft chains --- tutorials/tutorialCollisions.cpp | 96 +++++++++++++++++--------------- 1 file changed, 51 insertions(+), 45 deletions(-) diff --git a/tutorials/tutorialCollisions.cpp b/tutorials/tutorialCollisions.cpp index ea3916859d68c..66b6799b0eebc 100644 --- a/tutorials/tutorialCollisions.cpp +++ b/tutorials/tutorialCollisions.cpp @@ -57,13 +57,36 @@ const double default_spring_stiffness = 100; const double default_damping_coefficient = 10; const double default_ground_width = 2; -const double default_wall_thickness = 0.01; +const double default_wall_thickness = 0.1; const double default_wall_height = 1; const double default_spawn_range = 0.9*default_ground_width/2; using namespace dart::dynamics; using namespace dart::simulation; +void setupRing(const SkeletonPtr& chain) +{ + for(size_t i=6; i < chain->getNumDofs(); ++i) + { + DegreeOfFreedom* dof = chain->getDof(i); + dof->setSpringStiffness(default_spring_stiffness); + dof->setDampingCoefficient(default_damping_coefficient); + } + + size_t numEdges = chain->getNumBodyNodes(); + double angle = 2*M_PI/numEdges; + + for(size_t i=1; i < chain->getNumJoints(); ++i) + { + Joint* joint = chain->getJoint(i); + Eigen::Vector3d restPos = BallJoint::convertToPositions(Eigen::Matrix3d( + Eigen::AngleAxisd(angle, Eigen::Vector3d(0, 1, 0)))); + + for(size_t j=0; j<3; ++j) + joint->setRestPosition(j, restPos[j]); + } +} + class MyWindow : public dart::gui::SimWindow { public: @@ -90,7 +113,7 @@ class MyWindow : public dart::gui::SimWindow break; case '2': - addRing(mOriginalSoftChain->clone()); + addChain(mOriginalSoftChain->clone()); break; case 'r': @@ -106,7 +129,7 @@ class MyWindow : public dart::gui::SimWindow protected: - void addRing(const SkeletonPtr& chain) + void addChain(const SkeletonPtr& chain) { // Set the starting position for the chain Eigen::Vector6d positions(Eigen::Vector6d::Zero()); @@ -181,12 +204,19 @@ class MyWindow : public dart::gui::SimWindow angular_speed = mDistribution(mMT) * maximum_start_w; } -// Eigen::Vector3d v = speed * Eigen::Vector3d(cos(angle), 0.0, sin(angle)); -// Eigen::Vector3d w = angular_speed * Eigen::Vector3d::UnitY(); -// center.setClassicDerivatives(v, w); + Eigen::Vector3d v = speed * Eigen::Vector3d(cos(angle), 0.0, sin(angle)); + Eigen::Vector3d w = angular_speed * Eigen::Vector3d::UnitY(); + center.setClassicDerivatives(v, w); // Use the reference frames to set the velocity of the Skeleton's root chain->getJoint(0)->setVelocities(ref.getSpatialVelocity()); + } + + void addRing(const SkeletonPtr& chain) + { + setupRing(chain); + + addChain(chain); // Create a closed loop to turn the chain into a ring BodyNode* head = chain->getBodyNode(0); @@ -279,52 +309,34 @@ BodyNode* addSoftShape(const SkeletonPtr& chain, const std::string& name, joint_properties.mT_ChildBodyToJoint = tf.inverse(); } + double soft_shape_width = 2*default_shape_width; SoftBodyNode::UniqueProperties soft_properties; if(SOFT_BOX == type) { soft_properties = SoftBodyNodeHelper::makeBoxProperties(Eigen::Vector3d( - default_shape_width, default_shape_width, default_shape_height), + soft_shape_width, soft_shape_width, default_shape_height), Eigen::Isometry3d::Identity(), Eigen::Vector3i(4,4,4), - default_shape_density*default_shape_height*pow(default_shape_width,2)); + default_shape_density*default_shape_height*pow(soft_shape_width,2)); } else if(SOFT_CYLINDER == type) { soft_properties = SoftBodyNodeHelper::makeCylinderProperties( - default_shape_width/2.0, default_shape_height, 8, 6, 3, + soft_shape_width/2.0, default_shape_height, 8, 3, 2, default_shape_density * default_shape_height - * pow(M_PI*default_shape_width/2.0, 2)); + * pow(M_PI*soft_shape_width/2.0, 2)); } + soft_properties.mKv = 100; + soft_properties.mKe = 0; + soft_properties.mDampCoeff = 5; SoftBodyNode* bn = chain->createJointAndBodyNodePair( parent, joint_properties, SoftBodyNode::Properties( BodyNode::Properties(), soft_properties)).second; - bn->getVisualizationShape(0)->setColor(dart::Color::Blue()); - - return bn; -} - -void setupChain(const SkeletonPtr& chain) -{ - for(size_t i=6; i < chain->getNumDofs(); ++i) - { - DegreeOfFreedom* dof = chain->getDof(i); - dof->setSpringStiffness(default_spring_stiffness); - dof->setDampingCoefficient(default_damping_coefficient); - } - - size_t numEdges = chain->getNumBodyNodes(); - double angle = 2*M_PI/numEdges; + bn->getVisualizationShape(0)->setColor(dart::Color::Red()); - for(size_t i=1; i < chain->getNumJoints(); ++i) - { - Joint* joint = chain->getJoint(i); - Eigen::Vector3d restPos = BallJoint::convertToPositions(Eigen::Matrix3d( - Eigen::AngleAxisd(angle, Eigen::Vector3d(0, 1, 0)))); - for(size_t j=0; j<3; ++j) - joint->setRestPosition(j, restPos[j]); - } + return bn; } SkeletonPtr createRigidChain() @@ -338,8 +350,6 @@ SkeletonPtr createRigidChain() bn = addRigidShape(chain, "rigid box 5", Shape::BOX, bn); bn = addRigidShape(chain, "rigid cyl 6", Shape::CYLINDER, bn); - setupChain(chain); - return chain; } @@ -348,13 +358,8 @@ SkeletonPtr createSoftChain() SkeletonPtr chain = Skeleton::create("soft_chain"); BodyNode* bn = addSoftShape(chain, "soft box 1", SOFT_BOX); - bn = addSoftShape(chain, "soft cyl 2", SOFT_CYLINDER, bn); - bn = addSoftShape(chain, "soft box 3", SOFT_BOX, bn); - bn = addSoftShape(chain, "soft cyl 4", SOFT_CYLINDER, bn); - bn = addSoftShape(chain, "soft box 5", SOFT_BOX, bn); - bn = addSoftShape(chain, "soft cyl 6", SOFT_CYLINDER, bn); - - setupChain(chain); +// bn = addSoftShape(chain, "soft cyl 2", SOFT_CYLINDER, bn); +// bn = addSoftShape(chain, "soft box 3", SOFT_BOX, bn); return chain; } @@ -396,8 +401,9 @@ SkeletonPtr createWall() bn->addVisualizationShape(shape); Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); - tf.translation() = Eigen::Vector3d(default_ground_width/2.0, 0, - default_wall_height/2.0); + tf.translation() = Eigen::Vector3d( + (default_ground_width + default_wall_thickness)/2.0, 0.0, + (default_wall_height + default_wall_thickness)/2.0); bn->getParentJoint()->setTransformFromParentBodyNode(tf); return wall; From 67cb2fb09036635f380fc9a2bda7f22f9b41379b Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Tue, 30 Jun 2015 00:50:25 -0400 Subject: [PATCH 33/65] nicer colors for the softBodies demo --- apps/softBodies/Main.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/apps/softBodies/Main.cpp b/apps/softBodies/Main.cpp index 3674b05abdb97..19c152a48f893 100644 --- a/apps/softBodies/Main.cpp +++ b/apps/softBodies/Main.cpp @@ -61,10 +61,11 @@ int main(int argc, char* argv[]) for(size_t j=0; jgetNumBodyNodes(); ++j) { dart::dynamics::BodyNode* bn = skel->getBodyNode(j); + Eigen::Vector3d color = dart::Color::Random(); for(size_t k=0; kgetNumVisualizationShapes(); ++k) { dart::dynamics::ShapePtr vs = bn->getVisualizationShape(k); - vs->setColor(dart::Color::Random()); + vs->setColor(color); } } } From 3d32915187a7548f4e8ba8c3cae3ad8cac5a76ae Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Tue, 30 Jun 2015 02:58:13 -0400 Subject: [PATCH 34/65] added accessor function for BodyNodes --- dart/constraint/JointConstraint.cpp | 12 ++++++++++++ dart/constraint/JointConstraint.h | 6 ++++++ 2 files changed, 18 insertions(+) diff --git a/dart/constraint/JointConstraint.cpp b/dart/constraint/JointConstraint.cpp index 2b54d4bc7ce47..c0bbce7feeeef 100644 --- a/dart/constraint/JointConstraint.cpp +++ b/dart/constraint/JointConstraint.cpp @@ -173,5 +173,17 @@ double JointConstraint::getConstraintForceMixing() return mConstraintForceMixing; } +//============================================================================== +dynamics::BodyNode* JointConstraint::getBodyNode1() const +{ + return mBodyNode1; +} + +//============================================================================== +dynamics::BodyNode* JointConstraint::getBodyNode2() const +{ + return mBodyNode2; +} + } // namespace constraint } // namespace dart diff --git a/dart/constraint/JointConstraint.h b/dart/constraint/JointConstraint.h index 580749fa6a05f..d93fc1f12a771 100644 --- a/dart/constraint/JointConstraint.h +++ b/dart/constraint/JointConstraint.h @@ -88,6 +88,12 @@ class JointConstraint : public ConstraintBase /// Get global constraint force mixing parameter static double getConstraintForceMixing(); + /// Get the first BodyNode that this constraint is associated with + dynamics::BodyNode* getBodyNode1() const; + + /// Get the second BodyNode that this constraint is associated with + dynamics::BodyNode* getBodyNode2() const; + protected: /// First body node dynamics::BodyNode* mBodyNode1; From 0307a6e2e7a0c407216d4c5d93f267a628428e37 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Tue, 30 Jun 2015 02:58:29 -0400 Subject: [PATCH 35/65] experimenting with collisions --- tutorials/tutorialCollisions.cpp | 83 +++++++++++++++++++++++++++----- 1 file changed, 70 insertions(+), 13 deletions(-) diff --git a/tutorials/tutorialCollisions.cpp b/tutorials/tutorialCollisions.cpp index 66b6799b0eebc..301b019c503d2 100644 --- a/tutorials/tutorialCollisions.cpp +++ b/tutorials/tutorialCollisions.cpp @@ -50,7 +50,7 @@ const double minimum_launch_angle = 20.0*M_PI/180.0; // rad const double maximum_launch_angle = 60.0*M_PI/180.0; // rad const double default_launch_angle = 45.0*M_PI/180.0; // rad -const double maximum_start_w = 10*M_PI; // rad/s +const double maximum_start_w = 6*M_PI; // rad/s const double default_start_w = 3*M_PI; // rad/s const double default_spring_stiffness = 100; @@ -99,7 +99,8 @@ class MyWindow : public dart::gui::SimWindow mDistribution(-1.0, std::nextafter(1.0, 2.0)), mOriginalRigidChain(rigidChain), mOriginalSoftChain(softChain), - mOriginalHybridChain(hybridChain) + mOriginalHybridChain(hybridChain), + mSkelCount(0) { setWorld(world); } @@ -109,13 +110,22 @@ class MyWindow : public dart::gui::SimWindow switch(key) { case '1': - addRing(mOriginalRigidChain->clone()); + addChain(mOriginalRigidChain->clone()); break; case '2': + addRing(mOriginalRigidChain->clone()); + break; + + case '3': addChain(mOriginalSoftChain->clone()); break; + case 'd': + if(mWorld->getNumSkeletons() > 2) + removeSkeleton(mWorld->getSkeleton(2)); + break; + case 'r': mRandomize = !mRandomize; std::cout << "Randomization: " << (mRandomize? "on" : "off") @@ -127,9 +137,23 @@ class MyWindow : public dart::gui::SimWindow } } + void displayTimer(int _val) override + { + // We remove playback and baking, because we want to be able to add and + // remove objects during runtime + int numIter = mDisplayTimeout / (mWorld->getTimeStep() * 1000); + if (mSimulating) + { + for (int i = 0; i < numIter; i++) + timeStepping(); + } + glutPostRedisplay(); + glutTimerFunc(mDisplayTimeout, refreshTimer, _val); + } + protected: - void addChain(const SkeletonPtr& chain) + bool addChain(const SkeletonPtr& chain) { // Set the starting position for the chain Eigen::Vector6d positions(Eigen::Vector6d::Zero()); @@ -148,7 +172,7 @@ class MyWindow : public dart::gui::SimWindow } // Add the chain to the world - chain->setName(chain->getName()+std::to_string(mWorld->getNumSkeletons())); + chain->setName(chain->getName()+std::to_string(mSkelCount++)); mWorld->addSkeleton(chain); // Compute collisions @@ -175,9 +199,9 @@ class MyWindow : public dart::gui::SimWindow if(collision) { mWorld->removeSkeleton(chain); - std::cout << "The new chain spawned in a collision. " + std::cout << "The new object spawned in a collision. " << "It will not be added to the world." << std::endl; - return; + return false; } // Create reference frames for setting the initial velocity @@ -210,23 +234,45 @@ class MyWindow : public dart::gui::SimWindow // Use the reference frames to set the velocity of the Skeleton's root chain->getJoint(0)->setVelocities(ref.getSpatialVelocity()); + + return true; } - void addRing(const SkeletonPtr& chain) + void addRing(const SkeletonPtr& ring) { - setupRing(chain); + setupRing(ring); - addChain(chain); + if(!addChain(ring)) + return; // Create a closed loop to turn the chain into a ring - BodyNode* head = chain->getBodyNode(0); - BodyNode* tail = chain->getBodyNode(chain->getNumBodyNodes()-1); + BodyNode* head = ring->getBodyNode(0); + BodyNode* tail = ring->getBodyNode(ring->getNumBodyNodes()-1); Eigen::Vector3d offset = Eigen::Vector3d(0, 0, -default_shape_height / 2.0); auto constraint = new dart::constraint::BallJointConstraint( head, tail, head->getWorldTransform() * offset); mWorld->getConstraintSolver()->addConstraint(constraint); + mJointConstraints.push_back(constraint); + } + + void removeSkeleton(const SkeletonPtr& skel) + { + for(size_t i=0; igetBodyNode1()->getSkeleton() == skel + || constraint->getBodyNode2()->getSkeleton() == skel) + { + mWorld->getConstraintSolver()->removeConstraint(constraint); + mJointConstraints.erase(mJointConstraints.begin()+i); + delete constraint; + break; // There should only be one constraint per skeleton + } + } + + mWorld->removeSkeleton(skel); } // std library objects that allow us to generate high-quality random numbers @@ -235,12 +281,18 @@ class MyWindow : public dart::gui::SimWindow std::mt19937 mMT; std::uniform_real_distribution mDistribution; + std::vector mJointConstraints; + SkeletonPtr mOriginalRigidChain; SkeletonPtr mOriginalSoftChain; SkeletonPtr mOriginalHybridChain; + /// Keep track of how many Skeletons we spawn to ensure we can give them all + /// unique names + size_t mSkelCount; + }; template @@ -403,7 +455,7 @@ SkeletonPtr createWall() Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); tf.translation() = Eigen::Vector3d( (default_ground_width + default_wall_thickness)/2.0, 0.0, - (default_wall_height + default_wall_thickness)/2.0); + (default_wall_height - default_wall_thickness)/2.0); bn->getParentJoint()->setTransformFromParentBodyNode(tf); return wall; @@ -423,6 +475,11 @@ int main(int argc, char* argv[]) MyWindow window(world, createRigidChain(), createSoftChain(), createHybridChain()); + std::cout << "space bar: simulation on/off" << std::endl; + std::cout << "'1': spawn a rigid chain" << std::endl; + std::cout << "'2': spawn a rigid ring" << std::endl; + std::cout << "'3': spawn a soft body" << std::endl; + glutInit(&argc, argv); window.initWindow(640, 480, "Collisions"); glutMainLoop(); From da3a168785a9bd996c943ad211af5a7938754075 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Tue, 30 Jun 2015 16:58:21 -0400 Subject: [PATCH 36/65] passing ShapePtr by const ref --- dart/dynamics/BodyNode.cpp | 4 ++-- dart/dynamics/BodyNode.h | 4 ++-- dart/dynamics/Entity.cpp | 4 ++-- dart/dynamics/Entity.h | 4 ++-- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index cf2561b55b02b..49509c5717aba 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/dart/dynamics/BodyNode.cpp @@ -461,7 +461,7 @@ double BodyNode::getRestitutionCoeff() const } //============================================================================== -void BodyNode::addCollisionShape(ShapePtr _shape) +void BodyNode::addCollisionShape(const ShapePtr& _shape) { if(nullptr == _shape) { @@ -491,7 +491,7 @@ void BodyNode::addCollisionShape(ShapePtr _shape) } //============================================================================== -void BodyNode::removeCollisionShape(ShapePtr _shape) +void BodyNode::removeCollisionShape(const ShapePtr& _shape) { if (nullptr == _shape) return; diff --git a/dart/dynamics/BodyNode.h b/dart/dynamics/BodyNode.h index 8ff7c5d847db3..b46b5f9b21b82 100644 --- a/dart/dynamics/BodyNode.h +++ b/dart/dynamics/BodyNode.h @@ -270,10 +270,10 @@ class BodyNode : public Frame, public SkeletonRefCountingBase //-------------------------------------------------------------------------- /// Add a collision Shape into the BodyNode - void addCollisionShape(ShapePtr _shape); + void addCollisionShape(const ShapePtr& _shape); /// Remove a collision Shape from this BodyNode - void removeCollisionShape(ShapePtr _shape); + void removeCollisionShape(const ShapePtr& _shape); /// Remove all collision Shapes from this BodyNode void removeAllCollisionShapes(); diff --git a/dart/dynamics/Entity.cpp b/dart/dynamics/Entity.cpp index 905078fe901c6..b381d5a2f2801 100644 --- a/dart/dynamics/Entity.cpp +++ b/dart/dynamics/Entity.cpp @@ -160,7 +160,7 @@ const std::string& Entity::getName() const } //============================================================================== -void Entity::addVisualizationShape(ShapePtr _shape) +void Entity::addVisualizationShape(const ShapePtr& _shape) { if (nullptr == _shape) return; @@ -179,7 +179,7 @@ void Entity::addVisualizationShape(ShapePtr _shape) } //============================================================================== -void Entity::removeVisualizationShape(ShapePtr _shape) +void Entity::removeVisualizationShape(const ShapePtr& _shape) { if (nullptr == _shape) return; diff --git a/dart/dynamics/Entity.h b/dart/dynamics/Entity.h index 063d154e68492..1b56b7a89d1bd 100644 --- a/dart/dynamics/Entity.h +++ b/dart/dynamics/Entity.h @@ -127,10 +127,10 @@ class Entity : public virtual common::Subject virtual const std::string& getName() const; /// Add a visualization Shape for this Entity - virtual void addVisualizationShape(ShapePtr _shape); + virtual void addVisualizationShape(const ShapePtr& _shape); /// Remove a visualization Shape from this Entity - virtual void removeVisualizationShape(ShapePtr _shape); + virtual void removeVisualizationShape(const ShapePtr& _shape); /// Remove all visualization Shapes from this Entity virtual void removeAllVisualizationShapes(); From 19708b357deb5e102566801c247deaedf83bf737 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Tue, 30 Jun 2015 16:58:54 -0400 Subject: [PATCH 37/65] copying SoftMeshShape properties over when cloning --- dart/dynamics/SoftBodyNode.cpp | 31 +++++++++++++++++++++++-------- dart/dynamics/SoftBodyNode.h | 4 ++-- 2 files changed, 25 insertions(+), 10 deletions(-) diff --git a/dart/dynamics/SoftBodyNode.cpp b/dart/dynamics/SoftBodyNode.cpp index 0d151f2357ea4..471def1ef3295 100644 --- a/dart/dynamics/SoftBodyNode.cpp +++ b/dart/dynamics/SoftBodyNode.cpp @@ -131,23 +131,32 @@ SoftBodyNode::~SoftBodyNode() } //============================================================================== -void SoftBodyNode::removeSoftBodyShapes() +ShapePtr SoftBodyNode::removeSoftBodyShapes() { - for(size_t i=0; i(mEntityP.mVizShapes[i].get())) - removeVisualizationShape(mEntityP.mVizShapes[i]); + if(dynamic_cast(mBodyP.mColShapes[i].get())) + { + oldShape = mBodyP.mColShapes[i]; + removeCollisionShape(oldShape); + } else ++i; } - for(size_t i=0; i(mBodyP.mColShapes[i].get())) - removeCollisionShape(mBodyP.mColShapes[i]); + if(dynamic_cast(mEntityP.mVizShapes[i].get())) + { + oldShape = mEntityP.mVizShapes[i]; + removeVisualizationShape(oldShape); + } else ++i; } + + return oldShape; } //============================================================================== @@ -163,7 +172,7 @@ void SoftBodyNode::setProperties(const Properties& _properties) void SoftBodyNode::setProperties(const UniqueProperties& _properties) { // SoftMeshShape pointers should not be copied between bodies - removeSoftBodyShapes(); + ShapePtr oldShape = removeSoftBodyShapes(); size_t newCount = _properties.mPointProps.size(); size_t oldCount = mPointMasses.size(); @@ -207,6 +216,12 @@ void SoftBodyNode::setProperties(const UniqueProperties& _properties) mSoftShape = std::shared_ptr(new SoftMeshShape(this)); addVisualizationShape(mSoftShape); addCollisionShape(mSoftShape); + + if(oldShape) // Copy the properties of the previous soft shape, if it exists + { + mSoftShape->setColor(oldShape->getRGBA()); + mSoftShape->setLocalTransform(oldShape->getLocalTransform()); + } } //============================================================================== diff --git a/dart/dynamics/SoftBodyNode.h b/dart/dynamics/SoftBodyNode.h index 85894590154a5..8dea4f7bab282 100644 --- a/dart/dynamics/SoftBodyNode.h +++ b/dart/dynamics/SoftBodyNode.h @@ -123,10 +123,10 @@ class SoftBodyNode : public BodyNode /// Set the Properties of this SoftBodyNode void setProperties(const UniqueProperties& _properties); - /// Remove any unwarranted SoftBodyShapes + /// Remove all SoftBodyShapes and return the last one that was encountered /// Note: This will be deprecated once VisualizationNodes and CollisionNodes /// are implemented. Please see #394. - void removeSoftBodyShapes(); + ShapePtr removeSoftBodyShapes(); /// Get the Properties of this SoftBodyNode From b6cdfe584f64aec0d21aef7f04c0cef84657360e Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Tue, 30 Jun 2015 17:16:53 -0400 Subject: [PATCH 38/65] finished collisions tutorial --- tutorials/tutorialCollisions-Finished.cpp | 617 ++++++++++++++++++++++ tutorials/tutorialCollisions.cpp | 382 ++++++-------- 2 files changed, 792 insertions(+), 207 deletions(-) create mode 100644 tutorials/tutorialCollisions-Finished.cpp diff --git a/tutorials/tutorialCollisions-Finished.cpp b/tutorials/tutorialCollisions-Finished.cpp new file mode 100644 index 0000000000000..7f1f49c8b0d7c --- /dev/null +++ b/tutorials/tutorialCollisions-Finished.cpp @@ -0,0 +1,617 @@ +/* + * Copyright (c) 2015, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/dart.h" + +const double default_shape_density = 1000; // kg/m^3 +const double default_shape_height = 0.1; // m +const double default_shape_width = 0.03; // m + +const double default_start_height = 0.4; // m + +const double minimum_start_v = 2.5; // m/s +const double maximum_start_v = 4.0; // m/s +const double default_start_v = 3.5; // m/s + +const double minimum_launch_angle = 30.0*M_PI/180.0; // rad +const double maximum_launch_angle = 70.0*M_PI/180.0; // rad +const double default_launch_angle = 45.0*M_PI/180.0; // rad + +const double maximum_start_w = 6*M_PI; // rad/s +const double default_start_w = 3*M_PI; // rad/s + +const double default_spring_stiffness = 0.5; +const double default_damping_coefficient = 0.05; + +const double default_ground_width = 2; +const double default_wall_thickness = 0.1; +const double default_wall_height = 1; +const double default_spawn_range = 0.9*default_ground_width/2; + +const double default_restitution = 0.6; + +const double default_vertex_stiffness = 100.0; +const double default_edge_stiffness = 10.0; +const double default_soft_damping = 5.0; + +using namespace dart::dynamics; +using namespace dart::simulation; + +void setupRing(const SkeletonPtr& ring) +{ + // Set the spring and damping coefficients for the degrees of freedom + for(size_t i=6; i < ring->getNumDofs(); ++i) + { + DegreeOfFreedom* dof = ring->getDof(i); + dof->setSpringStiffness(default_spring_stiffness); + dof->setDampingCoefficient(default_damping_coefficient); + } + + // Compute the joint angle needed to form a ring + size_t numEdges = ring->getNumBodyNodes(); + double angle = 2*M_PI/numEdges; + + // Set the BallJoints so that they have the correct rest position angle + for(size_t i=1; i < ring->getNumJoints(); ++i) + { + Joint* joint = ring->getJoint(i); + Eigen::Vector3d restPos = BallJoint::convertToPositions(Eigen::Matrix3d( + Eigen::AngleAxisd(angle, Eigen::Vector3d(0, 1, 0)))); + + for(size_t j=0; j<3; ++j) + joint->setRestPosition(j, restPos[j]); + } + + // Set the Joints to be in their rest positions + for(size_t i=6; i < ring->getNumDofs(); ++i) + { + DegreeOfFreedom* dof = ring->getDof(i); + dof->setPosition(dof->getRestPosition()); + } +} + +class MyWindow : public dart::gui::SimWindow +{ +public: + + MyWindow(const WorldPtr& world, const SkeletonPtr& ball, + const SkeletonPtr& hybridBody, const SkeletonPtr& rigidChain, + const SkeletonPtr& rigidRing) + : mRandomize(true), + mRD(), + mMT(mRD()), + mDistribution(-1.0, std::nextafter(1.0, 2.0)), + mOriginalBall(ball), + mOriginalHybridBody(hybridBody), + mOriginalRigidChain(rigidChain), + mOriginalRigidRing(rigidRing), + mSkelCount(0) + { + setWorld(world); + } + + void keyboard(unsigned char key, int x, int y) override + { + switch(key) + { + case '1': + addObject(mOriginalBall->clone()); + break; + + case '2': + addObject(mOriginalHybridBody->clone()); + break; + + case '3': + addObject(mOriginalRigidChain->clone()); + break; + + case '4': + addRing(mOriginalRigidRing->clone()); + break; + + case 'd': + if(mWorld->getNumSkeletons() > 2) + removeSkeleton(mWorld->getSkeleton(2)); + std::cout << "Remaining objects: " << mWorld->getNumSkeletons()-2 + << std::endl; + break; + + case 'r': + mRandomize = !mRandomize; + std::cout << "Randomization: " << (mRandomize? "on" : "off") + << std::endl; + break; + + default: + SimWindow::keyboard(key, x, y); + } + } + + void drawSkels() override + { + // Make sure lighting is turned on and that polygons get filled in + glEnable(GL_LIGHTING); + glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); + + SimWindow::drawSkels(); + } + + void displayTimer(int _val) override + { + // We remove playback and baking, because we want to be able to add and + // remove objects during runtime + int numIter = mDisplayTimeout / (mWorld->getTimeStep() * 1000); + if (mSimulating) + { + for (int i = 0; i < numIter; i++) + timeStepping(); + } + glutPostRedisplay(); + glutTimerFunc(mDisplayTimeout, refreshTimer, _val); + } + +protected: + + /// Add an object to the world and toss it at the wall + bool addObject(const SkeletonPtr& object) + { + // Set the starting position for the object + Eigen::Vector6d positions(Eigen::Vector6d::Zero()); + + // If randomization is on, we will randomize the starting y-location + if(mRandomize) + positions[4] = default_spawn_range*mDistribution(mMT); + + positions[5] = default_start_height; + object->getJoint(0)->setPositions(positions); + + // Add the object to the world + object->setName(object->getName()+std::to_string(mSkelCount++)); + mWorld->addSkeleton(object); + + // Compute collisions + dart::collision::CollisionDetector* detector = + mWorld->getConstraintSolver()->getCollisionDetector(); + detector->detectCollision(true, true); + + // Look through the collisions to see if the new object would start in + // collision with something + bool collision = false; + size_t collisionCount = detector->getNumContacts(); + for(size_t i = 0; i < collisionCount; ++i) + { + const dart::collision::Contact& contact = detector->getContact(i); + if(contact.bodyNode1.lock()->getSkeleton() == object + || contact.bodyNode2.lock()->getSkeleton() == object) + { + collision = true; + break; + } + } + + // Refuse to add the object if it is in collision + if(collision) + { + mWorld->removeSkeleton(object); + std::cout << "The new object spawned in a collision. " + << "It will not be added to the world." << std::endl; + return false; + } + + // Create reference frames for setting the initial velocity + Eigen::Isometry3d centerTf(Eigen::Isometry3d::Identity()); + centerTf.translation() = object->getCOM(); + SimpleFrame center(Frame::World(), "center", centerTf); + + SimpleFrame ref(¢er, "root"); + ref.setRelativeTransform(object->getBodyNode(0)->getTransform(¢er)); + + // Set the velocities of the reference frames so that we can easily give the + // Skeleton the linear and angular velocities that we want + double angle = default_launch_angle; + double speed = default_start_v; + double angular_speed = default_start_w; + if(mRandomize) + { + angle = (mDistribution(mMT) + 1.0)/2.0 * + (maximum_launch_angle - minimum_launch_angle) + minimum_launch_angle; + + speed = (mDistribution(mMT) + 1.0)/2.0 * + (maximum_start_v - minimum_start_v) + minimum_start_v; + + angular_speed = mDistribution(mMT) * maximum_start_w; + } + + Eigen::Vector3d v = speed * Eigen::Vector3d(cos(angle), 0.0, sin(angle)); + Eigen::Vector3d w = angular_speed * Eigen::Vector3d::UnitY(); + center.setClassicDerivatives(v, w); + + // Use the reference frames to set the velocity of the Skeleton's root + object->getJoint(0)->setVelocities(ref.getSpatialVelocity()); + + return true; + } + + /// Add a ring to the world, and create a BallJoint constraint to ensure that + /// it stays in a ring shape + void addRing(const SkeletonPtr& ring) + { + setupRing(ring); + + if(!addObject(ring)) + return; + + // Create a closed loop to turn the chain into a ring + BodyNode* head = ring->getBodyNode(0); + BodyNode* tail = ring->getBodyNode(ring->getNumBodyNodes()-1); + + // Compute the offset where the JointConstraint should be located + Eigen::Vector3d offset = Eigen::Vector3d(0, 0, -default_shape_height / 2.0); + auto constraint = new dart::constraint::BallJointConstraint( + head, tail, head->getWorldTransform() * offset); + + mWorld->getConstraintSolver()->addConstraint(constraint); + mJointConstraints.push_back(constraint); + } + + /// Remove a Skeleton and get rid of the constraint that was associated with + /// it, if one existed + void removeSkeleton(const SkeletonPtr& skel) + { + for(size_t i=0; igetBodyNode1()->getSkeleton() == skel + || constraint->getBodyNode2()->getSkeleton() == skel) + { + mWorld->getConstraintSolver()->removeConstraint(constraint); + mJointConstraints.erase(mJointConstraints.begin()+i); + delete constraint; + break; // There should only be one constraint per skeleton + } + } + + mWorld->removeSkeleton(skel); + } + + /// Flag to keep track of whether or not we are randomizing the tosses + bool mRandomize; + + // std library objects that allow us to generate high-quality random numbers + std::random_device mRD; + std::mt19937 mMT; + std::uniform_real_distribution mDistribution; + + /// History of the active JointConstraints so that we can properly delete them + /// when a Skeleton gets removed + std::vector mJointConstraints; + + /// A blueprint Skeleton that we will use to spawn balls + SkeletonPtr mOriginalBall; + + /// A blueprint Skeleton that we will use to spawn hybrid bodies + SkeletonPtr mOriginalHybridBody; + + /// A blueprint Skeleton that we will use to spawn rigid chains + SkeletonPtr mOriginalRigidChain; + + /// A blueprint Skeleton that we will use to spawn rigid rings + SkeletonPtr mOriginalRigidRing; + + /// Keep track of how many Skeletons we spawn to ensure we can give them all + /// unique names + size_t mSkelCount; + +}; + +/// Add a rigid body with the specified Joint type to a chain +template +BodyNode* addRigidBody(const SkeletonPtr& chain, const std::string& name, + Shape::ShapeType type, BodyNode* parent = nullptr) +{ + // Compute the transform for the center of the object + Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); + Eigen::Vector3d center = Eigen::Vector3d(0, 0, default_shape_height / 2.0); + tf.translation() = center; + + // Set the Joint properties + typename JointType::Properties properties; + properties.mName = name+"_joint"; + if(parent) + { + // If the body has a parent, we should position the joint to be in the + // middle of the centers of the two bodies + properties.mT_ParentBodyToJoint = tf; + properties.mT_ChildBodyToJoint = tf.inverse(); + } + + // Create the Joint and Body pair + BodyNode* bn = chain->createJointAndBodyNodePair( + parent, properties, BodyNode::Properties(name)).second; + + // Make the shape based on the requested Shape type + std::shared_ptr shape; + if(Shape::BOX == type) + { + shape = std::make_shared(Eigen::Vector3d( + default_shape_width, + default_shape_width, + default_shape_height)); + } + else if(Shape::CYLINDER == type) + { + shape = std::make_shared(default_shape_width/2, + default_shape_height); + } + else if(Shape::ELLIPSOID == type) + { + shape = std::make_shared( + default_shape_height*Eigen::Vector3d::Ones()); + } + + bn->addVisualizationShape(shape); + bn->addCollisionShape(shape); + + // Setup the inertia for the body + Inertia box_inertia; + double mass = default_shape_density * shape->getVolume(); + box_inertia.setMass(mass); + box_inertia.setMoment(shape->computeInertia(mass)); + bn->setInertia(box_inertia); + + // Set the coefficient of restitution to make the body more bouncy + bn->setRestitutionCoeff(default_restitution); + + return bn; +} + +enum SoftShapeType { + SOFT_BOX = 0, + SOFT_CYLINDER, + SOFT_ELLIPSOID +}; + +/// Add a soft body with the specified Joint type to a chain +template +BodyNode* addSoftShape(const SkeletonPtr& chain, const std::string& name, + SoftShapeType type, BodyNode* parent = nullptr) +{ + // Compute the transform for the center of the object + Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); + Eigen::Vector3d center = Eigen::Vector3d(0, 0, default_shape_height / 2.0); + tf.translation() = center; + + // Set the Joint properties + typename JointType::Properties joint_properties; + joint_properties.mName = name+"_joint"; + if(parent) + { + // If the body has a parent, we should position the joint to be in the + // middle of the centers of the two bodies + joint_properties.mT_ParentBodyToJoint = tf; + joint_properties.mT_ChildBodyToJoint = tf.inverse(); + } + + // Set the properties of the soft body + double soft_shape_width = 2*default_shape_width; + SoftBodyNode::UniqueProperties soft_properties; + // Use the SoftBodyNodeHelper class to create the geometries for the + // SoftBodyNode + if(SOFT_BOX == type) + { + soft_properties = SoftBodyNodeHelper::makeBoxProperties(Eigen::Vector3d( + soft_shape_width, soft_shape_width, default_shape_height), + Eigen::Isometry3d::Identity(), Eigen::Vector3i(4,4,4), + default_shape_density*default_shape_height*pow(soft_shape_width,2)); + } + else if(SOFT_CYLINDER == type) + { + soft_properties = SoftBodyNodeHelper::makeCylinderProperties( + soft_shape_width/2.0, default_shape_height, 8, 3, 2, + default_shape_density * default_shape_height + * pow(M_PI*soft_shape_width/2.0, 2)); + } + else if(SOFT_ELLIPSOID == type) + { + soft_properties = SoftBodyNodeHelper::makeEllipsoidProperties( + default_shape_height*Eigen::Vector3d::Ones(), 6, 6, + default_shape_density * 4.0/3.0*M_PI*pow(default_shape_height/2,3)); + } + soft_properties.mKv = default_vertex_stiffness; + soft_properties.mKe = default_edge_stiffness; + soft_properties.mDampCoeff = default_soft_damping; + + // Create the Joint and Body pair + SoftBodyNode* bn = chain->createJointAndBodyNodePair( + parent, joint_properties, SoftBodyNode::Properties( + BodyNode::Properties(name), soft_properties)).second; + + return bn; +} + +void setAllColors(const SkeletonPtr& object, const Eigen::Vector3d& color) +{ + // Set the color of all the shapes in the object + for(size_t i=0; i < object->getNumBodyNodes(); ++i) + { + BodyNode* bn = object->getBodyNode(i); + for(size_t j=0; j < bn->getNumVisualizationShapes(); ++j) + bn->getVisualizationShape(j)->setColor(color); + } +} + +SkeletonPtr createBall() +{ + SkeletonPtr ball = Skeleton::create("rigid_ball"); + + // Give the ball a body + addRigidBody(ball, "rigid ball", Shape::ELLIPSOID); + + setAllColors(ball, dart::Color::Red()); + + return ball; +} + +SkeletonPtr createRigidChain() +{ + SkeletonPtr chain = Skeleton::create("rigid_chain"); + + // Add bodies to the chain + BodyNode* bn = addRigidBody(chain, "rigid box 1", Shape::BOX); + bn = addRigidBody(chain, "rigid cyl 2", Shape::CYLINDER, bn); + bn = addRigidBody(chain, "rigid box 3", Shape::BOX, bn); + + setAllColors(chain, dart::Color::Orange()); + + return chain; +} + +SkeletonPtr createRigidRing() +{ + SkeletonPtr ring = Skeleton::create("rigid_ring"); + + // Add bodies to the ring + BodyNode* bn = addRigidBody(ring, "rigid box 1", Shape::BOX); + bn = addRigidBody(ring, "rigid cyl 2", Shape::CYLINDER, bn); + bn = addRigidBody(ring, "rigid box 3", Shape::BOX, bn); + bn = addRigidBody(ring, "rigid cyl 4", Shape::CYLINDER, bn); + bn = addRigidBody(ring, "rigid box 5", Shape::BOX, bn); + bn = addRigidBody(ring, "rigid cyl 6", Shape::CYLINDER, bn); + + setAllColors(ring, dart::Color::Blue()); + + return ring; +} + +SkeletonPtr createHybridBody() +{ + SkeletonPtr hybrid = Skeleton::create("hybrid"); + + // Add a soft body + BodyNode* bn = addSoftShape(hybrid, "soft sphere", SOFT_ELLIPSOID); + + // Add a rigid body attached by a WeldJoint + bn = hybrid->createJointAndBodyNodePair(bn).second; + bn->setName("rigid box"); + + double box_shape_height = default_shape_height; + std::shared_ptr box = std::make_shared( + box_shape_height*Eigen::Vector3d::Ones()); + Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); + tf.translation() = Eigen::Vector3d(box_shape_height/2.0, 0, 0); + box->setLocalTransform(tf); + + bn->addCollisionShape(box); + bn->addVisualizationShape(box); + + Inertia inertia; + inertia.setMass(default_shape_density*pow(box_shape_height,3)); + inertia.setMoment(box->computeInertia(inertia.getMass())); + bn->setInertia(inertia); + + setAllColors(hybrid, dart::Color::Green()); + + return hybrid; +} + +SkeletonPtr createGround() +{ + SkeletonPtr ground = Skeleton::create("ground"); + + BodyNode* bn = ground->createJointAndBodyNodePair().second; + + std::shared_ptr shape = std::make_shared( + Eigen::Vector3d(default_ground_width, default_ground_width, + default_wall_thickness)); + shape->setColor(Eigen::Vector3d(1.0, 1.0, 1.0)); + + bn->addCollisionShape(shape); + bn->addVisualizationShape(shape); + + return ground; +} + +SkeletonPtr createWall() +{ + SkeletonPtr wall = Skeleton::create("wall"); + + BodyNode* bn = wall->createJointAndBodyNodePair().second; + + std::shared_ptr shape = std::make_shared( + Eigen::Vector3d(default_wall_thickness, default_ground_width, + default_wall_height)); + shape->setColor(Eigen::Vector3d(0.8, 0.8, 0.8)); + + bn->addCollisionShape(shape); + bn->addVisualizationShape(shape); + + Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); + tf.translation() = Eigen::Vector3d( + (default_ground_width + default_wall_thickness)/2.0, 0.0, + (default_wall_height - default_wall_thickness)/2.0); + bn->getParentJoint()->setTransformFromParentBodyNode(tf); + + bn->setRestitutionCoeff(0.2); + + return wall; +} + +int main(int argc, char* argv[]) +{ + WorldPtr world = std::make_shared(); + world->addSkeleton(createGround()); + world->addSkeleton(createWall()); + + MyWindow window(world, createBall(), createHybridBody(), + createRigidChain(), createRigidRing()); + + std::cout << "space bar: simulation on/off" << std::endl; + std::cout << "'1': toss a rigid ball" << std::endl; + std::cout << "'2': toss a hybrid soft/rigid body" << std::endl; + std::cout << "'3': toss a rigid chain" << std::endl; + std::cout << "'4': toss a ring of rigid bodies" << std::endl; + + std::cout << "\n'd': delete the oldest object" << std::endl; + std::cout << "'r': toggle randomness" << std::endl; + + std::cout << "\nWarning: Let objects settle before tossing a new one, or the simulation could explode." << std::endl; + std::cout << " If the simulation freezes, you may need to force quit the application.\n" << std::endl; + + glutInit(&argc, argv); + window.initWindow(640, 480, "Collisions"); + glutMainLoop(); +} diff --git a/tutorials/tutorialCollisions.cpp b/tutorials/tutorialCollisions.cpp index 301b019c503d2..7eb27ebf37a9b 100644 --- a/tutorials/tutorialCollisions.cpp +++ b/tutorials/tutorialCollisions.cpp @@ -42,64 +42,64 @@ const double default_shape_width = 0.03; // m const double default_start_height = 0.4; // m -const double minimum_start_v = 1.0; // m/s +const double minimum_start_v = 2.5; // m/s const double maximum_start_v = 4.0; // m/s -const double default_start_v = 2; // m/s +const double default_start_v = 3.5; // m/s -const double minimum_launch_angle = 20.0*M_PI/180.0; // rad -const double maximum_launch_angle = 60.0*M_PI/180.0; // rad +const double minimum_launch_angle = 30.0*M_PI/180.0; // rad +const double maximum_launch_angle = 70.0*M_PI/180.0; // rad const double default_launch_angle = 45.0*M_PI/180.0; // rad const double maximum_start_w = 6*M_PI; // rad/s const double default_start_w = 3*M_PI; // rad/s -const double default_spring_stiffness = 100; -const double default_damping_coefficient = 10; +const double default_spring_stiffness = 0.5; +const double default_damping_coefficient = 0.05; const double default_ground_width = 2; const double default_wall_thickness = 0.1; const double default_wall_height = 1; const double default_spawn_range = 0.9*default_ground_width/2; +const double default_restitution = 0.6; + +const double default_vertex_stiffness = 100.0; +const double default_edge_stiffness = 10.0; +const double default_soft_damping = 5.0; + using namespace dart::dynamics; using namespace dart::simulation; -void setupRing(const SkeletonPtr& chain) +void setupRing(const SkeletonPtr& /*ring*/) { - for(size_t i=6; i < chain->getNumDofs(); ++i) - { - DegreeOfFreedom* dof = chain->getDof(i); - dof->setSpringStiffness(default_spring_stiffness); - dof->setDampingCoefficient(default_damping_coefficient); - } + // Set the spring and damping coefficients for the degrees of freedom + // Lesson 4a - size_t numEdges = chain->getNumBodyNodes(); - double angle = 2*M_PI/numEdges; + // Compute the joint angle needed to form a ring + // Lesson 4b - for(size_t i=1; i < chain->getNumJoints(); ++i) - { - Joint* joint = chain->getJoint(i); - Eigen::Vector3d restPos = BallJoint::convertToPositions(Eigen::Matrix3d( - Eigen::AngleAxisd(angle, Eigen::Vector3d(0, 1, 0)))); + // Set the BallJoints so that they have the correct rest position angle + // Lesson 4b - for(size_t j=0; j<3; ++j) - joint->setRestPosition(j, restPos[j]); - } + // Set the Joints to be in their rest positions + // Lesson 4c } class MyWindow : public dart::gui::SimWindow { public: - MyWindow(const WorldPtr& world, const SkeletonPtr& rigidChain, - const SkeletonPtr& softChain, const SkeletonPtr& hybridChain) + MyWindow(const WorldPtr& world, const SkeletonPtr& ball, + const SkeletonPtr& hybridBody, const SkeletonPtr& rigidChain, + const SkeletonPtr& rigidRing) : mRandomize(true), mRD(), mMT(mRD()), mDistribution(-1.0, std::nextafter(1.0, 2.0)), + mOriginalBall(ball), + mOriginalHybridBody(hybridBody), mOriginalRigidChain(rigidChain), - mOriginalSoftChain(softChain), - mOriginalHybridChain(hybridChain), + mOriginalRigidRing(rigidRing), mSkelCount(0) { setWorld(world); @@ -110,20 +110,26 @@ class MyWindow : public dart::gui::SimWindow switch(key) { case '1': - addChain(mOriginalRigidChain->clone()); + addObject(mOriginalBall->clone()); break; case '2': - addRing(mOriginalRigidChain->clone()); + addObject(mOriginalHybridBody->clone()); break; case '3': - addChain(mOriginalSoftChain->clone()); + addObject(mOriginalRigidChain->clone()); + break; + + case '4': + addRing(mOriginalRigidRing->clone()); break; case 'd': if(mWorld->getNumSkeletons() > 2) removeSkeleton(mWorld->getSkeleton(2)); + std::cout << "Remaining objects: " << mWorld->getNumSkeletons()-2 + << std::endl; break; case 'r': @@ -137,6 +143,15 @@ class MyWindow : public dart::gui::SimWindow } } + void drawSkels() override + { + // Make sure lighting is turned on and that polygons get filled in + glEnable(GL_LIGHTING); + glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); + + SimWindow::drawSkels(); + } + void displayTimer(int _val) override { // We remove playback and baking, because we want to be able to add and @@ -153,110 +168,52 @@ class MyWindow : public dart::gui::SimWindow protected: - bool addChain(const SkeletonPtr& chain) + /// Add an object to the world and toss it at the wall + bool addObject(const SkeletonPtr& /*object*/) { - // Set the starting position for the chain - Eigen::Vector6d positions(Eigen::Vector6d::Zero()); - - if(mRandomize) - positions[4] = default_spawn_range*mDistribution(mMT); - - positions[5] = default_start_height; - chain->getJoint(0)->setPositions(positions); - - // Wrap up the revolute joints of the chain - for(size_t i=6; i < chain->getNumDofs(); ++i) - { - DegreeOfFreedom* dof = chain->getDof(i); - dof->setPosition(dof->getRestPosition()); - } + // Set the starting position for the object + // Lesson 3a - // Add the chain to the world - chain->setName(chain->getName()+std::to_string(mSkelCount++)); - mWorld->addSkeleton(chain); + // Add the object to the world + // Lesson 3b // Compute collisions - dart::collision::CollisionDetector* detector = - mWorld->getConstraintSolver()->getCollisionDetector(); - detector->detectCollision(true, true); - - // Look through the collisions to see if the new ring would start in - // collision with something - bool collision = false; - size_t collisionCount = detector->getNumContacts(); - for(size_t i = 0; i < collisionCount; ++i) - { - const dart::collision::Contact& contact = detector->getContact(i); - if(contact.bodyNode1.lock()->getSkeleton() == chain - || contact.bodyNode2.lock()->getSkeleton() == chain) - { - collision = true; - break; - } - } + // Lesson 3c - // Refuse to add the chain if it is in collision - if(collision) - { - mWorld->removeSkeleton(chain); - std::cout << "The new object spawned in a collision. " - << "It will not be added to the world." << std::endl; - return false; - } + // Refuse to add the object if it is in collision + // Lesson 3c // Create reference frames for setting the initial velocity - Eigen::Isometry3d centerTf(Eigen::Isometry3d::Identity()); - centerTf.translation() = chain->getCOM(); - SimpleFrame center(Frame::World(), "center", centerTf); - - SimpleFrame ref(¢er, "root"); - ref.setRelativeTransform(chain->getBodyNode(0)->getTransform(¢er)); + // Lesson 3d // Set the velocities of the reference frames so that we can easily give the // Skeleton the linear and angular velocities that we want - double angle = default_launch_angle; - double speed = default_start_v; - double angular_speed = default_start_w; - if(mRandomize) - { - angle = (mDistribution(mMT) + 1.0)/2.0 * - (maximum_launch_angle - minimum_launch_angle) + minimum_launch_angle; - - speed = (mDistribution(mMT) + 1.0)/2.0 * - (maximum_start_v - minimum_start_v) + minimum_start_v; - - angular_speed = mDistribution(mMT) * maximum_start_w; - } - - Eigen::Vector3d v = speed * Eigen::Vector3d(cos(angle), 0.0, sin(angle)); - Eigen::Vector3d w = angular_speed * Eigen::Vector3d::UnitY(); - center.setClassicDerivatives(v, w); + // Lesson 3e // Use the reference frames to set the velocity of the Skeleton's root - chain->getJoint(0)->setVelocities(ref.getSpatialVelocity()); + // Lesson 3f return true; } + /// Add a ring to the world, and create a BallJoint constraint to ensure that + /// it stays in a ring shape void addRing(const SkeletonPtr& ring) { setupRing(ring); - if(!addChain(ring)) + if(!addObject(ring)) return; // Create a closed loop to turn the chain into a ring - BodyNode* head = ring->getBodyNode(0); - BodyNode* tail = ring->getBodyNode(ring->getNumBodyNodes()-1); - - Eigen::Vector3d offset = Eigen::Vector3d(0, 0, -default_shape_height / 2.0); - auto constraint = new dart::constraint::BallJointConstraint( - head, tail, head->getWorldTransform() * offset); + // Lesson 5 - mWorld->getConstraintSolver()->addConstraint(constraint); - mJointConstraints.push_back(constraint); + // Compute the offset where the JointConstraint should be located + // Lesson 5 } + /// Remove a Skeleton and get rid of the constraint that was associated with + /// it, if one existed void removeSkeleton(const SkeletonPtr& skel) { for(size_t i=0; iremoveSkeleton(skel); } - // std library objects that allow us to generate high-quality random numbers + /// Flag to keep track of whether or not we are randomizing the tosses bool mRandomize; + + // std library objects that allow us to generate high-quality random numbers std::random_device mRD; std::mt19937 mMT; std::uniform_real_distribution mDistribution; + /// History of the active JointConstraints so that we can properly delete them + /// when a Skeleton gets removed std::vector mJointConstraints; - SkeletonPtr mOriginalRigidChain; + /// A blueprint Skeleton that we will use to spawn balls + SkeletonPtr mOriginalBall; - SkeletonPtr mOriginalSoftChain; + /// A blueprint Skeleton that we will use to spawn hybrid bodies + SkeletonPtr mOriginalHybridBody; - SkeletonPtr mOriginalHybridChain; + /// A blueprint Skeleton that we will use to spawn rigid chains + SkeletonPtr mOriginalRigidChain; + + /// A blueprint Skeleton that we will use to spawn rigid rings + SkeletonPtr mOriginalRigidRing; /// Keep track of how many Skeletons we spawn to ensure we can give them all /// unique names @@ -295,130 +262,127 @@ class MyWindow : public dart::gui::SimWindow }; +/// Add a rigid body with the specified Joint type to a chain template -BodyNode* addRigidShape(const SkeletonPtr& chain, const std::string& name, - Shape::ShapeType type, BodyNode* parent = nullptr) +BodyNode* addRigidBody(const SkeletonPtr& /*chain*/, const std::string& /*name*/, + Shape::ShapeType /*type*/, BodyNode* /*parent*/ = nullptr) { - Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); - Eigen::Vector3d center = Eigen::Vector3d(0, 0, default_shape_height / 2.0); - tf.translation() = center; + // Compute the transform for the center of the object + // Lesson 1a - typename JointType::Properties properties; - properties.mName = name+"_joint"; - if(parent) - { - properties.mT_ParentBodyToJoint = tf; - properties.mT_ChildBodyToJoint = tf.inverse(); - } + // Set the Joint properties + // Lesson 1b - BodyNode* bn = chain->createJointAndBodyNodePair( - parent, properties, BodyNode::Properties(name)).second; + // Create the Joint and Body pair + // Lesson 1c + BodyNode* bn = nullptr; - std::shared_ptr shape; - if(Shape::BOX == type) - { - shape = std::make_shared(Eigen::Vector3d( - default_shape_width, - default_shape_width, - default_shape_height)); - } - else if(Shape::CYLINDER == type) - { - shape = std::make_shared(default_shape_width/2, - default_shape_height); - } + // Make the shape based on the requested Shape type + // Lesson 1d - bn->addVisualizationShape(shape); - bn->addCollisionShape(shape); + // Setup the inertia for the body + // Lesson 1e - Inertia box_inertia; - double mass = default_shape_density * shape->getVolume(); - box_inertia.setMass(mass); - box_inertia.setMoment(shape->computeInertia(mass)); - bn->setInertia(box_inertia); + // Set the coefficient of restitution to make the body more bouncy + // Lesson 1f return bn; } enum SoftShapeType { SOFT_BOX = 0, - SOFT_CYLINDER + SOFT_CYLINDER, + SOFT_ELLIPSOID }; +/// Add a soft body with the specified Joint type to a chain template -BodyNode* addSoftShape(const SkeletonPtr& chain, const std::string& name, - SoftShapeType type, BodyNode* parent = nullptr) +BodyNode* addSoftShape(const SkeletonPtr& /*chain*/, const std::string& /*name*/, + SoftShapeType /*type*/, BodyNode* /*parent*/ = nullptr) { - Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); - Eigen::Vector3d center = Eigen::Vector3d(0, 0, default_shape_height / 2.0); - tf.translation() = center; + // Compute the transform for the center of the object + // Lesson 2a - typename JointType::Properties joint_properties; - joint_properties.mName = name+"_joint"; - if(parent) - { - joint_properties.mT_ParentBodyToJoint = tf; - joint_properties.mT_ChildBodyToJoint = tf.inverse(); - } + // Set the Joint properties + // Lesson 2b - double soft_shape_width = 2*default_shape_width; - SoftBodyNode::UniqueProperties soft_properties; - if(SOFT_BOX == type) - { - soft_properties = SoftBodyNodeHelper::makeBoxProperties(Eigen::Vector3d( - soft_shape_width, soft_shape_width, default_shape_height), - Eigen::Isometry3d::Identity(), Eigen::Vector3i(4,4,4), - default_shape_density*default_shape_height*pow(soft_shape_width,2)); - } - else if(SOFT_CYLINDER == type) + // Set the properties of the soft body + // Lesson 2c + + // Create the Joint and Body pair + // Lesson 2d + SoftBodyNode* bn = nullptr; + + return bn; +} + +void setAllColors(const SkeletonPtr& object, const Eigen::Vector3d& color) +{ + // Set the color of all the shapes in the object + for(size_t i=0; i < object->getNumBodyNodes(); ++i) { - soft_properties = SoftBodyNodeHelper::makeCylinderProperties( - soft_shape_width/2.0, default_shape_height, 8, 3, 2, - default_shape_density * default_shape_height - * pow(M_PI*soft_shape_width/2.0, 2)); + BodyNode* bn = object->getBodyNode(i); + for(size_t j=0; j < bn->getNumVisualizationShapes(); ++j) + bn->getVisualizationShape(j)->setColor(color); } - soft_properties.mKv = 100; - soft_properties.mKe = 0; - soft_properties.mDampCoeff = 5; +} - SoftBodyNode* bn = chain->createJointAndBodyNodePair( - parent, joint_properties, SoftBodyNode::Properties( - BodyNode::Properties(), soft_properties)).second; +SkeletonPtr createBall() +{ + SkeletonPtr ball = Skeleton::create("rigid_ball"); - bn->getVisualizationShape(0)->setColor(dart::Color::Red()); + // Give the ball a body + addRigidBody(ball, "rigid ball", Shape::ELLIPSOID); + setAllColors(ball, dart::Color::Red()); - return bn; + return ball; } SkeletonPtr createRigidChain() { SkeletonPtr chain = Skeleton::create("rigid_chain"); - BodyNode* bn = addRigidShape(chain, "rigid box 1", Shape::BOX); - bn = addRigidShape(chain, "rigid cyl 2", Shape::CYLINDER, bn); - bn = addRigidShape(chain, "rigid box 3", Shape::BOX, bn); - bn = addRigidShape(chain, "rigid cyl 4", Shape::CYLINDER, bn); - bn = addRigidShape(chain, "rigid box 5", Shape::BOX, bn); - bn = addRigidShape(chain, "rigid cyl 6", Shape::CYLINDER, bn); + // Add bodies to the chain + BodyNode* bn = addRigidBody(chain, "rigid box 1", Shape::BOX); + bn = addRigidBody(chain, "rigid cyl 2", Shape::CYLINDER, bn); + bn = addRigidBody(chain, "rigid box 3", Shape::BOX, bn); + + setAllColors(chain, dart::Color::Orange()); return chain; } -SkeletonPtr createSoftChain() +SkeletonPtr createRigidRing() { - SkeletonPtr chain = Skeleton::create("soft_chain"); + SkeletonPtr ring = Skeleton::create("rigid_ring"); - BodyNode* bn = addSoftShape(chain, "soft box 1", SOFT_BOX); -// bn = addSoftShape(chain, "soft cyl 2", SOFT_CYLINDER, bn); -// bn = addSoftShape(chain, "soft box 3", SOFT_BOX, bn); + // Add bodies to the ring + BodyNode* bn = addRigidBody(ring, "rigid box 1", Shape::BOX); + bn = addRigidBody(ring, "rigid cyl 2", Shape::CYLINDER, bn); + bn = addRigidBody(ring, "rigid box 3", Shape::BOX, bn); + bn = addRigidBody(ring, "rigid cyl 4", Shape::CYLINDER, bn); + bn = addRigidBody(ring, "rigid box 5", Shape::BOX, bn); + bn = addRigidBody(ring, "rigid cyl 6", Shape::CYLINDER, bn); - return chain; + setAllColors(ring, dart::Color::Blue()); + + return ring; } -SkeletonPtr createHybridChain() +SkeletonPtr createHybridBody() { - return Skeleton::create("hybrid_chain"); + SkeletonPtr hybrid = Skeleton::create("hybrid"); + + // Add a soft body + /*BodyNode* bn =*/ addSoftShape(hybrid, "soft sphere", SOFT_ELLIPSOID); + + // Add a rigid body attached by a WeldJoint + // Lesson 2e + + setAllColors(hybrid, dart::Color::Green()); + + return hybrid; } SkeletonPtr createGround() @@ -430,7 +394,7 @@ SkeletonPtr createGround() std::shared_ptr shape = std::make_shared( Eigen::Vector3d(default_ground_width, default_ground_width, default_wall_thickness)); - shape->setColor(Eigen::Vector3d(0,0,0)); + shape->setColor(Eigen::Vector3d(1.0, 1.0, 1.0)); bn->addCollisionShape(shape); bn->addVisualizationShape(shape); @@ -447,7 +411,7 @@ SkeletonPtr createWall() std::shared_ptr shape = std::make_shared( Eigen::Vector3d(default_wall_thickness, default_ground_width, default_wall_height)); - shape->setColor(Eigen::Vector3d(0,0,0)); + shape->setColor(Eigen::Vector3d(0.8, 0.8, 0.8)); bn->addCollisionShape(shape); bn->addVisualizationShape(shape); @@ -458,27 +422,31 @@ SkeletonPtr createWall() (default_wall_height - default_wall_thickness)/2.0); bn->getParentJoint()->setTransformFromParentBodyNode(tf); + bn->setRestitutionCoeff(0.2); + return wall; } int main(int argc, char* argv[]) { - SkeletonPtr chain = createRigidChain(); - Eigen::Vector6d positions(Eigen::Vector6d::Zero()); - positions.tail<3>() = Eigen::Vector3d(0,0,3); - chain->getJoint(0)->setPositions(positions); - WorldPtr world = std::make_shared(); world->addSkeleton(createGround()); world->addSkeleton(createWall()); - MyWindow window(world, createRigidChain(), createSoftChain(), - createHybridChain()); + MyWindow window(world, createBall(), createHybridBody(), + createRigidChain(), createRigidRing()); std::cout << "space bar: simulation on/off" << std::endl; - std::cout << "'1': spawn a rigid chain" << std::endl; - std::cout << "'2': spawn a rigid ring" << std::endl; - std::cout << "'3': spawn a soft body" << std::endl; + std::cout << "'1': toss a rigid ball" << std::endl; + std::cout << "'2': toss a hybrid soft/rigid body" << std::endl; + std::cout << "'3': toss a rigid chain" << std::endl; + std::cout << "'4': toss a ring of rigid bodies" << std::endl; + + std::cout << "\n'd': delete the oldest object" << std::endl; + std::cout << "'r': toggle randomness" << std::endl; + + std::cout << "\nWarning: Let objects settle before tossing a new one, or the simulation could explode." << std::endl; + std::cout << " If the simulation freezes, you may need to force quit the application.\n" << std::endl; glutInit(&argc, argv); window.initWindow(640, 480, "Collisions"); From ad352870c497fd7bfb6fb8f90b85a40e85170e1c Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Tue, 30 Jun 2015 22:36:42 -0400 Subject: [PATCH 39/65] moved ShapePtr to the SmartPointers file --- dart/dynamics/Entity.h | 1 + dart/dynamics/Shape.h | 3 --- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/dart/dynamics/Entity.h b/dart/dynamics/Entity.h index 1b56b7a89d1bd..5d070b9038b68 100644 --- a/dart/dynamics/Entity.h +++ b/dart/dynamics/Entity.h @@ -44,6 +44,7 @@ #include "dart/common/Subject.h" #include "dart/common/Signal.h" #include "dart/dynamics/Shape.h" +#include "dart/dynamics/SmartPointer.h" namespace dart { namespace renderer { diff --git a/dart/dynamics/Shape.h b/dart/dynamics/Shape.h index 569b4cafae420..4d622cadc0b31 100644 --- a/dart/dynamics/Shape.h +++ b/dart/dynamics/Shape.h @@ -176,9 +176,6 @@ class Shape : public virtual common::Subject EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -typedef std::shared_ptr ShapePtr; -typedef std::shared_ptr ConstShapePtr; - } // namespace dynamics } // namespace dart From f11221558697a32702bd0ae78bed04c6098c21b1 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Tue, 30 Jun 2015 22:39:40 -0400 Subject: [PATCH 40/65] final touches to collisions tutorial --- tutorials/tutorialCollisions-Finished.cpp | 76 ++++++++++++----------- tutorials/tutorialCollisions.cpp | 31 ++++----- 2 files changed, 50 insertions(+), 57 deletions(-) diff --git a/tutorials/tutorialCollisions-Finished.cpp b/tutorials/tutorialCollisions-Finished.cpp index 7f1f49c8b0d7c..12538b58d27ca 100644 --- a/tutorials/tutorialCollisions-Finished.cpp +++ b/tutorials/tutorialCollisions-Finished.cpp @@ -88,8 +88,9 @@ void setupRing(const SkeletonPtr& ring) for(size_t i=1; i < ring->getNumJoints(); ++i) { Joint* joint = ring->getJoint(i); - Eigen::Vector3d restPos = BallJoint::convertToPositions(Eigen::Matrix3d( - Eigen::AngleAxisd(angle, Eigen::Vector3d(0, 1, 0)))); + Eigen::AngleAxisd rotation(angle, Eigen::Vector3d(0, 1, 0)); + Eigen::Vector3d restPos = BallJoint::convertToPositions( + Eigen::Matrix3d(rotation)); for(size_t j=0; j<3; ++j) joint->setRestPosition(j, restPos[j]); @@ -194,7 +195,7 @@ class MyWindow : public dart::gui::SimWindow // If randomization is on, we will randomize the starting y-location if(mRandomize) - positions[4] = default_spawn_range*mDistribution(mMT); + positions[4] = default_spawn_range * mDistribution(mMT); positions[5] = default_start_height; object->getJoint(0)->setPositions(positions); @@ -237,9 +238,6 @@ class MyWindow : public dart::gui::SimWindow centerTf.translation() = object->getCOM(); SimpleFrame center(Frame::World(), "center", centerTf); - SimpleFrame ref(¢er, "root"); - ref.setRelativeTransform(object->getBodyNode(0)->getTransform(¢er)); - // Set the velocities of the reference frames so that we can easily give the // Skeleton the linear and angular velocities that we want double angle = default_launch_angle; @@ -260,6 +258,9 @@ class MyWindow : public dart::gui::SimWindow Eigen::Vector3d w = angular_speed * Eigen::Vector3d::UnitY(); center.setClassicDerivatives(v, w); + SimpleFrame ref(¢er, "root_reference"); + ref.setRelativeTransform(object->getBodyNode(0)->getTransform(¢er)); + // Use the reference frames to set the velocity of the Skeleton's root object->getJoint(0)->setVelocities(ref.getSpatialVelocity()); @@ -280,9 +281,10 @@ class MyWindow : public dart::gui::SimWindow BodyNode* tail = ring->getBodyNode(ring->getNumBodyNodes()-1); // Compute the offset where the JointConstraint should be located - Eigen::Vector3d offset = Eigen::Vector3d(0, 0, -default_shape_height / 2.0); + Eigen::Vector3d offset = Eigen::Vector3d(0, 0, default_shape_height / 2.0); + offset = tail->getWorldTransform() * offset; auto constraint = new dart::constraint::BallJointConstraint( - head, tail, head->getWorldTransform() * offset); + head, tail, offset); mWorld->getConstraintSolver()->addConstraint(constraint); mJointConstraints.push_back(constraint); @@ -343,11 +345,6 @@ template BodyNode* addRigidBody(const SkeletonPtr& chain, const std::string& name, Shape::ShapeType type, BodyNode* parent = nullptr) { - // Compute the transform for the center of the object - Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); - Eigen::Vector3d center = Eigen::Vector3d(0, 0, default_shape_height / 2.0); - tf.translation() = center; - // Set the Joint properties typename JointType::Properties properties; properties.mName = name+"_joint"; @@ -355,6 +352,8 @@ BodyNode* addRigidBody(const SkeletonPtr& chain, const std::string& name, { // If the body has a parent, we should position the joint to be in the // middle of the centers of the two bodies + Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); + tf.translation() = Eigen::Vector3d(0, 0, default_shape_height / 2.0); properties.mT_ParentBodyToJoint = tf; properties.mT_ChildBodyToJoint = tf.inverse(); } @@ -364,7 +363,7 @@ BodyNode* addRigidBody(const SkeletonPtr& chain, const std::string& name, parent, properties, BodyNode::Properties(name)).second; // Make the shape based on the requested Shape type - std::shared_ptr shape; + ShapePtr shape; if(Shape::BOX == type) { shape = std::make_shared(Eigen::Vector3d( @@ -374,7 +373,7 @@ BodyNode* addRigidBody(const SkeletonPtr& chain, const std::string& name, } else if(Shape::CYLINDER == type) { - shape = std::make_shared(default_shape_width/2, + shape = std::make_shared(default_shape_width/2.0, default_shape_height); } else if(Shape::ELLIPSOID == type) @@ -387,11 +386,11 @@ BodyNode* addRigidBody(const SkeletonPtr& chain, const std::string& name, bn->addCollisionShape(shape); // Setup the inertia for the body - Inertia box_inertia; + Inertia inertia; double mass = default_shape_density * shape->getVolume(); - box_inertia.setMass(mass); - box_inertia.setMoment(shape->computeInertia(mass)); - bn->setInertia(box_inertia); + inertia.setMass(mass); + inertia.setMoment(shape->computeInertia(mass)); + bn->setInertia(inertia); // Set the coefficient of restitution to make the body more bouncy bn->setRestitutionCoeff(default_restitution); @@ -407,13 +406,10 @@ enum SoftShapeType { /// Add a soft body with the specified Joint type to a chain template -BodyNode* addSoftShape(const SkeletonPtr& chain, const std::string& name, - SoftShapeType type, BodyNode* parent = nullptr) +BodyNode* addSoftBody(const SkeletonPtr& chain, const std::string& name, + SoftShapeType type, BodyNode* parent = nullptr) { // Compute the transform for the center of the object - Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); - Eigen::Vector3d center = Eigen::Vector3d(0, 0, default_shape_height / 2.0); - tf.translation() = center; // Set the Joint properties typename JointType::Properties joint_properties; @@ -422,43 +418,49 @@ BodyNode* addSoftShape(const SkeletonPtr& chain, const std::string& name, { // If the body has a parent, we should position the joint to be in the // middle of the centers of the two bodies + Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); + tf.translation() = Eigen::Vector3d(0, 0, default_shape_height / 2.0); joint_properties.mT_ParentBodyToJoint = tf; joint_properties.mT_ChildBodyToJoint = tf.inverse(); } // Set the properties of the soft body - double soft_shape_width = 2*default_shape_width; SoftBodyNode::UniqueProperties soft_properties; // Use the SoftBodyNodeHelper class to create the geometries for the // SoftBodyNode if(SOFT_BOX == type) { - soft_properties = SoftBodyNodeHelper::makeBoxProperties(Eigen::Vector3d( - soft_shape_width, soft_shape_width, default_shape_height), - Eigen::Isometry3d::Identity(), Eigen::Vector3i(4,4,4), - default_shape_density*default_shape_height*pow(soft_shape_width,2)); + double width = default_shape_width, height = default_shape_height; + Eigen::Vector3d dims(width, width, height); + double mass = default_shape_density * dims[0]*dims[1]*dims[2]; + soft_properties = SoftBodyNodeHelper::makeBoxProperties( + dims, Eigen::Isometry3d::Identity(), Eigen::Vector3i(4,4,4), mass); } else if(SOFT_CYLINDER == type) { + double radius = default_shape_width/2.0; + double height = default_shape_height; + double mass = default_shape_density * height * pow(M_PI*radius, 2); soft_properties = SoftBodyNodeHelper::makeCylinderProperties( - soft_shape_width/2.0, default_shape_height, 8, 3, 2, - default_shape_density * default_shape_height - * pow(M_PI*soft_shape_width/2.0, 2)); + radius, height, 8, 3, 2, mass); } else if(SOFT_ELLIPSOID == type) { + double radius = default_shape_height/2.0; + Eigen::Vector3d dims = 2*radius*Eigen::Vector3d::Ones(); + double mass = default_shape_density * 4.0/3.0*M_PI*pow(radius,3); soft_properties = SoftBodyNodeHelper::makeEllipsoidProperties( - default_shape_height*Eigen::Vector3d::Ones(), 6, 6, - default_shape_density * 4.0/3.0*M_PI*pow(default_shape_height/2,3)); + dims, 6, 6, mass); } soft_properties.mKv = default_vertex_stiffness; soft_properties.mKe = default_edge_stiffness; soft_properties.mDampCoeff = default_soft_damping; // Create the Joint and Body pair + SoftBodyNode::Properties body_properties(BodyNode::Properties(name), + soft_properties); SoftBodyNode* bn = chain->createJointAndBodyNodePair( - parent, joint_properties, SoftBodyNode::Properties( - BodyNode::Properties(name), soft_properties)).second; + parent, joint_properties, body_properties).second; return bn; } @@ -522,7 +524,7 @@ SkeletonPtr createHybridBody() SkeletonPtr hybrid = Skeleton::create("hybrid"); // Add a soft body - BodyNode* bn = addSoftShape(hybrid, "soft sphere", SOFT_ELLIPSOID); + BodyNode* bn = addSoftBody(hybrid, "soft sphere", SOFT_ELLIPSOID); // Add a rigid body attached by a WeldJoint bn = hybrid->createJointAndBodyNodePair(bn).second; diff --git a/tutorials/tutorialCollisions.cpp b/tutorials/tutorialCollisions.cpp index 7eb27ebf37a9b..104cb7575d2bd 100644 --- a/tutorials/tutorialCollisions.cpp +++ b/tutorials/tutorialCollisions.cpp @@ -207,9 +207,6 @@ class MyWindow : public dart::gui::SimWindow // Create a closed loop to turn the chain into a ring // Lesson 5 - - // Compute the offset where the JointConstraint should be located - // Lesson 5 } /// Remove a Skeleton and get rid of the constraint that was associated with @@ -267,24 +264,21 @@ template BodyNode* addRigidBody(const SkeletonPtr& /*chain*/, const std::string& /*name*/, Shape::ShapeType /*type*/, BodyNode* /*parent*/ = nullptr) { - // Compute the transform for the center of the object - // Lesson 1a - // Set the Joint properties - // Lesson 1b + // Lesson 1a // Create the Joint and Body pair - // Lesson 1c + // Lesson 1b BodyNode* bn = nullptr; // Make the shape based on the requested Shape type - // Lesson 1d + // Lesson 1c // Setup the inertia for the body - // Lesson 1e + // Lesson 1d // Set the coefficient of restitution to make the body more bouncy - // Lesson 1f + // Lesson 1e return bn; } @@ -297,20 +291,17 @@ enum SoftShapeType { /// Add a soft body with the specified Joint type to a chain template -BodyNode* addSoftShape(const SkeletonPtr& /*chain*/, const std::string& /*name*/, - SoftShapeType /*type*/, BodyNode* /*parent*/ = nullptr) +BodyNode* addSoftBody(const SkeletonPtr& /*chain*/, const std::string& /*name*/, + SoftShapeType /*type*/, BodyNode* /*parent*/ = nullptr) { - // Compute the transform for the center of the object - // Lesson 2a - // Set the Joint properties - // Lesson 2b + // Lesson 2a // Set the properties of the soft body - // Lesson 2c + // Lesson 2b // Create the Joint and Body pair - // Lesson 2d + // Lesson 2c SoftBodyNode* bn = nullptr; return bn; @@ -375,7 +366,7 @@ SkeletonPtr createHybridBody() SkeletonPtr hybrid = Skeleton::create("hybrid"); // Add a soft body - /*BodyNode* bn =*/ addSoftShape(hybrid, "soft sphere", SOFT_ELLIPSOID); + /*BodyNode* bn =*/ addSoftBody(hybrid, "soft sphere", SOFT_ELLIPSOID); // Add a rigid body attached by a WeldJoint // Lesson 2e From d464123e28c1517156cadacdc36bb95f9abb48c6 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Tue, 30 Jun 2015 22:40:04 -0400 Subject: [PATCH 41/65] created documentation for collisions tutorial --- docs/readthedocs/tutorials/collisions.md | 708 +++++++++++++++++++++++ mkdocs.yml | 3 +- 2 files changed, 710 insertions(+), 1 deletion(-) create mode 100644 docs/readthedocs/tutorials/collisions.md diff --git a/docs/readthedocs/tutorials/collisions.md b/docs/readthedocs/tutorials/collisions.md new file mode 100644 index 0000000000000..cef56c9ce8d24 --- /dev/null +++ b/docs/readthedocs/tutorials/collisions.md @@ -0,0 +1,708 @@ +# Overview +This tutorial will show you how to programmatically create different kinds of +bodies and set initial conditions for Skeletons. It will also demonstrate some +use of DART's Frame Semantics. + +The tutorial consists of five Lessons covering the following topics: + +- Creating a rigid body +- Creating a soft body +- Setting initial conditions and taking advantage of Frames +- Setting joint spring and damping properties +- Creating a closed kinematic chain + +# Lesson 1: Creating a rigid body + +Start by going opening the Skeleton code [tutorialCollisions.cpp](http://). +Find the function named ``addRigidBody``. You will notice that this is a templated +function. If you're not familiar with templates, that's okay; we won't be doing +anything too complicated with them. Different Joint types in DART are managed by +a bunch of different classes, so we need to use templates if we want the same +function to work with a variety of Joint types. + +### Lesson 1a: Setting joint properties + +The first thing we'll want to do is set the Joint properties for our new body. +Whenever we create a BodyNode, we must also create a parent Joint for it. A +BodyNode needs a parent Joint, even if that BodyNode is the root of the Skeleton, +because we need its parent Joint to describe how it's attached to the world. A +root BodyNode could be attached to the world by any kind of Joint. Most often, +it will be attached by either a FreeJoint (if the body should be completely +free to move with respect to the world) or a WeldJoint (if the body should be +rigidly attached to the world, unable to move at all), but *any* Joint type +is permissible. + +Joint properties are managed in a nested class, which means it's a class which +is defined inside of another class. For example, ``RevoluteJoint`` properties are +managed in a class called ``RevoluteJoint::Properties`` while ``PrismaticJoint`` +properties are managed in a class called ``PrismaticJoint::Properties``. However, +both ``RevoluteJoint`` and ``PrismaticJoint`` inherit the ``SingleDofJoint`` class +so the ``RevoluteJoint::Properties`` and ``PrismaticJoint::Properties`` classes +both inherit the ``SingleDofJoint::Properties`` class. The difference is that +``RevoluteJoint::Properties`` also inherits ``RevoluteJoint::UniqueProperties`` +whereas ``PrismaticJoint::Properties`` inherits ``PrismaticJoint::UniqueProperties`` +instead. Many DART classes contain nested ``Properties`` classes like this which +are compositions of their base class's nested ``Properties`` class and their own +``UniqueProperties`` class. As you'll see later, this is useful for providing a +consistent API that works cleanly for fundamentally different types of classes. + +To create a ``Properties`` class for our Joint type, we'll want to say +```cpp +typename JointType::Properties properties; +``` + +We need to include the ``typename`` keywords because of how the syntax works for +templated functions. Leaving it out should make your compiler complain. + +From here, we can set the Joint properties in any way we'd like. There are only +a few things we care about right now: First, the Joint's name. Every Joint in a +Skeleton needs to have a non-empty unique name. Those are the only restrictions +that are placed on Joint names. If you try to make a Joint's name empty, it will +be given a default name. If you try to make a Joint's name non-unique, DART will +append a number tag to the end of the name in order to make it unique. It will +also print out a warning during run time, which can be an eyesore (because it +wants you to be aware when you are being negligent about naming things). For the +sake of simplicity, let's just give it a name based off its child BodyNode: + +```cpp +properties.mName = name+"_joint"; +``` + +Don't forget to uncomment the function arguments. + +Next we'll want to deal with offsetting the new BodyNode from its parent BodyNode. +We can use the following to check if there is a parent BodyNode: + +```cpp +if(parent) +{ + // TODO: offset the child from its parent +} +``` + +Inside the brackets, we'll want to create the offset between bodies: + +```cpp +Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); +``` + +An ``Eigen::Isometry3d`` is the Eigen library's version of a homogeneous +transformation matrix. Here we are initializing it to an Identity matrix to +start out. This is almost always something you should do when creating an +Eigen::Isometry3d, because otherwise its contents will be completely arbitrary +trash. + +We can easily compute the center point between the origins of the two bodies +using our default height value: + +```cpp +tf.translation() = Eigen::Vector3d(0, 0, default_shape_height / 2.0); +``` + +We can then offset the parent and child BodyNodes of this Joint using this +transform: + +```cpp +properties.mT_ParentBodyToJoint = tf; +properties.mT_ChildBodyToJoint = tf.inverse(); +``` + +Remember that all of that code should go inside the ``if(parent)`` condition. +We do not want to create this offset for root BodyNodes, because later on we +will rely on the assumption that the root Joint origin is lined up with the +root BodyNode origin. + +### Lesson 1b: Create a Joint and BodyNode pair + +A single function is used to simultaneously create a new Joint and its child +BodyNode. It's important to note that a Joint cannot be created without a +child BodyNode to accompany it, and a BodyNode cannot be created with parent +Joint to attach it to something. A parent Joint without a child BodyNode or +vice-versa would be non-physical and nonsensical, so we don't allow it. + +Use the following to create a new Joint & BodyNode, and obtain a pointer to +that new BodyNode: + +```cpp +BodyNode* bn = chain->createJointAndBodyNodePair( + parent, properties, BodyNode::Properties(name)).second; +``` + +There's a lot going on in this function, so let's break it down for a moment: + +```cpp +chain->createJointAndBodyNodePair +``` + +This is a Skeleton member function that takes template arguments. The first +template argument specifies the type of Joint that you want to create. In our +case, the type of Joint we want to create is actually a template argument of +our current function, so we just pass that argument along. The second template +argument of ``createJointAndBodyNodePair`` allows us to specify the BodyNode +type that we want to create, but the default argument is a standard rigid +BodyNode, so we can leave the second argument blank. + +```cpp +(parent, properties, BodyNode::Properties(name)) +``` + +Now for the function arguments: The first specifies the parent BodyNode. In the +event that you want to create a root BodyNode, you can simply pass in a nullptr +as the parent. The second argument is a ``JointType::Properties`` struct, so we +pass in the ``properties`` object that we created earlier. The third argument is +a ``BodyNode::Properties`` struct, but we're going to set the BodyNode properties +later, so we'll just toss the name in and leave the rest as default values. + +Now notice the very last thing on this line of code: + +```cpp +.second; +``` + +The function actually returns a ``std::pair`` of pointers to the new Joint and +new BodyNode that were just created, but we only care about grabbing the +BodyNode once the function is finished, so we can append ``.second`` to the end +of the line so that we just grab the BodyNode pointer and ignore the Joint +pointer. The joint will of course still be created; we just have no need to +access it at this point. + +### Lesson 1c: Make a shape for the body + +We'll take advantage of the Shape::ShapeType enumeration to specify what kind of +Shape we want to produce for the body. In particular, we'll allow the user to +specify three types of Shapes: ``Shape::BOX``, ``Shape::CYLINDER``, and +``Shape::ELLIPSOID``. + +```cpp +ShapePtr shape; +if(Shape::BOX == type) +{ + // TODO: Make a box +} +else if(Shape::CYLINDER == type) +{ + // TODO: Make a cylinder +} +else if(SHAPE::ELLIPSOID == type) +{ + // TODO: Make an ellipsoid +} +``` + +``ShapePtr`` is simply a typedef for ``std::shared_ptr``. DART has this +typedef in order to improve space usage and readability, because this type gets +used very often. + +Now we want to construct each of the Shape types within their conditional +statements. Each constructor is a bit different. + +For box we pass in an Eigen::Vector3d that contains the three dimensions of the box: + +```cpp +shape = std::make_shared(Eigen::Vector3d( + default_shape_width, + default_shape_width, + default_shape_height)); +``` + +For cylinder we pass in a radius and a height: + +```cpp +shape = std::make_shared(default_shape_width/2.0, + default_shape_height); +``` + +For ellipsoid we pass in an Eigen::Vector3d that contains the lengths of the three axes: + +```cpp +shape = std::make_shared( + default_shape_height*Eigen::Vector3d::Ones()); +``` + +Since we actually want a sphere, all three axis lengths will be equal, so we can +create an Eigen::Vector3d filled with ones by using ``Eigen::Vector3d::Ones()`` +and then multiply it by the length that we actually want for the three components. + +Finally, we want to add this shape as a visualization **and** collision shape for +the BodyNode: + +```cpp +bn->addVisualizationShape(shape); +bn->addCollisionShape(shape); +``` + +We want to do this no matter which type was selected, so those two lines of code +should be after all the condition statements. + +### Lesson 1d: Set up the inertia properties for the body + +For the simulations to be physically accurate, it's important for the inertia +properties of the body to match up with the geometric properties of the shape. +We can create an ``Inertia`` object and set its values based on the shape's +geometry, then give that ``Inertia`` to the BodyNode. + +```cpp +Inertia inertia; +double mass = default_shape_density * shape->getVolume(); +inertia.setMass(mass); +inertia.setMoment(shape->computeInertia(mass)); +bn->setInertia(inertia); +``` + +### Lesson 1e: Set the coefficient of restitution + +This is very easily done with the following function: + +```cpp +bn->setRestitutionCoeff(default_restitution); +``` + +# Lesson 2: Creating a soft body + +Find the templated function named ``addSoftBody``. This function will have a +role identical to the ``addRigidBody`` function from earlier. + +### Lesson 2a: Set the Joint properties + +This portion is exactly the same as Lesson 1a. You can even copy the code +directly from there if you'd like to. + +### Lesson 2b: Set the properties of the soft body + +Last time we set the BodyNode properties after creating it, but this time +we'll set them beforehand. + +First, let's create a struct for the properties that are unique to SoftBodyNodes: + +```cpp +SoftBodyNode::UniqueProperties soft_properties; +``` + +Later we will combine this with a standard ``BodyNode::Properties`` struct, but +for now let's fill it in. Up above we defined an enumeration for a couple +different SoftBodyNode types. There is no official DART-native enumeration +for this, we created our own to use for this function. We'll want to fill in +the ``SoftBodyNode::UniqueProperties`` struct based off of this enumeration: + +```cpp +if(SOFT_BOX == type) +{ + // TODO: make a soft box +} +else if(SOFT_CYLINDER == type) +{ + // TODO: make a soft cylinder +} +else if(SOFT_ELLIPSOID == type) +{ + // TODO: make a soft ellipsoid +} +``` + +Each of these types has a static function in the ``SoftBodyNodeHelper`` class +that will set up your ``UniqueProperties`` for you. The arguments for each of +the functions are a bit complicated, so here is how to call it for each type: + +For the SOFT_BOX: +```cpp +double width = default_shape_width, height = default_shape_height; +Eigen::Vector3d dims(width, width, height); +double mass = default_shape_density * dims[0]*dims[1]*dims[2]; +soft_properties = SoftBodyNodeHelper::makeBoxProperties( + dims, Eigen::Isometry3d::Identity(), Eigen::Vector3i(4,4,4), mass); +``` + +For the SOFT_CYLINDER: +```cpp +double radius = default_shape_width/2.0; +double height = default_shape_height; +double mass = default_shape_density * height * pow(M_PI*radius, 2); +soft_properties = SoftBodyNodeHelper::makeCylinderProperties( + radius, height, 8, 3, 2, mass); +``` + +And for the SOFT_ELLIPSOID: +```cpp +double radius = default_shape_height/2.0; +Eigen::Vector3d dims = 2*radius*Eigen::Vector3d::Ones(); +double mass = default_shape_density * 4.0/3.0*M_PI*pow(radius,3); +soft_properties = SoftBodyNodeHelper::makeEllipsoidProperties( + dims, 6, 6, mass); +``` + +Feel free to play around with the different parameters, like number of slices +and number of stacks. However, be aware that some of those parameters have a +minimum value, usually of 2 or 3. During runtime, you should be warned if you +try to create one with a parameter that's too small. + +Lastly, we'll want to fill in the softness coefficients: + +```cpp +soft_properties.mKv = default_vertex_stiffness; +soft_properties.mKe = default_edge_stiffness; +soft_properties.mDampCoeff = default_soft_damping; +``` + +### Lesson 2c: Create the Joint and Soft Body pair + +This step is very similar to Lesson 1b, except now we'll want to specify +that we're creating a soft BodyNode. First, let's create a full +``SoftBodyNode::Properties``: + +```cpp +SoftBodyNode::Properties body_properties(BodyNode::Properties(name), + soft_properties); +``` + +This will combine the ``UniqueProperties`` of the SoftBodyNode with the +standard properties of a BodyNode. Now we can pass the whole thing into +the creation function: + +```cpp +SoftBodyNode* bn = chain->createJointAndBodyNodePair( + parent, joint_properties, body_properties).second; +``` + +Notice that this time it will return a ``SoftBodyNode`` pointer rather than a +normal ``BodyNode`` pointer. This is one of the advantages of templates! + +The ``SoftBodyNodeHelper`` function will automagically set up the inertia and shape +properties of the SoftBodyNode ahead of time, so we're done in this function. + + +# Lesson 3: Setting initial conditions and taking advantage of Frames + +Find the ``addObject`` function in the ``MyWorld`` class. This function will +be called whenever the user requests for an object to be added to the world. +In this function, we want to set up the initial conditions for the object so +that it gets thrown at the wall. We also want to make sure that it's not in +collision with anything at the time that it's added, because that would result +in problems for the simulation. + +### Lesson 3a: Set the starting position for the object + +We want to position the object in a reasonable place for us to throw it at the +wall. We also want to have the ability to randomize its location along the y-axis. + +First, let's create a zero vector for the position: +```cpp +Eigen::Vector6d positions(Eigen::Vector6d::Zero()); +``` + +You'll notice that this is an Eigen::Vector**6**d rather than the usual +Eigen::Vector**3**d. This vector has six components because the root BodyNode +has 6 degrees of freedom: three for orientation and three for translation. +Because we follow Roy Featherstone's Spatial Vector convention, the **first** +three components are for **orientation** using a logmap (also known as angle-axis) +and the **last** three components are for **translation**. + +First, if randomness is turned on, we'll set the y-translation to a randomized +value: + +```cpp +if(mRandomize) + positions[4] = default_spawn_range * mDistribution(mMT); +``` + +``mDistribution(mMT)`` will generate a random value in the range \[-1, 1\] +inclusive because of how we initialized the classes in the constructor of +``MyWindow``. + +Then we always set the height to the default value: +```cpp +positions[5] = default_start_height; +``` + +Finally, we use this vector to set the positions of the root Joint: +```cpp +object->getJoint(0)->setPositions(positions); +``` + +We trust that the root Joint is a FreeJoint with 6 degrees of freedom because +of how we constructed all the objects that are going to be thrown at the wall: +They were all given a FreeJoint between the world and the root BodyNode. + +### Lesson 3b: Add the object to the world + +Every object in the world is required to have a non-empty unique name. Just like +Joint names in a Skeleton, if we pass a Skeleton into a world with a non-unique +name, the world will print out a complaint to us and then rename it. So avoid the +ugly printout, we'll make sure the new object has a unique name ahead of time: + +```cpp +object->setName(object->getName()+std::to_string(mSkelCount++)); +``` + +And now we can add it to the world without any complaints: +```cpp +mWorld->addSkeleton(object); +``` + +### Lesson 3d: Compute collisions + +Now that we've added the Skeleton to the world, we want to make sure that it +wasn't actually placed inside of something accidentally. If an object in a +simulation starts off inside of another object, it can result in extremely +non-physical simulations, perhaps even breaking the simulation entirely. +We can access the world's collision detector directly to check make sure the +new object is collision-free: + +```cpp +dart::collision::CollisionDetector* detector = + mWorld->getConstraintSolver()->getCollisionDetector(); +detector->detectCollision(true, true); +``` + +Now we shouldn't be surprised if the *other* objects are in collision with each +other, so we'll want to look through the list of collisions and check whether +any of them are the new Skeleton: + +```cpp +bool collision = false; +size_t collisionCount = detector->getNumContacts(); +for(size_t i = 0; i < collisionCount; ++i) +{ + const dart::collision::Contact& contact = detector->getContact(i); + if(contact.bodyNode1.lock()->getSkeleton() == object + || contact.bodyNode2.lock()->getSkeleton() == object) + { + collision = true; + break; + } +} +``` + +If the new Skeleton *was* in collision with anything, we'll want to remove it +from the world and abandon our attempt to add it: + +```cpp +if(collision) +{ + mWorld->removeSkeleton(object); + std::cout << "The new object spawned in a collision. " + << "It will not be added to the world." << std::endl; + return false; +} +``` + +Of course we should also print out a message so that user understands why we +didn't throw a new object. + +### Lesson 3d: Creating reference frames + +DART has a unique feature that we call Frame Semantics. The Frame Semantics of +DART allow you to create reference frames and use them to get and set data +relative to arbitrary frames. There are two crucial Frame types currently used +in DART: ``BodyNode``s and ``SimpleFrame``s. + +The BodyNode class does not allow you to explicitly set its transform, velocity, +or acceleration properties, because those are all strictly functions of the +degrees of freedom that the BodyNode depends on. Because of this, the BodyNode +is not a very convenient class if you want to create an arbitrary frame of +reference. Instead, DART offers the ``SimpleFrame`` class which gives you the +freedom of arbitarily attaching it to any parent Frame and setting its transform, +velocity, and acceleration to whatever you'd like. This makes SimpleFrame useful +for specifying arbitrary reference frames. + +We're going to set up a couple SimpleFrames and use them to easily specify the +velocity properties that we want the Skeleton to have. First, we'll place a +SimpleFrame at the Skeleton's center of mass: + +```cpp +Eigen::Isometry3d centerTf(Eigen::Isometry3d::Identity()); +centerTf.translation() = object->getCOM(); +SimpleFrame center(Frame::World(), "center", centerTf); +``` + +Calling ``object->getCOM()`` will tell us the center of mass location with +respect to the World Frame. We use that to set the translation of the +SimpleFrame's relative transform so that the origin of the SimpleFrame will be +located at the object's center of mass. + +Now we'll set what we want the object's angular and linear speeds to be: + +```cpp +double angle = default_launch_angle; +double speed = default_start_v; +double angular_speed = default_start_w; +if(mRandomize) +{ + angle = (mDistribution(mMT) + 1.0)/2.0 * + (maximum_launch_angle - minimum_launch_angle) + minimum_launch_angle; + + speed = (mDistribution(mMT) + 1.0)/2.0 * + (maximum_start_v - minimum_start_v) + minimum_start_v; + + angular_speed = mDistribution(mMT) * maximum_start_w; +} +``` + +We just use the default values unless randomization is turned on. + +Now we'll convert those speeds into directional velocities: + +```cpp +Eigen::Vector3d v = speed * Eigen::Vector3d(cos(angle), 0.0, sin(angle)); +Eigen::Vector3d w = angular_speed * Eigen::Vector3d::UnitY(); +``` + +And now we'll use those vectors to set the velocity properties of the SimpleFrame: + +```cpp +center.setClassicDerivatives(v, w); +``` + +The ``SimpleFrame::setClassicDerivatives()`` allows you to set the classic linear +and angular velocities and accelerations of a SimpleFrame with respect to its +parent Frame, which in this case is the World Frame. In DART, classic velocity and +acceleration vectors are explicitly differentiated from spatial velocity and +acceleration vectors. If you are unfamiliar with the term "spatial vector", then +you'll most likely want to work in terms of classic velocity and acceleration. + +Now we want to create a new SimpleFrame that will be a child of the previous one: + +```cpp +SimpleFrame ref(¢er, "root_reference"); +``` + +And we want the origin of this new Frame to line up with the root BodyNode of +our object: + +```cpp +ref.setRelativeTransform(object->getBodyNode(0)->getTransform(¢er)); +``` + +Now we'll use this reference frame to set the velocity of the root BodyNode. +By setting the velocity of the root BodyNode equal to the velocity of this +reference frame, we will ensure that the overall velocity of Skeleton's center +of mass is equal to the velocity of the ``center`` Frame from earlier. + +```cpp +object->getJoint(0)->setVelocities(ref.getSpatialVelocity()); +``` + +Note that the FreeJoint uses spatial velocity and spatial acceleration for its +degrees of freedom. + +Now we're ready to toss around objects! + +# Lesson 4: Setting joint spring and damping properties + +Find the ``setupRing`` function. This is where we'll setup a chain of BodyNodes +so that it behaves more like a closed ring. + +### Lesson 4a: Set the spring and damping coefficients + +We'll want to set the stiffness and damping coefficients of only the +DegreesOfFreedom that are **between** two consecutive BodyNodes. The first +six degrees of freedom are between the root BodyNode and the World, so we don't +want to change the stiffness of them, or else the object will hover unnaturally +in the air. But all the rest of the degrees of freedom should be set: + +```cpp +for(size_t i=6; i < ring->getNumDofs(); ++i) +{ + DegreeOfFreedom* dof = ring->getDof(i); + dof->setSpringStiffness(default_spring_stiffness); + dof->setDampingCoefficient(default_damping_coefficient); +} +``` + +### Lesson 4b: Set the rest positions of the joints + +We want to make sure that the ring's rest position works well for the structure +it has. Using basic geometry, we know we can compute the exterior angle on each +edge of a polygon like so: + +```cpp +size_t numEdges = ring->getNumBodyNodes(); +double angle = 2*M_PI/numEdges; +``` + +Now it's important to remember that the joints we have between the BodyNodes are +BallJoints, which use logmaps (a.k.a. angle-axis) to represent their positions. +The BallJoint class provides a convenience function for converting rotations +into a position vector for a BallJoint. A similar function also exists for +EulerJoint and FreeJoint. + +```cpp +for(size_t i=1; i < ring->getNumJoints(); ++i) +{ + Joint* joint = ring->getJoint(i); + Eigen::AngleAxisd rotation(angle, Eigen::Vector3d(0, 1, 0)); + Eigen::Vector3d restPos = BallJoint::convertToPositions( + Eigen::Matrix3d(rotation)); + + // TODO: Set the rest position +} +``` + +Now we can set the rest positions component-wise: + +```cpp + for(size_t j=0; j<3; ++j) + joint->setRestPosition(j, restPos[j]); +``` + +### Lesson 4c: Set the Joints to be in their rest positions + +Finally, we should set the ring so that all the degrees of freedom (past the +root BodyNode) start out in their rest positions: + +```cpp +for(size_t i=6; i < ring->getNumDofs(); ++i) +{ + DegreeOfFreedom* dof = ring->getDof(i); + dof->setPosition(dof->getRestPosition()); +} +``` + +# Lesson 5: Create a closed kinematic chain + +Find the ``addRing`` function in ``MyWindow``. In here, we'll want to create a +dynamic constraint that attaches the first and last BodyNodes of the chain +together by a BallJoint-style constraint. + +First we'll grab the BodyNodes that we care about: + +```cpp +BodyNode* head = ring->getBodyNode(0); +BodyNode* tail = ring->getBodyNode(ring->getNumBodyNodes()-1); +``` + +Now we want to compute the offset where the BallJoint constraint should be located: + +```cpp +Eigen::Vector3d offset = Eigen::Vector3d(0, 0, default_shape_height / 2.0); +offset = tail->getWorldTransform() * offset; +``` + +The offset will be located half the default height up from the center of the +tail BodyNode. + +Now we have everything we need to construct the constraint: + +```cpp +auto constraint = new dart::constraint::BallJointConstraint( + head, tail, offset); +``` + +In order for the constraint to work, we'll need to add it to the world's +constraint solver: + +```cpp +mWorld->getConstraintSolver()->addConstraint(constraint); +``` + +And in order to properly clean up the constraint when removing BodyNodes, we'll +want to add it to our list of constraints: + +```cpp +mJointConstraints.push_back(constraint); +``` + +And that's it! You're ready to run the full tutorialCollisions application! + +**When running the application, keep in mind that the dynamics of collisions are +finnicky, so you may see some unstable and even completely non-physical behavior. +If the application freezes, you may need to force quit out of it.** diff --git a/mkdocs.yml b/mkdocs.yml index e41608ab478cc..98e730d62fff4 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -16,5 +16,6 @@ pages: - Tutorials: - 'Introduction': 'tutorials/introduction.md' - 'Pendulum': 'tutorials/multi-pendulum.md' + - 'Collisions' : 'tutorials/collisions.md' - 'Manipulator': 'tutorials/dominoes.md' - - 'Biped': 'tutorials/biped.md' \ No newline at end of file + - 'Biped': 'tutorials/biped.md' From 3332edd732124a1a269ddddac6eca7b2c63ea3c0 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Wed, 1 Jul 2015 01:30:10 -0400 Subject: [PATCH 42/65] including smart pointers in Shape.h --- dart/dynamics/Shape.h | 1 + 1 file changed, 1 insertion(+) diff --git a/dart/dynamics/Shape.h b/dart/dynamics/Shape.h index 4d622cadc0b31..971063976ed6a 100644 --- a/dart/dynamics/Shape.h +++ b/dart/dynamics/Shape.h @@ -44,6 +44,7 @@ #include "dart/math/Geometry.h" #include "dart/common/Subject.h" +#include "dart/dynamics/SmartPointer.h" namespace dart { namespace renderer { From d1911c1605bb4833e88fdcb6ca5700ee4ec1d4d8 Mon Sep 17 00:00:00 2001 From: Karen Liu Date: Wed, 1 Jul 2015 11:43:31 -0400 Subject: [PATCH 43/65] finished biped tuotial document; modified tutorialBiped-Finished.cpp --- docs/readthedocs/tutorials/biped.md | 471 ++++++++++++++---- .../tutorials/biped/IKObjective.png | Bin 0 -> 15541 bytes mkdocs.yml | 1 - tutorials/tutorialBiped-Finished.cpp | 60 ++- 4 files changed, 415 insertions(+), 117 deletions(-) create mode 100644 docs/readthedocs/tutorials/biped/IKObjective.png diff --git a/docs/readthedocs/tutorials/biped.md b/docs/readthedocs/tutorials/biped.md index c2a1e1ae027b8..ebc8b941d6a26 100644 --- a/docs/readthedocs/tutorials/biped.md +++ b/docs/readthedocs/tutorials/biped.md @@ -1,11 +1,8 @@ # Overview -This tutorial demonstrates the dynamic features in DART to facilitate -the development of controllers for bipedal and wheel-based robots. +This tutorial demonstrates the dynamic features in DART useful for +developing controllers for bipedal or wheel-based robots. The tutorial +consists of seven Lessons covering the following topics: -The tutorial consists of seven Lessons covering the following -topics: - -- SKEL file format. - Joint limits and self-collision. - Actuators types and management. - APIs for Jacobian matrices and other kinematic quantities. @@ -13,77 +10,93 @@ topics: - Skeleton editing. # Lesson 1: Joint limits and self-collision - -Let's start by opening the skeleton code -[tutorialBiped.cpp](http://). After creating a floor in -[main()](http://), [loadBiped()](http://) is called and a bipedal figure described -in SKEL format is loaded. Each SKEL file describes a -[World](http://dartsim.github.io/dart/d7/d41/classdart_1_1simulation_1_1World.html). Here +Let's start by locating the [main()](http://) function in +[tutorialBiped.cpp](http://). We first create a floor and call +[loadBiped()](http://) to load a +bipedal figure described in SKEL format, which is an XML format +representing a robot model. A SKEL file describes a +[World](http://dartsim.github.io/dart/d7/d41/classdart_1_1simulation_1_1World.html) +with one or more [Skeleton](http://)s in it. Here we load in a World from biped.skel and assign the bipedal figure to a [Skeleton](http://dartsim.github.io/dart/d3/d19/classdart_1_1dynamics_1_1Skeleton.html) pointer called *biped*. - +```cpp +SkeletonPtr loadBiped() +{ +... WorldPtr world = SkelParser::readWorld(DART_DATA_PATH"skel/biped.skel"); - assert(world != nullptr); - SkeletonPtr biped = world->getSkeleton("biped"); +... +} +``` -Running the skeleton code without any modification, you should see a -human-like character dropping on the ground and collapsing into -itself. Therefore, the first task in this Lesson is to enforce more realistic -joint limits. +Running the skeleton code without any modification (hit the spacebar), you should see a +human-like character collapse on the ground and fold in on +itself. Before we attempt to control the biped, let's first make the +biped a bit more realistic by enforcing more human-like joint limits. DART allows the user to set upper and lower bounds on each degree of -freedom in the SKEL file or using provided APIs. In biped.skel, you -should see the description of the right knee joint in this format: - - - ... - - 0.0 0.0 1.0 - +freedom in the SKEL file or using provided APIs. For example, you +should see the description of the right knee joint in biped.skel: + +```cpp + +... + + 0.0 0.0 1.0 + -3.14 0.0 - - - ... - - + + +... + +``` The <upper> and <lower> tags make sure that the knee can only flex but -not extend in one direction. Alternatively, you can directly specify the joint limits +not extend. Alternatively, you can directly specify the joint limits in the code using [setPositionUpperLimit](http://dartsim.github.io/dart/d6/d5b/classdart_1_1dynamics_1_1Joint.html#aa0635643a0a8c1f22edb8243e86ea801) and [setPositionLowerLimit](http://dartsim.github.io/dart/d6/d5b/classdart_1_1dynamics_1_1Joint.html#adadee231309b62cd3e3d904f75f2a969). In either case, the joint limits on the biped will not be activated until you call [setPositionLimited](http://dartsim.github.io/dart/d6/d5b/classdart_1_1dynamics_1_1Joint.html#a3212ca5f7893cfd9a5422ab17df4038b) - +```cpp +SkeletonPtr loadBiped() +{ +... for(size_t i = 0; i < biped->getNumJoints(); ++i) biped->getJoint(i)->setPositionLimited(true); - +... +} +``` Once the joint limits are set, the next task is to enforce self-collision. By default, DART does not check self-collision within a skeleton. You can enable self-collision checking on the biped by - +```cpp +SkeletonPtr loadBiped() +{ +... biped->enableSelfCollision(); - +... +} +``` This function will enable self-collision on every non-adjacent pair of body nodes. If you wish to also enable self-collision on adjacent body nodes, set the optional parameter to true: - - biped->enableSelfCollision(true); - +```cpp +biped->enableSelfCollision(true); +``` Running the program again, you should see that the character is still floppy like a ragdoll, but now the joints do not bend backward and the body nodes do not penetrate each other anymore. -# Lesson 2: Proportional-derivative (PD) control +# Lesson 2: Proportional-derivative control To actively control its own motion, the biped must exert internal forces using actuators. In this Lesson, we will design one of the simplest controllers to produce internal forces that make the biped -hold a target pose. The proportional-derivative (PD) control has -the form of Τ = -kp (θ - +hold a target pose. The proportional-derivative (PD) control computes +control force by Τ = -kp (θ - θtarget) - kd θ̇, where θ and θ̇ are the current position and velocity of a degree of freedom, θtarget is the target position set by the @@ -91,62 +104,75 @@ controller, and kp and kd are the stiffness and damping coefficients. The detailed description of a PD controller can be found [here](https://en.wikipedia.org/wiki/PID_controller). -The first task is to set the biped to a desired pose in -[setInitialPose()](htt[:\\). To set -each degree of freedom individually, you can use [setPosition](http://dartsim.github.io/dart/d3/d19/classdart_1_1dynamics_1_1Skeleton.html#ac2036ea4998f688173d19ace0edab841): - - biped->setPosition(biped->getDof("j_thigh_left_z")->getIndexInSkeleton(), 0.15); - +The first task is to set the biped to a particular configuration. You +can use +[setPosition](http://dartsim.github.io/dart/d3/d19/classdart_1_1dynamics_1_1Skeleton.html#ac2036ea4998f688173d19ace0edab841) +to set each degree of freedom individually: +```cpp +void setInitialPose(SkeletonPtr biped) +{ +... + biped->setPosition(biped->getDof("j_thigh_left_z")->getIndexInSkeleton(), 0.15); +... +} +``` Here the degree of freedom named "j_thigh_left_z" is set to 0.15 radian. Note that each degree of freedom in a skeleton has a numerical -index which can be obtained by +index which can be accessed by [getIndexInSkeleton](http://dartsim.github.io/dart/de/db7/classdart_1_1dynamics_1_1DegreeOfFreedom.html#add2ec1d2f979e9056b466b1be5ee1a86). You can also set the entire configuration using a vector that holds the positions of all the degreed of freedoms using [setPositions](http://dartsim.github.io/dart/d3/d19/classdart_1_1dynamics_1_1Skeleton.html#aee6d1a2be46c277602fae2f1d47762ef). -Let's set more degrees of freedoms in the lower +You can set more degrees of freedoms in the lower body to create a roughly stable standing pose. - biped->setPosition(biped->getDof("j_thigh_left_z")->getIndexInSkeleton(), 0.15); - biped->setPosition(biped->getDof("j_thigh_right_z")->getIndexInSkeleton(), 0.15); - biped->setPosition(biped->getDof("j_shin_left")->getIndexInSkeleton(), -0.4); - biped->setPosition(biped->getDof("j_shin_right")->getIndexInSkeleton(), -0.4); - biped->setPosition(biped->getDof("j_heel_left_1")->getIndexInSkeleton(), 0.25); - biped->setPosition(biped->getDof("j_heel_right_1")->getIndexInSkeleton(), 0.25); - -Now let's take a look at the constructor of our Controller in the +```cpp +biped->setPosition(biped->getDof("j_thigh_left_z")->getIndexInSkeleton(), 0.15); +biped->setPosition(biped->getDof("j_thigh_right_z")->getIndexInSkeleton(), 0.15); +biped->setPosition(biped->getDof("j_shin_left")->getIndexInSkeleton(), -0.4); +biped->setPosition(biped->getDof("j_shin_right")->getIndexInSkeleton(), -0.4); +biped->setPosition(biped->getDof("j_heel_left_1")->getIndexInSkeleton(), 0.25); +biped->setPosition(biped->getDof("j_heel_right_1")->getIndexInSkeleton(), 0.25); +``` + +Now the biped will start in this configuration, but will not maintain +this configuration as soon as the simulation starts. We need a +controller to make this happen. Let's take a look at the constructor of our Controller in the skeleton code: - ... +```cpp +Controller(const SkeletonPtr& biped) +{ +... for(size_t i = 0; i < 6; ++i) { - mKp(i, i) = 0.0; - mKd(i, i) = 0.0; + mKp(i, i) = 0.0; + mKd(i, i) = 0.0; } for(size_t i = 6; i < mBiped->getNumDofs(); ++i) { - mKp(i, i) = 1000; - mKd(i, i) = 50; + mKp(i, i) = 1000; + mKd(i, i) = 50; } - ... - + + setTargetPositions(mBiped->getPositions()); +} +``` Here we arbitrarily define the stiffness and damping coefficients to 1000 and 50, except for the first six degrees of freedom. Because the global translation and rotation of the biped are not actuated, the -first six degrees of freedom at the root should not exert any internal +first six degrees of freedom at the root do not exert any internal force. Therefore, we set the stiffness and damping coefficients to -zero. - -At the end of the constructor, we set the target position of the PD -controller to the current configuration of the biped: - - setTargetPositions(mBiped->getPositions()); - -[Controller::addForces()](http://) computes the forces generated by the PD -controller and adds these forces to the internal forces of biped using [setForces](http://dartsim.github.io/dart/d3/d19/classdart_1_1dynamics_1_1Skeleton.html#a9a6a9b792fa39639d3af613907d2d8ed): - +zero. At the end of the constructor, we set the target position of the PD +controller to the current configuration of the biped. + +With these settings, we can compute the forces generated by the PD +controller and add them to the internal forces of biped using [setForces](http://dartsim.github.io/dart/d3/d19/classdart_1_1dynamics_1_1Skeleton.html#a9a6a9b792fa39639d3af613907d2d8ed): +```cpp +void addPDForces() +{ Eigen::VectorXd q = mBiped->getPositions(); Eigen::VectorXd dq = mBiped->getVelocities(); @@ -155,7 +181,8 @@ controller and adds these forces to the internal forces of biped using [setForce mForces += p + d; mBiped->setForces(mForces); - +} +``` Note that the PD control force is *added* to the current internal force stored in mForces instead of overriding it. @@ -166,26 +193,33 @@ high. As soon as the biped deviates from the target position, huge internal forces are generated to cause the numerical simulation to blow up. -So let's lower those coefficients a bit. It turns out that each of -the degrees of freedom needs to be individually tuned depending on the -many factors such as the inertial properties of the body nodes, the -type of joints, and the current configuration of the system. Figuring -out an appropriate set of coefficients can be a tedious process -difficult to generalize across new tasks or different skeletons. In -the next Lesson, we will introduce a much more efficient way to -stabilize the PD controllers without endless tuning and +So let's lower those coefficients a bit. It turns out that each of the +degrees of freedom needs to be individually tuned depending on many +factors, such as the inertial properties of the body nodes, the type +and properties of joints, and the current configuration of the +system. Figuring out an appropriate set of coefficients can be a +tedious process difficult to generalize across new tasks or different +skeletons. In the next Lesson, we will introduce a much more efficient +way to stabilize the PD controllers without endless tuning and trial-and-errors. -# Lesson 3: Stable proportional-derivative (SPD) control +# Lesson 3: Stable PD control SPD is a variation of PD control proposed by [Jie Tan](http://www.cc.gatech.edu/~jtan34/project/spd.html). The basic idea of SPD is to compute control force using the predicted state at the next time step, instead of the current state. This Lesson -will only demonstrates the implementation of SPD using DART. +will only demonstrates the implementation of SPD using DART without +going into details of SPD derivation. -The implementation of SPD involves accessing dynamic quantities in Lagrange's equations of motion. Fortunately, these quantities are readily available via DART APIs. [Controller::addSPDForces()](\http://) shows the full implementation of SPD: +The implementation of SPD involves accessing the current dynamic +quantities in Lagrange's equations of motion. Fortunately, these +quantities are readily available via DART APIs, which makes the full +implementation of SPD simple and concise: +```cpp +void addSPDForces() +{ Eigen::VectorXd q = mBiped->getPositions(); Eigen::VectorXd dq = mBiped->getVelocities(); @@ -196,6 +230,8 @@ The implementation of SPD involves accessing dynamic quantities in Lagrange's eq mForces += p + d - mKd * qddot * mBiped->getTimeStep(); mBiped->setForces(mForces); +} +``` You can get mass matrix, Coriolis force, gravitational force, and constraint force projected onto generalized coordinates using function @@ -207,3 +243,260 @@ and respectively. Constraint forces include forces due to contacts, joint limits, and other joint constraints set by the user (e.g. the weld joint constraint in the multi-pendulum tutorial). + +With SPD, a wide range of stiffness and damping coefficients will all +result in stable motion. In fact, you can just leave them to our +original values: 1000 and 50. By holding the initial pose, now the biped +can stand on the ground in balance indefinitely. However, if you apply +an external push force on the biped (hit ',' or '.' key to apply a +backward or forward push), the biped loses its balance quickly. We +will demonstrate a more robust balance controller in the next Lesson. + + +# Lesson 4: Ankle strategy + +Ankle (or hip) strategy is an effective way to maintain standing +balance. The idea is to adjust the target position of ankles according +to the deviation between the center of mass and the center of pressure +projected on the ground. A simple linear feedback rule is used to +update the target ankle position: θa = -kp +(x - p) - kd (ẋ - ṗ), where x and p indicate the +center of mass and center of pressure in the anterior-posterior +axis. kp and kd are the feedback gains defined +by the user. + +To implement ankle strategy, let's first compute the deviation between +the center of mass and an approximated center of pressure in the +anterior-posterior axis: + +```cpp +void addAnkleStrategyForces() +{ +Eigen::Vector3d COM = mBiped->getCOM(); +Eigen::Vector3d offset(0.05, 0, 0); +Eigen::Vector3d COP = mBiped->getBodyNode("h_heel_left")-> getTransform() * offset; +double diff = COM[0] - COP[0]; +... +} +``` + +DART provides various APIs to access useful kinematic information. For +example, [getCOM](http://) returns the center of mass of the skeleton and +[getTransform](http://) returns transformation of the body node with +respect to any coordinate frame specified by the parameter (world +coordinate frame as default). DART APIs also come in handy when +computing the derivative term, - kd (ẋ - ṗ): + +```cpp +void addAnkleStrategyForces() +{ +... + Eigen::Vector3d dCOM = mBiped->getCOMLinearVelocity(); + Eigen::Vector3d dCOP = mBiped->getBodyNode("h_heel_left")->getLinearVelocity(offset); + double dDiff = dCOM[0] - dCOP[0]; +... +} +``` + +The linear/angular velocity of any point in any coordinate frame can be +easily accessed in DART. The following table summarizes the APIs for +accessing velocity. + +| Function name | Description | +|-------------|-----------| +|Content Cell | Content Cell | + +The remaining of the ankle strategy implementation is just the matter +of parameters tuning. We found that using different feedback rules for +falling forward and backward results in more stable controller. + +# Lesson 5: Skeleton editing + +DART provides various functions to copy, delete, split, and merge +parts of skeletons to alleviate the pain of building simulation models from +scratch. In this Lesson, we will load a skateboard model from a SKEL +file and merge our biped with the skateboard to create a wheel-based +robot. + +We first load a skateboard from [skateboard.skel](http:\\): + +```cpp +void modifyBipedWithSkateboard(SkeletonPtr biped) +{ + WorldPtr world = SkelParser::readWorld(DART_DATA_PATH"skel/skateboard.skel"); + SkeletonPtr skateboard = world->getSkeleton(0); +... +} +``` + +Our goal is to make the skateboard skeleton a subtree of the biped +skeleton connected to the left heel body node via a newly created +Euler joint. To do so, you need to first create an instance of +[EulerJoint::Properties](http:\\) for this new joint. + +```cpp +void modifyBipedWithSkateboard(SkeletonPtr biped) +{ +... + EulerJoint::Properties properties = EulerJoint::Properties(); + properties.mT_ChildBodyToJoint.translation() = Eigen::Vector3d(0, 0.1, 0); +... +} +``` + +Here we increase the vertical distance between the child body node and +the joint by 0.1m to give some space between the skateboard and the +left foot. Now you can merge the skateboard and the biped using this new Euler +joint by + +```cpp +void modifyBipedWithSkateboard(SkeletonPtr biped) +{ +... + skateboard->getRootBodyNode()->moveTo(biped->getBodyNode("h_heel_left"), properties); +} +``` + +There are many other functions you can use to edit skeletons. Here is +a table of relevant functions for quick references. + +| Function Name | Example | Description | +|-------------|----------------|-----------| +| moveTo | bd1->moveTo(bd2, BallJoint::Properties)| Move the body node bd1 and its subtree under the body node bd2 via a default ball joint. | +| Content Cell | Content Cell | Content Cell | + + +# Lesson 6: Actuator types + +DART provides five types of actuator. Each joint can select its own +actuator type. + +| Type | Description | +|-----|-----------| +|Content Cell | Content Cell | + + +In this Lesson, we will switch the type of actuators for the wheels +from the default FORCE type to VELOCITY type. + +```cpp +void setVelocityAccuators(SkeletonPtr biped) +{ + Joint* wheel1 = biped->getJoint("joint_front_left"); + wheel1->setActuatorType(Joint::VELOCITY); +... +} +``` + +Once all four wheels are set to VELOCITY actuator type, you can these +actuators by directly setting the desired velocity: + +```cpp +void setWheelCommands() +{ +... + int index1 = mBiped->getDof("joint_front_left_2")->getIndexInSkeleton(); + mBiped->setCommand(index1, mSpeed); +... +} +``` + +Note that [setCommand](http://) only exerts commanding force in the current time step. If you wish the +wheel to continue spinning at a particular speed, [setCommand](http://) +needs to be called at every time step. + +We also set the stiffness and damping coefficients for the wheels to zero. + +```cpp +void setWheelCommands() +{ + int wheelFirstIndex = mBiped->getDof("joint_front_left_1")->getIndexInSkeleton(); + for (size_t i = wheelFirstIndex; i < mBiped->getNumDofs(); ++i) + { + mKp(i, i) = 0.0; + mKd(i, i) = 0.0; + } +... +} +``` + +This is because we do not want the velocity-based actuators to +incorrectly affect the computation of SPD. If we use simple PD +control scheme, the values of these spring and damping coefficients do not +affect the dynamics of the system. + +Let's simulate what we've got so far. The biped now is connecting to the +skateboard through a Euler joint. Once the simulation starts, you can +use 'a' and 's' to increase or decrease the wheel speed. However, the +biped falls on the floor immediately because the current target pose is not +balanced for one-foot stance. We need to find a better target +pose. + +# Lesson 7: Inverse kinematics + +Instead of manually designing a target pose, this time we will solve for +a balanced pose by formulating an inverse kinematics (IK) problem and +solving it using gradient descent method. In this example, a balanced +pose is defined as a pose where the center of mass is well supported +by the ground contact and the left foot lies flat on the ground. As +such, we cast IK as an optimization problem that minimizes the +horizontal deviation between the center of mass and the center of the +left foot, as well as the vertical distance of the four corners of the +left foot from the ground: + + + +where c and p indicate the projected center of mass and center of +pressure on the ground, and *pi* indicates the vertical height of one +corner of the left foot. + +To compute the gradient of the above objective function, we need to evaluate +the partial derivatives of each objective term with respect to the +degrees of freedom, i.e., the computation of Jacobian matrix. DART +provides a comprensive set of APIs for accessing various types of +Jacobian. In this example, computing the gradient of the first term of +the objective function requires the Jacobian of the +center of mass of the skeleton, as well as the Jacobian of the center +of mass of a body node: + +```cpp +Eigen::VectorXd solveIK(SkeletonPtr biped) +{ +... + Eigen::Vector3d localCOM = leftHeel->getCOM(leftHeel); + LinearJacobian jacobian = biped->getCOMLinearJacobian() - biped->getLinearJacobian(leftHeel, localCOM); +... +} +``` + +[getCOMLinearJacobian](http://) returns the linear Jacobian of the +center of mass of the skeleton, while [getLinearJacobian](http://) +returns the Jacobian of a point on a body node. The body node and the +local coordinate of the point are specified as parameters to this +function. Here the point of interest is the center of mass of the left +foot, which local coordinates can be accessed by [getCOM](http://) +with a parameter indicating the left foot being the frame of +reference. We use[getLinearJacobian](http://) again to compute the +gradient of the second term of the objective function: + +```cpp +Eigen::VectorXd solveIK(SkeletonPtr biped) +{ +... + Eigen::Vector3d offset(0.0, -0.04, -0.03); + gradient = biped->getLinearJacobian(leftHeel, offset).row(1); +... +} +``` + +The comprehensive list of Jacobian APIs can be found in the following +table: + +| Function name | Description | +|-----|-----------| +|Content Cell | Content Cell | + +This Lesson concludes the entire Biped tutorial. You should see a biped +standing stably on the skateboard. With moderate +acceleration/deceleration on the skateboard, the biped is able to +maintain balance and hold the one-foot stance pose. diff --git a/docs/readthedocs/tutorials/biped/IKObjective.png b/docs/readthedocs/tutorials/biped/IKObjective.png new file mode 100644 index 0000000000000000000000000000000000000000..d5d0f39ca5d9a020682fa751800249a95e4ce124 GIT binary patch literal 15541 zcmeIZXIPU<7d8qQDJn=&P`ZGy1%XgPZ=tu)JJJ$b2uUC$p(q_hL_nkpNE3qeBA|d& zK?S5ql_Fibp!9waw_D$H{+$2c$8`ad$;>@7YprLkH8b}UuBW3$d4cf)5fKrkhPtu= z5z#sF({evK>FHky8o-y+e*h;i1WZI!7Jc#1?)>Rzc9^;Wgowz8n}{eNgotSWv?^eM zhzJKHB3iN~B9clXB4Th$t*%ENO{R}oVAcTE&r?*KLG>ZKvt{oJO!^(1S{2b`NpI`HI z@Phvt3FYzcvQ8H$`12bP{U10QiKnvX>KZ}V~jIJ7&>X2=`HK^faaJv`x1b-07eI+27*NyOXw>Cv2MnAEXW;Wq5*Rd#u+O)@@X0i z2{{NWVjLaqMa0#yP7XjlaV%WYLQulR+0#zVNy1SHVE_T1fxzswyn$l&YG9BBN)3X= zI6B}>b%gm~N?K@fZxNI_*v(DR%*-BUinK8IauxQFuy-^SGP3uuQ}Z;m6E)F01A!_k z>X^Ihpj9mJDi#=3RjiV*Fjh_64zFdRgLANVaMCw6P_@%E#3*WE3@l8{HN@Q1aWGLG z9h|xy%3as%3`9j8r)D4s6oS}cbTk!3pdNf$>QIP|uqP6!ZfvB3^iWm9qBWp!S8ZK= zJD9zNu8M`d17(fe#A+Cwf#48!AgG=hTEYWlkM|J6i6fl##U1ouKvRf=g|Mio zyOx_F)=d+RH*rF#xoh(YA`H~MwDcuZjj;$LK2zZ{5G{QN6K@@FEey`oSRLlBh428W zg52%IT^)q9y!6Z@2O^r!ib1#yNDtLwX}b!SkyxoZUOg3 zD`U~7co8$Oy`Zvzmk36j&)wL;P07y4NXOLC0;;9rrKPBd5EDbInweqo215Fp+LD4$ zh~cl9xT~4t^^8EGd=OVpB@;)ak(<7Xp(qrE@^W(51esg7I2(Cs^LcoKw2%gZ5V*d$ zx&c_o(_PZo8|LC-?BH;=93N*1dpI705qAPRnc*;e7!M0qAp@A5x&hA2)Z9(fT^Q*O zF+y2LSQzOdE%-FN@S-?`3R2IA&m86K0o6VO(Sze8l=Mu1SWiPCoW8QVG28%W2NKuR zRu%Jb#Asn462d4CtdTPgY3FICZl(eQxkB_1x<*h{EiGpUDC`Wx-PFs=-Nn__UED~; z)!Yc{4H1MJVpYW~fQF_>cQGi$RL{`E2W%%PC@G@opl2@*HG14&9P<3~KgD@sgePeq&7d4Q*uA7;wg^L~>$fx7% zX6)i6f|C@}Q#=DPLKr)!Xb5TJkp`y<^Ar>U0+FUzy!ff(0vZ`v2=ZyVs@QozA!?$g z2FfsJRc#|3K?5V4u{%^)1n+I2@e4%4UQ)zM*Z?7h)^mqL9Na{-)dfL9F6ws5AVo)n zof8i2=`F4(Y9@#R={uVl>%(<{r;Y`v?d4Fe!>f`;G`xAR|TnI#I$|HRE2;_X2L2McO{IN zB1Y^CLO>0Y6qH%JXjzyhc8OiHyrQ?fDXNCbk^3H;&NbmsKcYtr-~RF`T>6{@guC^E2C2ro z9NQ%Q3?VvrCoLf|ko$H8r1irNU+Bog@`x*{e&k1QJkwv^-Cn8g_V`x4Q{8>^egEUV z`uWsuS$Ufsx%sr8wY4Y~5@K=%A`m@88>gA^mGkr=88117#yRl2dQ|oa29Bax@a%yi6#5%v6>x zWoGezi}=|$}Fs6t1Q3s@s~lKIw6j)BKb4w{(FQoB+~gQoGvJ1*!=#vh^ykJD&5gg>$dJ%& zpwsSeo#zDn?~HB{*C8gW=oeW+gr_JJuU}vJYdO@{$cazFbyQ?ox z=@8>&8mlu2P%DZ3W7b=tzvZ}@+DKKRr$Wwn)Tk;w$xT+< z&T$^kO-@W5^gp9sAl}jF8@NCT4bmf~*Z*68Jq#p6mMY_Y!TIUco{OoqIf9XdG;csu zx0WjJpWy@3sCxpAHA{*cY>)fF#~RjB^GCiLV~$H~2kG?n9V(5Xe@T`IP@|PmnqNI| zqEhW}5?)?I#s^E!AO) z%Y(>dpIP2|Z0D80KjQiAM1J@D>skH3#s87{|4*6SPH)_wGYw??c|n>R{z1xxjB>IPk&p@$$oi1ia1p{`QK@+Q(U1p13`Sta3!q+a-P|v0F1Q>Ye#^oE^C*_ zI4s#`JnTDaWqT2wgAa{Yoanvaa^YXAtx%iv;cI8xU2yb|&m+|xapG_AAKO8=wMG#I z_t<@Qz-@G!KlJ}JwS7C^CDb8R#9YEp|LPLBev?I3Lc@sWmm}1RZwyXoq`bCeq%aC- zSDqN~bXW5H7@RF^ZB>3SFDc{cFjZvbq7Z@WRm0+w6Wl3QlLI344-pURVUJ9SkoSKY zLm-$o$*f?^R_*voZ%2VZ1x}?#OFLQfHL_Z@5=SP_Z42=TJmERH;>{VrK_KZcBX z`+A{$MQlCiIYy0tZ3TJiVBfJ#ekkE5$}_04%24kN@Y3SUD7g|k?`NGPXu=|C<)BOF zSl8geg*)*rh*=J+e||C_Pk%Og;|B`oo1$XusYbK39ZEJbiZzf7R}6!6dOc)_67xV| zWuwf(oUNwfljj2e_p*n6sY)qu`a$j^PbK>?Kv&gQhU?^*K8GtRwB*<*g>PHwDaEcC zo^Tc61KxK$KH}suuz}aW=e(Yd+1Fj3@D$qdoR6$+X;$*jOR49SScH&P)=WFjDm~@O zV=0D6Cx@t5*Kw3WD{({M_{6J6vSs^F0LiK6Gb?*1FylGXzB#U;vVQDDWhEyAenIXY z-n(P=`P-YEnz5N^IXjDMTc0DRd?_31y5uDpJI1Je!@|^G78@Iqc{rc=qDzLva8e(3 z$#;i5B)N`8 zTX~N}m2|RJRZZ_wGVjN(hwiW-sd_5sCwxs7!@m0W8Wn$;9nzfVSkB@hHbBgpP_Ln z-_+Z7*?VkAs)zhhH3aomxp)o#i7VsV8@)kk{%bnlqZiYIb5d!(yN&JMc9ksC*<_hI zUqWR(8o|Z*K}UPME$(hPJXW~u8&t0p!=4QGT`Fd3lii4n985#wRH#Z~h#6 za#1z*o&9dekaA~bX!F|4GaEryjw7y&XIH+-n7B*MtEHpGcOA;_i83%ODNkg^&e+UO zEgf}DzMft}@Iu1V)OY7D`|4VJ;GZ4coaMV6z5at@mdXs%$KTM}PeigsF>8HNy|^^^ z+LgV!4)reEMvB;1Y>4eu_O5B7^)kv>A)?#jK|%yGguQxAuSF%CbUu;nX+zsZ#KBvn z^{}`6tPIGia`lE_^xaPXbOzSo9Xw(-d8kKCR^Iw7saW6%EK$w(4gI`1&G89M5^c8S zY3izI_r1XNR#B4gS{A}!>q*|7S!a@G3({Dq;#R?RR*xxTc>_xSUF1}mT+ryXR5Ce# z)0wO#@Tak>onu&81;wj3fQe@h777*WsI15K?OS5kvyTtDGyQ0L%sxDuFf!F;qb84| zSx}y!xvTsY%`yV56s(7*<0fnoA9Szz|Q zm4^7)2*~rjuNJL+rW27oRjorIh;dSX6|P@v4v^~ebbicc{oy!ed+K6^-x#-J94F`CmFR9OA{)t61kf33wgM-IG_( zdFVNWA}~px8`*zeq(*0ml+Ndy(yVS*rK0I!I{J1;f-ThejLFWvWPl7vlSGL$IGs0g%zibO<6ZNjMiP!Zt~;!@sVhA>rgP3-Giw2W4<;1% z-u9FRtcL~5tW~aW1R__!vN@8}3hOqkm#JvZwl#q_PdAFfM{P|^{HZ_c1FKe+SQNh0 ziIAV8gB*S<9wunruIG;Y;6+|&X2`TCot(z*dRvn0)UBk5hMgE3v~!EF=~<_fkNj9< z*QP!T#tCY+tq>`DVkBz0-(p_kg{cQv0!u0nS)cD^y>}Bl zAUzDDka4PaYsPm?J@-0`pSITl*kI4K-o;>Ar>7>>o__5Gn-|mA)=J42;06PiOPz_0 zN4rl=+)RC+@C~c7~_W6)-zj6vO)tQL@+ZIdF5jm*Z`2^czyn!(yhFI0t<0^1Ccj;{ z=Ij$hU2AsY+LSbKpW~ky#Hkc|0iy5?9&#U&w}2^U)sT^!rE>p75$h0zOxjX72QFKb z;K)9*9SnytI1pY%s#HI{I%&%X*bIze;7hv?SS?@bOBX%5ZdgPF$GvYm5$;ZeG}tHqX#PwevGZW@m3Fu2fPZYi z>4W6QXjNg!&#niYcx`IusZau&uxxj~%~8dqU9W=1P(%HfA1a1*rrG?M4;YFU zCH{4Jq>Mg$`bVkz(G8Bg2YG}NkZ*8aR72-xqBf4zw>MkQca%(f0=+*3@zqb?vw_5; z(iesIaeVkp+AovFW}nw?RB}WURJp86DBjpt`qwThUA6p98R2izDyke!H*D)DzHNMx z?eT7?I@x9;8aggC@^;zL&Pvi(Ha0B@CS&julWAm8Ri?MK3l9b+g+?*$)SN2B0I zQ_Xb~^%A;5{0r>{s$0ibwVrP}WHr^K>*~`B^?jS?KZ*KyPkoS|fNJ;V7IKqs|r>y#Z85Dw)emF8lBri12*T2UMiIonNxa?P6V4YX25mwkegq;rG6{ zkbB(Qf%QDcC1=Mzdj|j^s>M-1x#hNjsE4*gsf#-yfMGHmAPN}@}h-TWNS0y3Li?y7}vPZvg}C{vF57nLfJ#h zHR$=;HKYuvH73X*KoDM9+f?!$D% zGom6d;&eT-LTyhD7rx|_r3setfbE#&{dkT@GBpnvgE_CclnUIDFJ7}jzh8P##lPD# zhlY+Cm_IW}&v1^{UX2PLX>eg29EH#-)?Q>N`{**g?bsZK|esl2&#q1X&kG^=& z4zKiN@wl)j)ZKAB1KO|#6x@oYXCoTgI|<<~ZT7;1WW|gDVm6CIAMV zU%d$J49CSyfvj3+NyQp;?0gvTbj$3wni42pZ^TR&TZ}iSAk1;EMgrqxOxuOkD-MQ| zxw-S6qzHgcgx&`uw_cXSFUCzou<_tN8q|rs)yxd6nW>_MCx27<_S(Z#Hs$cvjA7zn zwN^?C3me=%3a+J*g+2`j!VPjCoI9H%cFEwOb9ZwWWxS(RUZbtE7r%#BcPCp*Q@&bl z>FP`=qD3H0TljR(b@-_UdNq2+G>2ZuPUKekc zj$#_PGXbqBTrD2@tYW(Di593TFB>7>=9?{{jpP&y9&`Jy-dC$TIhJ4bwMb_gx35~> ziG-w%P0M(UL4EF}SI>V8U#9XOIn6?h(q#r?e|$+xFE0`=Zi>KwUSeL}uC{PD3)yvu z+bbxP+QVu4D%YB}>wfgC!C1bD zxxpsa%(|R^Bl(%(kv>aQQCjGHVOO`U_`5WB*3?_G3#0R;N(FCktlx`|zh0mQtBQNiaw`-*0KC$fM9-fRMAGc$Cc*b2 zHjQH_NBLpP-d!3?Cc1L+Gk_e?Yl@ z%sC=_iGS>>t6;R*9&zp8YZZo{-e>Tp@E8JtQ<@;-RE}MjyD1A zl0RsLl=8XAX~-3Ehlt6y!468w`=~i#wwC%3VCF@7-wV+L&RH zBjz8>-i&p!lsRJX0g%AB-2y?!VONQrGncvg=6MNBKG;`!xDt@4nEH@^WVA-tu-+jI zbN~7fl#Je?kG;0|O%+%0`=8+z#*=r+74@_s`LSE)wDVSN)Ehk>_h2|fnMy&^QjBpJ zMfRmJj0u6YRE*6jpCP3rEuWjxBFXFBmFt%SdFWD>P+!A)!%0sMB)mRbY$ZHO_?~rA z)+3OdjFQ{#0Y`k(&142Dbq%}SP3kRghG zdkCx$ff)#Onq^Jsf9mnSQZ_Fmt0x8##H!^dI7-R!k9;&M``neB^Pp(G`{N53SqHbA z9jJkk(T903L_JuR@&h$_Hm&+mQTErNO0TOR^=-f5)eTIIDY=h(c`>lrbt_!Y!=$q^JVGuI@=(yqer`@RxnME@e z4tad@YQkol7u#bvn8aJR<3QFQo`_uH@kGrb+Xk|B(`a@TpuQ(I&^H1q%QIEAnL1Qh ztL!h9QXa9ZpIYAp)NFbOYD-l7BevNVIFi>^{$)I$!*}H%zoI1XHG`V)CtUnXg-jvu z&&s=>)g>>227H6`5{?BKESB-Y6OYY4!ot7TTtgaW8z`~fQBID?Qz7xWGWsgzyI;Y^ zEy9{}j3G+j;>H_!ad+KMi?PkOekf{kP4{zWfO|On*lH4vbk&x}V{L?Fdn<0~U-us| za+Lj`wuXWaDJ;>Yd+uaFtX@DLIej}IbNcA4*Jb~v^swGJzbvgv2BICC;~J2j+{C2C z;aA%tr@ZlayN2+FNTS6hhE8O<(M41XKPhDZ{ zP77&;ofzY&ECVA2o039=^*6??%Pzn)&XYo}7{80g3`1R2qBc7}YTq4trnqD!ap7KF zs}nSM$XayC?&^Mw`Y4o8u3>N+HH1v5u?8;!T|pWxO}LN&rF?y=cP?t9)D^B4J_%)jw>!@PMAJZR~}%qP7$_#sepXQ@V)8k&B5@LWBI z-tN;(f}7UI97_5)A5O+kbRiB6x@GUX(z_6BxSS?L3EJFOzN}ekzlclhsWTd8v(aF-p~IY%SbO?bdWukfr=~o?t*N|DDA) zy=N3bpWd;W9=hx0sgR1Fe=5%svJ5cP{je<+dRua}D&wB-52Sfk_ncLEVKdV|JB}Sn z2Q=^h$vf$N-Q5r`lr|(#wP!^U&uhs7QP;deeknP?B#-T3g=T#*%&!@Qk0wl`wr?Dij6MQZ0GE`9Cejy6&2F>JwYqxaUYti<;DfTJ5_~- z^zxE`I=Cy4(HZ^un#BB3aHL6kR<_Pg{IKCii#LY81`OY$+6@`Biw~8#GD$TARaPQ| zWf|@7n3;#wf{booY>IcK7lf-hk)x_gy9Ol+4qr_{ z6i`*qEsUM)jJ2H85HG{u5zgheK=x)SkFT;Vy|UhH|8zk^CwIjm@D&*VYH0x*IG#zO zVFIY?OtwAQUalYQPs+=F08SQ+s?dZ@rj||0KbpN6Ykkcox&JBq8*zi>;pYYF74Ni< zJ)c++QaWEwoOh|Y%tIJzT<{2YhxK-WU^LRD3Zw9tH!0KfZBlL7J91Cto8sd*V@5bK zh-;+Oj*cDM!d53&Uvh9UA1G6+v25W2BW-uc`&mK`QHrs6%TzIjJlcHaE9AGIHCh*d z3(;~ND|u^{0(CSj1qv%i!Kt~4%SBd0sv}j7RZ_Z_Im}H&20lYKGu8EPSz!=iKs?xgX;cR_F4qjA*$s*1xX&jkM~B_7Z3fgBt9Pt20WqRS<3ZA zi<|2sj5*JcaM-z2V28~`%8(mC3<(|LWRHfFAsNXJeJ`3G@UCl=qZdQEnf_>&)6g|h zN29+r$%BWyrimejx3W&v}3xfq`TA-Kw28%0uSy6Kjzdz_B0P1{u&k3biPtfnrO zb4du3_&j`EuCC`)c@j-RK+s!n$ChMh0<_3wOrW|#{<1fokjmg*mE63dbcKBLet@-G z9k;<~K__0fCF)jW4N-sS}F=S`5V@N6$ijJorRO zQiCo`E-o=+*^1qmHHl~paALg~6&)V%j?+_>u}~#dDnc~owwe>K<%7YxqK7T?5IN-a zG3%SL@Ufz=Uz}^4mRvAql_iy2TPo)qi^Re#&+8@abH-8?#ikHYS+q|Nx+B0oZ)nYtYUEB%j z=b5xgxiKFnl;M)mbT7%IDnIMsc^4un@!~Sw5A&=?9HG1@A`h`I${otMT!3S7?jY0~87FnoBIAdJ_(ge~*}e*uXC3}}-k4#WEds8Lgh;jaPtc#d z4uu`9h-z%;xNcW`x+l~oS@ZOX2SXhrYsy}AxvytQrGoq1K7K+lVbqD))BR#ZU);j| zr62LNBS!CLUt&p)(H`cy)c44ZO%slm%Re?uGXyly0hUsow&a(1=C|Ufw)8}Ye-s~~ z7+o#UFWY5#>X2?qSBKVa9TxplcwRGGF0i@$!S^lC!3AdLb=HSnfl;B09t@I)Z*j>os1=iL>fvQ2&tZ{9OQIt(1|7ek($^_9|1&6t_|gWTobr3XH5V8bA^?SpY~QZ%{dB)8~7woJ13-wan5K|PKf zWBJeSBtYgnnP1P3E;)(4c~v_xki`Hg7^-NvRk8l{wbT$2ITWkw6kG5bTH^T)I3Z=HoXady;N-^LEV!gL_E7RT7P()b4(dL zFGep|u{dIM9I=vnws#(6wx=QgT&nZcY1LR(gNa$pvNx-7yC=O=?DFIR&QD-9Ky40$F(=VG-l0t^C^fZ zdoWA`9xFLL}%ct zV33;u&?#s=P`#^xwX72E({`bDX!Bl4&(#f^`OwiHOJBE@EkhmRHcEm-ecCPuHVTt3 zcosyJqnxTA)fhU807Qw6{6Ejhy?mPRF)pmY(YvSTgInbHxSrhWbhTJmmQOT2hU)S1S%3!@j#Z`ka3;>H+c?tEpg08H(FF#CWGny3- zULOu_U?SRW{e3RY-UA}>$-OL@-1uCL>~h$qobD*!NZ3WTQZ?8N$ESMfpU0t_$_k^O zmG+7nGgLO*XlayhUEZe)VWEc{5H@ugjCLalnTN{GrR$V5qvhI{Et?S5Q$8a!Dg9ph zsx$nBIVp96ej|r6vg-kpXS|lcF6!M3Q@Zhz$DDvo16Ox0ui+j12F_0H{nQ1@4<4!- zmKAq%E@pM-(1TUvXhQhTkDngdCw;qQV2zrJZ={#AHh(!|bwJXkONu)d2YVGZ36fII z&&?8%|B}zbr9ei?MO~Vvu()V5*vIs+fde$mj&jpoT zJAieZ2F?<8dRLCNlI0q;a$VoO3eQ<4<9AHEkqq7fCodN}ERZy)^Fbr7o7)vWmuj$*uc7tYn_h zVE$wYadW$>vw`In@SeCLZ2JJBbeC(9#?a2XE|qzy#Z^btV=LNbjS4$9&eUJqcHIHh z2fDjqKKOn4%|}{f(T|M=7Zdv^%Af!#RnM-1)U`2eaDXZ&@9Ayz@HfVS<)vOP2ClQq z%uq4IFhA-ZboPoTUQs@|$A$k$$b{}GQb8AY=G+I*o0xWTJX>I^S#=A%SzWD50v&kvieo1!aUo!@WTW{^ zy0g?hlzF$@gVPB=IN8WAMK;>RiK>sjMXU-5w!K>WI&y9pu=nm^r17%b^X=T>&lf_W*C0|C3XigJtf7_+g ziwra-koEP5OLkino$t0NX&(#v?KANg=zOZ=oBIsg(PHsJd%xK-rEVl695bYNKfASx z#@Kq|6EKMn)a^K~b>OLXaVa)5@AU!fpNk2ac512nu&z%4N>*|P*KPAJQ)VOSyn?Uw;ev)tVE_}24;K;5;`K@8%qrds248UMSgX`zZN?^TfEnOq2J~B72!N0 zVc)mUiC{M|P^ilrb5Ijpk6RXdS`c9hhn0^%I^L7+Xf70OQMWr~OXkz|pTy z)9)F2l(=HYMmW~Wub7JbsC{~YXA8hTwPwBH{`52JgW>n6PeD6FvgA{|HJ;9d9V%&` z$i?Km{eOnu?;WM=i;8o596jZxV;nk)Ulg9(08b^0Rxhj19NN3n0dDr3fAz)asg>)# zqaksx?Y-XgZ7~w{J2KXC*$#C$1WnL%*|%D7OaxK)oj^$ z(j!`k2n)|4e~jzC2K(GYMW}e4VfOV+?!sAv-TXO6%9v zyE(Oy>moo@;QQK4R|$eLoNgH5_+FzJum4Enu&Sfs?WTaC69XGpV1VOSs*&T6UElDm z>m`W9F>f1*C9T>QP9PFLHOaik*n#Pq<=pm`EJYgSPz5K7_1eXiyz9oToBG#=E{7F= z`~IN}a5zy6Bdn^zXje&pFz_*xRmTbiI%gH_(=25%etLB0S(pH6UB9v#lv~0DsTV>()4yT{X`1xCvj* zO-oa+3=Ua-$A0D-&aV72N&xL90iP+|R<3V(28GatOb;)Z{SWa<@RZqzdmC8X{6@`d z)4RV9>5uk(^ttyIIruMDU4|3Dq_Dm$P+%>HMKWeYjGt9Vr=HSLEotg68t%dkV&D6p z*e0b;l6HCza`o4)XKP<#e#N^szBl*lQuAL@UgIZ z6OQz!hf4bkA2HoTIsdW8h{-E-6!>PQh=xMn?}m3Zdle>q1L2tSYu=RDttVfR3;WCk z`SFL&`ttOU{8I|2zK-bV2F(GrwolV8j(`^OOG1h0j_(Z|BQ-&{$bONwep1IWgh-_Mc}TY-?0SYZ zI5_`eXOdTdGgdqZ+SitJui-1*N#2{03f_wTUfAdOyU_@HhVPzkrGAfkM~^g9cVxxt zMdbFdTyCXa(rL2zyOj)yHkHgZ%DX$}e%e%w_m|3HL6ho}zcUx2z^~-McP>#5w{vYC zT`vPE1@NnOx^Xf5?#)h3+rxm@p@fkwX7VQF6-<9rZ+k`|L+Gt6S92=;;6<#Z1q8bO4~th-jh*=# zBCFIFaAh;ZHe#`*Kb=`po(^!XkH9?PXcu?>kDT>1CyQb@{jZm8jbr0Mlbr-KaKI$1 zlkh)y?9|k!57sLe0Z&?HHcht%PzySDV~xLT1CTi<(yhhDe=b+zKg9!sPnps9-C?3M`w#L)UOVomL)bvRZP?sJB%z+!DKOFZoZo+)ern#9`E++JGER4X{>Le< z$K~~eKttxgjiN?7fI=~`n+o@rrwCB!03@+jTuPn(+hk6un%dMg_wX;6{_bzV?RKwY zPX8~=#v3G-oqJaeeXG@NZr9w+_mO1yUrw;9qJp%931r;VM{#E}#_a(pw&Ystiwaj* zgnmckEvkQ&5~xqtGrSx|XpL)D>y&N9PKe%~cQ?d)dnv3}X$3@-Sbd2j{a56TuPN^J zA&$ep`ui~+<2=yYQC6tqZm+2%Q_Hw-kt}9etM~X0rfw$a_EJ*Y2K5jrm;YfVQviy~ zC0a$lXnd_{3)E&J;O7^1Wvcr)T+^Kd8b?z<$$SB; zXI4x~9P-SaKgWIdKbxh#u^#ncwf({BotM5Hn{6M;nq6(4jig60qx&l07Q^Si7FU`B z9sZ7ew&bU;ov4QBQa9tP{fh}F<;=0hBKN&-OBYHF(4?{VXR2BJhfZ|Le;l^0q$2yl zcr9$2n`U3lMaWzTmegJTHs6rNE=Vto75?8shyXqswyiEKRI_g{zjxi7zVcO&E~hWy zyMXiX{c@@9auINnN^TD9O>hsb^TR(DZb;%2G}$3{ap8n(cUm4~ws9HYmvga?RNsfK z-~IwTMuASGj=n(ak0x#10CH><3$7P!S zBoAC8t#YbcH`$OErJS(;cJ51ArdOrHkz*f&wvDK4+vYeS@o5^GvE9&-Izlk%I6m)` zOM>`c$K#{Reviy9tWy1itm*U4c75m^Dz2yedeY)(9^aoyfGmp3q(G?v*XX4DlLocaX*1IRgYk}jB5iOj_tHcoku9#=QmrT+FJXMV<32yiU_h^Q@`m?b9W=gYwwDmu!= Iinc-j4@E&WjsO4v literal 0 HcmV?d00001 diff --git a/mkdocs.yml b/mkdocs.yml index e41608ab478cc..4b8e8be976244 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -9,7 +9,6 @@ copyright: Copyright (c) 2011-2015, Georgia Tech Research Corporation # Build directories theme: readthedocs docs_dir: docs/readthedocs -site_dir: docs/readthedocs/site pages: - Home: 'index.md' diff --git a/tutorials/tutorialBiped-Finished.cpp b/tutorials/tutorialBiped-Finished.cpp index b50037a42bb9f..f3f1b8373b3fb 100644 --- a/tutorials/tutorialBiped-Finished.cpp +++ b/tutorials/tutorialBiped-Finished.cpp @@ -56,7 +56,6 @@ class Controller /// Constructor Controller(const SkeletonPtr& biped) : mBiped(biped), - mPreOffset(0.0), mSpeed(0.0) { int nDofs = mBiped->getNumDofs(); @@ -129,31 +128,40 @@ class Controller void addAnkleStrategyForces() { Eigen::Vector3d COM = mBiped->getCOM(); - Eigen::Vector3d approximatedCOP = mBiped->getBodyNode("h_heel_left")-> - getTransform()* Eigen::Vector3d(0.05, 0, 0); - double offset = COM[0] - approximatedCOP[0]; + // Approximated center of pressure in sagittal axis + Eigen::Vector3d offset(0.05, 0, 0); + Eigen::Vector3d COP = mBiped->getBodyNode("h_heel_left")-> + getTransform() * offset; + double diff = COM[0] - COP[0]; + + Eigen::Vector3d dCOM = mBiped->getCOMLinearVelocity(); + Eigen::Vector3d dCOP = mBiped->getBodyNode("h_heel_left")-> + getLinearVelocity(offset); + double dDiff = dCOM[0] - dCOP[0]; + int lHeelIndex = mBiped->getDof("j_heel_left_1")->getIndexInSkeleton(); int rHeelIndex = mBiped->getDof("j_heel_right_1")->getIndexInSkeleton(); int lToeIndex = mBiped->getDof("j_toe_left")->getIndexInSkeleton(); int rToeIndex = mBiped->getDof("j_toe_right")->getIndexInSkeleton(); - if (offset < 0.1 && offset > 0.0) { + if(diff < 0.1 && diff >= 0.0) { + // Feedback rule for recovering forward push double k1 = 200.0; double k2 = 100.0; - double kd = 10.0; - mForces[lHeelIndex] += -k1 * offset + kd * (mPreOffset - offset); - mForces[lToeIndex] += -k2 * offset + kd * (mPreOffset - offset); - mForces[rHeelIndex] += -k1 * offset + kd * (mPreOffset - offset); - mForces[rToeIndex] += -k2 * offset + kd * (mPreOffset - offset); - } else if (offset > -0.2 && offset < -0.05) { + double kd = 10; + mForces[lHeelIndex] += -k1 * diff - kd * dDiff; + mForces[lToeIndex] += -k2 * diff - kd * dDiff; + mForces[rHeelIndex] += -k1 * diff - kd * dDiff; + mForces[rToeIndex] += -k2 * diff - kd * dDiff; + }else if(diff > -0.2 && diff < -0.05) { + // Feedback rule for recovering backward push double k1 = 2000.0; double k2 = 100.0; - double kd = 100.0; - mForces[lHeelIndex] += -k1 * offset + kd * (mPreOffset - offset); - mForces[lToeIndex] += -k2 * offset + kd * (mPreOffset - offset); - mForces[rHeelIndex] += -k1 * offset + kd * (mPreOffset - offset); - mForces[rToeIndex] += -k2 * offset + kd * (mPreOffset - offset); - } - mPreOffset = offset; + double kd = 100; + mForces[lHeelIndex] += -k1 * diff - kd * dDiff; + mForces[lToeIndex] += -k2 * diff - kd * dDiff; + mForces[rHeelIndex] += -k1 * diff - kd * dDiff; + mForces[rToeIndex] += -k2 * diff - kd * dDiff; + } mBiped->setForces(mForces); } @@ -162,7 +170,8 @@ class Controller { int wheelFirstIndex = mBiped->getDof("joint_front_left_1")->getIndexInSkeleton(); - for (size_t i = wheelFirstIndex; i < mBiped->getNumDofs(); ++i){ + for (size_t i = wheelFirstIndex; i < mBiped->getNumDofs(); ++i) + { mKp(i, i) = 0.0; mKd(i, i) = 0.0; } @@ -198,10 +207,7 @@ class Controller /// Target positions for the PD controllers Eigen::VectorXd mTargetPositions; - - /// For ankle strategy: Error in the previous timestep - double mPreOffset; - + /// For velocity actuator: Current speed of the skateboard double mSpeed; }; @@ -346,8 +352,7 @@ void modifyBipedWithSkateboard(SkeletonPtr biped) // Set the actuator type for four wheel joints to "VELOCITY" (Lesson 6 Answer) void setVelocityAccuators(SkeletonPtr biped) -{ - +{ Joint* wheel1 = biped->getJoint("joint_front_left"); Joint* wheel2 = biped->getJoint("joint_front_right"); Joint* wheel3 = biped->getJoint("joint_back_left"); @@ -361,7 +366,7 @@ void setVelocityAccuators(SkeletonPtr biped) // Solve for a balanced pose using IK (Lesson 7 Answer) Eigen::VectorXd solveIK(SkeletonPtr biped) { - // Modify the intial pose to stand on one foot + // Modify the intial pose to one-foot stance before IK biped->setPosition(biped->getDof("j_shin_right")-> getIndexInSkeleton(), -1.4); biped->setPosition(biped->getDof("j_bicep_left_x")-> @@ -377,8 +382,9 @@ Eigen::VectorXd solveIK(SkeletonPtr biped) for(size_t i = 0; i < default_ik_iterations; ++i) { Eigen::Vector3d deviation = biped->getCOM() - leftHeel->getCOM(); + Eigen::Vector3d localCOM = leftHeel->getCOM(leftHeel); LinearJacobian jacobian = biped->getCOMLinearJacobian() - - biped->getLinearJacobian(leftHeel, leftHeel->getCOM(leftHeel)); + biped->getLinearJacobian(leftHeel, localCOM); // Sagittal deviation double error = deviation[0]; From f38907f89c5819cfa6c86556d76c61afe5fda8e2 Mon Sep 17 00:00:00 2001 From: Karen Liu Date: Wed, 1 Jul 2015 16:06:32 -0400 Subject: [PATCH 44/65] included random library in tutorialCollisions --- tutorials/tutorialCollisions-Finished.cpp | 2 +- tutorials/tutorialCollisions.cpp | 5 +++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/tutorials/tutorialCollisions-Finished.cpp b/tutorials/tutorialCollisions-Finished.cpp index 12538b58d27ca..ce2bb73b46020 100644 --- a/tutorials/tutorialCollisions-Finished.cpp +++ b/tutorials/tutorialCollisions-Finished.cpp @@ -33,7 +33,7 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ - +#include #include "dart/dart.h" const double default_shape_density = 1000; // kg/m^3 diff --git a/tutorials/tutorialCollisions.cpp b/tutorials/tutorialCollisions.cpp index 104cb7575d2bd..bfe7cfce99fe0 100644 --- a/tutorials/tutorialCollisions.cpp +++ b/tutorials/tutorialCollisions.cpp @@ -33,7 +33,7 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ - +#include #include "dart/dart.h" const double default_shape_density = 1000; // kg/m^3 @@ -69,6 +69,7 @@ const double default_soft_damping = 5.0; using namespace dart::dynamics; using namespace dart::simulation; +using namespace dart::gui; void setupRing(const SkeletonPtr& /*ring*/) { @@ -85,7 +86,7 @@ void setupRing(const SkeletonPtr& /*ring*/) // Lesson 4c } -class MyWindow : public dart::gui::SimWindow +class MyWindow : public SimWindow { public: From 116a3ae2d2f538c3d0345aa258ceaa39f11b74f2 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Wed, 1 Jul 2015 16:11:35 -0400 Subject: [PATCH 45/65] fixed the collisions tutorial to be more realistic --- tutorials/tutorialCollisions-Finished.cpp | 21 +++++++++++++++++---- tutorials/tutorialCollisions.cpp | 11 +++++++++-- 2 files changed, 26 insertions(+), 6 deletions(-) diff --git a/tutorials/tutorialCollisions-Finished.cpp b/tutorials/tutorialCollisions-Finished.cpp index 12538b58d27ca..23da9e98e8969 100644 --- a/tutorials/tutorialCollisions-Finished.cpp +++ b/tutorials/tutorialCollisions-Finished.cpp @@ -53,8 +53,9 @@ const double default_launch_angle = 45.0*M_PI/180.0; // rad const double maximum_start_w = 6*M_PI; // rad/s const double default_start_w = 3*M_PI; // rad/s -const double default_spring_stiffness = 0.5; -const double default_damping_coefficient = 0.05; +const double ring_spring_stiffness = 0.5; +const double ring_damping_coefficient = 0.05; +const double default_damping_coefficient = 0.005; const double default_ground_width = 2; const double default_wall_thickness = 0.1; @@ -76,8 +77,8 @@ void setupRing(const SkeletonPtr& ring) for(size_t i=6; i < ring->getNumDofs(); ++i) { DegreeOfFreedom* dof = ring->getDof(i); - dof->setSpringStiffness(default_spring_stiffness); - dof->setDampingCoefficient(default_damping_coefficient); + dof->setSpringStiffness(ring_spring_stiffness); + dof->setDampingCoefficient(ring_damping_coefficient); } // Compute the joint angle needed to form a ring @@ -395,6 +396,13 @@ BodyNode* addRigidBody(const SkeletonPtr& chain, const std::string& name, // Set the coefficient of restitution to make the body more bouncy bn->setRestitutionCoeff(default_restitution); + if(parent) + { + Joint* joint = bn->getParentJoint(); + for(size_t i=0; i < joint->getNumDofs(); ++i) + joint->getDof(i)->setDampingCoefficient(default_damping_coefficient); + } + return bn; } @@ -462,6 +470,11 @@ BodyNode* addSoftBody(const SkeletonPtr& chain, const std::string& name, SoftBodyNode* bn = chain->createJointAndBodyNodePair( parent, joint_properties, body_properties).second; + Inertia inertia; + inertia.setMoment(1e-8*Eigen::Matrix3d::Identity()); + inertia.setMass(1e-8); + bn->setInertia(inertia); + return bn; } diff --git a/tutorials/tutorialCollisions.cpp b/tutorials/tutorialCollisions.cpp index 104cb7575d2bd..3c362e99b9cdd 100644 --- a/tutorials/tutorialCollisions.cpp +++ b/tutorials/tutorialCollisions.cpp @@ -53,8 +53,9 @@ const double default_launch_angle = 45.0*M_PI/180.0; // rad const double maximum_start_w = 6*M_PI; // rad/s const double default_start_w = 3*M_PI; // rad/s -const double default_spring_stiffness = 0.5; -const double default_damping_coefficient = 0.05; +const double ring_spring_stiffness = 0.5; +const double ring_damping_coefficient = 0.05; +const double default_damping_coefficient = 0.005; const double default_ground_width = 2; const double default_wall_thickness = 0.1; @@ -280,6 +281,9 @@ BodyNode* addRigidBody(const SkeletonPtr& /*chain*/, const std::string& /*name*/ // Set the coefficient of restitution to make the body more bouncy // Lesson 1e + // Set damping to make the simulation more stable + // Lesson 1f + return bn; } @@ -304,6 +308,9 @@ BodyNode* addSoftBody(const SkeletonPtr& /*chain*/, const std::string& /*name*/, // Lesson 2c SoftBodyNode* bn = nullptr; + // Zero out the inertia for the underlying BodyNode + // Lesson 2d + return bn; } From 9e91b31208a8c03f8543343bca56b1954ef11638 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Wed, 1 Jul 2015 16:11:59 -0400 Subject: [PATCH 46/65] update collisions tutorial documentation --- docs/readthedocs/tutorials/collisions.md | 40 +++++++++++++++++++++--- 1 file changed, 36 insertions(+), 4 deletions(-) diff --git a/docs/readthedocs/tutorials/collisions.md b/docs/readthedocs/tutorials/collisions.md index cef56c9ce8d24..253e131b8d3be 100644 --- a/docs/readthedocs/tutorials/collisions.md +++ b/docs/readthedocs/tutorials/collisions.md @@ -257,6 +257,25 @@ This is very easily done with the following function: bn->setRestitutionCoeff(default_restitution); ``` +### Lesson 1f: Set the damping coefficient + +In real life, joints have friction. This pulls energy out of systems over time, +and makes those systems more stable. In our simulation, we'll ignore air +friction, but we'll add friction in the joints between bodies in order to have +better numerical and dynamic stability: + +```cpp +if(parent) +{ + Joint* joint = bn->getParentJoint(); + for(size_t i=0; i < joint->getNumDofs(); ++i) + joint->getDof(i)->setDampingCoefficient(default_damping_coefficient); +} +``` + +If this BodyNode has a parent BodyNode, then we set damping coefficients of its +Joint to a default value. + # Lesson 2: Creating a soft body Find the templated function named ``addSoftBody``. This function will have a @@ -366,9 +385,22 @@ SoftBodyNode* bn = chain->createJointAndBodyNodePair( Notice that this time it will return a ``SoftBodyNode`` pointer rather than a normal ``BodyNode`` pointer. This is one of the advantages of templates! -The ``SoftBodyNodeHelper`` function will automagically set up the inertia and shape -properties of the SoftBodyNode ahead of time, so we're done in this function. +### Lesson 2d: Zero out the BodyNode inertia +A SoftBodyNode has two sources of inertia: the underlying inertia of the +standard BodyNode class, and the point mass inertias of its soft skin. In our +case, we only want the point mass inertias, so we should zero out the standard +BodyNode inertia. However, zeroing out inertia values can be very dangerous, +because it can easily result in singularities. So instead of completely zeroing +them out, we will just make them small enough that they don't impact the +simulation: + +```cpp +Inertia inertia; +inertia.setMoment(1e-8*Eigen::Matrix3d::Identity()); +inertia.setMass(1e-8); +bn->setInertia(inertia); +``` # Lesson 3: Setting initial conditions and taking advantage of Frames @@ -603,8 +635,8 @@ in the air. But all the rest of the degrees of freedom should be set: for(size_t i=6; i < ring->getNumDofs(); ++i) { DegreeOfFreedom* dof = ring->getDof(i); - dof->setSpringStiffness(default_spring_stiffness); - dof->setDampingCoefficient(default_damping_coefficient); + dof->setSpringStiffness(ring_spring_stiffness); + dof->setDampingCoefficient(ring_damping_coefficient); } ``` From 0c5ed5ae614bcaa991d6effaff386ad33788157e Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Wed, 1 Jul 2015 16:12:41 -0400 Subject: [PATCH 47/65] some small cleanup --- tutorials/tutorialMultiPendulum-Finished.cpp | 51 +++++++++----------- tutorials/tutorialMultiPendulum.cpp | 41 ++++++++++------ 2 files changed, 49 insertions(+), 43 deletions(-) diff --git a/tutorials/tutorialMultiPendulum-Finished.cpp b/tutorials/tutorialMultiPendulum-Finished.cpp index e81e90e63a250..2b2cc879debe9 100644 --- a/tutorials/tutorialMultiPendulum-Finished.cpp +++ b/tutorials/tutorialMultiPendulum-Finished.cpp @@ -272,33 +272,19 @@ class MyWindow : public dart::gui::SimWindow } } - if(mBodyForce) + if(!mBodyForce) { - // Apply body forces based on user input, and color the body shape red - for(size_t i = 0; i < mPendulum->getNumBodyNodes(); ++i) + // Apply joint torques based on user input, and color the Joint shape red + for(size_t i = 0; i < mPendulum->getNumDofs(); ++i) { if(mForceCountDown[i] > 0) { - BodyNode* bn = mPendulum->getBodyNode(i); - const ShapePtr& shape = bn->getVisualizationShape(1); - shape->setColor(dart::Color::Red()); - - if(mPositiveSign) - { - bn->addExtForce( - default_force * Eigen::Vector3d::UnitX(), - Eigen::Vector3d(-default_width / 2.0, 0.0, default_height / 2.0), - true, true); - } - else - { - bn->addExtForce( - -default_force * Eigen::Vector3d::UnitX(), - Eigen::Vector3d(default_width / 2.0, 0.0, default_height / 2.0), - true, true); - } + DegreeOfFreedom* dof = mPendulum->getDof(i); + dof->setForce( mPositiveSign? default_torque : -default_torque ); - bn->addVisualizationShape(mArrow); + BodyNode* bn = dof->getChildBodyNode(); + const ShapePtr& shape = bn->getVisualizationShape(0); + shape->setColor(dart::Color::Red()); --mForceCountDown[i]; } @@ -306,17 +292,26 @@ class MyWindow : public dart::gui::SimWindow } else { - // Apply joint torques based on user input, and color the Joint shape red - for(size_t i = 0; i < mPendulum->getNumDofs(); ++i) + // Apply body forces based on user input, and color the body shape red + for(size_t i = 0; i < mPendulum->getNumBodyNodes(); ++i) { if(mForceCountDown[i] > 0) { - DegreeOfFreedom* dof = mPendulum->getDof(i); - BodyNode* bn = dof->getChildBodyNode(); - const ShapePtr& shape = bn->getVisualizationShape(0); + BodyNode* bn = mPendulum->getBodyNode(i); + + Eigen::Vector3d force = default_force * Eigen::Vector3d::UnitX(); + Eigen::Vector3d location(-default_width / 2.0, 0.0, default_height / 2.0); + if(!mPositiveSign) + { + force = -force; + location[0] = -location[0]; + } + bn->addExtForce(force, location, true, true); - dof->setForce(mPositiveSign ? default_torque : -default_torque); + const ShapePtr& shape = bn->getVisualizationShape(1); shape->setColor(dart::Color::Red()); + bn->addVisualizationShape(mArrow); + --mForceCountDown[i]; } } diff --git a/tutorials/tutorialMultiPendulum.cpp b/tutorials/tutorialMultiPendulum.cpp index 156568942eb80..e2c4e9a529b07 100644 --- a/tutorials/tutorialMultiPendulum.cpp +++ b/tutorials/tutorialMultiPendulum.cpp @@ -101,36 +101,37 @@ class MyWindow : public dart::gui::SimWindow } } - void applyForce(size_t) + void applyForce(size_t index) { - // Lesson 1 + if(index < mForceCountDown.size()) + mForceCountDown[index] = default_countdown; } void changeRestPosition(double) { - // Lesson 2 + // Lesson 2a } void changeStiffness(double) { - // Lesson 3 + // Lesson 2b } void changeDamping(double) { - // Lesson 4 + // Lesson 2c } /// Add a constraint to attach the final link to the world void addConstraint() { - // Lesson 5 + // Lesson 3a } /// Remove any existing constraint, allowing the pendulum to flail freely void removeConstraint() { - // Lesson 5 + // Lesson 3b } /// Handle keyboard input @@ -215,22 +216,32 @@ class MyWindow : public dart::gui::SimWindow void timeStepping() override { // Reset all the shapes to be Blue - // Lesson 1 + // Lesson 1a - if(mBodyForce) + if(!mBodyForce) { - // Apply body forces based on user input, and color the body shape red - for(size_t i = 0; i < mPendulum->getNumBodyNodes(); ++i) + // Apply joint torques based on user input, and color the Joint shape red + for(size_t i = 0; i < mPendulum->getNumDofs(); ++i) { - // Lesson 1b + if(mForceCountDown[i] > 0) + { + // Lesson 1b + + --mForceCountDown[i]; + } } } else { - // Apply joint torques based on user input, and color the Joint shape red - for(size_t i = 0; i < mPendulum->getNumDofs(); ++i) + // Apply body forces based on user input, and color the body shape red + for(size_t i = 0; i < mPendulum->getNumBodyNodes(); ++i) { - // Lesson 1a + if(mForceCountDown[i] > 0) + { + // Lesson 1c + + --mForceCountDown[i]; + } } } From 7f5b5e39a94a936e1cb678a94cd69ac2cad6bf08 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Wed, 1 Jul 2015 16:13:04 -0400 Subject: [PATCH 48/65] beginning documentation for pendulum tutorial --- docs/readthedocs/tutorials/multi-pendulum.md | 202 ++++++++++++++++++- 1 file changed, 196 insertions(+), 6 deletions(-) diff --git a/docs/readthedocs/tutorials/multi-pendulum.md b/docs/readthedocs/tutorials/multi-pendulum.md index 0063e634a11e1..b14a342c13428 100644 --- a/docs/readthedocs/tutorials/multi-pendulum.md +++ b/docs/readthedocs/tutorials/multi-pendulum.md @@ -1,13 +1,203 @@ # Overview -# Section 1 +This tutorial will demonstrate some basic interaction with DART's dynamics +API during simulation. This will show you how to: + +- Change the colors of shapes +- Add/remove shapes from visualization +- Apply internal forces in the joints +- Apply external forces to the bodies +- Alter the implicit spring and damping properties of joints +- Add/remove dynamic constraints + +# Lesson 1: Changing shapes and applying forces + +We have a pendulum with five bodies, and we want to be able to apply forces to +them during simulation. Additionally, we want to visualize these forces so we +can more easily interpret what is happening in the simulation. For this reason, +we'll discuss visualizing and forces at the same time. + +### Lesson 1a: Reset everything to default appearance + +At each step, we'll want to make sure that everything starts out with its default +appearance. The default is for everything to be blue and there not to be any +arrow attached to any body. + +Find the function named ``timeStepping`` in the ``MyWindow`` class. The top of +this function is where we will want to reset everything to the default appearance. + +In DART, an articulated dynamics model is represented by a ``Skeleton``. A +Skeleton is a structure that consists of ``BodyNode``s (bodies) which are +connected by ``Joint``s. Every Joint has a child BodyNode, and every BodyNode +has a parent Joint. Even the root BodyNode has a Joint that attaches it to the +World. + +Each BodyNode contains visualization ``Shape``s that will be rendered during +simulation. In our case, each BodyNode has two shapes: + +- One shape to visualize the parent joint +- One shape to visualize the body + +The default appearance for everything is to be colored blue, so we'll want to +iterate through these two Shapes in each BodyNode, setting their colors to blue. + +```cpp +for(size_t i = 0; i < mPendulum->getNumBodyNodes(); ++i) +{ + BodyNode* bn = mPendulum->getBodyNode(i); + for(size_t j = 0; j < 2; ++j) + { + const ShapePtr& shape = bn->getVisualizationShape(j); + + shape->setColor(dart::Color::Blue()); + } + + // TODO: Remove any arrows +} +``` + +Additionally, there is the possibility that some BodyNodes will have an arrow +shape attached if the user had been applying an external body force to it. By +default, this arrow should not be attached, so in the outer for-loop, we should +check for arrows and remove them: + +```cpp +if(bn->getNumVisualizationShapes() == 3) +{ + bn->removeVisualizationShape(mArrow); +} +``` + +Now everything will be reset to the default appearance. + +### Lesson 1b: Apply joint torques based on user input + +The ``MyWindow`` class in this tutorial has a variable called ``mForceCountDown`` +which is a ``std::vector`` whose entries get set to a value of +``default_countdown`` each time the user presses a number key. If an entry in +``mForceCountDown`` is greater than zero, then that implies that the user wants +a force to be applied for that entry. + +There are two ways that forces can be applied: + +- As an internal joint force +- As an external body force + +First we'll consider applying a Joint force. Inside the for-loop that goes +through each ``DegreeOfFreedom`` using ``getNumDofs()``, there is an +if-statement for ``mForceCountDown``. In that if-statement, we'll grab the +relevant DegreeOfFreedom and set its generalized (joint) force: + +```cpp +DegreeOfFreedom* dof = mPendulum->getDof(i); +dof->setForce( mPositiveSign? default_torque : -default_torque ); +``` + +The ``mPositiveSign`` boolean gets toggled when the user presses the minus sign +'-' key. We use this boolean to decide whether the applied force should be +positive or negative. + +Now we'll want to visualize the fact that a Joint force is being applied. We'll +do this by highlighting the joint with the color red. First we'll grab the Shape +that corresponds to this Joint: + +```cpp +BodyNode* bn = dof->getChildBodyNode(); +const ShapePtr& shape = bn->getVisualizationShape(0); +``` + +Because of the way the pendulum bodies were constructed, we trust that the +zeroth indexed visualization shape will be the shape that depicts the joint. +So now we will color it red: + +```cpp +shape->setColor(dart::Color::Red()); +``` + +### Lesson 1c: Apply body forces based on user input + +If mBodyForce is true, we'll want to apply an external force to the body instead +of an internal force in the joint. First, inside the for-loop that iterates +through each ``BodyNode`` using ``getNumBodyNodes()``, there is an if-statement +for ``mForceCountDown``. In that if-statement, we'll grab the relevant BodyNode: + +```cpp +BodyNode* bn = mPendulum->getBodyNode(i); +``` + +Now we'll create an ``Eigen::Vector3d`` that describes the force and another one +that describes the location for that force. An ``Eigen::Vector3d`` is the Eigen +C++ library's version of a three-dimensional mathematical vector. Note that the +``d`` at the end of the name stands for ``double``, not for "dimension". An +Eigen::Vector3f would be a three-dimensional vector of floats, and an +Eigen::Vector3i would be a three-dimensional vector of integers. + +```cpp +Eigen::Vector3d force = default_force * Eigen::Vector3d::UnitX(); +Eigen::Vector3d location(-default_width / 2.0, 0.0, default_height / 2.0); +``` + +The force will have a magnitude of ``default_force`` and it will point in the +positive x-direction. The location of the force will be in the center of the +negative x side of the body, as if a finger on the negative side is pushing the +body in the positive direction. However, we need to account for sign changes: + +```cpp +if(!mPositiveSign) +{ + force = -force; + location[0] = -location[0]; +} +``` + +That will flip the signs whenever the user is requesting a negative force. + +Now we can add the external force: + +```cpp +bn->addExtForce(force, location, true, true); +``` + +The two ``true`` booleans at the end are indicating to DART that both the force +and the location vectors are being expressed with respect to the body frame. + +Now we'll want to visualize the force being applied to the body. First, we'll +grab the Shape for the body and color it red: + +```cpp +const ShapePtr& shape = bn->getVisualizationShape(1); +shape->setColor(dart::Color::Red()); +``` + +Last time we grabbed the 0-index visualization shape, because we trusted that +it was the shape that represented the parent Joint. This time we're grabbing +the 1-index visualization shape, because we trust that it is the shape for the +body. + +Now we'll want to add an arrow to the visualization shapes of the body to +represent the applied force. The ``MyWindow`` class already provides the arrow +shape; we just need to add it: + +```cpp +bn->addVisualizationShape(mArrow); +``` + + + + + + + + + + + + + + + -## Subsection 1 -## Subsection 2 -# Section 2 -## Subsection 1 -## Subsection 2 From 7a67fd1ebca4bcbfd472334627024eccd9a9111c Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Wed, 1 Jul 2015 19:40:01 -0400 Subject: [PATCH 49/65] finished pendulum tutorial documentation --- docs/readthedocs/tutorials/multi-pendulum.md | 129 +++++++++++++++++++ tutorials/tutorialMultiPendulum-Finished.cpp | 7 +- tutorials/tutorialMultiPendulum.cpp | 4 +- 3 files changed, 135 insertions(+), 5 deletions(-) diff --git a/docs/readthedocs/tutorials/multi-pendulum.md b/docs/readthedocs/tutorials/multi-pendulum.md index b14a342c13428..ee44c611e3bcc 100644 --- a/docs/readthedocs/tutorials/multi-pendulum.md +++ b/docs/readthedocs/tutorials/multi-pendulum.md @@ -182,22 +182,151 @@ shape; we just need to add it: bn->addVisualizationShape(mArrow); ``` +# Lesson 2: Set spring and damping properties for joints +DART allows Joints to have implicit spring and damping properties. By default, +these properties are zeroed out, so a joint will only exhibit the forces that +are given to it by the ``Joint::setForces`` function. However, you can give a +non-zero spring coefficient to a joint so that it behaves according to Hooke's +Law, and you can give a non-zero damping coefficient to a joint which will +result in linear damping. These forces are computed using implicit methods in +order to improve numerical stability. +### Lesson 2a: Set joint spring rest position +First let's see how to get and set the rest positions. +Find the function named ``changeRestPosition`` in the ``MyWindow`` class. This +function will be called whenever the user presses the 'q' or 'a' button. We want +those buttons to curl and uncurl the rest positions for the pendulum. To start, +we'll go through all the generalized coordinates and change their rest positions +by ``delta``: +```cpp +for(size_t i = 0; i < mPendulum->getNumDofs(); ++i) +{ + DegreeOfFreedom* dof = mPendulum->getDof(i); + double q0 = dof->getRestPosition() + delta; + + dof->setRestPosition(q0); +} +``` + +However, it's important to note that the system can become somewhat unstable if +we allow it to curl up too much, so let's put a limit on the magnitude of the +rest angle. Right before ``dof->setRestPosition(q0);`` we can put: +```cpp +if(std::abs(q0) > 90.0 * M_PI / 180.0) + q0 = (q0 > 0)? (90.0 * M_PI / 180.0) : -(90.0 * M_PI / 180.0); +``` + +And there's one last thing to consider: the first joint of the pendulum is a +BallJoint. BallJoints have three degrees of freedom, which means if we alter +the rest positions of *all* of the pendulum's degrees of freedom, then the +pendulum will end up curling out of the x-z plane. You can allow this to happen +if you want, or you can prevent it from happening by zeroing out the rest +positions of the BallJoint's other two degrees of freedom: + +```cpp +mPendulum->getDof(0)->setRestPosition(0.0); +mPendulum->getDof(2)->setRestPosition(0.0); +``` +### Lesson 2b: Set joint spring stiffness +Changing the rest position does not accomplish anything without having any +spring stiffness. We can change the spring stiffness as follows: +```cpp +for(size_t i = 0; i < mPendulum->getNumDofs(); ++i) +{ + DegreeOfFreedom* dof = mPendulum->getDof(i); + double stiffness = dof->getSpringStiffness() + delta; + dof->setSpringStiffness(stiffness); +} +``` +However, it's important to realize that if the spring stiffness were ever to +become negative, we would get some very nasty explosive behavior. It's also a +bad idea to just trust the user to avoid decrementing it into being negative. +So before the line ``dof->setSpringStiffness(stiffness);`` you'll want to put: +```cpp +if(stiffness < 0.0) + stiffness = 0.0; +``` +### Lesson 2c: Set joint damping +Joint damping can be thought of as friction inside the joint actuator. It +applies a resistive force to the joint which is proportional to the generalized +velocities of the joint. This draws energy out of the system and generally +results in more stable behavior. +The API for getting and setting the damping is just like the API for stiffness: +```cpp +for(size_t i = 0; i < mPendulum->getNumDofs(); ++i) +{ + DegreeOfFreedom* dof = mPendulum->getDof(i); + double damping = dof->getDampingCoefficient() + delta; + if(damping < 0.0) + damping = 0.0; + dof->setDampingCoefficient(damping); +} +``` +Again, we want to make sure that the damping coefficient is never negative. In +fact, a negative damping coefficient would be far more harmful than a negative +stiffness coefficient. +# Lesson 3: Adding and removing dynamic constraints + +Dynamic constraints in DART allow you to attach two BodyNodes together according +to a selection of a few different Joint-style constraints. This allows you to +create closed loop constraints, which is not possible using standard Joints. +You can also create a dynamic constraint that attaches a BodyNode to the World +instead of to another BodyNode. + +In our case, we want to attach the last BodyNode to the World with a BallJoint +style constraint whenever the function ``addConstraint()`` gets called. First, +let's grab the last BodyNode in the pendulum: + +```cpp +BodyNode* tip = mPendulum->getBodyNode(mPendulum->getNumBodyNodes() - 1); +``` + +Now we'll want to compute the location that the constraint should have. We want +to connect the very end of the tip to the world, so the location would be: + +```cpp +Eigen::Vector3d location = + tip->getTransform() * Eigen::Vector3d(0.0, 0.0, default_height); +``` + +Now we can create the BallJointConstraint: + +```cpp +mBallConstraint = new dart::constraint::BallJointConstraint(tip, location); +``` + +And then add it to the world: + +```cpp +mWorld->getConstraintSolver()->addConstraint(mBallConstraint); +``` + +Now we also want to be able to remove this constraint. In the function +``removeConstraint()``, we can put the following code: + +```cpp +mWorld->getConstraintSolver()->removeConstraint(mBallConstraint); +delete mBallConstraint; +mBallConstraint = nullptr; +``` +Currently DART does not use smart pointers for dynamic constraints, so they +need to be explicitly deleted. This may be revised in a later version of DART. +**Now you are ready to run the demo!** diff --git a/tutorials/tutorialMultiPendulum-Finished.cpp b/tutorials/tutorialMultiPendulum-Finished.cpp index 2b2cc879debe9..c4fc738c39f96 100644 --- a/tutorials/tutorialMultiPendulum-Finished.cpp +++ b/tutorials/tutorialMultiPendulum-Finished.cpp @@ -117,7 +117,7 @@ class MyWindow : public dart::gui::SimWindow // The system becomes numerically unstable when the rest position exceeds // 90 degrees if(std::abs(q0) > 90.0 * M_PI / 180.0) - q0 = q0 > 0 ? 90.0 * M_PI / 180.0 : -90.0 * M_PI / 180.0; + q0 = (q0 > 0)? (90.0 * M_PI / 180.0) : -(90.0 * M_PI / 180.0); dof->setRestPosition(q0); } @@ -158,8 +158,9 @@ class MyWindow : public dart::gui::SimWindow BodyNode* tip = mPendulum->getBodyNode(mPendulum->getNumBodyNodes() - 1); // Attach the last link to the world - mBallConstraint = new dart::constraint::BallJointConstraint( - tip, tip->getTransform() * Eigen::Vector3d(0.0, 0.0, default_height)); + Eigen::Vector3d location = + tip->getTransform() * Eigen::Vector3d(0.0, 0.0, default_height); + mBallConstraint = new dart::constraint::BallJointConstraint(tip, location); mWorld->getConstraintSolver()->addConstraint(mBallConstraint); } diff --git a/tutorials/tutorialMultiPendulum.cpp b/tutorials/tutorialMultiPendulum.cpp index e2c4e9a529b07..9f1fe50381fc1 100644 --- a/tutorials/tutorialMultiPendulum.cpp +++ b/tutorials/tutorialMultiPendulum.cpp @@ -125,13 +125,13 @@ class MyWindow : public dart::gui::SimWindow /// Add a constraint to attach the final link to the world void addConstraint() { - // Lesson 3a + // Lesson 3 } /// Remove any existing constraint, allowing the pendulum to flail freely void removeConstraint() { - // Lesson 3b + // Lesson 3 } /// Handle keyboard input From eee1c767d197793d63ab379e4f2dd82b575e81a5 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Wed, 1 Jul 2015 20:56:04 -0400 Subject: [PATCH 50/65] beginning dominoes tutorial --- docs/readthedocs/tutorials/dominoes.md | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/docs/readthedocs/tutorials/dominoes.md b/docs/readthedocs/tutorials/dominoes.md index 0063e634a11e1..3d920e14789b6 100644 --- a/docs/readthedocs/tutorials/dominoes.md +++ b/docs/readthedocs/tutorials/dominoes.md @@ -1,13 +1,14 @@ # Overview -# Section 1 +This tutorial will demonstrate some of the more advanced features of DART's +dynamics API which allow you to write robust controllers that work for real +dynamic systems, such as robotic manipulators. We will show you how to: -## Subsection 1 +- Clone Skeletons +- Load a URDF +- Write a stable PD controller w/ gravity and coriolis compensation +- Write an operational space controller -## Subsection 2 +# Lesson 1 -# Section 2 -## Subsection 1 - -## Subsection 2 From cc9130142f9e79323833a2122077682efc1a3cbe Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Thu, 2 Jul 2015 15:27:55 -0400 Subject: [PATCH 51/65] fixed up some inconsistencies --- docs/readthedocs/tutorials/collisions.md | 66 +++++++++++++++++++++-- tutorials/tutorialCollisions-Finished.cpp | 35 ++++++++---- tutorials/tutorialCollisions.cpp | 10 ++-- 3 files changed, 94 insertions(+), 17 deletions(-) diff --git a/docs/readthedocs/tutorials/collisions.md b/docs/readthedocs/tutorials/collisions.md index 253e131b8d3be..4218ae688b58f 100644 --- a/docs/readthedocs/tutorials/collisions.md +++ b/docs/readthedocs/tutorials/collisions.md @@ -326,7 +326,8 @@ For the SOFT_BOX: ```cpp double width = default_shape_width, height = default_shape_height; Eigen::Vector3d dims(width, width, height); -double mass = default_shape_density * dims[0]*dims[1]*dims[2]; +double mass = 2*dims[0]*dims[1] + 2*dims[0]*dims[2] + 2*dims[1]*dims[2]; +mass *= default_shape_density * default_skin_thickness; soft_properties = SoftBodyNodeHelper::makeBoxProperties( dims, Eigen::Isometry3d::Identity(), Eigen::Vector3i(4,4,4), mass); ``` @@ -335,7 +336,12 @@ For the SOFT_CYLINDER: ```cpp double radius = default_shape_width/2.0; double height = default_shape_height; -double mass = default_shape_density * height * pow(M_PI*radius, 2); +// Mass of center +double mass = default_shape_density * height * 2*M_PI*radius + * default_skin_thickness; +// Mass of top and bottom +mass += 2 * default_shape_density * M_PI*pow(radius,2) + * default_skin_thickness; soft_properties = SoftBodyNodeHelper::makeCylinderProperties( radius, height, 8, 3, 2, mass); ``` @@ -344,7 +350,8 @@ And for the SOFT_ELLIPSOID: ```cpp double radius = default_shape_height/2.0; Eigen::Vector3d dims = 2*radius*Eigen::Vector3d::Ones(); -double mass = default_shape_density * 4.0/3.0*M_PI*pow(radius,3); +double mass = default_shape_density * 4.0*M_PI*pow(radius, 2) + * default_skin_thickness; soft_properties = SoftBodyNodeHelper::makeEllipsoidProperties( dims, 6, 6, mass); ``` @@ -402,6 +409,59 @@ inertia.setMass(1e-8); bn->setInertia(inertia); ``` +### Lesson 2e: Make the shape transparent + +To help us visually distinguish between the soft and rigid portions of a body, +we can make the soft part of the shape transparent. Upon creation, a SoftBodyNode +will have exactly one visualization shape: the soft shape visualizer. We can +grab that shape and reduce the value of its alpha channel: + +``` +Eigen::Vector4d color = bn->getVisualizationShape(0)->getRGBA(); +color[3] = 0.4; +bn->getVisualizationShape(0)->setRGBA(color); +``` + +### Lesson 2f: Add a rigid body attached by a WeldJoint + +To make the shape more interesting, we can attach a protruding rigid body using +a WeldJoint. Find the ``createHybridBody()`` function and see where we call the +``addSoftBody`` function. Just below this, we'll create a new rigid body with a +WeldJoint attachment: + +```cpp +bn = hybrid->createJointAndBodyNodePair(bn).second; +bn->setName("rigid box"); +``` + +Now we can give the new rigid BodyNode a regular box shape: + +```cpp +double box_shape_height = default_shape_height; +std::shared_ptr box = std::make_shared( + box_shape_height*Eigen::Vector3d::Ones()); + +bn->addCollisionShape(box); +bn->addVisualizationShape(box); +``` + +To make the box protrude, we'll shift it away from the center of its parent: + +```cpp +Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); +tf.translation() = Eigen::Vector3d(box_shape_height/2.0, 0, 0); +bn->getParentJoint()->setTransformFromParentBodyNode(tf); +``` + +And be sure to set its inertia, or else the simulation will not be realistic: + +```cpp +Inertia inertia; +inertia.setMass(default_shape_density * box->getVolume()); +inertia.setMoment(box->computeInertia(inertia.getMass())); +bn->setInertia(inertia); +``` + # Lesson 3: Setting initial conditions and taking advantage of Frames Find the ``addObject`` function in the ``MyWorld`` class. This function will diff --git a/tutorials/tutorialCollisions-Finished.cpp b/tutorials/tutorialCollisions-Finished.cpp index 23da9e98e8969..b20ad2733e060 100644 --- a/tutorials/tutorialCollisions-Finished.cpp +++ b/tutorials/tutorialCollisions-Finished.cpp @@ -39,6 +39,7 @@ const double default_shape_density = 1000; // kg/m^3 const double default_shape_height = 0.1; // m const double default_shape_width = 0.03; // m +const double default_skin_thickness = 1e-3; // m const double default_start_height = 0.4; // m @@ -55,7 +56,7 @@ const double default_start_w = 3*M_PI; // rad/s const double ring_spring_stiffness = 0.5; const double ring_damping_coefficient = 0.05; -const double default_damping_coefficient = 0.005; +const double default_damping_coefficient = 0.001; const double default_ground_width = 2; const double default_wall_thickness = 0.1; @@ -65,7 +66,7 @@ const double default_spawn_range = 0.9*default_ground_width/2; const double default_restitution = 0.6; const double default_vertex_stiffness = 100.0; -const double default_edge_stiffness = 10.0; +const double default_edge_stiffness = 1.0; const double default_soft_damping = 5.0; using namespace dart::dynamics; @@ -417,8 +418,6 @@ template BodyNode* addSoftBody(const SkeletonPtr& chain, const std::string& name, SoftShapeType type, BodyNode* parent = nullptr) { - // Compute the transform for the center of the object - // Set the Joint properties typename JointType::Properties joint_properties; joint_properties.mName = name+"_joint"; @@ -440,7 +439,8 @@ BodyNode* addSoftBody(const SkeletonPtr& chain, const std::string& name, { double width = default_shape_width, height = default_shape_height; Eigen::Vector3d dims(width, width, height); - double mass = default_shape_density * dims[0]*dims[1]*dims[2]; + double mass = 2*dims[0]*dims[1] + 2*dims[0]*dims[2] + 2*dims[1]*dims[2]; + mass *= default_shape_density * default_skin_thickness; soft_properties = SoftBodyNodeHelper::makeBoxProperties( dims, Eigen::Isometry3d::Identity(), Eigen::Vector3i(4,4,4), mass); } @@ -448,7 +448,12 @@ BodyNode* addSoftBody(const SkeletonPtr& chain, const std::string& name, { double radius = default_shape_width/2.0; double height = default_shape_height; - double mass = default_shape_density * height * pow(M_PI*radius, 2); + // Mass of center + double mass = default_shape_density * height * 2*M_PI*radius + * default_skin_thickness; + // Mass of top and bottom + mass += 2 * default_shape_density * M_PI*pow(radius,2) + * default_skin_thickness; soft_properties = SoftBodyNodeHelper::makeCylinderProperties( radius, height, 8, 3, 2, mass); } @@ -456,7 +461,8 @@ BodyNode* addSoftBody(const SkeletonPtr& chain, const std::string& name, { double radius = default_shape_height/2.0; Eigen::Vector3d dims = 2*radius*Eigen::Vector3d::Ones(); - double mass = default_shape_density * 4.0/3.0*M_PI*pow(radius,3); + double mass = default_shape_density * 4.0*M_PI*pow(radius, 2) + * default_skin_thickness; soft_properties = SoftBodyNodeHelper::makeEllipsoidProperties( dims, 6, 6, mass); } @@ -470,11 +476,17 @@ BodyNode* addSoftBody(const SkeletonPtr& chain, const std::string& name, SoftBodyNode* bn = chain->createJointAndBodyNodePair( parent, joint_properties, body_properties).second; + // Zero out the inertia for the underlying BodyNode Inertia inertia; inertia.setMoment(1e-8*Eigen::Matrix3d::Identity()); inertia.setMass(1e-8); bn->setInertia(inertia); + // Make the shape transparent + Eigen::Vector4d color = bn->getVisualizationShape(0)->getRGBA(); + color[3] = 0.4; + bn->getVisualizationShape(0)->setRGBA(color); + return bn; } @@ -546,15 +558,16 @@ SkeletonPtr createHybridBody() double box_shape_height = default_shape_height; std::shared_ptr box = std::make_shared( box_shape_height*Eigen::Vector3d::Ones()); - Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); - tf.translation() = Eigen::Vector3d(box_shape_height/2.0, 0, 0); - box->setLocalTransform(tf); bn->addCollisionShape(box); bn->addVisualizationShape(box); + Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); + tf.translation() = Eigen::Vector3d(box_shape_height/2.0, 0, 0); + bn->getParentJoint()->setTransformFromParentBodyNode(tf); + Inertia inertia; - inertia.setMass(default_shape_density*pow(box_shape_height,3)); + inertia.setMass(default_shape_density * box->getVolume()); inertia.setMoment(box->computeInertia(inertia.getMass())); bn->setInertia(inertia); diff --git a/tutorials/tutorialCollisions.cpp b/tutorials/tutorialCollisions.cpp index 3c362e99b9cdd..73a664ca035ea 100644 --- a/tutorials/tutorialCollisions.cpp +++ b/tutorials/tutorialCollisions.cpp @@ -39,6 +39,7 @@ const double default_shape_density = 1000; // kg/m^3 const double default_shape_height = 0.1; // m const double default_shape_width = 0.03; // m +const double default_skin_thickness = 1e-3; // m const double default_start_height = 0.4; // m @@ -55,7 +56,7 @@ const double default_start_w = 3*M_PI; // rad/s const double ring_spring_stiffness = 0.5; const double ring_damping_coefficient = 0.05; -const double default_damping_coefficient = 0.005; +const double default_damping_coefficient = 0.001; const double default_ground_width = 2; const double default_wall_thickness = 0.1; @@ -65,7 +66,7 @@ const double default_spawn_range = 0.9*default_ground_width/2; const double default_restitution = 0.6; const double default_vertex_stiffness = 100.0; -const double default_edge_stiffness = 10.0; +const double default_edge_stiffness = 1.0; const double default_soft_damping = 5.0; using namespace dart::dynamics; @@ -311,6 +312,9 @@ BodyNode* addSoftBody(const SkeletonPtr& /*chain*/, const std::string& /*name*/, // Zero out the inertia for the underlying BodyNode // Lesson 2d + // Make the shape transparent + // Lesson 2e + return bn; } @@ -376,7 +380,7 @@ SkeletonPtr createHybridBody() /*BodyNode* bn =*/ addSoftBody(hybrid, "soft sphere", SOFT_ELLIPSOID); // Add a rigid body attached by a WeldJoint - // Lesson 2e + // Lesson 2f setAllColors(hybrid, dart::Color::Green()); From b5d6bdde3cde6393acc312b6ac195fa9c8fc82c4 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Thu, 2 Jul 2015 16:24:26 -0400 Subject: [PATCH 52/65] added Fuschia color --- dart/math/Helpers.h | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/dart/math/Helpers.h b/dart/math/Helpers.h index dbb9b76a98726..d79dddb41f57d 100644 --- a/dart/math/Helpers.h +++ b/dart/math/Helpers.h @@ -268,6 +268,16 @@ inline Eigen::Vector3d Red() return Eigen::Vector3d(0.9, 0.1, 0.1); } +inline Eigen::Vector3d Fuschia() +{ + return Eigen::Vector3d(1.0, 0.0, 0.5); +} + +inline Eigen::Vector4d Fuschia(double alpha) +{ + return Eigen::Vector4d(1.0, 0.0, 0.5, alpha); +} + inline Eigen::Vector4d Orange(double alpha) { return Eigen::Vector4d(1.0, 0.63, 0.0, alpha); From b5ee84c625b8ba7175ba7fb939c07d085721f9f9 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Thu, 2 Jul 2015 16:24:40 -0400 Subject: [PATCH 53/65] added a standard soft body shape option --- docs/readthedocs/tutorials/collisions.md | 59 ++++++++++++++++++--- tutorials/tutorialCollisions-Finished.cpp | 63 ++++++++++++++++++----- tutorials/tutorialCollisions.cpp | 44 ++++++++++++---- 3 files changed, 136 insertions(+), 30 deletions(-) diff --git a/docs/readthedocs/tutorials/collisions.md b/docs/readthedocs/tutorials/collisions.md index 4218ae688b58f..833b42e6c76c2 100644 --- a/docs/readthedocs/tutorials/collisions.md +++ b/docs/readthedocs/tutorials/collisions.md @@ -324,7 +324,10 @@ the functions are a bit complicated, so here is how to call it for each type: For the SOFT_BOX: ```cpp -double width = default_shape_width, height = default_shape_height; +// Make a wide and short box +double width = default_shape_height, height = 2*default_shape_width; +Eigen::Vector3d dims(width, width, height); + Eigen::Vector3d dims(width, width, height); double mass = 2*dims[0]*dims[1] + 2*dims[0]*dims[2] + 2*dims[1]*dims[2]; mass *= default_shape_density * default_skin_thickness; @@ -334,8 +337,9 @@ soft_properties = SoftBodyNodeHelper::makeBoxProperties( For the SOFT_CYLINDER: ```cpp -double radius = default_shape_width/2.0; -double height = default_shape_height; +// Make a wide and short cylinder +double radius = default_shape_height/2.0, height = 2*default_shape_width; + // Mass of center double mass = default_shape_density * height * 2*M_PI*radius * default_skin_thickness; @@ -422,12 +426,51 @@ color[3] = 0.4; bn->getVisualizationShape(0)->setRGBA(color); ``` -### Lesson 2f: Add a rigid body attached by a WeldJoint +### Lesson 2f: Give a hard bone to the SoftBodyNode + +SoftBodyNodes are intended to be used as soft skins that are attached to rigid +bones. We can create a rigid shape, place it in the SoftBodyNode, and give some +inertia to the SoftBodyNode's base BodyNode class, to act as the inertia of the +bone. + +Find the function ``createSoftBody()``. Underneath the call to ``addSoftBody``, +we can create a box shape that matches the dimensions of the soft box, but scaled +down: + +```cpp +double width = default_shape_height, height = 2*default_shape_width; +Eigen::Vector3d dims(width, width, height); +dims *= 0.6; +std::shared_ptr box = std::make_shared(dims); +``` + +And then we can add that shape to the visualization and collision shapes of the +SoftBodyNode, just like normal: + +```cpp +bn->addCollisionShape(box); +bn->addVisualizationShape(box); +``` + +And we'll want to make sure that we set the inertia of the underlying BodyNode, +or else the behavior will not be realistic: + +```cpp +Inertia inertia; +inertia.setMass(default_shape_density * box->getVolume()); +inertia.setMoment(box->computeInertia(inertia.getMass())); +bn->setInertia(inertia); +``` + +Note that the inertia of the inherited BodyNode is independent of the inertia +of the SoftBodyNode's skin. + +### Lesson 2g: Add a rigid body attached by a WeldJoint -To make the shape more interesting, we can attach a protruding rigid body using -a WeldJoint. Find the ``createHybridBody()`` function and see where we call the -``addSoftBody`` function. Just below this, we'll create a new rigid body with a -WeldJoint attachment: +To make a more interesting hybrid shape, we can attach a protruding rigid body +to a SoftBodyNode using a WeldJoint. Find the ``createHybridBody()`` function +and see where we call the ``addSoftBody`` function. Just below this, we'll +create a new rigid body with a WeldJoint attachment: ```cpp bn = hybrid->createJointAndBodyNodePair(bn).second; diff --git a/tutorials/tutorialCollisions-Finished.cpp b/tutorials/tutorialCollisions-Finished.cpp index b20ad2733e060..31c43e76a4bc6 100644 --- a/tutorials/tutorialCollisions-Finished.cpp +++ b/tutorials/tutorialCollisions-Finished.cpp @@ -65,7 +65,7 @@ const double default_spawn_range = 0.9*default_ground_width/2; const double default_restitution = 0.6; -const double default_vertex_stiffness = 100.0; +const double default_vertex_stiffness = 1000.0; const double default_edge_stiffness = 1.0; const double default_soft_damping = 5.0; @@ -111,13 +111,14 @@ class MyWindow : public dart::gui::SimWindow public: MyWindow(const WorldPtr& world, const SkeletonPtr& ball, - const SkeletonPtr& hybridBody, const SkeletonPtr& rigidChain, - const SkeletonPtr& rigidRing) + const SkeletonPtr& softBody, const SkeletonPtr& hybridBody, + const SkeletonPtr& rigidChain, const SkeletonPtr& rigidRing) : mRandomize(true), mRD(), mMT(mRD()), mDistribution(-1.0, std::nextafter(1.0, 2.0)), mOriginalBall(ball), + mOriginalSoftBody(softBody), mOriginalHybridBody(hybridBody), mOriginalRigidChain(rigidChain), mOriginalRigidRing(rigidRing), @@ -135,14 +136,18 @@ class MyWindow : public dart::gui::SimWindow break; case '2': - addObject(mOriginalHybridBody->clone()); + addObject(mOriginalSoftBody->clone()); break; case '3': - addObject(mOriginalRigidChain->clone()); + addObject(mOriginalHybridBody->clone()); break; case '4': + addObject(mOriginalRigidChain->clone()); + break; + + case '5': addRing(mOriginalRigidRing->clone()); break; @@ -327,6 +332,9 @@ class MyWindow : public dart::gui::SimWindow /// A blueprint Skeleton that we will use to spawn balls SkeletonPtr mOriginalBall; + /// A blueprint Skeleton that we will use to spawn soft bodies + SkeletonPtr mOriginalSoftBody; + /// A blueprint Skeleton that we will use to spawn hybrid bodies SkeletonPtr mOriginalHybridBody; @@ -397,6 +405,7 @@ BodyNode* addRigidBody(const SkeletonPtr& chain, const std::string& name, // Set the coefficient of restitution to make the body more bouncy bn->setRestitutionCoeff(default_restitution); + // Set damping to make the simulation more stable if(parent) { Joint* joint = bn->getParentJoint(); @@ -437,8 +446,10 @@ BodyNode* addSoftBody(const SkeletonPtr& chain, const std::string& name, // SoftBodyNode if(SOFT_BOX == type) { - double width = default_shape_width, height = default_shape_height; + // Make a wide and short box + double width = default_shape_height, height = 2*default_shape_width; Eigen::Vector3d dims(width, width, height); + double mass = 2*dims[0]*dims[1] + 2*dims[0]*dims[2] + 2*dims[1]*dims[2]; mass *= default_shape_density * default_skin_thickness; soft_properties = SoftBodyNodeHelper::makeBoxProperties( @@ -446,8 +457,9 @@ BodyNode* addSoftBody(const SkeletonPtr& chain, const std::string& name, } else if(SOFT_CYLINDER == type) { - double radius = default_shape_width/2.0; - double height = default_shape_height; + // Make a wide and short cylinder + double radius = default_shape_height/2.0, height = 2*default_shape_width; + // Mass of center double mass = default_shape_density * height * 2*M_PI*radius * default_skin_thickness; @@ -544,6 +556,32 @@ SkeletonPtr createRigidRing() return ring; } +SkeletonPtr createSoftBody() +{ + SkeletonPtr soft = Skeleton::create("soft"); + + // Add a soft body + BodyNode* bn = addSoftBody(soft, "soft box", SOFT_BOX); + + // Add a rigid collision geometry and inertia + double width = default_shape_height, height = 2*default_shape_width; + Eigen::Vector3d dims(width, width, height); + dims *= 0.6; + std::shared_ptr box = std::make_shared(dims); + + bn->addCollisionShape(box); + bn->addVisualizationShape(box); + + Inertia inertia; + inertia.setMass(default_shape_density * box->getVolume()); + inertia.setMoment(box->computeInertia(inertia.getMass())); + bn->setInertia(inertia); + + setAllColors(soft, dart::Color::Fuschia()); + + return soft; +} + SkeletonPtr createHybridBody() { SkeletonPtr hybrid = Skeleton::create("hybrid"); @@ -624,14 +662,15 @@ int main(int argc, char* argv[]) world->addSkeleton(createGround()); world->addSkeleton(createWall()); - MyWindow window(world, createBall(), createHybridBody(), + MyWindow window(world, createBall(), createSoftBody(), createHybridBody(), createRigidChain(), createRigidRing()); std::cout << "space bar: simulation on/off" << std::endl; std::cout << "'1': toss a rigid ball" << std::endl; - std::cout << "'2': toss a hybrid soft/rigid body" << std::endl; - std::cout << "'3': toss a rigid chain" << std::endl; - std::cout << "'4': toss a ring of rigid bodies" << std::endl; + std::cout << "'2': toss a soft body" << std::endl; + std::cout << "'3': toss a hybrid soft/rigid body" << std::endl; + std::cout << "'4': toss a rigid chain" << std::endl; + std::cout << "'5': toss a ring of rigid bodies" << std::endl; std::cout << "\n'd': delete the oldest object" << std::endl; std::cout << "'r': toggle randomness" << std::endl; diff --git a/tutorials/tutorialCollisions.cpp b/tutorials/tutorialCollisions.cpp index 73a664ca035ea..524d5752b9b22 100644 --- a/tutorials/tutorialCollisions.cpp +++ b/tutorials/tutorialCollisions.cpp @@ -65,7 +65,7 @@ const double default_spawn_range = 0.9*default_ground_width/2; const double default_restitution = 0.6; -const double default_vertex_stiffness = 100.0; +const double default_vertex_stiffness = 1000.0; const double default_edge_stiffness = 1.0; const double default_soft_damping = 5.0; @@ -92,13 +92,14 @@ class MyWindow : public dart::gui::SimWindow public: MyWindow(const WorldPtr& world, const SkeletonPtr& ball, - const SkeletonPtr& hybridBody, const SkeletonPtr& rigidChain, - const SkeletonPtr& rigidRing) + const SkeletonPtr& softBody, const SkeletonPtr& hybridBody, + const SkeletonPtr& rigidChain, const SkeletonPtr& rigidRing) : mRandomize(true), mRD(), mMT(mRD()), mDistribution(-1.0, std::nextafter(1.0, 2.0)), mOriginalBall(ball), + mOriginalSoftBody(softBody), mOriginalHybridBody(hybridBody), mOriginalRigidChain(rigidChain), mOriginalRigidRing(rigidRing), @@ -116,14 +117,18 @@ class MyWindow : public dart::gui::SimWindow break; case '2': - addObject(mOriginalHybridBody->clone()); + addObject(mOriginalSoftBody->clone()); break; case '3': - addObject(mOriginalRigidChain->clone()); + addObject(mOriginalHybridBody->clone()); break; case '4': + addObject(mOriginalRigidChain->clone()); + break; + + case '5': addRing(mOriginalRigidRing->clone()); break; @@ -246,6 +251,9 @@ class MyWindow : public dart::gui::SimWindow /// A blueprint Skeleton that we will use to spawn balls SkeletonPtr mOriginalBall; + /// A blueprint Skeleton that we will use to spawn soft bodies + SkeletonPtr mOriginalSoftBody; + /// A blueprint Skeleton that we will use to spawn hybrid bodies SkeletonPtr mOriginalHybridBody; @@ -372,6 +380,21 @@ SkeletonPtr createRigidRing() return ring; } +SkeletonPtr createSoftBody() +{ + SkeletonPtr soft = Skeleton::create("soft"); + + // Add a soft body + /*BodyNode* bn =*/ addSoftBody(soft, "soft box", SOFT_BOX); + + // Add a rigid collision geometry and inertia + // Lesson 2f + + setAllColors(soft, dart::Color::Fuschia()); + + return soft; +} + SkeletonPtr createHybridBody() { SkeletonPtr hybrid = Skeleton::create("hybrid"); @@ -380,7 +403,7 @@ SkeletonPtr createHybridBody() /*BodyNode* bn =*/ addSoftBody(hybrid, "soft sphere", SOFT_ELLIPSOID); // Add a rigid body attached by a WeldJoint - // Lesson 2f + // Lesson 2g setAllColors(hybrid, dart::Color::Green()); @@ -435,14 +458,15 @@ int main(int argc, char* argv[]) world->addSkeleton(createGround()); world->addSkeleton(createWall()); - MyWindow window(world, createBall(), createHybridBody(), + MyWindow window(world, createBall(), createSoftBody(), createHybridBody(), createRigidChain(), createRigidRing()); std::cout << "space bar: simulation on/off" << std::endl; std::cout << "'1': toss a rigid ball" << std::endl; - std::cout << "'2': toss a hybrid soft/rigid body" << std::endl; - std::cout << "'3': toss a rigid chain" << std::endl; - std::cout << "'4': toss a ring of rigid bodies" << std::endl; + std::cout << "'2': toss a soft body" << std::endl; + std::cout << "'3': toss a hybrid soft/rigid body" << std::endl; + std::cout << "'4': toss a rigid chain" << std::endl; + std::cout << "'5': toss a ring of rigid bodies" << std::endl; std::cout << "\n'd': delete the oldest object" << std::endl; std::cout << "'r': toggle randomness" << std::endl; From 23d34134f1c87bcf2081bcbabcb1962574fd4616 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Thu, 2 Jul 2015 16:27:37 -0400 Subject: [PATCH 54/65] added random header just in case --- tutorials/tutorialCollisions-Finished.cpp | 1 + tutorials/tutorialCollisions.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/tutorials/tutorialCollisions-Finished.cpp b/tutorials/tutorialCollisions-Finished.cpp index 31c43e76a4bc6..84920ddb7d7f1 100644 --- a/tutorials/tutorialCollisions-Finished.cpp +++ b/tutorials/tutorialCollisions-Finished.cpp @@ -35,6 +35,7 @@ */ #include "dart/dart.h" +#include const double default_shape_density = 1000; // kg/m^3 const double default_shape_height = 0.1; // m diff --git a/tutorials/tutorialCollisions.cpp b/tutorials/tutorialCollisions.cpp index 524d5752b9b22..42bac45917cdb 100644 --- a/tutorials/tutorialCollisions.cpp +++ b/tutorials/tutorialCollisions.cpp @@ -35,6 +35,7 @@ */ #include "dart/dart.h" +#include const double default_shape_density = 1000; // kg/m^3 const double default_shape_height = 0.1; // m From 1034fefaf7664ecef590132c02459385c58261d2 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Thu, 2 Jul 2015 20:55:44 -0400 Subject: [PATCH 55/65] finished documenation for domino tutorial --- docs/readthedocs/tutorials/dominoes.md | 522 +++++++++++++++++++++++- tutorials/tutorialDominoes-Finished.cpp | 30 +- tutorials/tutorialDominoes.cpp | 27 +- 3 files changed, 546 insertions(+), 33 deletions(-) diff --git a/docs/readthedocs/tutorials/dominoes.md b/docs/readthedocs/tutorials/dominoes.md index 3d920e14789b6..fe68b77c7c8b9 100644 --- a/docs/readthedocs/tutorials/dominoes.md +++ b/docs/readthedocs/tutorials/dominoes.md @@ -9,6 +9,526 @@ dynamic systems, such as robotic manipulators. We will show you how to: - Write a stable PD controller w/ gravity and coriolis compensation - Write an operational space controller -# Lesson 1 +# Lesson 1: Cloning Skeletons +There are often times where you might want to create an exact replica of an +existing Skeleton. DART offers cloning functionality that allows you to do this +very easily. +### Lesson 1a: Create a new domino + +Creating a new domino is straightforward. Find the function ``attemptToCreateDomino`` +in the ``MyWindow`` class. The class has a member called ``mFirstDomino`` which +is the original domino created when the program starts up. To make a new one, +we can just clone it: + +```cpp +SkeletonPtr newDomino = mFirstDomino->clone(); +``` + +But keep in mind that every Skeleton that gets added to a world requires its own +unique name. Creating a clone will keep the original name, so we should we give +the new copy its own name: + +```cpp +newDomino->setName("domino #" + std::to_string(mDominoes.size() + 1)); +``` + +So the easy part is finished, but now we need to get the domino to the correct +position. First, let's grab the last domino that was placed in the environment: + +```cpp +const SkeletonPtr& lastDomino = mDominoes.size() > 0 ? + mDominoes.back() : mFirstDomino; +``` + +Now we should compute what we want its position to be. The ``MyWindow`` class +keeps a member called ``mTotalAngle`` which tracks how much the line of dominoes +has turned so far. We'll use that to figure out what translational offset the +new domino should have from the last domino: + +```cpp +Eigen::Vector3d dx = default_distance * Eigen::Vector3d( + cos(mTotalAngle), sin(mTotalAngle), 0.0); +``` + +And now we can compute the total position of the new domino. First, we'll copy +the positions of the last domino: + +```cpp +Eigen::Vector6d x = lastDomino->getPositions(); +``` + +And then we'll add the translational offset to it: + +```cpp +x.tail<3>() += dx; +``` + +Remember that the domino's root joint is a FreeJoint which has six degrees of +freedom: the first three are for orientation and last three are for translation. + +Finally, we should add on the change in angle for the new domino: + +```cpp +x[2] = mTotalAngle + angle; +``` + +Be sure to uncomment the ``angle`` argument of the function. + +Now we can use ``x`` to set the positions of the domino: + +```cpp +newDomino->setPositions(x); +``` + +The root FreeJoint is the only joint in the domino's Skeleton, so we can just +use the ``Skeleton::setPositions`` function to set it. + +Now we'll add the Skeleton to the world: + +```cpp +mWorld->addSkeleton(newDomino); +``` + +### Lesson 1b: Make sure no dominoes are in collision + +Similar to **Lesson 3** of the **Collisions** tutorial, we'll want to make sure +that the newly inserted Skeleton is not starting out in collision with anything, +because this could make for a very ugly (perhaps even broken) simulation. + +First, we'll tell the world to compute collisions: + +```cpp +dart::collision::CollisionDetector* detector = + mWorld->getConstraintSolver()->getCollisionDetector(); +detector->detectCollision(true, true); +``` + +Now we'll look through and see if any dominoes are in collision with anything +besides the floor. We ignore collisions with the floor because, mathemetically +speaking, if they are in contact with the floor then they register as being in +collision. But we want the dominoes to be in contact with the floor, so this is +okay. + +```cpp +bool dominoCollision = false; +size_t collisionCount = detector->getNumContacts(); +for(size_t i = 0; i < collisionCount; ++i) +{ + // If neither of the colliding BodyNodes belongs to the floor, then we + // know the new domino is in contact with something it shouldn't be + const dart::collision::Contact& contact = detector->getContact(i); + if(contact.bodyNode1.lock()->getSkeleton() != mFloor + && contact.bodyNode2.lock()->getSkeleton() != mFloor) + { + dominoCollision = true; + break; + } +} +``` + +The only object that could possibly have collided with something else is the +new domino, because we don't allow the application to create new things except +for the dominoes. So if this registered as true, then we should take the new +domino out of the world: + +```cpp +if(dominoCollision) +{ + // Remove the new domino, because it is penetrating an existing one + mWorld->removeSkeleton(newDomino); +} +``` + +Otherwise, if the new domino is in an okay position, we should add it to the +history: + +```cpp +else +{ + // Record the latest domino addition + mAngles.push_back(angle); + mDominoes.push_back(newDomino); + mTotalAngle += angle; +} +``` + +### Lesson 1c: Delete the last domino added + +Ordinarily, removing a Skeleton from a scene is just a matter of calling the +``World::removeSkeleton`` function, but we have a little bit of bookkeeping to +take care of for our particular application. First, we should check whether +there are any dominoes to actually remove: + +```cpp +if(mDominoes.size() > 0) +{ + // TODO: Remove Skeleton +} +``` + +Then we should grab the last domino in the history, remove it from the history, +and then take it out of the world: + +```cpp +SkeletonPtr lastDomino = mDominoes.back(); +mDominoes.pop_back(); +mWorld->removeSkeleton(lastDomino); +``` + +The ``SkeletonPtr`` class is really a ``std::shared_ptr`` so we don't +need to worry about ever calling ``delete`` on it. Instead, its resources will +be freed when ``lastDomino`` goes out of scope. + +We should also make sure to do the bookkeepping for the angles: + +```cpp +mTotalAngle -= mAngles.back(); +mAngles.pop_back(); +``` + +**Now we can add and remove dominoes from the scene. Feel free to give it a try.** + +### Lesson 1d: Apply a force to the first domino + +But just setting up dominoes isn't much fun without being able to knock them +down. We can quickly and easily knock down the dominoes by magically applying +a force to the first one. In the ``timeStepping`` function of ``MyWindow`` there +is a label for **Lesson 1d**. This spot will get visited whenever the user +presses 'f', so we'll apply an external force to the first domino here: + +```cpp +Eigen::Vector3d force = default_push_force * Eigen::Vector3d::UnitX(); +Eigen::Vector3d location = + default_domino_height / 2.0 * Eigen::Vector3d::UnitZ(); +mFirstDomino->getBodyNode(0)->addExtForce(force, location); +``` + +# Lesson 2: Loading and controlling a robotic manipulator + +Striking something with a magical force is convenient, but not very believable. +Instead, let's load a robotic manipulator and have it push over the first domino. + +### Lesson 2a: Load a URDF file + +Our manipulator is going to be loaded from a URDF file. URDF files are loaded +by the ``dart::utils::DartLoader`` class (pending upcoming changes to DART's +loading system). First, create a loader: + +```cpp +dart::utils::DartLoader loader; +``` + +Note that many URDF files use ROS's ``package:`` scheme to specify the locations +of the resources that need to be loaded. We won't be using this in our example, +but in general you should use the function ``DartLoader::addPackageDirectory`` +to specify the locations of these packages, because DART does not have the same +package resolving abilities of ROS. + +Now we'll have ``loader`` parse the file into a Skeleton: + +```cpp +SkeletonPtr manipulator = + loader.parseSkeleton(DART_DATA_PATH"urdf/KR5/KR5 sixx R650.urdf"); +``` + +And we should give the Skeleton a convenient name: + +```cpp +manipulator->setName("manipulator"); +``` + +Now we'll want to initialize the location and configuration of the manipulator. +Experimentation has demonstrated that the following setup is good for our purposes: + +```cpp +// Position its base in a reasonable way +Eigen::Isometry3d tf = Eigen::Isometry3d::Identity(); +tf.translation() = Eigen::Vector3d(-0.65, 0.0, 0.0); +manipulator->getJoint(0)->setTransformFromParentBodyNode(tf); + +// Get it into a useful configuration +manipulator->getDof(1)->setPosition(140.0 * M_PI / 180.0); +manipulator->getDof(2)->setPosition(-140.0 * M_PI / 180.0); +``` + +And lastly, be sure to return the Skeleton that we loaded rather than the dummy +Skeleton that was originally there: + +```cpp +return manipulator; +``` + +**Feel free to load up the application to see the manipulator in the scene, +although all it will be able to do is collapse pitifully onto the floor.** + +### Lesson 2b: Grab the desired joint angles + +To make the manipulator actually useful, we'll want to have the ``Controller`` +control its joint forces. For it to do that, the ``Controller`` class will need +to be informed of what we want the manipulator's joint angles to be. This is +easily done in the constructor of the ``Controller`` class: + +```cpp +mQDesired = mManipulator->getPositions(); +``` + +The function ``Skeleton::getPositions`` will get all the generalized coordinate +positions of all the joints in the Skeleton, stacked in a single vector. These +Skeleton API functions are useful when commanding or controlling an entire +Skeleton with a single mathematical expression. + +### Lesson 2c: Write a stable PD controller for the manipulator + +Now that we know what configuration we want the manipulator to hold, we can +write a PD controller that keeps them in place. Find the function ``setPDForces`` +in the ``Controller`` class. + +First, we'll grab the current positions and velocities: + +```cpp +Eigen::VectorXd q = mManipulator->getPositions(); +Eigen::VectorXd dq = mManipulator->getVelocities(); +``` + +Additionally, we'll integrate the position forward by one timestep: + +```cpp +q += dq * mManipulator->getTimeStep(); +``` + +This is not necessary for writing a regular PD controller, but instead this is +to write a "stable PD" controller which has some better numerical stability +properties than an ordinary discrete PD controller. You can try running with and +without this line to see what effect it has on the stability. + +Now we'll compute our joint position error: + +```cpp +Eigen::VectorXd q_err = mQDesired - q; +``` + +And our joint velocity error, assuming our desired joint velocity is zero: + +```cpp +Eigen::VectorXd dq_err = -dq; +``` + +Now we can grab our mass matrix, which we will use to scale our force terms: + +```cpp +const Eigen::MatrixXd& M = mManipulator->getMassMatrix(); +``` + +And then combine all this into a PD controller that computes forces to minimize +our error: + +```cpp +mForces = M * (mKpPD * q_err + mKdPD * dq_err); +``` + +Now we're ready to set these forces on the manipulator: + +```cpp +mManipulator->setForces(mForces); +``` + +**Feel free to give this PD controller a try to see how effective it is.** + +### Lesson 2d: Compensate for gravity and Coriolis forces + +One of the key features of DART is the ability to easily compute the gravity and +Coriolis forces, allowing you to write much higher quality controllers than you +would be able to otherwise. This is easily done like so: + +```cpp +const Eigen::VectorXd& Cg = mManipulator->getCoriolisAndGravityForces(); +``` + +And now we can update our control law by just slapping this term onto the end +of the equation: + +```cpp +mForces = M * (mKpPD * q_err + mKdPD * dq_err) + Cg; +``` + +**Give this new PD controller a try to see how its performance compares to the +one without compensation** + +# Lesson 3: Writing an operational space controller + +While PD controllers are simply and handy, operational space controllers can be +much more elegant and useful for performing tasks. Operational space controllers +allow us to unify geometric tasks (like getting the end effector to a particular +spot) and dynamics tasks (like applying a certain force with the end effector) +all while remaining stable and smooth. + +### Lesson 3a: Set up the information needed for an OS controller + +Unlike PD controllers, an operational space controller needs more information +than just desired joint angles. + +First, we'll grab the last BodyNode on the manipulator and treat it as an end +effector: + +```cpp +mEndEffector = mManipulator->getBodyNode(mManipulator->getNumBodyNodes() - 1); +``` + +But we don't want to use the origin of the BodyNode frame as the origin of our +Operational Space controller; instead we want to use a slight offset, to get to +the tool area of the last BodyNode: + +```cpp +mOffset = default_endeffector_offset * Eigen::Vector3d::UnitX(); +``` + +Also, our target will be the spot on top of the first domino, so we'll create a +reference frame and place it there. First, create the SimpleFrame: + +```cpp +mTarget = std::make_shared(Frame::World(), "target"); +``` + +Then compute the transform needed to get from the center of the domino to the +top of the domino: + +```cpp +Eigen::Isometry3d target_offset(Eigen::Isometry3d::Identity()); +target_offset.translation() = + default_domino_height / 2.0 * Eigen::Vector3d::UnitZ(); +``` + +And then we should rotate the target's coordinate frame to make sure that lines +up with the end effector's reference frame, otherwise the manipulator might try +to push on the domino from a very strange angle: + +```cpp +target_offset.linear() = + mEndEffector->getTransform(domino->getBodyNode(0)).linear(); +``` + +Now we'll set the target so that it has a transform of ``target_offset`` with +respect to the frame of the domino: + +```cpp +mTarget->setTransform(target_offset, domino->getBodyNode(0)); +``` + +And this gives us all the information we need to write an Operational Space +controller. + +### Lesson 3b: Computing forces for OS Controller + +Find the function ``setOperationalSpaceForces()``. This is where we'll compute +the forces for our operational space controller. + +One of the key ingredients in an operational space controller is the mass matrix. +We can get this easily, just like we did for the PD controller: + +```cpp +const Eigen::MatrixXd& M = mManipulator->getMassMatrix(); +``` + +Next we'll want the Jacobian of the tool offset in the end effector. We can get +it easily with this function: + +```cpp +Jacobian J = mEndEffector->getWorldJacobian(mOffset); +``` + +But operational space controllers typically use the Moore-Penrose pseudoinverse +of the Jacobian rather than the Jacobian itself. There are many ways to compute +the pseudoinverse of the Jacobian, but a simple way is like this: + +```cpp +Eigen::MatrixXd pinv_J = J.transpose() * (J * J.transpose() + + 0.0025 * Eigen::Matrix6d::Identity()).inverse(); +``` + +Note that this pseudoinverse is also damped so that it behaves better around +singularities. This is method for computing the pseudoinverse is not very +efficient in terms of the number of mathematical operations it performs, but +it is plenty fast for our application. Consider using methods based on Singular +Value Decomposition if you need to compute the pseudoinverse as fast as possible. + +Next we'll want the time derivative of the Jacobian, as well as its pseudoinverse: + +```cpp +// Compute the Jacobian time derivative +Jacobian dJ = mEndEffector->getJacobianClassicDeriv(mOffset); + +// Comptue the pseudo-inverse of the Jacobian time derivative +Eigen::MatrixXd pinv_dJ = dJ.transpose() * (dJ * dJ.transpose() + + 0.0025 * Eigen::Matrix6d::Identity()).inverse(); +``` + +Notice that here we're compute the **classic** derivative, which means the +derivative of the Jacobian with respect to time in classical coordinates rather +than spatial coordinates. If you use spatial vector arithmetic, then you'll want +to use ``BodyNode::getJacobianSpatialDeriv`` instead. + +Now we can compute the linear components of error: + +```cpp +Eigen::Vector6d e; +e.tail<3>() = mTarget->getWorldTransform().translation() + - mEndEffector->getWorldTransform() * mOffset; +``` + +And then the angular components of error: + +```cpp +Eigen::AngleAxisd aa(mTarget->getTransform(mEndEffector).linear()); +e.head<3>() = aa.angle() * aa.axis(); +``` + +Then the time derivative of error, assuming our desired velocity is zero: + +```cpp +Eigen::Vector6d de = -mEndEffector->getSpatialVelocity( + mOffset, mTarget.get(), Frame::World()); +``` + +Like with the PD controller, we can mix in terms to compensate for gravity and +Coriolis forces: + +```cpp +const Eigen::VectorXd& Cg = mManipulator->getCoriolisAndGravityForces(); +``` + +The gains for the operational space controller need to be in matrix form, but +we're storing the gains as scalars, so we'll need to conver them: + +```cpp +Eigen::Matrix6d Kp = mKpOS * Eigen::Matrix6d::Identity(); + +size_t dofs = mManipulator->getNumDofs(); +Eigen::MatrixXd Kd = mKdOS * Eigen::MatrixXd::Identity(dofs, dofs); +``` + +And we'll need to compute the joint forces needed to achieve our desired end +effector force. This is easily done using the Jacobian transpose: + +```cpp +Eigen::Vector6d fDesired = Eigen::Vector6d::Zero(); +fDesired[3] = default_push_force; +Eigen::VectorXd f = J.transpose() * fDesired; +``` + +And now we can mix everything together into the single control law: + +```cpp +Eigen::VectorXd dq = mManipulator->getVelocities(); +mForces = M * (pinv_J * Kp * de + pinv_dJ * Kp * e) + - Kd * dq + Kd * pinv_J * Kp * e + Cg + f; +``` + +Then don't forget to pass the forces into the manipulator: + +```cpp +mManipulator->setForces(mForces); +``` + +**Now you're ready to try out the full dominoes app!** diff --git a/tutorials/tutorialDominoes-Finished.cpp b/tutorials/tutorialDominoes-Finished.cpp index a6d858e30e53b..71b2f4798fb0d 100644 --- a/tutorials/tutorialDominoes-Finished.cpp +++ b/tutorials/tutorialDominoes-Finished.cpp @@ -76,6 +76,9 @@ class Controller // Compute the body frame offset for the end effector mOffset = default_endeffector_offset * Eigen::Vector3d::UnitX(); + // Create a target reference frame + mTarget = std::make_shared(Frame::World(), "target"); + // Create a transform from the center of the domino to the top of the domino Eigen::Isometry3d target_offset(Eigen::Isometry3d::Identity()); target_offset.translation() = @@ -86,8 +89,7 @@ class Controller target_offset.linear() = mEndEffector->getTransform(domino->getBodyNode(0)).linear(); - // Place the _target SimpleFrame at the top of the domino - mTarget = std::make_shared(Frame::World(), "target"); + // Place the mTarget SimpleFrame at the top of the domino mTarget->setTransform(target_offset, domino->getBodyNode(0)); // Set PD control gains @@ -106,10 +108,6 @@ class Controller if(nullptr == mManipulator) return; - // Compute the joint forces needed to compensate for Coriolis forces and - // gravity - const Eigen::VectorXd& Cg = mManipulator->getCoriolisAndGravityForces(); - // Compute the joint position error Eigen::VectorXd q = mManipulator->getPositions(); Eigen::VectorXd dq = mManipulator->getVelocities(); @@ -120,6 +118,10 @@ class Controller // Compute the joint velocity error Eigen::VectorXd dq_err = -dq; + // Compute the joint forces needed to compensate for Coriolis forces and + // gravity + const Eigen::VectorXd& Cg = mManipulator->getCoriolisAndGravityForces(); + // Compute the desired joint forces const Eigen::MatrixXd& M = mManipulator->getMassMatrix(); mForces = M * (mKpPD * q_err + mKdPD * dq_err) + Cg; @@ -139,7 +141,7 @@ class Controller Jacobian J = mEndEffector->getWorldJacobian(mOffset); // Compute the pseudo-inverse of the Jacobian Eigen::MatrixXd pinv_J = J.transpose() * (J * J.transpose() - + 0.0025 * Eigen::Matrix6d::Identity()).inverse(); + + 0.0025 * Eigen::Matrix6d::Identity()).inverse(); // Compute the Jacobian time derivative Jacobian dJ = mEndEffector->getJacobianClassicDeriv(mOffset); @@ -240,6 +242,10 @@ class MyWindow : public dart::gui::SimWindow // with anything (other than the floor), then discard it. void attemptToCreateDomino(double angle) { + // Create a new domino + SkeletonPtr newDomino = mFirstDomino->clone(); + newDomino->setName("domino #" + std::to_string(mDominoes.size() + 1)); + const SkeletonPtr& lastDomino = mDominoes.size() > 0 ? mDominoes.back() : mFirstDomino; @@ -253,9 +259,6 @@ class MyWindow : public dart::gui::SimWindow // Adjust the angle for the new domino x[2] = mTotalAngle + angle; - // Create the new domino - SkeletonPtr newDomino = mFirstDomino->clone(); - newDomino->setName("domino #" + std::to_string(mDominoes.size() + 1)); newDomino->setPositions(x); mWorld->addSkeleton(newDomino); @@ -357,9 +360,10 @@ class MyWindow : public dart::gui::SimWindow // order to push it over if(mForceCountDown > 0) { - mFirstDomino->getBodyNode(0)->addExtForce( - default_push_force * Eigen::Vector3d::UnitX(), - default_domino_height / 2.0 * Eigen::Vector3d::UnitZ()); + Eigen::Vector3d force = default_push_force * Eigen::Vector3d::UnitX(); + Eigen::Vector3d location = + default_domino_height / 2.0 * Eigen::Vector3d::UnitZ(); + mFirstDomino->getBodyNode(0)->addExtForce(force, location); --mForceCountDown; } diff --git a/tutorials/tutorialDominoes.cpp b/tutorials/tutorialDominoes.cpp index 03922b25620b6..be0214c4e53f9 100644 --- a/tutorials/tutorialDominoes.cpp +++ b/tutorials/tutorialDominoes.cpp @@ -68,24 +68,11 @@ class Controller : mManipulator(manipulator) { // Grab the current joint angles to use them as desired angles - // Lesson 2c + // Lesson 2b - // Grab the last body in the manipulator, and use it as an end effector + // Set up the information needed for an operational space controller // Lesson 3a - // Compute the body frame offset for the end effector - // Lesson 3b - - // Create a transform from the center of the domino to the top of the domino - // Lesson 3c - - // Rotate the transform so that it matches the orientation of the end - // effector - // Lesson 3d - - // Place the mTarget SimpleFrame at the top of the domino - // Lesson 3e - // Set PD control gains mKpPD = 200.0; mKdPD = 20.0; @@ -99,15 +86,17 @@ class Controller /// Coriolis forces void setPDForces() { - // Lesson 2b + // Write a stable PD controller + // Lesson 2c + // Compensate for gravity and Coriolis forces // Lesson 2d } /// Compute an operational space controller to push on the first domino void setOperationalSpaceForces() { - // Lesson 3f + // Lesson 3b } protected: @@ -166,9 +155,9 @@ class MyWindow : public dart::gui::SimWindow // Attempt to create a new domino. If the new domino would be in collision // with anything (other than the floor), then discard it. - void attemptToCreateDomino(double) + void attemptToCreateDomino(double /*angle*/) { - // Create the new domino + // Create a new domino // Lesson 1a // Look through the collisions to see if any dominoes are penetrating From 6bfc19d5f39dff9a1a508ff860cc6e15d016abe9 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Thu, 2 Jul 2015 21:04:10 -0400 Subject: [PATCH 56/65] fixed typo --- tutorials/tutorialDominoes-Finished.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tutorials/tutorialDominoes-Finished.cpp b/tutorials/tutorialDominoes-Finished.cpp index 71b2f4798fb0d..8d02695fa530d 100644 --- a/tutorials/tutorialDominoes-Finished.cpp +++ b/tutorials/tutorialDominoes-Finished.cpp @@ -509,7 +509,7 @@ int main(int argc, char* argv[]) std::cout << std::endl; std::cout << "spacebar: Begin simulation (you can no longer create or remove dominoes)" << std::endl; std::cout << "'p': replay simulation" << std::endl; - std::cout << "'f': Push the first domino with a disembodies force so that it falls over" << std::endl; + std::cout << "'f': Push the first domino with a disembodied force so that it falls over" << std::endl; std::cout << "'r': Push the first domino with the manipulator so that it falls over" << std::endl; std::cout << "'v': Turn contact force visualization on/off" << std::endl; From 33f88bf2810e69b05b26571ffce884eb8a3ab33b Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Thu, 2 Jul 2015 22:18:04 -0400 Subject: [PATCH 57/65] updated introduction for DART --- docs/readthedocs/index.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/readthedocs/index.md b/docs/readthedocs/index.md index 7d4bd55eb1a01..ba2ce0fbc332f 100644 --- a/docs/readthedocs/index.md +++ b/docs/readthedocs/index.md @@ -2,6 +2,6 @@ -DART (Dynamic Animation and Robotics Toolkit) is a collaborative, cross-platform, open source library created by the Georgia Tech Graphics Lab and Humanoid Robotics Lab. The library provides data structures and algorithms for kinematic and dynamic applications in robotics and computer animation. DART is distinguished by its accuracy and stability due to its use of generalized coordinates to represent articulated rigid body systems and computation of Lagrange's equations derived from D.Alembert's principle to describe the dynamics of motion. For developers, in contrast to many popular physics engines which view the simulator as a black box, DART gives full access to internal kinematic and dynamic quantities, such as the mass matrix, Coriolis and centrifugal forces, transformation matrices and their derivatives. DART also provides efficient computation of Jacobian matrices for arbitrary body points and coordinate frames. Contact and collision are handled using an implicit time-stepping, velocity-based LCP (linear-complementarity problem) to guarantee non-penetration, directional friction, and approximated Coulomb friction cone conditions. For collision detection, DART uses FCL developed by Willow Garage and the UNC Gamma Lab. DART has applications in robotics and computer animation because it features a multibody dynamic simulator and tools for control and motion planning. Multibody dynamic simulation in DART is an extension of RTQL8, an open source software created by the Georgia Tech Graphics Lab. For ease of use, we separately provide the open source GRIP environment library which simplifies user interface, world loading, visualization and video recording. +DART (Dynamic Animation and Robotics Toolkit) is a collaborative, cross-platform, open source library created by the Georgia Tech Graphics Lab and Humanoid Robotics Lab. The library provides data structures and algorithms for kinematic and dynamic applications in robotics and computer animation. DART is distinguished by its accuracy and stability due to its use of generalized coordinates to represent articulated rigid body systems and Featherstone's Articulated Body Algorithm to compute the dynamics of motion. For developers, in contrast to many popular physics engines which view the simulator as a black box, DART gives full access to internal kinematic and dynamic quantities, such as the mass matrix, Coriolis and centrifugal forces, transformation matrices and their derivatives. DART also provides efficient computation of Jacobian matrices for arbitrary body points and coordinate frames. The frame semantics of DART allows users to define arbitrary reference frames (both inertial and non-inertial) and use those frames to specify or request data. For air-tight code safety, forward kinematics and dynamics values are updated automatically through lazy evaluation, making DART suitable for real time controllers. Contacts and collisions are handled using an implicit time-stepping, velocity-based LCP (linear-complementarity problem) to guarantee non-penetration, directional friction, and approximated Coulomb friction cone conditions. For collision detection, DART uses FCL developed by Willow Garage and the UNC Gamma Lab. DART has applications in robotics and computer animation because it features a multibody dynamic simulator and tools for control and motion planning. Multibody dynamic simulation in DART is an extension of RTQL8, an open source software created by the Georgia Tech Graphics Lab. DART is fully integrated with Gazebo 2.0, a multi-robot simulator for outdoor environments. DART supports most features provided in Gazebo, including the link structure, sensor simulation, and all the joint types. DART also supports models in URDF, SDF, and VSK file formats. From 2329367d52eb897a46523ed7cbcb5732dfbcd537 Mon Sep 17 00:00:00 2001 From: Karen Liu Date: Sun, 5 Jul 2015 21:00:29 -0400 Subject: [PATCH 58/65] edited multi-pendulum tutorial document --- docs/readthedocs/tutorials/multi-pendulum.md | 64 +++++++++++++++++++- 1 file changed, 62 insertions(+), 2 deletions(-) diff --git a/docs/readthedocs/tutorials/multi-pendulum.md b/docs/readthedocs/tutorials/multi-pendulum.md index ee44c611e3bcc..5695e3d0d8086 100644 --- a/docs/readthedocs/tutorials/multi-pendulum.md +++ b/docs/readthedocs/tutorials/multi-pendulum.md @@ -3,6 +3,7 @@ This tutorial will demonstrate some basic interaction with DART's dynamics API during simulation. This will show you how to: +- Create a basic program to simulate a dynamic system - Change the colors of shapes - Add/remove shapes from visualization - Apply internal forces in the joints @@ -10,7 +11,66 @@ API during simulation. This will show you how to: - Alter the implicit spring and damping properties of joints - Add/remove dynamic constraints -# Lesson 1: Changing shapes and applying forces +# Lesson 0: Simulate a passive multi-pendulum + +This is a warmup lesson that demonstrates how to set up a simulation +program in DART. The example we will use throughout this tutorial is a +pendulum with five rigid bodies swinging under gravity. DART allows +the user to build various articulated rigid/soft body systems from +scratch. It also loads models in URDF, SDF, and SKEL formats as +demonstrated in the later tutorials. + +In DART, an articulated dynamics model is represented by a +``Skeleton``. In the ``main`` function, we first create an empty +skeleton named *pendulum*. + +```cpp +SkeletonPtr pendulum = Skeleton::create("pendulum"); +``` + +A Skeleton is a structure that consists of ``BodyNode``s (bodies) which are +connected by ``Joint``s. Every Joint has a child BodyNode, and every BodyNode +has a parent Joint. Even the root BodyNode has a Joint that attaches it to the +World. In the function ``makeRootBody``, we create a pair of a +``BallJoint`` and a BodyNode, and attach this pair to the currently +empty pendulum skeleton. + +```cpp +BodyNodePtr bn = + pendulum->createJointAndBodyNodePair(nullptr, properties, BodyNode::Properties(name)).second; +``` + +Note that the first parameters is a nullptr, which indicates that +this new BodyNode is the root of the pendulum. If we wish to append +the new BodyNode to an existing BodyNode in the pendulum, +we can do so by passing the pointer of the existing BodyNode as +the first parameter. In fact, this is how we add more BodyNodes to +the pendulum in the function ``addBody``: + +```cpp +BodyNodePtr bn = + pendulum->createJointAndBodyNodePair(parent, properties, BodyNode::Properties(name)).second; +``` +The simplest way to set up a simulation program in DART is to use +``SimWindow`` class. A SimWindow owns an instance of ``World`` and +simulates all the Skeletons in the World. In this example, we create a World with the +pendulum skeleton in it, and assign the World to an instance of +``MyWindow``, a subclass derived from SimWindow. + +```cpp +WorldPtr world(new World); +world->addSkeleton(pendulum); +MyWindow window(world); +``` + +Every single time step, the ``timeStepping`` function will be called +and the state of the World will be simulated. The user can override +the default timeStepping function to customize the simulation +routine. For example, one can incorporate sensors, actuators, or user +interaction in the forward simulation. + + +# Lesson 1: Change shapes and applying forces We have a pendulum with five bodies, and we want to be able to apply forces to them during simulation. Additionally, we want to visualize these forces so we @@ -281,7 +341,7 @@ Again, we want to make sure that the damping coefficient is never negative. In fact, a negative damping coefficient would be far more harmful than a negative stiffness coefficient. -# Lesson 3: Adding and removing dynamic constraints +# Lesson 3: Add and remove dynamic constraints Dynamic constraints in DART allow you to attach two BodyNodes together according to a selection of a few different Joint-style constraints. This allows you to From 52ac1fc306e089db3e5eb2a1268eb09594dc9cce Mon Sep 17 00:00:00 2001 From: Karen Liu Date: Fri, 17 Jul 2015 13:41:11 -0400 Subject: [PATCH 59/65] edited tutorial docs --- docs/readthedocs/index.md | 4 +- docs/readthedocs/tutorials/biped.md | 114 +++++++++---------- docs/readthedocs/tutorials/collisions.md | 2 + docs/readthedocs/tutorials/dominoes.md | 2 + docs/readthedocs/tutorials/multi-pendulum.md | 10 +- 5 files changed, 66 insertions(+), 66 deletions(-) diff --git a/docs/readthedocs/index.md b/docs/readthedocs/index.md index ba2ce0fbc332f..0ff50004cad73 100644 --- a/docs/readthedocs/index.md +++ b/docs/readthedocs/index.md @@ -2,6 +2,6 @@ -DART (Dynamic Animation and Robotics Toolkit) is a collaborative, cross-platform, open source library created by the Georgia Tech Graphics Lab and Humanoid Robotics Lab. The library provides data structures and algorithms for kinematic and dynamic applications in robotics and computer animation. DART is distinguished by its accuracy and stability due to its use of generalized coordinates to represent articulated rigid body systems and Featherstone's Articulated Body Algorithm to compute the dynamics of motion. For developers, in contrast to many popular physics engines which view the simulator as a black box, DART gives full access to internal kinematic and dynamic quantities, such as the mass matrix, Coriolis and centrifugal forces, transformation matrices and their derivatives. DART also provides efficient computation of Jacobian matrices for arbitrary body points and coordinate frames. The frame semantics of DART allows users to define arbitrary reference frames (both inertial and non-inertial) and use those frames to specify or request data. For air-tight code safety, forward kinematics and dynamics values are updated automatically through lazy evaluation, making DART suitable for real time controllers. Contacts and collisions are handled using an implicit time-stepping, velocity-based LCP (linear-complementarity problem) to guarantee non-penetration, directional friction, and approximated Coulomb friction cone conditions. For collision detection, DART uses FCL developed by Willow Garage and the UNC Gamma Lab. DART has applications in robotics and computer animation because it features a multibody dynamic simulator and tools for control and motion planning. Multibody dynamic simulation in DART is an extension of RTQL8, an open source software created by the Georgia Tech Graphics Lab. +DART (Dynamic Animation and Robotics Toolkit) is a collaborative, cross-platform, open source library created by the Georgia Tech Graphics Lab and Humanoid Robotics Lab. The library provides data structures and algorithms for kinematic and dynamic applications in robotics and computer animation. DART is distinguished by its accuracy and stability due to its use of generalized coordinates to represent articulated rigid body systems and Featherstone's Articulated Body Algorithm to compute the dynamics of motion. For developers, in contrast to many popular physics engines which view the simulator as a black box, DART gives full access to internal kinematic and dynamic quantities, such as the mass matrix, Coriolis and centrifugal forces, transformation matrices and their derivatives. DART also provides efficient computation of Jacobian matrices for arbitrary body points and coordinate frames. The frame semantics of DART allows users to define arbitrary reference frames (both inertial and non-inertial) and use those frames to specify or request data. For air-tight code safety, forward kinematics and dynamics values are updated automatically through lazy evaluation, making DART suitable for real time controllers. Contacts and collisions are handled using an implicit time-stepping, velocity-based LCP (linear-complementarity problem) to guarantee non-penetration, directional friction, and approximated Coulomb friction cone conditions. DART has applications in robotics and computer animation because it features a multibody dynamic simulator and tools for control and motion planning. Multibody dynamic simulation in DART is an extension of RTQL8, an open source software created by the Georgia Tech Graphics Lab. -DART is fully integrated with Gazebo 2.0, a multi-robot simulator for outdoor environments. DART supports most features provided in Gazebo, including the link structure, sensor simulation, and all the joint types. DART also supports models in URDF, SDF, and VSK file formats. +DART is fully integrated with Gazebo, a multi-robot simulator for outdoor environments. DART supports most features provided in Gazebo, including the link structure, sensor simulation, and all the joint types. DART also supports models in URDF, SDF, and VSK file formats. diff --git a/docs/readthedocs/tutorials/biped.md b/docs/readthedocs/tutorials/biped.md index ebc8b941d6a26..8b50f3a25030f 100644 --- a/docs/readthedocs/tutorials/biped.md +++ b/docs/readthedocs/tutorials/biped.md @@ -9,17 +9,15 @@ consists of seven Lessons covering the following topics: - APIs for dynamic quantities. - Skeleton editing. +Please reference the source code in **tutorialBiped.cpp** and **tutorialBiped-Finished.cpp**. + # Lesson 1: Joint limits and self-collision -Let's start by locating the [main()](http://) function in -[tutorialBiped.cpp](http://). We first create a floor and call -[loadBiped()](http://) to load a -bipedal figure described in SKEL format, which is an XML format -representing a robot model. A SKEL file describes a -[World](http://dartsim.github.io/dart/d7/d41/classdart_1_1simulation_1_1World.html) -with one or more [Skeleton](http://)s in it. Here -we load in a World from biped.skel and assign the bipedal figure to a -[Skeleton](http://dartsim.github.io/dart/d3/d19/classdart_1_1dynamics_1_1Skeleton.html) -pointer called *biped*. +Let's start by locating the ``main`` function in tutorialBiped.cpp. We first create a floor +and call ``loadBiped`` to load a bipedal figure described in SKEL +format, which is an XML format representing a robot model. A SKEL file +describes a ``World`` with one or more ``Skeleton``s in it. Here we +load in a World from **biped.skel** and assign the bipedal figure to a +``Skeleton`` pointer called *biped*. ```cpp SkeletonPtr loadBiped() @@ -31,14 +29,14 @@ SkeletonPtr loadBiped() } ``` -Running the skeleton code without any modification (hit the spacebar), you should see a +Running the skeleton code (hit the spacebar) without any modification, you should see a human-like character collapse on the ground and fold in on itself. Before we attempt to control the biped, let's first make the biped a bit more realistic by enforcing more human-like joint limits. DART allows the user to set upper and lower bounds on each degree of freedom in the SKEL file or using provided APIs. For example, you -should see the description of the right knee joint in biped.skel: +should see the description of the right knee joint in **biped.skel**: ```cpp @@ -56,10 +54,11 @@ should see the description of the right knee joint in biped.skel: The <upper> and <lower> tags make sure that the knee can only flex but not extend. Alternatively, you can directly specify the joint limits in the code using -[setPositionUpperLimit](http://dartsim.github.io/dart/d6/d5b/classdart_1_1dynamics_1_1Joint.html#aa0635643a0a8c1f22edb8243e86ea801) -and [setPositionLowerLimit](http://dartsim.github.io/dart/d6/d5b/classdart_1_1dynamics_1_1Joint.html#adadee231309b62cd3e3d904f75f2a969). +``setPositionUpperLimit`` and ``setPositionLowerLimit``. + +In either case, the joint limits on the biped will not be activated +until you call ``setPositionLimited``: -In either case, the joint limits on the biped will not be activated until you call [setPositionLimited](http://dartsim.github.io/dart/d6/d5b/classdart_1_1dynamics_1_1Joint.html#a3212ca5f7893cfd9a5422ab17df4038b) ```cpp SkeletonPtr loadBiped() { @@ -105,9 +104,8 @@ damping coefficients. The detailed description of a PD controller can be found [here](https://en.wikipedia.org/wiki/PID_controller). The first task is to set the biped to a particular configuration. You -can use -[setPosition](http://dartsim.github.io/dart/d3/d19/classdart_1_1dynamics_1_1Skeleton.html#ac2036ea4998f688173d19ace0edab841) -to set each degree of freedom individually: +can use ``setPosition`` to set each degree of freedom individually: + ```cpp void setInitialPose(SkeletonPtr biped) { @@ -119,12 +117,12 @@ void setInitialPose(SkeletonPtr biped) Here the degree of freedom named "j_thigh_left_z" is set to 0.15 radian. Note that each degree of freedom in a skeleton has a numerical index which can be accessed by -[getIndexInSkeleton](http://dartsim.github.io/dart/de/db7/classdart_1_1dynamics_1_1DegreeOfFreedom.html#add2ec1d2f979e9056b466b1be5ee1a86). You +``getIndexInSkeleton``. You can also set the entire configuration using a vector that holds the positions of all the degreed of freedoms using -[setPositions](http://dartsim.github.io/dart/d3/d19/classdart_1_1dynamics_1_1Skeleton.html#aee6d1a2be46c277602fae2f1d47762ef). +``setPositions``. -You can set more degrees of freedoms in the lower +We continue to set more degrees of freedoms in the lower body to create a roughly stable standing pose. ```cpp @@ -138,7 +136,7 @@ biped->setPosition(biped->getDof("j_heel_right_1")->getIndexInSkeleton(), 0.25); Now the biped will start in this configuration, but will not maintain this configuration as soon as the simulation starts. We need a -controller to make this happen. Let's take a look at the constructor of our Controller in the +controller to make this happen. Let's take a look at the constructor of our ``Controller`` in the skeleton code: ```cpp @@ -160,6 +158,7 @@ Controller(const SkeletonPtr& biped) setTargetPositions(mBiped->getPositions()); } ``` + Here we arbitrarily define the stiffness and damping coefficients to 1000 and 50, except for the first six degrees of freedom. Because the global translation and rotation of the biped are not actuated, the @@ -169,7 +168,8 @@ zero. At the end of the constructor, we set the target position of the PD controller to the current configuration of the biped. With these settings, we can compute the forces generated by the PD -controller and add them to the internal forces of biped using [setForces](http://dartsim.github.io/dart/d3/d19/classdart_1_1dynamics_1_1Skeleton.html#a9a6a9b792fa39639d3af613907d2d8ed): +controller and add them to the internal forces of biped using ``setForces``: + ```cpp void addPDForces() { @@ -209,12 +209,12 @@ SPD is a variation of PD control proposed by [Jie Tan](http://www.cc.gatech.edu/~jtan34/project/spd.html). The basic idea of SPD is to compute control force using the predicted state at the next time step, instead of the current state. This Lesson -will only demonstrates the implementation of SPD using DART without +will only demonstrate the implementation of SPD using DART without going into details of SPD derivation. The implementation of SPD involves accessing the current dynamic quantities in Lagrange's equations of motion. Fortunately, these -quantities are readily available via DART APIs, which makes the full +quantities are readily available via DART API, which makes the full implementation of SPD simple and concise: ```cpp @@ -235,22 +235,22 @@ void addSPDForces() You can get mass matrix, Coriolis force, gravitational force, and constraint force projected onto generalized coordinates using function -calls [getMassMatrix](http://dartsim.github.io/dart/d3/d19/classdart_1_1dynamics_1_1Skeleton.html#a1998cb27dd892d259da109509f313830), -[getCoriolisForces](http://dartsim.github.io/dart/d3/d19/classdart_1_1dynamics_1_1Skeleton.html#aeffe03aff506e206f79c5074b3886f08), -[getGravityForces](http://dartsim.github.io/dart/d3/d19/classdart_1_1dynamics_1_1Skeleton.html#a0d278dc0365a99729fdbbee7acf0bcd3), +calls ``getMassMatrix``, +``getCoriolisForces``, +``getGravityForces``, and -[getConstraintForces](http://dartsim.github.io/dart/d3/d19/classdart_1_1dynamics_1_1Skeleton.html#a4b46912a4f3966efb2e54f1f5a29a77b), +``getConstraintForces``, respectively. Constraint forces include forces due to contacts, joint limits, and other joint constraints set by the user (e.g. the weld joint constraint in the multi-pendulum tutorial). With SPD, a wide range of stiffness and damping coefficients will all result in stable motion. In fact, you can just leave them to our -original values: 1000 and 50. By holding the initial pose, now the biped +original values: 1000 and 50. By holding the target pose, now the biped can stand on the ground in balance indefinitely. However, if you apply an external push force on the biped (hit ',' or '.' key to apply a backward or forward push), the biped loses its balance quickly. We -will demonstrate a more robust balance controller in the next Lesson. +will demonstrate a more robust feedback controller in the next Lesson. # Lesson 4: Ankle strategy @@ -272,20 +272,20 @@ anterior-posterior axis: ```cpp void addAnkleStrategyForces() { -Eigen::Vector3d COM = mBiped->getCOM(); -Eigen::Vector3d offset(0.05, 0, 0); -Eigen::Vector3d COP = mBiped->getBodyNode("h_heel_left")-> getTransform() * offset; -double diff = COM[0] - COP[0]; + Eigen::Vector3d COM = mBiped->getCOM(); + Eigen::Vector3d offset(0.05, 0, 0); + Eigen::Vector3d COP = mBiped->getBodyNode("h_heel_left")->getTransform() * offset; + double diff = COM[0] - COP[0]; ... } ``` DART provides various APIs to access useful kinematic information. For -example, [getCOM](http://) returns the center of mass of the skeleton and -[getTransform](http://) returns transformation of the body node with +example, ``getCOM`` returns the center of mass of the skeleton and +``getTransform`` returns transformation of the body node with respect to any coordinate frame specified by the parameter (world coordinate frame as default). DART APIs also come in handy when -computing the derivative term, - kd (ẋ - ṗ): +computing the derivative term, -kd (ẋ - ṗ): ```cpp void addAnkleStrategyForces() @@ -300,7 +300,7 @@ void addAnkleStrategyForces() The linear/angular velocity of any point in any coordinate frame can be easily accessed in DART. The following table summarizes the APIs for -accessing velocity. +accessing various velocities. | Function name | Description | |-------------|-----------| @@ -308,7 +308,7 @@ accessing velocity. The remaining of the ankle strategy implementation is just the matter of parameters tuning. We found that using different feedback rules for -falling forward and backward results in more stable controller. +falling forward and backward result in more stable controller. # Lesson 5: Skeleton editing @@ -318,7 +318,7 @@ scratch. In this Lesson, we will load a skateboard model from a SKEL file and merge our biped with the skateboard to create a wheel-based robot. -We first load a skateboard from [skateboard.skel](http:\\): +We first load a skateboard from **skateboard.skel**: ```cpp void modifyBipedWithSkateboard(SkeletonPtr biped) @@ -329,10 +329,10 @@ void modifyBipedWithSkateboard(SkeletonPtr biped) } ``` -Our goal is to make the skateboard skeleton a subtree of the biped -skeleton connected to the left heel body node via a newly created +Our goal is to make the skateboard Skeleton a subtree of the biped +Skeleton connected to the left heel BodyNode via a newly created Euler joint. To do so, you need to first create an instance of -[EulerJoint::Properties](http:\\) for this new joint. +``EulerJoint::Properties`` for this new joint. ```cpp void modifyBipedWithSkateboard(SkeletonPtr biped) @@ -344,7 +344,7 @@ void modifyBipedWithSkateboard(SkeletonPtr biped) } ``` -Here we increase the vertical distance between the child body node and +Here we increase the vertical distance between the child BodyNode and the joint by 0.1m to give some space between the skateboard and the left foot. Now you can merge the skateboard and the biped using this new Euler joint by @@ -376,7 +376,7 @@ actuator type. |Content Cell | Content Cell | -In this Lesson, we will switch the type of actuators for the wheels +In this Lesson, we will switch the actuator type of the wheels from the default FORCE type to VELOCITY type. ```cpp @@ -388,8 +388,8 @@ void setVelocityAccuators(SkeletonPtr biped) } ``` -Once all four wheels are set to VELOCITY actuator type, you can these -actuators by directly setting the desired velocity: +Once all four wheels are set to VELOCITY actuator type, you can +command them by directly setting the desired velocity: ```cpp void setWheelCommands() @@ -401,8 +401,8 @@ void setWheelCommands() } ``` -Note that [setCommand](http://) only exerts commanding force in the current time step. If you wish the -wheel to continue spinning at a particular speed, [setCommand](http://) +Note that ``setCommand`` only exerts commanding force in the current time step. If you wish the +wheel to continue spinning at a particular speed, ``setCommand`` needs to be called at every time step. We also set the stiffness and damping coefficients for the wheels to zero. @@ -456,8 +456,8 @@ degrees of freedom, i.e., the computation of Jacobian matrix. DART provides a comprensive set of APIs for accessing various types of Jacobian. In this example, computing the gradient of the first term of the objective function requires the Jacobian of the -center of mass of the skeleton, as well as the Jacobian of the center -of mass of a body node: +center of mass of the Skeleton, as well as the Jacobian of the center +of mass of a BodyNode: ```cpp Eigen::VectorXd solveIK(SkeletonPtr biped) @@ -469,14 +469,14 @@ Eigen::VectorXd solveIK(SkeletonPtr biped) } ``` -[getCOMLinearJacobian](http://) returns the linear Jacobian of the -center of mass of the skeleton, while [getLinearJacobian](http://) -returns the Jacobian of a point on a body node. The body node and the +``getCOMLinearJacobian`` returns the linear Jacobian of the +center of mass of the Skeleton, while ``getLinearJacobian`` +returns the Jacobian of a point on a BodyNode. The BodyNode and the local coordinate of the point are specified as parameters to this function. Here the point of interest is the center of mass of the left -foot, which local coordinates can be accessed by [getCOM](http://) +foot, which local coordinates can be accessed by ``getCOM`` with a parameter indicating the left foot being the frame of -reference. We use[getLinearJacobian](http://) again to compute the +reference. We use ``getLinearJacobian`` again to compute the gradient of the second term of the objective function: ```cpp diff --git a/docs/readthedocs/tutorials/collisions.md b/docs/readthedocs/tutorials/collisions.md index 833b42e6c76c2..df02fb0ac4c91 100644 --- a/docs/readthedocs/tutorials/collisions.md +++ b/docs/readthedocs/tutorials/collisions.md @@ -11,6 +11,8 @@ The tutorial consists of five Lessons covering the following topics: - Setting joint spring and damping properties - Creating a closed kinematic chain +Please reference the source code in **tutorialCollision.cpp** and **tutorialCollision-Finished.cpp**. + # Lesson 1: Creating a rigid body Start by going opening the Skeleton code [tutorialCollisions.cpp](http://). diff --git a/docs/readthedocs/tutorials/dominoes.md b/docs/readthedocs/tutorials/dominoes.md index fe68b77c7c8b9..c3c1961df2125 100644 --- a/docs/readthedocs/tutorials/dominoes.md +++ b/docs/readthedocs/tutorials/dominoes.md @@ -9,6 +9,8 @@ dynamic systems, such as robotic manipulators. We will show you how to: - Write a stable PD controller w/ gravity and coriolis compensation - Write an operational space controller +Please reference the source code in **tutorialDominoes.cpp** and **tutorialDominoes-Finished.cpp**. + # Lesson 1: Cloning Skeletons There are often times where you might want to create an exact replica of an diff --git a/docs/readthedocs/tutorials/multi-pendulum.md b/docs/readthedocs/tutorials/multi-pendulum.md index 5695e3d0d8086..b2d3e81281640 100644 --- a/docs/readthedocs/tutorials/multi-pendulum.md +++ b/docs/readthedocs/tutorials/multi-pendulum.md @@ -11,6 +11,8 @@ API during simulation. This will show you how to: - Alter the implicit spring and damping properties of joints - Add/remove dynamic constraints +Please reference the source code in **tutorialMultiPendulum.cpp** and **tutorialMultiPendulum-Finished.cpp**. + # Lesson 0: Simulate a passive multi-pendulum This is a warmup lesson that demonstrates how to set up a simulation @@ -63,7 +65,7 @@ world->addSkeleton(pendulum); MyWindow window(world); ``` -Every single time step, the ``timeStepping`` function will be called +Every single time step, the ``MyWindow::timeStepping`` function will be called and the state of the World will be simulated. The user can override the default timeStepping function to customize the simulation routine. For example, one can incorporate sensors, actuators, or user @@ -86,12 +88,6 @@ arrow attached to any body. Find the function named ``timeStepping`` in the ``MyWindow`` class. The top of this function is where we will want to reset everything to the default appearance. -In DART, an articulated dynamics model is represented by a ``Skeleton``. A -Skeleton is a structure that consists of ``BodyNode``s (bodies) which are -connected by ``Joint``s. Every Joint has a child BodyNode, and every BodyNode -has a parent Joint. Even the root BodyNode has a Joint that attaches it to the -World. - Each BodyNode contains visualization ``Shape``s that will be rendered during simulation. In our case, each BodyNode has two shapes: From 0880efac2be728ecc770d8aac27c911b67afb9ec Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 4 Aug 2015 16:04:48 -0400 Subject: [PATCH 60/65] Add facebook comment and "like" button to the tutorial pages --- docs/readthedocs/index.md | 13 +++++++++++++ docs/readthedocs/tutorials/biped.md | 13 +++++++++++++ docs/readthedocs/tutorials/collisions.md | 13 +++++++++++++ docs/readthedocs/tutorials/dominoes.md | 13 +++++++++++++ docs/readthedocs/tutorials/introduction.md | 13 +++++++++++++ docs/readthedocs/tutorials/multi-pendulum.md | 13 +++++++++++++ 6 files changed, 78 insertions(+) diff --git a/docs/readthedocs/index.md b/docs/readthedocs/index.md index 0ff50004cad73..cdf1492ed58c0 100644 --- a/docs/readthedocs/index.md +++ b/docs/readthedocs/index.md @@ -5,3 +5,16 @@ DART (Dynamic Animation and Robotics Toolkit) is a collaborative, cross-platform, open source library created by the Georgia Tech Graphics Lab and Humanoid Robotics Lab. The library provides data structures and algorithms for kinematic and dynamic applications in robotics and computer animation. DART is distinguished by its accuracy and stability due to its use of generalized coordinates to represent articulated rigid body systems and Featherstone's Articulated Body Algorithm to compute the dynamics of motion. For developers, in contrast to many popular physics engines which view the simulator as a black box, DART gives full access to internal kinematic and dynamic quantities, such as the mass matrix, Coriolis and centrifugal forces, transformation matrices and their derivatives. DART also provides efficient computation of Jacobian matrices for arbitrary body points and coordinate frames. The frame semantics of DART allows users to define arbitrary reference frames (both inertial and non-inertial) and use those frames to specify or request data. For air-tight code safety, forward kinematics and dynamics values are updated automatically through lazy evaluation, making DART suitable for real time controllers. Contacts and collisions are handled using an implicit time-stepping, velocity-based LCP (linear-complementarity problem) to guarantee non-penetration, directional friction, and approximated Coulomb friction cone conditions. DART has applications in robotics and computer animation because it features a multibody dynamic simulator and tools for control and motion planning. Multibody dynamic simulation in DART is an extension of RTQL8, an open source software created by the Georgia Tech Graphics Lab. DART is fully integrated with Gazebo, a multi-robot simulator for outdoor environments. DART supports most features provided in Gazebo, including the link structure, sensor simulation, and all the joint types. DART also supports models in URDF, SDF, and VSK file formats. + +
+ + +
+ +
diff --git a/docs/readthedocs/tutorials/biped.md b/docs/readthedocs/tutorials/biped.md index 8b50f3a25030f..e7603bf8f0a6a 100644 --- a/docs/readthedocs/tutorials/biped.md +++ b/docs/readthedocs/tutorials/biped.md @@ -500,3 +500,16 @@ This Lesson concludes the entire Biped tutorial. You should see a biped standing stably on the skateboard. With moderate acceleration/deceleration on the skateboard, the biped is able to maintain balance and hold the one-foot stance pose. + +
+ + +
+ +
diff --git a/docs/readthedocs/tutorials/collisions.md b/docs/readthedocs/tutorials/collisions.md index df02fb0ac4c91..bfb1595238d34 100644 --- a/docs/readthedocs/tutorials/collisions.md +++ b/docs/readthedocs/tutorials/collisions.md @@ -843,3 +843,16 @@ And that's it! You're ready to run the full tutorialCollisions application! **When running the application, keep in mind that the dynamics of collisions are finnicky, so you may see some unstable and even completely non-physical behavior. If the application freezes, you may need to force quit out of it.** + +
+ + +
+ +
diff --git a/docs/readthedocs/tutorials/dominoes.md b/docs/readthedocs/tutorials/dominoes.md index c3c1961df2125..e8091161b0b5c 100644 --- a/docs/readthedocs/tutorials/dominoes.md +++ b/docs/readthedocs/tutorials/dominoes.md @@ -534,3 +534,16 @@ mManipulator->setForces(mForces); ``` **Now you're ready to try out the full dominoes app!** + +
+ + +
+ +
diff --git a/docs/readthedocs/tutorials/introduction.md b/docs/readthedocs/tutorials/introduction.md index 2bd6b871b0828..c9021145fdf94 100644 --- a/docs/readthedocs/tutorials/introduction.md +++ b/docs/readthedocs/tutorials/introduction.md @@ -5,3 +5,16 @@ the tutorial code in the directory: dart/tutorials. For each of the four tutorials, we provide the skeleton code as the starting point (e.g. tutorialMultiPendulum.cpp) and the final code as the answer to the tutorial (e.g. tutorialMultiPendulum-Finished.cpp). The examples are based on the APIs of DART 5.0. + +
+ + +
+ +
\ No newline at end of file diff --git a/docs/readthedocs/tutorials/multi-pendulum.md b/docs/readthedocs/tutorials/multi-pendulum.md index b2d3e81281640..992637a9d9b5f 100644 --- a/docs/readthedocs/tutorials/multi-pendulum.md +++ b/docs/readthedocs/tutorials/multi-pendulum.md @@ -386,3 +386,16 @@ Currently DART does not use smart pointers for dynamic constraints, so they need to be explicitly deleted. This may be revised in a later version of DART. **Now you are ready to run the demo!** + +
+ + +
+ +
From 0ed5f646e2035d4dd39ea54ce97e6adf7994aa7a Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 4 Aug 2015 22:13:01 -0400 Subject: [PATCH 61/65] Replace comment system from Facebook to Disqus to allow anonymous comments --- docs/readthedocs/index.md | 14 +++++++++++++- docs/readthedocs/tutorials/biped.md | 14 +++++++++++++- docs/readthedocs/tutorials/collisions.md | 14 +++++++++++++- docs/readthedocs/tutorials/dominoes.md | 14 +++++++++++++- docs/readthedocs/tutorials/introduction.md | 14 +++++++++++++- docs/readthedocs/tutorials/multi-pendulum.md | 14 +++++++++++++- 6 files changed, 78 insertions(+), 6 deletions(-) diff --git a/docs/readthedocs/index.md b/docs/readthedocs/index.md index cdf1492ed58c0..1d88b6d20df06 100644 --- a/docs/readthedocs/index.md +++ b/docs/readthedocs/index.md @@ -17,4 +17,16 @@ DART is fully integrated with Gazebo, a multi-robot simulator for outdoor enviro
-
+
+ + diff --git a/docs/readthedocs/tutorials/biped.md b/docs/readthedocs/tutorials/biped.md index e7603bf8f0a6a..fb69b62fcfb3b 100644 --- a/docs/readthedocs/tutorials/biped.md +++ b/docs/readthedocs/tutorials/biped.md @@ -512,4 +512,16 @@ maintain balance and hold the one-foot stance pose.
-
+
+ + diff --git a/docs/readthedocs/tutorials/collisions.md b/docs/readthedocs/tutorials/collisions.md index bfb1595238d34..d5d339e497269 100644 --- a/docs/readthedocs/tutorials/collisions.md +++ b/docs/readthedocs/tutorials/collisions.md @@ -855,4 +855,16 @@ If the application freezes, you may need to force quit out of it.**
-
+
+ + diff --git a/docs/readthedocs/tutorials/dominoes.md b/docs/readthedocs/tutorials/dominoes.md index e8091161b0b5c..e7169a29625a2 100644 --- a/docs/readthedocs/tutorials/dominoes.md +++ b/docs/readthedocs/tutorials/dominoes.md @@ -546,4 +546,16 @@ mManipulator->setForces(mForces);
-
+
+ + diff --git a/docs/readthedocs/tutorials/introduction.md b/docs/readthedocs/tutorials/introduction.md index c9021145fdf94..aef962a06efb1 100644 --- a/docs/readthedocs/tutorials/introduction.md +++ b/docs/readthedocs/tutorials/introduction.md @@ -17,4 +17,16 @@ the final code as the answer to the tutorial (e.g. tutorialMultiPendulum-Finishe
-
\ No newline at end of file +
+ + diff --git a/docs/readthedocs/tutorials/multi-pendulum.md b/docs/readthedocs/tutorials/multi-pendulum.md index 992637a9d9b5f..c2595333d35f0 100644 --- a/docs/readthedocs/tutorials/multi-pendulum.md +++ b/docs/readthedocs/tutorials/multi-pendulum.md @@ -398,4 +398,16 @@ need to be explicitly deleted. This may be revised in a later version of DART.
-
+
+ + From a556cea22933167d27e8bf095b54d5d33d1ff7b2 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sun, 27 Sep 2015 21:39:17 -0400 Subject: [PATCH 62/65] Fill uncompleted API tables --- docs/readthedocs/tutorials/biped.md | 63 +++++++++++++++++++---------- 1 file changed, 41 insertions(+), 22 deletions(-) diff --git a/docs/readthedocs/tutorials/biped.md b/docs/readthedocs/tutorials/biped.md index fb69b62fcfb3b..6ee0b6055aa45 100644 --- a/docs/readthedocs/tutorials/biped.md +++ b/docs/readthedocs/tutorials/biped.md @@ -298,13 +298,19 @@ void addAnkleStrategyForces() } ``` -The linear/angular velocity of any point in any coordinate frame can be -easily accessed in DART. The following table summarizes the APIs for -accessing various velocities. - -| Function name | Description | -|-------------|-----------| -|Content Cell | Content Cell | +The linear/angular velocity/acceleration of any point in any coordinate +frame can be easily accessed in DART. The full list of the APIs for accessing +various velocities/accelerations can be found in the [API Documentation](http://dartsim.github.io/dart/). The +following table summarizes the essential APIs. + +| Function Name | Description | +|------------------------|-------------| +| getSpatialVelocity | Return the spatial velocity of this BodyNode in the coordinates of the BodyNode. | +| getLinearVelocity | Return the linear portion of classical velocity of the BodyNode relative to some other BodyNode. | +| getAngularVelocity | Return the angular portion of classical velocity of this BodyNode relative to some other BodyNode. | +| getSpatialAcceleration | Return the spatial acceleration of this BodyNode in the coordinates of the BodyNode. | +| getLinearAcceleration | Return the linear portion of classical acceleration of the BodyNode relative to some other BodyNode. | +| getAngularAcceleration | Return the angular portion of classical acceleration of this BodyNode relative to some other BodyNode. | The remaining of the ankle strategy implementation is just the matter of parameters tuning. We found that using different feedback rules for @@ -358,12 +364,16 @@ void modifyBipedWithSkateboard(SkeletonPtr biped) ``` There are many other functions you can use to edit skeletons. Here is -a table of relevant functions for quick references. +a table of some relevant functions for quick references. -| Function Name | Example | Description | -|-------------|----------------|-----------| -| moveTo | bd1->moveTo(bd2, BallJoint::Properties)| Move the body node bd1 and its subtree under the body node bd2 via a default ball joint. | -| Content Cell | Content Cell | Content Cell | +| Function Name | Example | Description | +|-----------------------|-----------------------------------------------|-------------| +| remove | bd1->remove() | Remove the BodyNode bd1 and its subtree from their Skeleton. | +| moveTo | bd1->moveTo(bd2) | Move the BodyNode bd1 and its subtree under the BodyNode bd2. | +| split | auto newSkel = bd1->split("new skeleton")` | Remove the BodyNode bd1 and its subtree from their current Skeleton and move them into a newly created Skeleton with "new skeleton" name. | +| changeParentJointType | bd1->changeParentJointType<BallJoint>() | Change the Joint type of the BodyNode bd1's parent joint to BallJoint | +| copyTo | bd1->copyTo(bd2) | Create clones of the BodyNode bd1 and its subtree and attach the clones to the specified the BodyNode bd2. | +| copyAs | auto newSkel = bd1->copyAs("new skeleton") | Create clones of the BodyNode bd1 and its subtree and create a new Skeleton with "new skeleton" name to attach them to. | # Lesson 6: Actuator types @@ -371,10 +381,13 @@ a table of relevant functions for quick references. DART provides five types of actuator. Each joint can select its own actuator type. -| Type | Description | -|-----|-----------| -|Content Cell | Content Cell | - +| Type | Description | +|--------------|-------------| +| FORCE | Take joint force and return the resulting joint acceleration. | +| PASSIVE | Take nothing (joint force = 0) and return the resulting joint acceleration. | +| ACCELERATION | Take desired joint acceleration and return the joint force to achieve the acceleration. | +| VELOCITY | Take desired joint velocity and return the joint force to achieve the velocity. | +| LOCKED | Lock the joint by setting the joint velocity and acceleration to zero and return the joint force to lock the joint. | In this Lesson, we will switch the actuator type of the wheels from the default FORCE type to VELOCITY type. @@ -489,12 +502,18 @@ Eigen::VectorXd solveIK(SkeletonPtr biped) } ``` -The comprehensive list of Jacobian APIs can be found in the following -table: - -| Function name | Description | -|-----|-----------| -|Content Cell | Content Cell | +The full list of Jacobian APIs can be found in the [API Documentation](http://dartsim.github.io/dart/). The +following table summarizes the essential APIs. + +| Function Name | Description | +|-------------------------|-------------| +| getJacobian | Return the generalized Jacobian targeting the origin of the BodyNode. The Jacobian is expressed in the Frame of this BodyNode. | +| getLinearJacobian | Return the linear Jacobian targeting the origin of the BodyNode. You can specify a coordinate Frame to express the Jacobian in. | +| getAngularJacobian | Return the angular Jacobian targeting the origin of the BodyNode. You can specify a coordinate Frame to express the Jacobian in. | +| getJacobianSpatialDeriv | Return the spatial time derivative of the generalized Jacobian targeting the origin of the BodyNode. The Jacobian is expressed in the BodyNode's coordinate Frame. | +| getJacobianClassicDeriv | Return the classical time derivative of the generalized Jacobian targeting the origin of the BodyNode. The Jacobian is expressed in the World coordinate Frame. | +| getLinearJacobianDeriv | Return the linear Jacobian (classical) time derivative, in terms of any coordinate Frame. | +| getAngularJacobianDeriv | Return the angular Jacobian (classical) time derivative, in terms of any coordinate Frame. | This Lesson concludes the entire Biped tutorial. You should see a biped standing stably on the skateboard. With moderate From 1127f24bcc20b419da9a09334cf0485619473958 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 30 Sep 2015 10:26:40 -0400 Subject: [PATCH 63/65] Delete unnecessary whitespaces --- apps/vehicle/Main.cpp | 2 +- apps/vehicle/MyWindow.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/apps/vehicle/Main.cpp b/apps/vehicle/Main.cpp index f5c7573333a1c..c5c688bb1fc74 100644 --- a/apps/vehicle/Main.cpp +++ b/apps/vehicle/Main.cpp @@ -52,7 +52,7 @@ int main(int argc, char* argv[]) assert(myWorld != nullptr); Eigen::Vector3d gravity(0.0, -9.81, 0.0); myWorld->setGravity(gravity); - + // create a window and link it to the world MyWindow window; window.setWorld(myWorld); diff --git a/apps/vehicle/MyWindow.cpp b/apps/vehicle/MyWindow.cpp index f4b8490699d3c..c47674333abc4 100644 --- a/apps/vehicle/MyWindow.cpp +++ b/apps/vehicle/MyWindow.cpp @@ -51,8 +51,8 @@ void MyWindow::timeStepping() { dart::dynamics::SkeletonPtr vehicle = mWorld->getSkeleton("car_skeleton"); assert(vehicle != 0); - size_t dof = vehicle->getNumDofs(); - + size_t dof = vehicle->getNumDofs(); + Eigen::VectorXd q = vehicle->getPositions(); Eigen::VectorXd dq = vehicle->getVelocities(); Eigen::VectorXd tau = Eigen::VectorXd::Zero(dof); From 2a04bc89a8751b8e6cc8fc4b7ea3454ced57794a Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 30 Sep 2015 12:01:05 -0400 Subject: [PATCH 64/65] Add newline at EOF of .gitignore --- .gitignore | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index b8c4ec59c0735..8aadbe300a507 100644 --- a/.gitignore +++ b/.gitignore @@ -22,4 +22,4 @@ TAGS *~ /nbproject/ /doxygen/html/ -docs/readthedocs/site \ No newline at end of file +docs/readthedocs/site From 12be7a167e608e9b5b1463beb530d9c5af980d20 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 30 Sep 2015 12:39:37 -0400 Subject: [PATCH 65/65] Add hyper links --- docs/readthedocs/index.md | 4 ++-- docs/readthedocs/tutorials/biped.md | 4 ++-- docs/readthedocs/tutorials/collisions.md | 4 ++-- docs/readthedocs/tutorials/dominoes.md | 2 +- docs/readthedocs/tutorials/introduction.md | 6 +++--- docs/readthedocs/tutorials/multi-pendulum.md | 2 +- 6 files changed, 11 insertions(+), 11 deletions(-) diff --git a/docs/readthedocs/index.md b/docs/readthedocs/index.md index 1d88b6d20df06..147cb58687a12 100644 --- a/docs/readthedocs/index.md +++ b/docs/readthedocs/index.md @@ -2,9 +2,9 @@ -DART (Dynamic Animation and Robotics Toolkit) is a collaborative, cross-platform, open source library created by the Georgia Tech Graphics Lab and Humanoid Robotics Lab. The library provides data structures and algorithms for kinematic and dynamic applications in robotics and computer animation. DART is distinguished by its accuracy and stability due to its use of generalized coordinates to represent articulated rigid body systems and Featherstone's Articulated Body Algorithm to compute the dynamics of motion. For developers, in contrast to many popular physics engines which view the simulator as a black box, DART gives full access to internal kinematic and dynamic quantities, such as the mass matrix, Coriolis and centrifugal forces, transformation matrices and their derivatives. DART also provides efficient computation of Jacobian matrices for arbitrary body points and coordinate frames. The frame semantics of DART allows users to define arbitrary reference frames (both inertial and non-inertial) and use those frames to specify or request data. For air-tight code safety, forward kinematics and dynamics values are updated automatically through lazy evaluation, making DART suitable for real time controllers. Contacts and collisions are handled using an implicit time-stepping, velocity-based LCP (linear-complementarity problem) to guarantee non-penetration, directional friction, and approximated Coulomb friction cone conditions. DART has applications in robotics and computer animation because it features a multibody dynamic simulator and tools for control and motion planning. Multibody dynamic simulation in DART is an extension of RTQL8, an open source software created by the Georgia Tech Graphics Lab. +[DART](http://dartsim.github.io/) (Dynamic Animation and Robotics Toolkit) is a collaborative, cross-platform, open source library created by the [Georgia Tech](http://www.gatech.edu/) [Graphics Lab](http://www.cc.gatech.edu/~karenliu/Home.html) and [Humanoid Robotics Lab](http://www.golems.org/). The library provides data structures and algorithms for kinematic and dynamic applications in robotics and computer animation. DART is distinguished by its accuracy and stability due to its use of generalized coordinates to represent articulated rigid body systems and Featherstone's Articulated Body Algorithm to compute the dynamics of motion. For developers, in contrast to many popular physics engines which view the simulator as a black box, DART gives full access to internal kinematic and dynamic quantities, such as the mass matrix, Coriolis and centrifugal forces, transformation matrices and their derivatives. DART also provides efficient computation of Jacobian matrices for arbitrary body points and coordinate frames. The frame semantics of DART allows users to define arbitrary reference frames (both inertial and non-inertial) and use those frames to specify or request data. For air-tight code safety, forward kinematics and dynamics values are updated automatically through lazy evaluation, making DART suitable for real time controllers. Contacts and collisions are handled using an implicit time-stepping, velocity-based LCP (linear-complementarity problem) to guarantee non-penetration, directional friction, and approximated Coulomb friction cone conditions. DART has applications in robotics and computer animation because it features a multibody dynamic simulator and tools for control and motion planning. Multibody dynamic simulation in DART is an extension of [RTQL8](https://bitbucket.org/karenliu/rtql8), an open source software created by the Georgia Tech Graphics Lab. -DART is fully integrated with Gazebo, a multi-robot simulator for outdoor environments. DART supports most features provided in Gazebo, including the link structure, sensor simulation, and all the joint types. DART also supports models in URDF, SDF, and VSK file formats. +DART is fully integrated with [Gazebo](http://gazebosim.org/), a multi-robot simulator for outdoor environments. DART supports most features provided in Gazebo, including the link structure, sensor simulation, and all the joint types. DART also supports models in [URDF](http://wiki.ros.org/urdf), [SDF](http://sdformat.org/), and VSK file formats.