diff --git a/.gitignore b/.gitignore index ffde43bf85ede..8aadbe300a507 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 diff --git a/CMakeLists.txt b/CMakeLists.txt index f0ed35509495a..32298c4ec78d3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -66,6 +66,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) #=============================================================================== @@ -315,6 +316,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) @@ -482,6 +484,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}") @@ -558,6 +561,10 @@ if(NOT BUILD_CORE_ONLY) add_subdirectory(apps) endif() + if(DART_BUILD_TUTORIALS) + add_subdirectory(tutorials) + endif() + endif() #=============================================================================== 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) 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); } } } diff --git a/dart/collision/CollisionDetector.cpp b/dart/collision/CollisionDetector.cpp index 919f03e148ff9..73f99dd903701 100644 --- a/dart/collision/CollisionDetector.cpp +++ b/dart/collision/CollisionDetector.cpp @@ -300,6 +300,9 @@ bool CollisionDetector::getPairCollidable(const CollisionNode* _node1, if (!isValidIndex(mCollidablePairs, index1, index2)) return false; + if (index1 == index2) + return false; + return mCollidablePairs[index1][index2]; } 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; 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 d8897d2f91659..a81066feff94a 100644 --- a/dart/dynamics/BodyNode.h +++ b/dart/dynamics/BodyNode.h @@ -274,10 +274,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 153ddcdd6ec9d..c740e641dada1 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 { @@ -129,10 +130,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(); diff --git a/dart/dynamics/Shape.h b/dart/dynamics/Shape.h index 569b4cafae420..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 { @@ -176,9 +177,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 diff --git a/dart/dynamics/SoftBodyNode.cpp b/dart/dynamics/SoftBodyNode.cpp index 4297d9a83d95e..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()); + } } //============================================================================== @@ -1175,6 +1190,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) diff --git a/dart/dynamics/SoftBodyNode.h b/dart/dynamics/SoftBodyNode.h index 006245ec98fd8..a52c112a7dea0 100644 --- a/dart/dynamics/SoftBodyNode.h +++ b/dart/dynamics/SoftBodyNode.h @@ -127,10 +127,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 diff --git a/dart/math/Helpers.h b/dart/math/Helpers.h index effc18681c375..2c3a0207378f4 100644 --- a/dart/math/Helpers.h +++ b/dart/math/Helpers.h @@ -268,6 +268,26 @@ 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); +} + +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/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/docs/readthedocs/index.md b/docs/readthedocs/index.md new file mode 100644 index 0000000000000..147cb58687a12 --- /dev/null +++ b/docs/readthedocs/index.md @@ -0,0 +1,32 @@ +# Welcome to DART + + + +[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](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. + +
+ + +
+ +
+ + diff --git a/docs/readthedocs/logo.png b/docs/readthedocs/logo.png new file mode 100644 index 0000000000000..6c593af571b07 Binary files /dev/null and b/docs/readthedocs/logo.png differ diff --git a/docs/readthedocs/tutorials/biped.md b/docs/readthedocs/tutorials/biped.md new file mode 100644 index 0000000000000..ccde200c9c8bb --- /dev/null +++ b/docs/readthedocs/tutorials/biped.md @@ -0,0 +1,546 @@ +# Overview +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: + +- Joint limits and self-collision. +- Actuators types and management. +- APIs for Jacobian matrices and other kinematic quantities. +- APIs for dynamic quantities. +- Skeleton editing. + +Please reference the source code in [**tutorialBiped.cpp**](https://github.com/dartsim/dart/blob/release-5.1/tutorials/tutorialBiped.cpp) and [**tutorialBiped-Finished.cpp**](https://github.com/dartsim/dart/blob/release-5.1/tutorials/tutorialBiped-Finished.cpp). + +# Lesson 1: Joint limits and self-collision +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**](https://github.com/dartsim/dart/blob/release-5.1/data/skel/biped.skel) and assign the bipedal figure to a +``Skeleton`` pointer called *biped*. + +```cpp +SkeletonPtr loadBiped() +{ +... + WorldPtr world = SkelParser::readWorld(DART_DATA_PATH"skel/biped.skel"); + SkeletonPtr biped = world->getSkeleton("biped"); +... +} +``` + +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**: + +```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. Alternatively, you can directly specify the joint limits +in the code using +``setPositionUpperLimit`` and ``setPositionLowerLimit``. + +In either case, the joint limits on the biped will not be activated +until you call ``setPositionLimited``: + +```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: +```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 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 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 +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 particular configuration. You +can use ``setPosition`` 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 accessed by +``getIndexInSkeleton``. You +can also set the entire configuration using a vector that holds the +positions of all the degreed of freedoms using +``setPositions``. + +We continue to set more degrees of freedoms in the lower +body to create a roughly stable standing pose. + +```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; + } + + for(size_t i = 6; i < mBiped->getNumDofs(); ++i) + { + 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 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. + +With these settings, we can compute the forces generated by the PD +controller and add them to the internal forces of biped using ``setForces``: + +```cpp +void addPDForces() +{ + 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 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 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 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 API, which makes the full +implementation of SPD simple and concise: + +```cpp +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); +} +``` + +You can get mass matrix, Coriolis force, gravitational force, and +constraint force projected onto generalized coordinates using function +calls ``getMassMatrix``, +``getCoriolisForces``, +``getGravityForces``, +and +``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 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 feedback 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`` 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 (ẋ - ṗ): + +```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/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 +falling forward and backward result 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**: + +```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 BodyNode via a newly created +Euler joint. To do so, you need to first create an instance of +``EulerJoint::Properties`` 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 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 + +```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 some relevant functions for quick references. + +| 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 + +DART provides five types of actuator. Each joint can select its own +actuator type. + +| 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. + +```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 +command them 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`` 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. + +```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 BodyNode: + +```cpp +Eigen::VectorXd solveIK(SkeletonPtr biped) +{ +... + Eigen::Vector3d localCOM = leftHeel->getCOM(leftHeel); + LinearJacobian jacobian = biped->getCOMLinearJacobian() - biped->getLinearJacobian(leftHeel, localCOM); +... +} +``` + +``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`` +with a parameter indicating the left foot being the frame of +reference. We use ``getLinearJacobian`` 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 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 +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 0000000000000..d5d0f39ca5d9a Binary files /dev/null and b/docs/readthedocs/tutorials/biped/IKObjective.png differ diff --git a/docs/readthedocs/tutorials/collisions.md b/docs/readthedocs/tutorials/collisions.md new file mode 100644 index 0000000000000..9452bb7427264 --- /dev/null +++ b/docs/readthedocs/tutorials/collisions.md @@ -0,0 +1,870 @@ +# 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 + +Please reference the source code in [**tutorialCollision.cpp**](https://github.com/dartsim/dart/blob/release-5.1/tutorials/tutorialCollision.cpp) and [**tutorialCollision-Finished.cpp**](https://github.com/dartsim/dart/blob/release-5.1/tutorials/tutorialCollision-Finished.cpp). + +# Lesson 1: Creating a rigid body + +Start by going opening the Skeleton code [tutorialCollisions.cpp](https://github.com/dartsim/dart/blob/release-5.1/tutorials/tutorialCollision.cpp). +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 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 +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 +// 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; +soft_properties = SoftBodyNodeHelper::makeBoxProperties( + dims, Eigen::Isometry3d::Identity(), Eigen::Vector3i(4,4,4), mass); +``` + +For the SOFT_CYLINDER: +```cpp +// 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; +// 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); +``` + +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*M_PI*pow(radius, 2) + * default_skin_thickness; +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! + +### 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 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: 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 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; +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 +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(ring_spring_stiffness); + dof->setDampingCoefficient(ring_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/docs/readthedocs/tutorials/dominoes.md b/docs/readthedocs/tutorials/dominoes.md new file mode 100644 index 0000000000000..2d6830c574a20 --- /dev/null +++ b/docs/readthedocs/tutorials/dominoes.md @@ -0,0 +1,561 @@ +# Overview + +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: + +- Clone Skeletons +- Load a URDF +- Write a stable PD controller w/ gravity and coriolis compensation +- Write an operational space controller + +Please reference the source code in [**tutorialDominoes.cpp**](https://github.com/dartsim/dart/blob/release-5.1/tutorials/tutorialDominoes.cpp) and [**tutorialDominoes-Finished.cpp**](https://github.com/dartsim/dart/blob/release-5.1/tutorials/tutorialDominoes-Finished.cpp). + +# 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/docs/readthedocs/tutorials/introduction.md b/docs/readthedocs/tutorials/introduction.md new file mode 100644 index 0000000000000..45e8134eddc72 --- /dev/null +++ b/docs/readthedocs/tutorials/introduction.md @@ -0,0 +1,32 @@ +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](https://github.com/dartsim/dart/tree/release-5.1/tutorials). For each of the +four tutorials, we provide the skeleton code as the starting point +(e.g. [tutorialMultiPendulum.cpp](https://github.com/dartsim/dart/blob/release-5.1/tutorials/tutorialMultiPendulum.cpp)) and +the final code as the answer to the tutorial (e.g. [tutorialMultiPendulum-Finished.cpp](https://github.com/dartsim/dart/blob/release-5.1/tutorials/tutorialMultiPendulum-Finished.cpp)). The examples are based on the APIs of DART 5.0. + +
+ + +
+ +
+ + diff --git a/docs/readthedocs/tutorials/multi-pendulum.md b/docs/readthedocs/tutorials/multi-pendulum.md new file mode 100644 index 0000000000000..64e95b70d1ad5 --- /dev/null +++ b/docs/readthedocs/tutorials/multi-pendulum.md @@ -0,0 +1,413 @@ +# Overview + +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 +- Apply external forces to the bodies +- Alter the implicit spring and damping properties of joints +- Add/remove dynamic constraints + +Please reference the source code in [**tutorialMultiPendulum.cpp**](https://github.com/dartsim/dart/blob/release-5.1/tutorials/tutorialMultiPendulum.cpp) and [**tutorialMultiPendulum-Finished.cpp**](https://github.com/dartsim/dart/blob/release-5.1/tutorials/tutorialMultiPendulum-Finished.cpp). + +# 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 ``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 +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 +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. + +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); +``` + +# 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: 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 +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/mkdocs.yml b/mkdocs.yml new file mode 100644 index 0000000000000..11140ebea446e --- /dev/null +++ b/mkdocs.yml @@ -0,0 +1,20 @@ +# Project information +site_name: DART Documentation +repo_url: https://github.com/dartsim/dart.git +repo_name: GitHub +site_description: Dynamic Animation and Robotics Toolkit +# site_favicon: null # todo: add dart.ico +copyright: Copyright (c) 2011-2015, Georgia Tech Research Corporation + +# Build directories +theme: readthedocs +docs_dir: docs/readthedocs + +pages: +- Home: 'index.md' +- Tutorials: + - 'Introduction': 'tutorials/introduction.md' + - 'Pendulum': 'tutorials/multi-pendulum.md' + - 'Collisions' : 'tutorials/collisions.md' + - 'Manipulator': 'tutorials/dominoes.md' + - 'Biped': 'tutorials/biped.md' 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/tutorialBiped-Finished.cpp b/tutorials/tutorialBiped-Finished.cpp new file mode 100644 index 0000000000000..da666d7d10bef --- /dev/null +++ b/tutorials/tutorialBiped-Finished.cpp @@ -0,0 +1,502 @@ +/* + * 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), + 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() + { + 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(); + // 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(diff < 0.1 && diff >= 0.0) { + // Feedback rule for recovering forward push + double k1 = 200.0; + double k2 = 100.0; + 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; + 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); + } + + // 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 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 + for(size_t i = 0; i < biped->getNumJoints(); ++i) + biped->getJoint(i)->setPositionLimited(true); + + // Enable self collision check but ignore adjacent bodies + biped->enableSelfCollision(); + + 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")-> + 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); +} + +// 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) +{ + // 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")-> + 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"); + double initialHeight = -0.8; + + 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, localCOM); + + // 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 2 + setInitialPose(biped); + + // Lesson 5 + modifyBipedWithSkateboard(biped); + + // Lesson 6 + setVelocityAccuators(biped); + + // Lesson 7 + Eigen::VectorXd balancedPose = solveIK(biped); + 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()); +#endif + + 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 new file mode 100644 index 0000000000000..8b89bf5fe3499 --- /dev/null +++ b/tutorials/tutorialBiped.cpp @@ -0,0 +1,339 @@ +/* + * 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 < biped->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 + void addPDForces() + { + // Lesson 2 + } + + /// Add commanind forces from Stable-PD controllers + void addSPDForces() + { + // Lesson 3 + } + + /// add commanding forces from ankle strategy + void addAnkleStrategyForces() + { + // Lesson 4 + } + + // Send velocity commands on wheel actuators + void setWheelCommands() + { + // 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; + + /// 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 +SkeletonPtr loadBiped() +{ + // Lesson 1 + + // 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; +} + +// 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) +{ + // 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[]) +{ + 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->setGravity(Eigen::Vector3d(0.0, -9.81, 0.0)); + +#ifdef HAVE_BULLET_COLLISION + world->getConstraintSolver()->setCollisionDetector( + new dart::collision::BulletCollisionDetector()); +#endif + + 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/tutorialCollisions-Finished.cpp b/tutorials/tutorialCollisions-Finished.cpp new file mode 100644 index 0000000000000..8b4ee80d5f5c0 --- /dev/null +++ b/tutorials/tutorialCollisions-Finished.cpp @@ -0,0 +1,685 @@ +/* + * 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 +#include "dart/dart.h" +#include + +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 + +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 ring_spring_stiffness = 0.5; +const double ring_damping_coefficient = 0.05; +const double default_damping_coefficient = 0.001; + +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 = 1000.0; +const double default_edge_stiffness = 1.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(ring_spring_stiffness); + dof->setDampingCoefficient(ring_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::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]); + } + + // 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& 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), + 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(mOriginalSoftBody->clone()); + break; + + case '3': + addObject(mOriginalHybridBody->clone()); + break; + + case '4': + addObject(mOriginalRigidChain->clone()); + break; + + case '5': + 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); + + // 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); + + 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()); + + 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); + offset = tail->getWorldTransform() * offset; + auto constraint = new dart::constraint::BallJointConstraint( + head, tail, 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 soft bodies + SkeletonPtr mOriginalSoftBody; + + /// 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) +{ + // 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 + 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(); + } + + // 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 + ShapePtr 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.0, + 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 inertia; + double mass = default_shape_density * shape->getVolume(); + 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); + + // Set damping to make the simulation more stable + if(parent) + { + Joint* joint = bn->getParentJoint(); + for(size_t i=0; i < joint->getNumDofs(); ++i) + joint->getDof(i)->setDampingCoefficient(default_damping_coefficient); + } + + 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* addSoftBody(const SkeletonPtr& chain, const std::string& name, + SoftShapeType type, BodyNode* parent = nullptr) +{ + // 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 + 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 + SoftBodyNode::UniqueProperties soft_properties; + // Use the SoftBodyNodeHelper class to create the geometries for the + // SoftBodyNode + if(SOFT_BOX == type) + { + // 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( + dims, Eigen::Isometry3d::Identity(), Eigen::Vector3i(4,4,4), mass); + } + else if(SOFT_CYLINDER == type) + { + // 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; + // 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); + } + 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*M_PI*pow(radius, 2) + * default_skin_thickness; + soft_properties = SoftBodyNodeHelper::makeEllipsoidProperties( + 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, 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; +} + +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 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"); + + // Add a soft body + BodyNode* bn = addSoftBody(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()); + + 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 * box->getVolume()); + 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(), 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 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; + + 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 new file mode 100644 index 0000000000000..5d6cc45dd0023 --- /dev/null +++ b/tutorials/tutorialCollisions.cpp @@ -0,0 +1,482 @@ +/* + * 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 +#include "dart/dart.h" +#include + +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 + +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 ring_spring_stiffness = 0.5; +const double ring_damping_coefficient = 0.05; +const double default_damping_coefficient = 0.001; + +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 = 1000.0; +const double default_edge_stiffness = 1.0; +const double default_soft_damping = 5.0; + +using namespace dart::dynamics; +using namespace dart::simulation; +using namespace dart::gui; + +void setupRing(const SkeletonPtr& /*ring*/) +{ + // Set the spring and damping coefficients for the degrees of freedom + // Lesson 4a + + // Compute the joint angle needed to form a ring + // Lesson 4b + + // Set the BallJoints so that they have the correct rest position angle + // Lesson 4b + + // Set the Joints to be in their rest positions + // Lesson 4c +} + +class MyWindow : public SimWindow +{ +public: + + MyWindow(const WorldPtr& world, const SkeletonPtr& ball, + 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), + 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(mOriginalSoftBody->clone()); + break; + + case '3': + addObject(mOriginalHybridBody->clone()); + break; + + case '4': + addObject(mOriginalRigidChain->clone()); + break; + + case '5': + 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 + // Lesson 3a + + // Add the object to the world + // Lesson 3b + + // Compute collisions + // Lesson 3c + + // Refuse to add the object if it is in collision + // Lesson 3c + + // Create reference frames for setting the initial velocity + // 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 + // Lesson 3e + + // Use the reference frames to set the velocity of the Skeleton's root + // 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(!addObject(ring)) + return; + + // Create a closed loop to turn the chain into a ring + // 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; 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 soft bodies + SkeletonPtr mOriginalSoftBody; + + /// 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) +{ + // Set the Joint properties + // Lesson 1a + + // Create the Joint and Body pair + // Lesson 1b + BodyNode* bn = nullptr; + + // Make the shape based on the requested Shape type + // Lesson 1c + + // Setup the inertia for the body + // Lesson 1d + + // 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; +} + +enum SoftShapeType { + SOFT_BOX = 0, + SOFT_CYLINDER, + SOFT_ELLIPSOID +}; + +/// Add a soft body with the specified Joint type to a chain +template +BodyNode* addSoftBody(const SkeletonPtr& /*chain*/, const std::string& /*name*/, + SoftShapeType /*type*/, BodyNode* /*parent*/ = nullptr) +{ + // Set the Joint properties + // Lesson 2a + + // Set the properties of the soft body + // Lesson 2b + + // Create the Joint and Body pair + // Lesson 2c + SoftBodyNode* bn = nullptr; + + // Zero out the inertia for the underlying BodyNode + // Lesson 2d + + // Make the shape transparent + // Lesson 2e + + 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 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"); + + // Add a soft body + /*BodyNode* bn =*/ addSoftBody(hybrid, "soft sphere", SOFT_ELLIPSOID); + + // Add a rigid body attached by a WeldJoint + // Lesson 2g + + 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(), 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 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; + + 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/tutorialDominoes-Finished.cpp b/tutorials/tutorialDominoes-Finished.cpp new file mode 100644 index 0000000000000..8d02695fa530d --- /dev/null +++ b/tutorials/tutorialDominoes-Finished.cpp @@ -0,0 +1,519 @@ +/* + * 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) + : mManipulator(manipulator) + { + // Grab the current joint angles to use them as desired angles + mQDesired = mManipulator->getPositions(); + + // Grab the last body in the manipulator, and use it as an end effector + mEndEffector = mManipulator->getBodyNode(mManipulator->getNumBodyNodes() - 1); + + // 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() = + default_domino_height / 2.0 * Eigen::Vector3d::UnitZ(); + + // Rotate the transform so that it matches the orientation of the end + // effector + target_offset.linear() = + mEndEffector->getTransform(domino->getBodyNode(0)).linear(); + + // Place the mTarget SimpleFrame at the top of the domino + mTarget->setTransform(target_offset, domino->getBodyNode(0)); + + // Set PD control gains + mKpPD = 200.0; + mKdPD = 20.0; + + // Set operational space control gains + mKpOS = 5.0; + mKdOS = 0.01; + } + + /// Compute a stable PD controller that also compensates for gravity and + /// Coriolis forces + void setPDForces() + { + if(nullptr == mManipulator) + return; + + // Compute the joint position error + Eigen::VectorXd q = mManipulator->getPositions(); + Eigen::VectorXd dq = mManipulator->getVelocities(); + q += dq * mManipulator->getTimeStep(); + + Eigen::VectorXd q_err = mQDesired - q; + + // 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; + + mManipulator->setForces(mForces); + } + + /// Compute an operational space controller to push on the first domino + void setOperationalSpaceForces() + { + if(nullptr == mManipulator) + return; + + const Eigen::MatrixXd& M = mManipulator->getMassMatrix(); + + // Compute the Jacobian + 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 = 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>() = mTarget->getWorldTransform().translation() + - mEndEffector->getWorldTransform() * mOffset; + + // Compute the angular error + Eigen::AngleAxisd aa(mTarget->getTransform(mEndEffector).linear()); + e.head<3>() = aa.angle() * aa.axis(); + + // Compute the time derivative of the error + 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 = mManipulator->getCoriolisAndGravityForces(); + + // Turn the control gains into matrix form + Eigen::Matrix6d Kp = mKpOS * Eigen::Matrix6d::Identity(); + + 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(); + fDesired[3] = default_push_force; + Eigen::VectorXd f = J.transpose() * fDesired; + + // Compute the control forces + Eigen::VectorXd dq = mManipulator->getVelocities(); + mForces = M * (pinv_J * Kp * de + pinv_dJ * Kp * e) + - Kd * dq + Kd * pinv_J * Kp * e + Cg + f; + + mManipulator->setForces(mForces); + } + +protected: + + /// The manipulator Skeleton that we will be controlling + SkeletonPtr mManipulator; + + /// The target pose for the controller + SimpleFramePtr mTarget; + + /// End effector for the manipulator + BodyNodePtr mEndEffector; + + /// Desired joint positions when not applying the operational space controller + Eigen::VectorXd mQDesired; + + /// The offset of the end effector from the body origin of the last BodyNode + /// in the manipulator + Eigen::Vector3d mOffset; + + /// Control gains for the proportional error terms in the PD controller + double mKpPD; + + /// Control gains for the derivative error terms in the PD controller + double mKdPD; + + /// Control gains for the proportional error terms in the operational + /// space controller + double mKpOS; + + /// Control gains for the derivative error terms in the operational space + /// controller + double mKdOS; + + /// Joint forces for the manipulator (output of the Controller) + Eigen::VectorXd mForces; +}; + +class MyWindow : public dart::gui::SimWindow +{ +public: + + MyWindow(const WorldPtr& world) + : mTotalAngle(0.0), + mHasEverRun(false), + mForceCountDown(0), + mPushCountDown(0) + { + setWorld(world); + mFirstDomino = world->getSkeleton("domino"); + mFloor = world->getSkeleton("floor"); + + 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) + { + // 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; + + // Compute the position for the new domino + Eigen::Vector3d dx = default_distance * Eigen::Vector3d( + cos(mTotalAngle), sin(mTotalAngle), 0.0); + + Eigen::Vector6d x = lastDomino->getPositions(); + x.tail<3>() += dx; + + // Adjust the angle for the new domino + x[2] = mTotalAngle + angle; + + 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() != mFloor + && contact.bodyNode2.lock()->getSkeleton() != mFloor) + { + 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 + mAngles.push_back(angle); + mDominoes.push_back(newDomino); + mTotalAngle += angle; + } + } + + // Delete the last domino that was added to the scene. (Do not delete the + // original domino) + void deleteLastDomino() + { + if(mDominoes.size() > 0) + { + SkeletonPtr lastDomino = mDominoes.back(); + mDominoes.pop_back(); + mWorld->removeSkeleton(lastDomino); + + mTotalAngle -= mAngles.back(); + mAngles.pop_back(); + } + } + + void keyboard(unsigned char key, int x, int y) override + { + if(!mHasEverRun) + { + 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 ' ': + mHasEverRun = true; + break; + } + } + else + { + switch(key) + { + case 'f': + mForceCountDown = default_force_duration; + break; + + case 'r': + mPushCountDown = 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(mForceCountDown > 0) + { + 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; + } + + // Run the controller for the manipulator + if(mPushCountDown > 0) + { + mController->setOperationalSpaceForces(); + --mPushCountDown; + } + else + { + mController->setPDForces(); + } + + SimWindow::timeStepping(); + } + +protected: + + /// Base domino. Used to clone new dominoes. + SkeletonPtr mFirstDomino; + + /// Floor of the scene + SkeletonPtr mFloor; + + /// History of the dominoes that have been created + std::vector mDominoes; + + /// History of the angles that the user has specified + std::vector mAngles; + + /// Sum of all angles so far + double mTotalAngle; + + /// Set to true the first time spacebar is pressed + bool mHasEverRun; + + /// The first domino will be pushed by a disembodied force while the value of + /// this is greater than zero + int mForceCountDown; + + /// The manipulator will attempt to push on the first domino while the value + /// of this is greater than zero + int mPushCountDown; + + std::unique_ptr mController; + +}; + +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 << "'p': replay simulation" << 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; + + glutInit(&argc, argv); + window.initWindow(640, 480, "Dominoes"); + glutMainLoop(); +} diff --git a/tutorials/tutorialDominoes.cpp b/tutorials/tutorialDominoes.cpp new file mode 100644 index 0000000000000..be0214c4e53f9 --- /dev/null +++ b/tutorials/tutorialDominoes.cpp @@ -0,0 +1,359 @@ +/* + * 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 defaultmEndEffectormOffset = 0.05; + +using namespace dart::dynamics; +using namespace dart::simulation; +using namespace dart::math; + +class Controller +{ +public: + + Controller(const SkeletonPtr& manipulator, const SkeletonPtr& /*domino*/) + : mManipulator(manipulator) + { + // Grab the current joint angles to use them as desired angles + // Lesson 2b + + // Set up the information needed for an operational space controller + // Lesson 3a + + // Set PD control gains + mKpPD = 200.0; + mKdPD = 20.0; + + // Set operational space control gains + mKpOS = 5.0; + mKdOS = 0.01; + } + + /// Compute a stable PD controller that also compensates for gravity and + /// Coriolis forces + void setPDForces() + { + // 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 3b + } + +protected: + + /// The manipulator Skeleton that we will be controlling + SkeletonPtr mManipulator; + + /// The target pose for the controller + SimpleFramePtr mTarget; + + /// End effector for the manipulator + BodyNodePtr mEndEffector; + + /// Desired joint positions when not applying the operational space controller + Eigen::VectorXd mQDesired; + + /// The offset of the end effector from the body origin of the last BodyNode + /// in the manipulator + Eigen::Vector3d mOffset; + + /// Control gains for the proportional error terms in the PD controller + double mKpPD; + + /// Control gains for the derivative error terms in the PD controller + double mKdPD; + + /// Control gains for the proportional error terms in the operational + /// space controller + double mKpOS; + + /// Control gains for the derivative error terms in the operational space + /// controller + double mKdOS; + + /// Joint forces for the manipulator (output of the Controller) + Eigen::VectorXd mForces; +}; + +class MyWindow : public dart::gui::SimWindow +{ +public: + + MyWindow(const WorldPtr& world) + : mTotalAngle(0.0), + mHasEverRun(false), + mForceCountDown(0), + mPushCountDown(0) + { + setWorld(world); + mFirstDomino = world->getSkeleton("domino"); + mFloor = world->getSkeleton("floor"); + + 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*/) + { + // Create a new domino + // Lesson 1a + + // 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() + { + // Lesson 1c + } + + void keyboard(unsigned char key, int x, int y) override + { + if(!mHasEverRun) + { + 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 ' ': + mHasEverRun = true; + break; + } + } + else + { + switch(key) + { + case 'f': + mForceCountDown = default_force_duration; + break; + + case 'r': + mPushCountDown = 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(mForceCountDown > 0) + { + // Lesson 1d + --mForceCountDown; + } + + // Run the controller for the manipulator + if(mPushCountDown > 0) + { + mController->setOperationalSpaceForces(); + --mPushCountDown; + } + else + { + mController->setPDForces(); + } + + SimWindow::timeStepping(); + } + +protected: + + /// Base domino. Used to clone new dominoes. + SkeletonPtr mFirstDomino; + + /// Floor of the scene + SkeletonPtr mFloor; + + /// History of the dominoes that have been created + std::vector mDominoes; + + /// History of the angles that the user has specified + std::vector mAngles; + + /// Sum of all angles so far + double mTotalAngle; + + /// Set to true the first time spacebar is pressed + bool mHasEverRun; + + /// The first domino will be pushed by a disembodied force while the value of + /// this is greater than zero + int mForceCountDown; + + /// The manipulator will attempt to push on the first domino while the value + /// of this is greater than zero + int mPushCountDown; + + std::unique_ptr mController; + +}; + +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() +{ + // Lesson 2a + return Skeleton::create("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 << "'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; + + glutInit(&argc, argv); + window.initWindow(640, 480, "Dominoes"); + glutMainLoop(); +} diff --git a/tutorials/tutorialMultiPendulum-Finished.cpp b/tutorials/tutorialMultiPendulum-Finished.cpp new file mode 100644 index 0000000000000..c4fc738c39f96 --- /dev/null +++ b/tutorials/tutorialMultiPendulum-Finished.cpp @@ -0,0 +1,471 @@ +/* + * 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 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 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) + : mBallConstraint(nullptr), + mPositiveSign(true), + mBodyForce(false) + { + setWorld(world); + + // Find the Skeleton named "pendulum" within the World + mPendulum = world->getSkeleton("pendulum"); + + // Make sure that the pendulum was found in the World + assert(mPendulum != nullptr); + + mForceCountDown.resize(mPendulum->getNumDofs(), 0); + + ArrowShape::Properties arrow_properties; + arrow_properties.mRadius = 0.05; + 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() + { + mPositiveSign = !mPositiveSign; + if(mPositiveSign) + { + 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 + { + 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 < mForceCountDown.size()) + mForceCountDown[index] = default_countdown; + } + + void changeRestPosition(double delta) + { + for(size_t i = 0; i < mPendulum->getNumDofs(); ++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); + + dof->setRestPosition(q0); + } + + // Only curl up along one axis in the BallJoint + mPendulum->getDof(0)->setRestPosition(0.0); + mPendulum->getDof(2)->setRestPosition(0.0); + } + + void changeStiffness(double delta) + { + for(size_t i = 0; i < mPendulum->getNumDofs(); ++i) + { + DegreeOfFreedom* dof = mPendulum->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 < mPendulum->getNumDofs(); ++i) + { + DegreeOfFreedom* dof = mPendulum->getDof(i); + double damping = dof->getDampingCoefficient() + delta; + if(damping < 0.0) + damping = 0.0; + dof->setDampingCoefficient(damping); + } + } + + /// Add a constraint to attach the final link to the world + void addConstraint() + { + // Get the last body in the pendulum + BodyNode* tip = mPendulum->getBodyNode(mPendulum->getNumBodyNodes() - 1); + + // Attach the last link to the world + Eigen::Vector3d location = + tip->getTransform() * Eigen::Vector3d(0.0, 0.0, default_height); + mBallConstraint = new dart::constraint::BallJointConstraint(tip, location); + mWorld->getConstraintSolver()->addConstraint(mBallConstraint); + } + + /// Remove any existing constraint, allowing the pendulum to flail freely + void removeConstraint() + { + mWorld->getConstraintSolver()->removeConstraint(mBallConstraint); + delete mBallConstraint; + mBallConstraint = nullptr; + } + + /// Handle keyboard input + void keyboard(unsigned char key, int x, int y) override + { + switch(key) + { + case '-': + changeDirection(); + break; + + 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 '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(mBallConstraint) + removeConstraint(); + else + addConstraint(); + break; + } + + case 'f': + mBodyForce = !mBodyForce; + break; + + default: + SimWindow::keyboard(key, x, y); + } + } + + void timeStepping() override + { + // Reset all the shapes to be Blue + 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()); + } + + // 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(mArrow); + } + } + + if(!mBodyForce) + { + // 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) + { + DegreeOfFreedom* dof = mPendulum->getDof(i); + dof->setForce( mPositiveSign? default_torque : -default_torque ); + + BodyNode* bn = dof->getChildBodyNode(); + const ShapePtr& shape = bn->getVisualizationShape(0); + shape->setColor(dart::Color::Red()); + + --mForceCountDown[i]; + } + } + } + else + { + // 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) + { + 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); + + const ShapePtr& shape = bn->getVisualizationShape(1); + shape->setColor(dart::Color::Red()); + bn->addVisualizationShape(mArrow); + + --mForceCountDown[i]; + } + } + } + + // Step the simulation forward + SimWindow::timeStepping(); + } + +protected: + + /// An arrow shape that we will use to visualize applied forces + std::shared_ptr mArrow; + + /// The pendulum that we will be perturbing + SkeletonPtr mPendulum; + + /// Pointer to the ball constraint that we will be turning on and off + dart::constraint::BallJointConstraint* mBallConstraint; + + /// Number of iterations before clearing a force entry + std::vector mForceCountDown; + + /// Whether a force should be applied in the positive or negative direction + 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 mBodyForce; +}; + +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 << "'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; + 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; + 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); + window.initWindow(640, 480, "Multi-Pendulum Tutorial"); + glutMainLoop(); +} diff --git a/tutorials/tutorialMultiPendulum.cpp b/tutorials/tutorialMultiPendulum.cpp new file mode 100644 index 0000000000000..9f1fe50381fc1 --- /dev/null +++ b/tutorials/tutorialMultiPendulum.cpp @@ -0,0 +1,398 @@ +/* + * 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 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 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) + : mBallConstraint(nullptr), + mPositiveSign(true), + mBodyForce(false) + { + setWorld(world); + + // Find the Skeleton named "pendulum" within the World + mPendulum = world->getSkeleton("pendulum"); + + // Make sure that the pendulum was found in the World + assert(mPendulum != nullptr); + + mForceCountDown.resize(mPendulum->getNumDofs(), 0); + + ArrowShape::Properties arrow_properties; + arrow_properties.mRadius = 0.05; + 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() + { + mPositiveSign = !mPositiveSign; + if(mPositiveSign) + { + 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 + { + 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 < mForceCountDown.size()) + mForceCountDown[index] = default_countdown; + } + + void changeRestPosition(double) + { + // Lesson 2a + } + + void changeStiffness(double) + { + // Lesson 2b + } + + void changeDamping(double) + { + // Lesson 2c + } + + /// Add a constraint to attach the final link to the world + void addConstraint() + { + // Lesson 3 + } + + /// Remove any existing constraint, allowing the pendulum to flail freely + void removeConstraint() + { + // Lesson 3 + } + + /// Handle keyboard input + void keyboard(unsigned char key, int x, int y) override + { + switch(key) + { + case '-': + changeDirection(); + break; + + 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 '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(mBallConstraint) + removeConstraint(); + else + addConstraint(); + break; + } + + case 'f': + mBodyForce = !mBodyForce; + break; + + default: + SimWindow::keyboard(key, x, y); + } + } + + void timeStepping() override + { + // Reset all the shapes to be Blue + // Lesson 1a + + if(!mBodyForce) + { + // 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) + { + // Lesson 1b + + --mForceCountDown[i]; + } + } + } + else + { + // 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) + { + // Lesson 1c + + --mForceCountDown[i]; + } + } + } + + // Step the simulation forward + SimWindow::timeStepping(); + } + +protected: + + /// An arrow shape that we will use to visualize applied forces + std::shared_ptr mArrow; + + /// The pendulum that we will be perturbing + SkeletonPtr mPendulum; + + /// Pointer to the ball constraint that we will be turning on and off + dart::constraint::BallJointConstraint* mBallConstraint; + + /// Number of iterations before clearing a force entry + std::vector mForceCountDown; + + /// Whether a force should be applied in the positive or negative direction + 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 mBodyForce; +}; + +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 << "'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; + 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; + 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); + window.initWindow(640, 480, "Multi-Pendulum Tutorial"); + glutMainLoop(); +}