diff --git a/CHANGELOG.md b/CHANGELOG.md index 5b1016f714448..c67db0b52c02d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,10 +5,12 @@ * Dynamics * Added `computeLagrangian()` to `MetaSkeleton` and `BodyNode`: [#746](https://github.com/dartsim/dart/pull/746) + * Added `SphereShape`: [#745](https://github.com/dartsim/dart/pull/745) * Misc improvements and bug fixes * Added `virtual Shape::getType()` and deprecated `ShapeType Shpae::getShapeType()`: [#724](https://github.com/dartsim/dart/pull/724) + * Fixed typo: [#756](https://github.com/dartsim/dart/pull/756), [#755](https://github.com/dartsim/dart/pull/755) ### DART 6.0.1 (2016-XX-XX) diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index d424aa0518264..37adde7bd30d5 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -44,6 +44,7 @@ #include "dart/collision/bullet/BulletCollisionGroup.hpp" #include "dart/dynamics/ShapeFrame.hpp" #include "dart/dynamics/Shape.hpp" +#include "dart/dynamics/SphereShape.hpp" #include "dart/dynamics/BoxShape.hpp" #include "dart/dynamics/EllipsoidShape.hpp" #include "dart/dynamics/CylinderShape.hpp" @@ -391,6 +392,7 @@ btCollisionShape* BulletCollisionDetector::createBulletCollisionShape( const dynamics::ConstShapePtr& shape) { using dynamics::Shape; + using dynamics::SphereShape; using dynamics::BoxShape; using dynamics::EllipsoidShape; using dynamics::CylinderShape; @@ -401,7 +403,16 @@ btCollisionShape* BulletCollisionDetector::createBulletCollisionShape( const auto& shapeType = shape->getType(); btCollisionShape* bulletCollisionShape = nullptr; - if (BoxShape::getStaticType() == shapeType) + if (SphereShape::getStaticType() == shapeType) + { + assert(dynamic_cast(shape.get())); + + const auto sphere = static_cast(shape.get()); + const auto radius = sphere->getRadius(); + + bulletCollisionShape = new btSphereShape(radius); + } + else if (BoxShape::getStaticType() == shapeType) { assert(dynamic_cast(shape.get())); @@ -417,15 +428,8 @@ btCollisionShape* BulletCollisionDetector::createBulletCollisionShape( const auto ellipsoid = static_cast(shape.get()); const Eigen::Vector3d& size = ellipsoid->getSize(); - if (ellipsoid->isSphere()) - { - bulletCollisionShape = new btSphereShape(size[0] * 0.5); - } - else - { - bulletCollisionShape = createBulletEllipsoidMesh( - size[0], size[1], size[2]); - } + bulletCollisionShape = createBulletEllipsoidMesh( + size[0], size[1], size[2]); } else if (CylinderShape::getStaticType() == shapeType) { diff --git a/dart/collision/dart/DARTCollide.cpp b/dart/collision/dart/DARTCollide.cpp index 6bd5e5c5d7cfa..7c3e3e0101253 100644 --- a/dart/collision/dart/DARTCollide.cpp +++ b/dart/collision/dart/DARTCollide.cpp @@ -35,6 +35,7 @@ #include #include "dart/math/Helpers.hpp" +#include "dart/dynamics/SphereShape.hpp" #include "dart/dynamics/BoxShape.hpp" #include "dart/dynamics/EllipsoidShape.hpp" #include "dart/dynamics/CylinderShape.hpp" @@ -1262,11 +1263,51 @@ int collide(CollisionObject* o1, CollisionObject* o2, CollisionResult& result) const Eigen::Isometry3d& T1 = o1->getTransform(); const Eigen::Isometry3d& T2 = o2->getTransform(); - if (dynamics::BoxShape::getStaticType() == shapeType1) + if (dynamics::SphereShape::getStaticType() == shapeType1) + { + const auto* sphere0 + = static_cast(shape1.get()); + + if (dynamics::SphereShape::getStaticType() == shapeType2) + { + const auto* sphere1 + = static_cast(shape2.get()); + + return collideSphereSphere( + o1, o2, sphere0->getRadius(), T1, sphere1->getRadius(), T2, result); + } + else if (dynamics::BoxShape::getStaticType() == shapeType2) + { + const auto* box1 + = static_cast(shape2.get()); + + return collideSphereBox( + o1, o2, sphere0->getRadius(), T1, box1->getSize(), T2, result); + } + else if (dynamics::EllipsoidShape::getStaticType() == shapeType2) + { + const auto* ellipsoid1 + = static_cast(shape2.get()); + + return collideSphereSphere(o1, o2, + sphere0->getRadius(), T1, + ellipsoid1->getSize()[0] * 0.5, T2, + result); + } + } + else if (dynamics::BoxShape::getStaticType() == shapeType1) { const auto* box0 = static_cast(shape1.get()); - if (dynamics::BoxShape::getStaticType() == shapeType2) + if (dynamics::SphereShape::getStaticType() == shapeType2) + { + const auto* sphere1 + = static_cast(shape2.get()); + + return collideBoxSphere( + o1, o2, box0->getSize(), T1, sphere1->getRadius(), T2, result); + } + else if (dynamics::BoxShape::getStaticType() == shapeType2) { const auto* box1 = static_cast(shape2.get()); @@ -1292,7 +1333,17 @@ int collide(CollisionObject* o1, CollisionObject* o2, CollisionResult& result) const auto* ellipsoid0 = static_cast(shape1.get()); - if (dynamics::BoxShape::getStaticType() == shapeType2) + if (dynamics::SphereShape::getStaticType() == shapeType2) + { + const auto* sphere1 + = static_cast(shape2.get()); + + return collideSphereSphere(o1, o2, + ellipsoid0->getSize()[0] * 0.5, T1, + sphere1->getRadius(), T2, + result); + } + else if (dynamics::BoxShape::getStaticType() == shapeType2) { const auto* box1 = static_cast(shape2.get()); diff --git a/dart/collision/dart/DARTCollisionDetector.cpp b/dart/collision/dart/DARTCollisionDetector.cpp index b051b40e58d32..0de1d015452f4 100644 --- a/dart/collision/dart/DARTCollisionDetector.cpp +++ b/dart/collision/dart/DARTCollisionDetector.cpp @@ -37,6 +37,7 @@ #include "dart/collision/dart/DARTCollisionObject.hpp" #include "dart/collision/dart/DARTCollisionGroup.hpp" #include "dart/dynamics/ShapeFrame.hpp" +#include "dart/dynamics/SphereShape.hpp" #include "dart/dynamics/BoxShape.hpp" #include "dart/dynamics/EllipsoidShape.hpp" @@ -239,6 +240,9 @@ void warnUnsupportedShapeType(const dynamics::ShapeFrame* shapeFrame) const auto& shape = shapeFrame->getShape(); const auto& shapeType = shape->getType(); + if (shapeType == dynamics::SphereShape::getStaticType()) + return; + if (shapeType == dynamics::BoxShape::getStaticType()) return; diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index c746cd4944428..187a131ce6ad3 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -50,6 +50,7 @@ #include "dart/collision/fcl/tri_tri_intersection_test.hpp" #include "dart/dynamics/ShapeFrame.hpp" #include "dart/dynamics/Shape.hpp" +#include "dart/dynamics/SphereShape.hpp" #include "dart/dynamics/BoxShape.hpp" #include "dart/dynamics/EllipsoidShape.hpp" #include "dart/dynamics/CylinderShape.hpp" @@ -808,6 +809,7 @@ FCLCollisionDetector::createFCLCollisionGeometry( const FCLCollisionGeometryDeleter& deleter) { using dynamics::Shape; + using dynamics::SphereShape; using dynamics::BoxShape; using dynamics::EllipsoidShape; using dynamics::CylinderShape; @@ -818,7 +820,19 @@ FCLCollisionDetector::createFCLCollisionGeometry( fcl::CollisionGeometry* geom = nullptr; const auto& shapeType = shape->getType(); - if (BoxShape::getStaticType() == shapeType) + if (SphereShape::getStaticType() == shapeType) + { + assert(dynamic_cast(shape.get())); + + auto* sphere = static_cast(shape.get()); + const auto radius = sphere->getRadius(); + + if (FCLCollisionDetector::PRIMITIVE == type) + geom = new fcl::Sphere(radius); + else + geom = createEllipsoid(radius*2.0, radius*2.0, radius*2.0); + } + else if (BoxShape::getStaticType() == shapeType) { assert(dynamic_cast(shape.get())); @@ -839,18 +853,11 @@ FCLCollisionDetector::createFCLCollisionGeometry( if (FCLCollisionDetector::PRIMITIVE == type) { - if (ellipsoid->isSphere()) - { - geom = new fcl::Sphere(size[0] * 0.5); - } - else - { #if FCL_VERSION_AT_LEAST(0,4,0) - geom = new fcl::Ellipsoid(FCLTypes::convertVector3(size * 0.5)); + geom = new fcl::Ellipsoid(FCLTypes::convertVector3(size * 0.5)); #else - geom = createEllipsoid(size[0], size[1], size[2]); + geom = createEllipsoid(size[0], size[1], size[2]); #endif - } } else { diff --git a/dart/dynamics/Shape.hpp b/dart/dynamics/Shape.hpp index ac6623a71c1ad..b0fc9f4c5d21c 100644 --- a/dart/dynamics/Shape.hpp +++ b/dart/dynamics/Shape.hpp @@ -50,6 +50,7 @@ class Shape : public virtual common::Subject /// \deprecated Deprecated in 6.1. Please use getType() instead. enum ShapeType { + SPHERE, BOX, ELLIPSOID, CYLINDER, diff --git a/dart/dynamics/SoftBodyNode.cpp b/dart/dynamics/SoftBodyNode.cpp index ff9d62965378d..e59a485f2c2af 100644 --- a/dart/dynamics/SoftBodyNode.cpp +++ b/dart/dynamics/SoftBodyNode.cpp @@ -2509,6 +2509,26 @@ void SoftBodyNodeHelper::setSinglePointMass(SoftBodyNode* _softBodyNode, _dampingCoeff)); } +//============================================================================== +SoftBodyNode::UniqueProperties SoftBodyNodeHelper::makeSphereProperties( + double _radius, + std::size_t _nSlices, + std::size_t _nStacks, + double _totalMass, + double _vertexStiffness, + double _edgeStiffness, + double _dampingCoeff) +{ + return makeEllipsoidProperties( + Eigen::Vector3d::Constant(_radius*2.0), + _nSlices, + _nStacks, + _totalMass, + _vertexStiffness, + _edgeStiffness, + _dampingCoeff); +} + //============================================================================== SoftBodyNode::UniqueProperties SoftBodyNodeHelper::makeEllipsoidProperties( const Eigen::Vector3d& _size, diff --git a/dart/dynamics/SoftBodyNode.hpp b/dart/dynamics/SoftBodyNode.hpp index dadcade78f5b5..c7032ce22f63a 100644 --- a/dart/dynamics/SoftBodyNode.hpp +++ b/dart/dynamics/SoftBodyNode.hpp @@ -393,6 +393,16 @@ class SoftBodyNodeHelper double _edgeStiffness = DART_DEFAULT_EDGE_STIFNESS, double _dampingCoeff = DART_DEFAULT_DAMPING_COEFF); + /// Create a Properties struct for an Sphere-shaped SoftBodyNode + static SoftBodyNode::UniqueProperties makeSphereProperties( + double _radius, + std::size_t _nSlices, + std::size_t _nStacks, + double _totalMass, + double _vertexStiffness = DART_DEFAULT_VERTEX_STIFFNESS, + double _edgeStiffness = DART_DEFAULT_EDGE_STIFNESS, + double _dampingCoeff = DART_DEFAULT_DAMPING_COEFF); + /// Create a Properties struct for an Ellipsoid-shaped SoftBodyNode static SoftBodyNode::UniqueProperties makeEllipsoidProperties( const Eigen::Vector3d& _size, diff --git a/dart/dynamics/SphereShape.cpp b/dart/dynamics/SphereShape.cpp new file mode 100644 index 0000000000000..7f7fc7c2ec739 --- /dev/null +++ b/dart/dynamics/SphereShape.cpp @@ -0,0 +1,115 @@ +/* + * Copyright (c) 2016, Graphics Lab, Georgia Tech Research Corporation + * Copyright (c) 2016, Humanoid Lab, Georgia Tech Research Corporation + * Copyright (c) 2016, Personal Robotics Lab, Carnegie Mellon University + * All rights reserved. + * + * 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/dynamics/SphereShape.hpp" + +#include "dart/math/Helpers.hpp" + +namespace dart { +namespace dynamics { + +//============================================================================== +SphereShape::SphereShape(double radius) + : Shape(SPHERE) +{ + setRadius(radius); +} + +//============================================================================== +SphereShape::~SphereShape() +{ + // Do nothing +} + +//============================================================================== +const std::string& SphereShape::getType() const +{ + return getStaticType(); +} + +//============================================================================== +const std::string& SphereShape::getStaticType() +{ + static const std::string type("SphereShape"); + return type; +} + +//============================================================================== +void SphereShape::setRadius(double radius) +{ + assert(radius > 0.0); + + mRadius = radius; + + mBoundingBox.setMin(Eigen::Vector3d::Constant(-radius)); + mBoundingBox.setMax(Eigen::Vector3d::Constant(radius)); + + updateVolume(); +} + +//============================================================================== +double SphereShape::getRadius() const +{ + return mRadius; +} + +//============================================================================== +double SphereShape::computeVolume(double radius) +{ + return math::constantsd::pi() * 4.0 / 3.0 * std::pow(radius, 3) ; +} + +//============================================================================== +Eigen::Matrix3d SphereShape::computeInertia(double radius, double mass) +{ + Eigen::Matrix3d inertia = Eigen::Matrix3d::Identity(); + + inertia(0, 0) = 2.0 / 5.0 * mass * std::pow(radius, 2); + inertia(1, 1) = inertia(0, 0); + inertia(2, 2) = inertia(0, 0); + + return inertia; +} + +//============================================================================== +Eigen::Matrix3d SphereShape::computeInertia(double mass) const +{ + return computeInertia(mRadius, mass); +} + +//============================================================================== +void SphereShape::updateVolume() +{ + mVolume = computeVolume(mRadius); +} + +} // namespace dynamics +} // namespace dart diff --git a/dart/dynamics/SphereShape.hpp b/dart/dynamics/SphereShape.hpp new file mode 100644 index 0000000000000..ed256ae503e21 --- /dev/null +++ b/dart/dynamics/SphereShape.hpp @@ -0,0 +1,82 @@ +/* + * Copyright (c) 2016, Graphics Lab, Georgia Tech Research Corporation + * Copyright (c) 2016, Humanoid Lab, Georgia Tech Research Corporation + * Copyright (c) 2016, Personal Robotics Lab, Carnegie Mellon University + * All rights reserved. + * + * 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. + */ + +#ifndef DART_DYNAMICS_SPHERESHAPE_HPP_ +#define DART_DYNAMICS_SPHERESHAPE_HPP_ + +#include "dart/dynamics/Shape.hpp" + +namespace dart { +namespace dynamics { + +class SphereShape : public Shape +{ +public: + /// Constructor. + explicit SphereShape(double radius); + + /// Destructor. + virtual ~SphereShape(); + + // Documentation inherited. + const std::string& getType() const; + + /// Returns shape type for this class + static const std::string& getStaticType(); + + /// Set radius of this box. + void setRadius(double radius); + + /// Get radius of this box. + double getRadius() const; + + /// Compute volume from given properties + static double computeVolume(double radius); + + /// Compute moments of inertia of a Sphere + static Eigen::Matrix3d computeInertia(double radius, double mass); + + // Documentation inherited. + Eigen::Matrix3d computeInertia(double mass) const override; + +protected: + // Documentation inherited. + void updateVolume() override; + +private: + /// Radius of this Sphere + double mRadius; +}; + +} // namespace dynamics +} // namespace dart + +#endif // DART_DYNAMICS_SPHERESHAPE_HPP_ diff --git a/dart/gui/GlutWindow.cpp b/dart/gui/GlutWindow.cpp index 874f6136652e1..ba7c1d82b2077 100644 --- a/dart/gui/GlutWindow.cpp +++ b/dart/gui/GlutWindow.cpp @@ -219,7 +219,7 @@ inline GlutWindow* GlutWindow::current() { return mWindows.at(i); } } - std::cout << "An unknown error occured!" << std::endl; + std::cout << "An unknown error occurred!" << std::endl; exit(0); } diff --git a/dart/gui/OpenGLRenderInterface.cpp b/dart/gui/OpenGLRenderInterface.cpp index f5ef365b527f1..e0c141c9e10a1 100644 --- a/dart/gui/OpenGLRenderInterface.cpp +++ b/dart/gui/OpenGLRenderInterface.cpp @@ -143,21 +143,24 @@ void OpenGLRenderInterface::scale(const Eigen::Vector3d& _scale) { glScaled(_scale[0], _scale[1], _scale[2]); } -void OpenGLRenderInterface::drawEllipsoid(const Eigen::Vector3d& _size) { - glScaled(_size(0), _size(1), _size(2)); +void OpenGLRenderInterface::drawSphere(double radius) +{ + GLint slices = 16; + GLint stacks = 16; - GLdouble radius = 0.5; - GLint slices = 16; - GLint stacks = 16; + // Code taken from glut/lib/glut_shapes.c + QUAD_OBJ_INIT; + gluQuadricDrawStyle(quadObj, GLU_FILL); + gluQuadricNormals(quadObj, GLU_SMOOTH); + //gluQuadricTexture(quadObj, GL_TRUE); - // Code taken from glut/lib/glut_shapes.c - QUAD_OBJ_INIT; - gluQuadricDrawStyle(quadObj, GLU_FILL); - gluQuadricNormals(quadObj, GLU_SMOOTH); - //gluQuadricTexture(quadObj, GL_TRUE); + gluSphere(quadObj, radius, slices, stacks); +} - gluSphere(quadObj, radius, slices, stacks); - //glut/lib/glut_shapes.c +void OpenGLRenderInterface::drawEllipsoid(const Eigen::Vector3d& _size) { + glScaled(_size(0), _size(1), _size(2)); + + drawSphere(0.5); } void OpenGLRenderInterface::drawCube(const Eigen::Vector3d& _size) { diff --git a/dart/gui/OpenGLRenderInterface.hpp b/dart/gui/OpenGLRenderInterface.hpp index cc26ecd2eb69f..4e3c9cd40fab5 100644 --- a/dart/gui/OpenGLRenderInterface.hpp +++ b/dart/gui/OpenGLRenderInterface.hpp @@ -80,6 +80,7 @@ class OpenGLRenderInterface : public RenderInterface { void compileList(dynamics::Shape* _shape); GLuint compileList(const Eigen::Vector3d& _scale, const aiScene* _mesh); + void drawSphere(double _radius) override; void drawEllipsoid(const Eigen::Vector3d& _size) override; void drawCube(const Eigen::Vector3d& _size) override; void drawCylinder(double _radius, double _height) override; diff --git a/dart/gui/RenderInterface.cpp b/dart/gui/RenderInterface.cpp index faee15a9903ac..55db08ebd32f7 100644 --- a/dart/gui/RenderInterface.cpp +++ b/dart/gui/RenderInterface.cpp @@ -101,6 +101,10 @@ void RenderInterface::scale(const Eigen::Vector3d& /*_scale*/) { } +void RenderInterface::drawSphere(double /*_radius*/) +{ +} + void RenderInterface::drawEllipsoid(const Eigen::Vector3d& /*_size*/) { } diff --git a/dart/gui/RenderInterface.hpp b/dart/gui/RenderInterface.hpp index edc081996aa93..5216d3e744238 100644 --- a/dart/gui/RenderInterface.hpp +++ b/dart/gui/RenderInterface.hpp @@ -90,6 +90,7 @@ class RenderInterface { virtual void transform(const Eigen::Isometry3d& _transform); //glMultMatrix virtual void scale(const Eigen::Vector3d& _scale); //glScale + virtual void drawSphere(double _radius); virtual void drawEllipsoid(const Eigen::Vector3d& _size); virtual void drawCube(const Eigen::Vector3d& _size); virtual void drawCylinder(double _radius, double _height); diff --git a/dart/gui/SimWindow.cpp b/dart/gui/SimWindow.cpp index 4245ee711dc8e..fc7eed6e823e5 100644 --- a/dart/gui/SimWindow.cpp +++ b/dart/gui/SimWindow.cpp @@ -44,6 +44,7 @@ #include "dart/simulation/World.hpp" #include "dart/dynamics/Skeleton.hpp" #include "dart/dynamics/SoftBodyNode.hpp" +#include "dart/dynamics/SphereShape.hpp" #include "dart/dynamics/BoxShape.hpp" #include "dart/dynamics/EllipsoidShape.hpp" #include "dart/dynamics/CylinderShape.hpp" @@ -157,7 +158,7 @@ void SimWindow::draw() { mRI->setPenColor(Eigen::Vector3d(0.2, 0.2, 0.8)); mRI->pushMatrix(); glTranslated(v[0], v[1], v[2]); - mRI->drawEllipsoid(Eigen::Vector3d(0.02, 0.02, 0.02)); + mRI->drawSphere(0.01); mRI->popMatrix(); } } @@ -176,7 +177,7 @@ void SimWindow::draw() { mRI->setPenColor(Eigen::Vector3d(0.2, 0.2, 0.8)); mRI->pushMatrix(); glTranslated(v[0], v[1], v[2]); - mRI->drawEllipsoid(Eigen::Vector3d(0.02, 0.02, 0.02)); + mRI->drawSphere(0.01); mRI->popMatrix(); } } @@ -395,6 +396,7 @@ void SimWindow::drawShape(const dynamics::Shape* shape, mRI->setPenColor(color); using dynamics::Shape; + using dynamics::SphereShape; using dynamics::BoxShape; using dynamics::EllipsoidShape; using dynamics::CylinderShape; @@ -405,7 +407,12 @@ void SimWindow::drawShape(const dynamics::Shape* shape, const auto& shapeType = shape->getType(); - if (BoxShape::getStaticType() == shapeType) + if (SphereShape::getStaticType() == shapeType) + { + const auto* sphere = static_cast(shape); + mRI->drawSphere(sphere->getRadius()); + } + else if (BoxShape::getStaticType() == shapeType) { const auto* box = static_cast(shape); mRI->drawCube(box->getSize()); @@ -473,7 +480,7 @@ void SimWindow::drawPointMasses( mRI->setPenColor(Eigen::Vector4d(0.8, 0.3, 0.3, 1.0)); else mRI->setPenColor(color); - mRI->drawEllipsoid(Eigen::Vector3d::Constant(0.01)); + mRI->drawSphere(0.005); mRI->popMatrix(); // render point at the resting position @@ -484,7 +491,7 @@ void SimWindow::drawPointMasses( mRI->setPenColor(Eigen::Vector4d(0.8, 0.3, 0.3, 1.0)); else mRI->setPenColor(color); - mRI->drawEllipsoid(Eigen::Vector3d::Constant(0.01)); + mRI->drawSphere(0.005); mRI->popMatrix(); } } @@ -520,7 +527,7 @@ void SimWindow::drawMarker(const dynamics::Marker* marker, mRI->pushMatrix(); mRI->translate(marker->getLocalPosition()); - mRI->drawEllipsoid(Eigen::Vector3d::Constant(0.01)); + mRI->drawSphere(0.005); mRI->popMatrix(); mRI->popName(); diff --git a/dart/gui/osg/ShapeFrameNode.cpp b/dart/gui/osg/ShapeFrameNode.cpp index 76e2072f3bec2..343b7cbeaba3b 100644 --- a/dart/gui/osg/ShapeFrameNode.cpp +++ b/dart/gui/osg/ShapeFrameNode.cpp @@ -36,6 +36,7 @@ #include "dart/gui/osg/ShapeFrameNode.hpp" #include "dart/gui/osg/Utils.hpp" #include "dart/gui/osg/render/ShapeNode.hpp" +#include "dart/gui/osg/render/SphereShapeNode.hpp" #include "dart/gui/osg/render/BoxShapeNode.hpp" #include "dart/gui/osg/render/EllipsoidShapeNode.hpp" #include "dart/gui/osg/render/CylinderShapeNode.hpp" @@ -48,6 +49,7 @@ #include "dart/dynamics/Frame.hpp" #include "dart/dynamics/ShapeFrame.hpp" #include "dart/dynamics/Entity.hpp" +#include "dart/dynamics/SphereShape.hpp" #include "dart/dynamics/BoxShape.hpp" #include "dart/dynamics/EllipsoidShape.hpp" #include "dart/dynamics/CylinderShape.hpp" @@ -170,7 +172,16 @@ void ShapeFrameNode::createShapeNode( const auto& shapeType = shape->getType(); - if(BoxShape::getStaticType() == shapeType) + if(SphereShape::getStaticType() == shapeType) + { + std::shared_ptr es = + std::dynamic_pointer_cast(shape); + if(es) + mShapeNode = new render::SphereShapeNode(es, this); + else + warnAboutUnsuccessfulCast(shapeType, mShapeFrame->getName()); + } + else if(BoxShape::getStaticType() == shapeType) { std::shared_ptr bs = std::dynamic_pointer_cast(shape); diff --git a/dart/gui/osg/SupportPolygonVisual.cpp b/dart/gui/osg/SupportPolygonVisual.cpp index 5e10f448ecf6d..76794683f86ba 100644 --- a/dart/gui/osg/SupportPolygonVisual.cpp +++ b/dart/gui/osg/SupportPolygonVisual.cpp @@ -32,7 +32,7 @@ #include "dart/dynamics/Skeleton.hpp" #include "dart/dynamics/BodyNode.hpp" #include "dart/dynamics/SimpleFrame.hpp" -#include "dart/dynamics/EllipsoidShape.hpp" +#include "dart/dynamics/SphereShape.hpp" #include "dart/math/Helpers.hpp" #include "dart/gui/osg/SupportPolygonVisual.hpp" @@ -153,8 +153,8 @@ void SupportPolygonVisual::setCentroidRadius(double radius) mCentroidRadius = radius; const dart::dynamics::ShapePtr& shape = mCentroid->getShape(); - std::static_pointer_cast(shape)->setSize( - mCentroidRadius/2.0*Eigen::Vector3d::Ones()); + std::static_pointer_cast(shape)->setRadius( + mCentroidRadius/4.0); shape->addDataVariance(dart::dynamics::Shape::DYNAMIC_PRIMITIVE); } @@ -192,8 +192,8 @@ void SupportPolygonVisual::setCenterOfMassRadius(double radius) mComRadius = radius; const dart::dynamics::ShapePtr& shape = mCom->getShape(); - std::static_pointer_cast(shape)->setSize( - mComRadius/2.0*Eigen::Vector3d::Ones()); + std::static_pointer_cast(shape)->setRadius( + mComRadius/4.0); shape->addDataVariance(dart::dynamics::Shape::DYNAMIC_PRIMITIVE); } @@ -370,8 +370,7 @@ void SupportPolygonVisual::initialize() mCentroidRadius = 0.12; mCentroid->setShape( - std::make_shared( - mCentroidRadius/2.0*Eigen::Vector3d::Ones())); + std::make_shared(mCentroidRadius/4.0)); mCentroid->getVisualAspect(true)->setColor( Eigen::Vector4d(color[0], color[1], color[2], color[3])); @@ -387,8 +386,7 @@ void SupportPolygonVisual::initialize() dart::dynamics::Frame::World(), "com"); mComRadius = mCentroidRadius; - mCom->setShape(std::make_shared( - mComRadius/2.0*Eigen::Vector3d::Ones())); + mCom->setShape(std::make_shared(mComRadius/4.0)); mCom->getShape()->addDataVariance(dart::dynamics::Shape::DYNAMIC_COLOR); mComNode = new ShapeFrameNode(mCom.get(), nullptr); diff --git a/dart/gui/osg/examples/osgOperationalSpaceControl.cpp b/dart/gui/osg/examples/osgOperationalSpaceControl.cpp index b60a77226d148..f28059dea5abb 100644 --- a/dart/gui/osg/examples/osgOperationalSpaceControl.cpp +++ b/dart/gui/osg/examples/osgOperationalSpaceControl.cpp @@ -71,7 +71,7 @@ class OperationalSpaceControlWorld : public dart::gui::osg::WorldNode Eigen::Isometry3d tf = mEndEffector->getWorldTransform(); tf.pretranslate(mOffset); mTarget = std::make_shared(Frame::World(), "target", tf); - ShapePtr ball(new EllipsoidShape(Eigen::Vector3d(0.05,0.05,0.05))); + ShapePtr ball(new SphereShape(0.025)); mTarget->setShape(ball); mTarget->getVisualAspect(true)->setColor(Eigen::Vector3d(0.9,0,0)); mWorld->addSimpleFrame(mTarget); diff --git a/dart/gui/osg/render/SphereShapeNode.cpp b/dart/gui/osg/render/SphereShapeNode.cpp new file mode 100644 index 0000000000000..596ab86abcfc6 --- /dev/null +++ b/dart/gui/osg/render/SphereShapeNode.cpp @@ -0,0 +1,220 @@ +/* + * Copyright (c) 2016, Graphics Lab, Georgia Tech Research Corporation + * Copyright (c) 2016, Humanoid Lab, Georgia Tech Research Corporation + * Copyright (c) 2016, Personal Robotics Lab, Carnegie Mellon University + * All rights reserved. + * + * 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 +#include +#include + +#include "dart/gui/osg/render/SphereShapeNode.hpp" +#include "dart/gui/osg/Utils.hpp" + +#include "dart/dynamics/SphereShape.hpp" +#include "dart/dynamics/SimpleFrame.hpp" + +namespace dart { +namespace gui { +namespace osg { +namespace render { + +class SphereShapeGeode : public ShapeNode, public ::osg::Geode +{ +public: + + SphereShapeGeode(dart::dynamics::SphereShape* shape, + ShapeFrameNode* parentShapeFrame, + SphereShapeNode* parentNode); + + void refresh(); + void extractData(); + +protected: + + virtual ~SphereShapeGeode(); + + SphereShapeNode* mParentNode; + dart::dynamics::SphereShape* mSphereShape; + SphereShapeDrawable* mDrawable; + +}; + +//============================================================================== +class SphereShapeDrawable : public ::osg::ShapeDrawable +{ +public: + + SphereShapeDrawable(dart::dynamics::SphereShape* shape, + dart::dynamics::VisualAspect* visualAspect, + SphereShapeGeode* parent); + + void refresh(bool firstTime); + +protected: + + virtual ~SphereShapeDrawable(); + + dart::dynamics::SphereShape* mSphereShape; + dart::dynamics::VisualAspect* mVisualAspect; + SphereShapeGeode* mParent; + +}; + +//============================================================================== +SphereShapeNode::SphereShapeNode( + std::shared_ptr shape, + ShapeFrameNode* parent) + : ShapeNode(shape, parent, this), + mSphereShape(shape), + mGeode(nullptr) +{ + extractData(true); + setNodeMask(mVisualAspect->isHidden()? 0x0 : ~0x0); +} + +//============================================================================== +void SphereShapeNode::refresh() +{ + mUtilized = true; + + setNodeMask(mVisualAspect->isHidden()? 0x0 : ~0x0); + + if(mShape->getDataVariance() == dart::dynamics::Shape::STATIC) + return; + + extractData(false); +} + +//============================================================================== +void SphereShapeNode::extractData(bool /*firstTime*/) +{ + if(nullptr == mGeode) + { + mGeode = new SphereShapeGeode(mSphereShape.get(), mParentShapeFrameNode, this); + addChild(mGeode); + return; + } + + mGeode->refresh(); +} + +//============================================================================== +SphereShapeNode::~SphereShapeNode() +{ + // Do nothing +} + +//============================================================================== +SphereShapeGeode::SphereShapeGeode(dart::dynamics::SphereShape* shape, + ShapeFrameNode* parentShapeFrame, + SphereShapeNode* parentNode) + : ShapeNode(parentNode->getShape(), parentShapeFrame, this), + mParentNode(parentNode), + mSphereShape(shape), + mDrawable(nullptr) +{ + getOrCreateStateSet()->setMode(GL_BLEND, ::osg::StateAttribute::ON); + extractData(); +} + +//============================================================================== +void SphereShapeGeode::refresh() +{ + mUtilized = true; + + extractData(); +} + +//============================================================================== +void SphereShapeGeode::extractData() +{ + if(nullptr == mDrawable) + { + mDrawable = new SphereShapeDrawable(mSphereShape, mVisualAspect, this); + addDrawable(mDrawable); + return; + } + + mDrawable->refresh(false); +} + +//============================================================================== +SphereShapeGeode::~SphereShapeGeode() +{ + // Do nothing +} + +//============================================================================== +SphereShapeDrawable::SphereShapeDrawable( + dart::dynamics::SphereShape* shape, + dart::dynamics::VisualAspect* visualAspect, + SphereShapeGeode* parent) + : mSphereShape(shape), + mVisualAspect(visualAspect), + mParent(parent) +{ + refresh(true); +} + +//============================================================================== +void SphereShapeDrawable::refresh(bool firstTime) +{ + if(mSphereShape->getDataVariance() == dart::dynamics::Shape::STATIC) + setDataVariance(::osg::Object::STATIC); + else + setDataVariance(::osg::Object::DYNAMIC); + + if(mSphereShape->checkDataVariance(dart::dynamics::Shape::DYNAMIC_PRIMITIVE) + || firstTime) + { + ::osg::ref_ptr<::osg::Sphere> osg_shape = nullptr; + osg_shape = new ::osg::Sphere(::osg::Vec3(0,0,0), mSphereShape->getRadius()); + + setShape(osg_shape); + dirtyDisplayList(); + } + + if(mSphereShape->checkDataVariance(dart::dynamics::Shape::DYNAMIC_COLOR) + || firstTime) + { + setColor(eigToOsgVec4(mVisualAspect->getRGBA())); + } +} + +//============================================================================== +SphereShapeDrawable::~SphereShapeDrawable() +{ + // Do nothing +} + +} // namespace render +} // namespace osg +} // namespace gui +} // namespace dart diff --git a/dart/gui/osg/render/SphereShapeNode.hpp b/dart/gui/osg/render/SphereShapeNode.hpp new file mode 100644 index 0000000000000..74c4f7e83a76d --- /dev/null +++ b/dart/gui/osg/render/SphereShapeNode.hpp @@ -0,0 +1,78 @@ +/* + * Copyright (c) 2016, Graphics Lab, Georgia Tech Research Corporation + * Copyright (c) 2016, Humanoid Lab, Georgia Tech Research Corporation + * Copyright (c) 2016, Personal Robotics Lab, Carnegie Mellon University + * All rights reserved. + * + * 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. + */ + +#ifndef DART_GUI_OSG_RENDER_SPHERESHAPENODE_HPP_ +#define DART_GUI_OSG_RENDER_SPHERESHAPENODE_HPP_ + +#include +#include + +#include "dart/gui/osg/render/ShapeNode.hpp" + +namespace dart { + +namespace dynamics { +class SphereShape; +} // namespace dynamics + +namespace gui { +namespace osg { +namespace render { + +class SphereShapeGeode; +class SphereShapeDrawable; + +class SphereShapeNode : public ShapeNode, public ::osg::MatrixTransform +{ +public: + + SphereShapeNode( + std::shared_ptr shape, + ShapeFrameNode* parent); + + void refresh(); + void extractData(bool firstTime); + +protected: + + virtual ~SphereShapeNode(); + + std::shared_ptr mSphereShape; + SphereShapeGeode* mGeode; + +}; + +} // namespace render +} // namespace osg +} // namespace gui +} // namespace dart + +#endif // DART_GUI_OSG_RENDER_SPHERESHAPENODE_HPP_ diff --git a/dart/math/Helpers.hpp b/dart/math/Helpers.hpp index 7dd11a30ebe63..94831aa28aa8e 100644 --- a/dart/math/Helpers.hpp +++ b/dart/math/Helpers.hpp @@ -340,12 +340,12 @@ inline Eigen::Vector3d Red() return Eigen::Vector3d(0.9, 0.1, 0.1); } -inline Eigen::Vector3d Fuschia() +inline Eigen::Vector3d Fuchsia() { return Eigen::Vector3d(1.0, 0.0, 0.5); } -inline Eigen::Vector4d Fuschia(double alpha) +inline Eigen::Vector4d Fuchsia(double alpha) { return Eigen::Vector4d(1.0, 0.0, 0.5, alpha); } diff --git a/dart/utils/CompositeResourceRetriever.cpp b/dart/utils/CompositeResourceRetriever.cpp index 9a855f0a7ef29..aca849313a3ca 100644 --- a/dart/utils/CompositeResourceRetriever.cpp +++ b/dart/utils/CompositeResourceRetriever.cpp @@ -51,8 +51,8 @@ bool CompositeResourceRetriever::addSchemaRetriever( { if(!_resourceRetriever) { - dterr << "[CompositeResourceRetriever::addSchemaRetriever] Recieved nullptr" - " ResourceRetriever; skipping this entry.\n"; + dterr << "[CompositeResourceRetriever::addSchemaRetriever] Receieved" + " nullptr ResourceRetriever; skipping this entry.\n"; return false; } diff --git a/dart/utils/SkelParser.cpp b/dart/utils/SkelParser.cpp index c508cdd146d98..3ca8a8d2b7911 100644 --- a/dart/utils/SkelParser.cpp +++ b/dart/utils/SkelParser.cpp @@ -50,6 +50,7 @@ #include "dart/dynamics/BodyNode.hpp" #include "dart/dynamics/SoftBodyNode.hpp" #include "dart/dynamics/ShapeNode.hpp" +#include "dart/dynamics/SphereShape.hpp" #include "dart/dynamics/BoxShape.hpp" #include "dart/dynamics/CylinderShape.hpp" #include "dart/dynamics/EllipsoidShape.hpp" @@ -1173,7 +1174,16 @@ SkelBodyNode readSoftBodyNode( // geometry tinyxml2::XMLElement* geometryEle = getElement(softShapeEle, "geometry"); - if (hasElement(geometryEle, "box")) + if (hasElement(geometryEle, "sphere")) + { + tinyxml2::XMLElement* sphereEle = getElement(geometryEle, "sphere"); + const auto radius = getValueDouble(sphereEle, "radius"); + const auto nSlices = getValueUInt(sphereEle, "num_slices"); + const auto nStacks = getValueUInt(sphereEle, "num_stacks"); + newSoftBodyNode = dynamics::SoftBodyNodeHelper::makeSphereProperties( + radius, nSlices, nStacks, totalMass); + } + else if (hasElement(geometryEle, "box")) { tinyxml2::XMLElement* boxEle = getElement(geometryEle, "box"); Eigen::Vector3d size = getValueVector3d(boxEle, "size"); @@ -1185,8 +1195,8 @@ SkelBodyNode readSoftBodyNode( { tinyxml2::XMLElement* ellipsoidEle = getElement(geometryEle, "ellipsoid"); Eigen::Vector3d size = getValueVector3d(ellipsoidEle, "size"); - double nSlices = getValueDouble(ellipsoidEle, "num_slices"); - double nStacks = getValueDouble(ellipsoidEle, "num_stacks"); + const auto nSlices = getValueUInt(ellipsoidEle, "num_slices"); + const auto nStacks = getValueUInt(ellipsoidEle, "num_stacks"); newSoftBodyNode = dynamics::SoftBodyNodeHelper::makeEllipsoidProperties( size, nSlices, @@ -1259,7 +1269,13 @@ dynamics::ShapePtr readShape( assert(hasElement(vizEle, "geometry")); tinyxml2::XMLElement* geometryEle = getElement(vizEle, "geometry"); - if (hasElement(geometryEle, "box")) + if (hasElement(geometryEle, "sphere")) + { + tinyxml2::XMLElement* sphereEle = getElement(geometryEle, "sphere"); + const auto radius = getValueDouble(sphereEle, "radius"); + newShape = dynamics::ShapePtr(new dynamics::SphereShape(radius)); + } + else if (hasElement(geometryEle, "box")) { tinyxml2::XMLElement* boxEle = getElement(geometryEle, "box"); Eigen::Vector3d size = getValueVector3d(boxEle, "size"); diff --git a/dart/utils/VskParser.cpp b/dart/utils/VskParser.cpp index cf6121fe1dee9..d91b56ceb501b 100644 --- a/dart/utils/VskParser.cpp +++ b/dart/utils/VskParser.cpp @@ -425,7 +425,7 @@ bool readSegment(const tinyxml2::XMLElement* segment, if (!res) { - dtwarn << "[ParserVsk::readSegment] Faild to parse joint type.\n"; + dtwarn << "[ParserVsk::readSegment] Failed to parse joint type.\n"; return false; } @@ -571,7 +571,7 @@ bool readJoint(const std::string& jointType, } else { - dtwarn << "[ParserVsk::readSegment] Faild to parse joint type.\n"; + dtwarn << "[ParserVsk::readSegment] Failed to parse joint type.\n"; return false; } } diff --git a/dart/utils/sdf/SdfParser.cpp b/dart/utils/sdf/SdfParser.cpp index 9e9fee41cc355..fcbbf1a402e70 100644 --- a/dart/utils/sdf/SdfParser.cpp +++ b/dart/utils/sdf/SdfParser.cpp @@ -45,9 +45,9 @@ #include "dart/common/Uri.hpp" #include "dart/dynamics/BodyNode.hpp" #include "dart/dynamics/SoftBodyNode.hpp" +#include "dart/dynamics/SphereShape.hpp" #include "dart/dynamics/BoxShape.hpp" #include "dart/dynamics/CylinderShape.hpp" -#include "dart/dynamics/EllipsoidShape.hpp" #include "dart/dynamics/MeshShape.hpp" #include "dart/dynamics/WeldJoint.hpp" #include "dart/dynamics/PrismaticJoint.hpp" @@ -805,7 +805,16 @@ dynamics::SoftBodyNode::UniqueProperties readSoftBodyProperties( // geometry tinyxml2::XMLElement* geometryEle = getElement(softShapeEle, "geometry"); - if (hasElement(geometryEle, "box")) + if (hasElement(geometryEle, "sphere")) + { + tinyxml2::XMLElement* sphereEle = getElement(geometryEle, "sphere"); + const auto radius = getValueDouble(sphereEle, "radius"); + const auto nSlices = getValueUInt(sphereEle, "num_slices"); + const auto nStacks = getValueUInt(sphereEle, "num_stacks"); + softProperties = dynamics::SoftBodyNodeHelper::makeSphereProperties( + radius, nSlices, nStacks, totalMass); + } + else if (hasElement(geometryEle, "box")) { tinyxml2::XMLElement* boxEle = getElement(geometryEle, "box"); Eigen::Vector3d size = getValueVector3d(boxEle, "size"); @@ -817,8 +826,8 @@ dynamics::SoftBodyNode::UniqueProperties readSoftBodyProperties( { tinyxml2::XMLElement* ellipsoidEle = getElement(geometryEle, "ellipsoid"); Eigen::Vector3d size = getValueVector3d(ellipsoidEle, "size"); - double nSlices = getValueDouble(ellipsoidEle, "num_slices"); - double nStacks = getValueDouble(ellipsoidEle, "num_stacks"); + const auto nSlices = getValueUInt(ellipsoidEle, "num_slices"); + const auto nStacks = getValueUInt(ellipsoidEle, "num_stacks"); softProperties = dynamics::SoftBodyNodeHelper::makeEllipsoidProperties( size, nSlices, nStacks, totalMass); } @@ -872,22 +881,21 @@ dynamics::ShapePtr readShape( assert(hasElement(_shapelement, "geometry")); tinyxml2::XMLElement* geometryElement = getElement(_shapelement, "geometry"); - if (hasElement(geometryElement, "box")) + if (hasElement(geometryElement, "sphere")) { - tinyxml2::XMLElement* boxElement = getElement(geometryElement, "box"); + tinyxml2::XMLElement* sphereElement = getElement(geometryElement, "sphere"); - Eigen::Vector3d size = getValueVector3d(boxElement, "size"); + const auto radius = getValueDouble(sphereElement, "radius"); - newShape = dynamics::ShapePtr(new dynamics::BoxShape(size)); + newShape = dynamics::ShapePtr(new dynamics::SphereShape(radius)); } - else if (hasElement(geometryElement, "sphere")) + else if (hasElement(geometryElement, "box")) { - tinyxml2::XMLElement* ellipsoidElement = getElement(geometryElement, "sphere"); + tinyxml2::XMLElement* boxElement = getElement(geometryElement, "box"); - double radius = getValueDouble(ellipsoidElement, "radius"); - Eigen::Vector3d size(radius * 2, radius * 2, radius * 2); + Eigen::Vector3d size = getValueVector3d(boxElement, "size"); - newShape = dynamics::ShapePtr(new dynamics::EllipsoidShape(size)); + newShape = dynamics::ShapePtr(new dynamics::BoxShape(size)); } else if (hasElement(geometryElement, "cylinder")) { diff --git a/dart/utils/urdf/DartLoader.cpp b/dart/utils/urdf/DartLoader.cpp index 968098875c7d1..369c1aff6e929 100644 --- a/dart/utils/urdf/DartLoader.cpp +++ b/dart/utils/urdf/DartLoader.cpp @@ -47,8 +47,8 @@ #include "dart/dynamics/FreeJoint.hpp" #include "dart/dynamics/PlanarJoint.hpp" #include "dart/dynamics/Shape.hpp" +#include "dart/dynamics/SphereShape.hpp" #include "dart/dynamics/BoxShape.hpp" -#include "dart/dynamics/EllipsoidShape.hpp" #include "dart/dynamics/CylinderShape.hpp" #include "dart/dynamics/MeshShape.hpp" #include "dart/simulation/World.hpp" @@ -529,8 +529,7 @@ dynamics::ShapePtr DartLoader::createShape( // Sphere if(urdf::Sphere* sphere = dynamic_cast(_vizOrCol->geometry.get())) { - shape = dynamics::ShapePtr(new dynamics::EllipsoidShape( - 2.0 * sphere->radius * Eigen::Vector3d::Ones())); + shape = dynamics::ShapePtr(new dynamics::SphereShape(sphere->radius)); } // Box else if(urdf::Box* box = dynamic_cast(_vizOrCol->geometry.get())) diff --git a/data/skel/bullet_collision.skel b/data/skel/bullet_collision.skel index d818c768cd611..35803a5f9c816 100644 --- a/data/skel/bullet_collision.skel +++ b/data/skel/bullet_collision.skel @@ -7,7 +7,7 @@ bullet - + 0 -0.375 0 0 0 0 diff --git a/data/skel/cube.skel b/data/skel/cube.skel index 7b7c00426800c..386bb9072e625 100644 --- a/data/skel/cube.skel +++ b/data/skel/cube.skel @@ -7,7 +7,7 @@ fcl_mesh - + 0 -0.375 0 0 0 0 diff --git a/data/skel/cubes.skel b/data/skel/cubes.skel index 337cf6c88bcd2..a7938a902075a 100644 --- a/data/skel/cubes.skel +++ b/data/skel/cubes.skel @@ -7,7 +7,7 @@ dart - + 0 -0.375 0 0 0 0 diff --git a/data/skel/fullbody1.skel b/data/skel/fullbody1.skel index 563d4fda561e3..afe09222d0864 100644 --- a/data/skel/fullbody1.skel +++ b/data/skel/fullbody1.skel @@ -7,7 +7,7 @@ fcl_mesh - + false 0 -0.92 0 0 0 0 diff --git a/data/skel/ground.skel b/data/skel/ground.skel index 9ceb0fd51fa67..5aa7215f306d4 100644 --- a/data/skel/ground.skel +++ b/data/skel/ground.skel @@ -1,7 +1,7 @@ - + 0 -0.975 0 0 0 0 diff --git a/data/skel/joint_limit.skel b/data/skel/joint_limit.skel index c5c5232964d36..4c8ed4ef20c80 100644 --- a/data/skel/joint_limit.skel +++ b/data/skel/joint_limit.skel @@ -7,7 +7,7 @@ dart - + 0 -0.375 0 0 0 0 diff --git a/data/skel/mesh_collision.skel b/data/skel/mesh_collision.skel index 711a0445cef2e..475d0c5b30e7f 100644 --- a/data/skel/mesh_collision.skel +++ b/data/skel/mesh_collision.skel @@ -7,7 +7,7 @@ fcl_mesh - + 0 -0.375 0 0 0 0 diff --git a/data/skel/shapes.skel b/data/skel/shapes.skel index f4b981f14cedb..c546ec93b59fa 100644 --- a/data/skel/shapes.skel +++ b/data/skel/shapes.skel @@ -7,7 +7,7 @@ fcl - + 0 -0.375 0 0 0 0 diff --git a/data/skel/softBodies.skel b/data/skel/softBodies.skel index dab1c5dd9c295..b0984d16e1b80 100644 --- a/data/skel/softBodies.skel +++ b/data/skel/softBodies.skel @@ -7,7 +7,7 @@ fcl_mesh - + 0 -0.375 0 0 0 0 diff --git a/data/skel/soft_cubes.skel b/data/skel/soft_cubes.skel index 868d49e258331..b5c661cd35988 100644 --- a/data/skel/soft_cubes.skel +++ b/data/skel/soft_cubes.skel @@ -7,7 +7,7 @@ fcl_mesh - + 0 -0.375 0 0 0 0 diff --git a/data/skel/soft_open_chain.skel b/data/skel/soft_open_chain.skel index 2b3f2e2613b37..8ae67038e00f7 100644 --- a/data/skel/soft_open_chain.skel +++ b/data/skel/soft_open_chain.skel @@ -7,7 +7,7 @@ fcl_mesh - + 0 -0.375 0 0 0 0 diff --git a/data/skel/sphere.skel b/data/skel/sphere.skel index 731850f01765f..ef46c3731b95c 100644 --- a/data/skel/sphere.skel +++ b/data/skel/sphere.skel @@ -7,7 +7,7 @@ dart - + 0 -0.375 0 0 0 0 diff --git a/data/skel/spheres.skel b/data/skel/spheres.skel index 89d177289778a..1b0660f323115 100644 --- a/data/skel/spheres.skel +++ b/data/skel/spheres.skel @@ -7,7 +7,7 @@ fcl_mesh - + 0 -0.5 0 0 0 0 diff --git a/data/skel/test/box_stacking.skel b/data/skel/test/box_stacking.skel index ee374f882c14b..615742dc23ac7 100644 --- a/data/skel/test/box_stacking.skel +++ b/data/skel/test/box_stacking.skel @@ -6,7 +6,7 @@ 0 -9.81 0 fcl_mesh - + 0 -0.5 0 0 0 0 diff --git a/data/skel/test/double_pendulum_with_base.skel b/data/skel/test/double_pendulum_with_base.skel index 408e86a5f7bed..3e21f8c700d35 100644 --- a/data/skel/test/double_pendulum_with_base.skel +++ b/data/skel/test/double_pendulum_with_base.skel @@ -7,7 +7,7 @@ fcl_mesh - + 0 0 -0.55 0 0 0 diff --git a/data/skel/test/drop.skel b/data/skel/test/drop.skel index 7a9211cc4e33c..32bcc3a2fab77 100644 --- a/data/skel/test/drop.skel +++ b/data/skel/test/drop.skel @@ -6,7 +6,7 @@ 0 -9.81 0 fcl_mesh - + 0 -0.5 0 0 0 0 diff --git a/data/skel/test/drop_unrotated_box.skel b/data/skel/test/drop_unrotated_box.skel index 6f80ffad7e515..a8776ffb91497 100644 --- a/data/skel/test/drop_unrotated_box.skel +++ b/data/skel/test/drop_unrotated_box.skel @@ -6,7 +6,7 @@ 0 -9.81 0 fcl_mesh - + 0 -0.5 0 0 0 0 diff --git a/data/skel/test/file_info_world_test.skel b/data/skel/test/file_info_world_test.skel index 563d4fda561e3..afe09222d0864 100644 --- a/data/skel/test/file_info_world_test.skel +++ b/data/skel/test/file_info_world_test.skel @@ -7,7 +7,7 @@ fcl_mesh - + false 0 -0.92 0 0 0 0 diff --git a/data/skel/test/spheres.skel b/data/skel/test/spheres.skel index 89d177289778a..1b0660f323115 100644 --- a/data/skel/test/spheres.skel +++ b/data/skel/test/spheres.skel @@ -7,7 +7,7 @@ fcl_mesh - + 0 -0.5 0 0 0 0 diff --git a/data/skel/test/test_adaptive_deformable.skel b/data/skel/test/test_adaptive_deformable.skel index d10a4114afe8e..a06485b1cb9ff 100644 --- a/data/skel/test/test_adaptive_deformable.skel +++ b/data/skel/test/test_adaptive_deformable.skel @@ -7,7 +7,7 @@ fcl_mesh - + 0 -0.375 0 0 0 0 diff --git a/data/skel/test/test_drop_box.skel b/data/skel/test/test_drop_box.skel index 434ce0d680d77..39289df2fcba1 100644 --- a/data/skel/test/test_drop_box.skel +++ b/data/skel/test/test_drop_box.skel @@ -7,7 +7,7 @@ fcl_mesh - + 0 -0.375 0 0 0 0 diff --git a/data/skel/test/test_drop_box_offset.skel b/data/skel/test/test_drop_box_offset.skel index c990d98a6499e..773e21f90378d 100644 --- a/data/skel/test/test_drop_box_offset.skel +++ b/data/skel/test/test_drop_box_offset.skel @@ -7,7 +7,7 @@ fcl_mesh - + 0 -0.375 0 0 0 0 diff --git a/data/skel/test/test_drop_low_stiffness.skel b/data/skel/test/test_drop_low_stiffness.skel index 2ed8ed6494d01..f00d12b37c771 100644 --- a/data/skel/test/test_drop_low_stiffness.skel +++ b/data/skel/test/test_drop_low_stiffness.skel @@ -7,7 +7,7 @@ fcl_mesh - + 0 -0.375 0 0 0 0 diff --git a/data/skel/test/test_drop_sphere.skel b/data/skel/test/test_drop_sphere.skel index aa843fa2672f0..2b81fc10e9dec 100644 --- a/data/skel/test/test_drop_sphere.skel +++ b/data/skel/test/test_drop_sphere.skel @@ -7,7 +7,7 @@ fcl_mesh - + 0 -0.375 0 0 0 0 diff --git a/data/skel/two_cubes.skel b/data/skel/two_cubes.skel index 23a6ac24cf01e..779a9745672f8 100644 --- a/data/skel/two_cubes.skel +++ b/data/skel/two_cubes.skel @@ -7,7 +7,7 @@ dart - + 0 -0.375 0 0 0 0 diff --git a/tutorials/tutorialCollisions-Finished.cpp b/tutorials/tutorialCollisions-Finished.cpp index 1defbef81a9a6..09c37dc6d2b2e 100644 --- a/tutorials/tutorialCollisions-Finished.cpp +++ b/tutorials/tutorialCollisions-Finished.cpp @@ -568,7 +568,7 @@ SkeletonPtr createSoftBody() inertia.setMoment(box->computeInertia(inertia.getMass())); bn->setInertia(inertia); - setAllColors(soft, dart::Color::Fuschia()); + setAllColors(soft, dart::Color::Fuchsia()); return soft; } diff --git a/tutorials/tutorialCollisions.cpp b/tutorials/tutorialCollisions.cpp index 6b763f43cc298..06dbbdb56b0e9 100644 --- a/tutorials/tutorialCollisions.cpp +++ b/tutorials/tutorialCollisions.cpp @@ -391,7 +391,7 @@ SkeletonPtr createSoftBody() // Add a rigid collision geometry and inertia // Lesson 2f - setAllColors(soft, dart::Color::Fuschia()); + setAllColors(soft, dart::Color::Fuchsia()); return soft; } diff --git a/unittests/testCollision.cpp b/unittests/testCollision.cpp index 48fa85e7ea301..eec9562f4bead 100644 --- a/unittests/testCollision.cpp +++ b/unittests/testCollision.cpp @@ -615,6 +615,93 @@ TEST_F(COLLISION, SimpleFrames) testSimpleFrames(dart); } +//============================================================================== +void testSphereSphere(const std::shared_ptr& cd, + double tol = 1e-12) +{ + auto simpleFrame1 = Eigen::make_aligned_shared(Frame::World()); + auto simpleFrame2 = Eigen::make_aligned_shared(Frame::World()); + + ShapePtr shape1(new SphereShape(1.0)); + ShapePtr shape2(new SphereShape(0.5)); + simpleFrame1->setShape(shape1); + simpleFrame2->setShape(shape2); + + auto group1 = cd->createCollisionGroup(simpleFrame1.get()); + auto group2 = cd->createCollisionGroup(simpleFrame2.get()); + + EXPECT_EQ(group1->getNumShapeFrames(), 1u); + EXPECT_EQ(group2->getNumShapeFrames(), 1u); + + collision::CollisionOption option; + option.enableContact = true; + + collision::CollisionResult result; + + simpleFrame1->setTranslation(Eigen::Vector3d::Zero()); + simpleFrame2->setTranslation(Eigen::Vector3d(2.0, 0.0, 0.0)); + result.clear(); + EXPECT_FALSE(group1->collide(group2.get(), option, &result)); + EXPECT_TRUE(result.getNumContacts() == 0u); + + simpleFrame1->setTranslation(Eigen::Vector3d::Zero()); + simpleFrame2->setTranslation(Eigen::Vector3d(1.5, 0.0, 0.0)); + result.clear(); + EXPECT_TRUE(group1->collide(group2.get(), option, &result)); + EXPECT_TRUE(result.getNumContacts() == 1u); + EXPECT_TRUE(result.getContact(0).point.isApprox( + Eigen::Vector3d(1.0, 0.0, 0.0), tol)); + + simpleFrame1->setTranslation(Eigen::Vector3d::Zero()); + simpleFrame2->setTranslation(Eigen::Vector3d::Zero()); + result.clear(); + if (cd->getType() == FCLCollisionDetector::getStaticType()) + { + EXPECT_FALSE(group1->collide(group2.get(), option, &result)); + // FCL is not able to detect collisions when an object completely (strictly) + // contanins the other object (no collisions between the hulls) + } + else + { + EXPECT_TRUE(group1->collide(group2.get(), option, &result)); + EXPECT_TRUE(result.getNumContacts() == 1u); + } + // The positions of contact point are different depending on the collision + // detector. More comprehensive tests need to be added. +} + +//============================================================================== +TEST_F(COLLISION, SphereSphere) +{ + auto fcl_mesh_dart = FCLCollisionDetector::create(); + fcl_mesh_dart->setPrimitiveShapeType(FCLCollisionDetector::MESH); + fcl_mesh_dart->setContactPointComputationMethod(FCLCollisionDetector::DART); + testSphereSphere(fcl_mesh_dart); + + // auto fcl_prim_fcl = FCLCollisionDetector::create(); + // fcl_prim_fcl->setPrimitiveShapeType(FCLCollisionDetector::MESH); + // fcl_prim_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL); + // testSphereSphere(fcl_prim_fcl); + + // auto fcl_mesh_fcl = FCLCollisionDetector::create(); + // fcl_mesh_fcl->setPrimitiveShapeType(FCLCollisionDetector::PRIMITIVE); + // fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::DART); + // testSphereSphere(fcl_mesh_fcl); + + // auto fcl_mesh_fcl = FCLCollisionDetector::create(); + // fcl_mesh_fcl->setPrimitiveShapeType(FCLCollisionDetector::PRIMITIVE); + // fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL); + // testSphereSphere(fcl_mesh_fcl); + +#if HAVE_BULLET_COLLISION + auto bullet = BulletCollisionDetector::create(); + testSphereSphere(bullet); +#endif + + auto dart = DARTCollisionDetector::create(); + testSphereSphere(dart); +} + //============================================================================== bool checkBoundingBox(const Eigen::Vector3d& min, const Eigen::Vector3d& max, const Eigen::Vector3d& point, double tol = 1e-12)