From c1cac77b36c5a566e4d1f7b78f84818f4a590799 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 20 Jul 2016 15:10:27 -0400 Subject: [PATCH 01/16] Add SphereShape class --- dart/dynamics/Shape.hpp | 1 + dart/dynamics/SphereShape.cpp | 115 ++++++++++++++++++++++++++++++++++ dart/dynamics/SphereShape.hpp | 82 ++++++++++++++++++++++++ 3 files changed, 198 insertions(+) create mode 100644 dart/dynamics/SphereShape.cpp create mode 100644 dart/dynamics/SphereShape.hpp 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/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_ From 224b56fe8a3c9f68cffeead3ad722a70d381b4bf Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 20 Jul 2016 15:28:53 -0400 Subject: [PATCH 02/16] Update collision detectors for SphereShape --- .../bullet/BulletCollisionDetector.cpp | 24 ++++---- dart/collision/dart/DARTCollide.cpp | 57 ++++++++++++++++++- dart/collision/dart/DARTCollisionDetector.cpp | 4 ++ dart/collision/fcl/FCLCollisionDetector.cpp | 27 +++++---- 4 files changed, 89 insertions(+), 23 deletions(-) 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 6d5173c060b05..906789db38e84 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" @@ -812,6 +813,7 @@ FCLCollisionDetector::createFCLCollisionGeometry( const FCLCollisionGeometryDeleter& deleter) { using dynamics::Shape; + using dynamics::SphereShape; using dynamics::BoxShape; using dynamics::EllipsoidShape; using dynamics::CylinderShape; @@ -822,7 +824,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())); @@ -843,18 +857,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 { From 47700e279537df846c5217ea1760b606bbbc2ffd Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 20 Jul 2016 15:44:47 -0400 Subject: [PATCH 03/16] Update parsers for SphereShape --- dart/dynamics/SoftBodyNode.cpp | 20 ++++++++++++++++++++ dart/dynamics/SoftBodyNode.hpp | 10 ++++++++++ dart/utils/SkelParser.cpp | 24 ++++++++++++++++++++---- dart/utils/sdf/SdfParser.cpp | 34 +++++++++++++++++++++------------- dart/utils/urdf/DartLoader.cpp | 5 ++--- 5 files changed, 73 insertions(+), 20 deletions(-) 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/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/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())) From e55e4d9d24a4982d329ce6171992b332ee655c91 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 20 Jul 2016 16:18:45 -0400 Subject: [PATCH 04/16] Update renderers for SphereShape --- dart/gui/OpenGLRenderInterface.cpp | 27 +- dart/gui/OpenGLRenderInterface.hpp | 1 + dart/gui/RenderInterface.cpp | 4 + dart/gui/RenderInterface.hpp | 1 + dart/gui/SimWindow.cpp | 19 +- dart/gui/osg/ShapeFrameNode.cpp | 13 +- dart/gui/osg/SupportPolygonVisual.cpp | 16 +- .../examples/osgOperationalSpaceControl.cpp | 2 +- dart/gui/osg/render/SphereShapeNode.cpp | 233 ++++++++++++++++++ dart/gui/osg/render/SphereShapeNode.hpp | 78 ++++++ 10 files changed, 365 insertions(+), 29 deletions(-) create mode 100644 dart/gui/osg/render/SphereShapeNode.cpp create mode 100644 dart/gui/osg/render/SphereShapeNode.hpp 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..26db2e40da469 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/8.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..49f0c6feaad9f --- /dev/null +++ b/dart/gui/osg/render/SphereShapeNode.cpp @@ -0,0 +1,233 @@ +/* + * 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( mShape->checkDataVariance(dart::dynamics::Shape::DYNAMIC_TRANSFORM) + || mShape->checkDataVariance(dart::dynamics::Shape::DYNAMIC_PRIMITIVE) + || firstTime ) + { + Eigen::Matrix4d S(Eigen::Matrix4d::Zero()); + const double radius = mSphereShape->getRadius(); + S(0,0) = radius; + S(1,1) = radius; + S(2,2) = radius; + S(3,3) = 1.0; + setMatrix(eigToOsgMatrix(S)); + } + + 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_ From d8c7f07e8c85f9baedf28f284a6c441f757950d6 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 20 Jul 2016 22:26:30 -0400 Subject: [PATCH 05/16] Fix incorrect number --- dart/gui/osg/SupportPolygonVisual.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dart/gui/osg/SupportPolygonVisual.cpp b/dart/gui/osg/SupportPolygonVisual.cpp index 26db2e40da469..76794683f86ba 100644 --- a/dart/gui/osg/SupportPolygonVisual.cpp +++ b/dart/gui/osg/SupportPolygonVisual.cpp @@ -193,7 +193,7 @@ void SupportPolygonVisual::setCenterOfMassRadius(double radius) mComRadius = radius; const dart::dynamics::ShapePtr& shape = mCom->getShape(); std::static_pointer_cast(shape)->setRadius( - mComRadius/8.0); + mComRadius/4.0); shape->addDataVariance(dart::dynamics::Shape::DYNAMIC_PRIMITIVE); } From 602f983ea69f2f9ec2c4e92ef62ea0095f6ece4c Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 21 Jul 2016 07:06:21 -0400 Subject: [PATCH 06/16] Update API to support fcl 0.5.0 --- dart/collision/fcl/FCLCollisionDetector.cpp | 20 ++++++++++++++++---- dart/collision/fcl/FCLCollisionDetector.hpp | 15 ++++++++++++++- dart/collision/fcl/FCLCollisionObject.cpp | 4 ++++ dart/collision/fcl/FCLCollisionObject.hpp | 5 +++++ 4 files changed, 39 insertions(+), 5 deletions(-) diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index 426e50f103ca9..0e3c60f303c40 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -431,8 +431,6 @@ fcl::BVHModel* createEllipsoid(float _sizeX, float _sizeY, float _sizeZ) return model; } -#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,4) - //============================================================================== template fcl::BVHModel* createCylinder(double _baseRadius, double _topRadius, @@ -536,8 +534,6 @@ fcl::BVHModel* createCylinder(double _baseRadius, double _topRadius, return model; } -#endif - //============================================================================== template fcl::BVHModel* createMesh(float _scaleX, float _scaleY, float _scaleZ, @@ -782,7 +778,11 @@ std::unique_ptr FCLCollisionDetector::createCollisionObject( } //============================================================================== +#if FCL_VERSION_AT_LEAST(0,5,0) +std::shared_ptr +#else boost::shared_ptr +#endif FCLCollisionDetector::claimFCLCollisionGeometry( const dynamics::ConstShapePtr& shape) { @@ -805,7 +805,11 @@ FCLCollisionDetector::claimFCLCollisionGeometry( } //============================================================================== +#if FCL_VERSION_AT_LEAST(0,5,0) +std::shared_ptr +#else boost::shared_ptr +#endif FCLCollisionDetector::createFCLCollisionGeometry( const dynamics::ConstShapePtr& shape, FCLCollisionDetector::PrimitiveShape type, @@ -943,9 +947,17 @@ FCLCollisionDetector::createFCLCollisionGeometry( } if (geom) +#if FCL_VERSION_AT_LEAST(0,5,0) + return std::shared_ptr(geom, deleter); +#else return boost::shared_ptr(geom, deleter); +#endif else +#if FCL_VERSION_AT_LEAST(0,5,0) + return std::shared_ptr(); +#else return boost::shared_ptr(); +#endif } //============================================================================== diff --git a/dart/collision/fcl/FCLCollisionDetector.hpp b/dart/collision/fcl/FCLCollisionDetector.hpp index b808619022801..4e7c55bee98d5 100644 --- a/dart/collision/fcl/FCLCollisionDetector.hpp +++ b/dart/collision/fcl/FCLCollisionDetector.hpp @@ -36,6 +36,7 @@ #include #include // This should be removed once we migrate to fcl 0.5 #include "dart/collision/CollisionDetector.hpp" +#include "dart/collision/fcl/FCLTypes.hpp" namespace dart { namespace collision { @@ -130,7 +131,11 @@ class FCLCollisionDetector : public CollisionDetector /// Return fcl::CollisionGeometry associated with give Shape. New /// fcl::CollisionGeome will be created if it hasn't created yet. +#if FCL_VERSION_AT_LEAST(0,5,0) + std::shared_ptr claimFCLCollisionGeometry( +#else boost::shared_ptr claimFCLCollisionGeometry( +#endif const dynamics::ConstShapePtr& shape); protected: @@ -162,7 +167,11 @@ class FCLCollisionDetector : public CollisionDetector /// Create fcl::CollisionGeometry with the custom deleter /// FCLCollisionGeometryDeleter +#if FCL_VERSION_AT_LEAST(0,5,0) + std::shared_ptr createFCLCollisionGeometry( +#else boost::shared_ptr createFCLCollisionGeometry( +#endif const dynamics::ConstShapePtr& shape, FCLCollisionDetector::PrimitiveShape type, const FCLCollisionGeometryDeleter& deleter); @@ -170,7 +179,11 @@ class FCLCollisionDetector : public CollisionDetector private: using ShapeMap = std::map>; +#if FCL_VERSION_AT_LEAST(0,5,0) + std::weak_ptr>; +#else + boost::weak_ptr>; +#endif // TODO(JS): FCL replaced all the use of boost in version 0.5. Once we migrate // to 0.5 or greater, this also should be changed to // std::weak_ptr diff --git a/dart/collision/fcl/FCLCollisionObject.cpp b/dart/collision/fcl/FCLCollisionObject.cpp index f72d62158cdfc..11e6dfce9c57d 100644 --- a/dart/collision/fcl/FCLCollisionObject.cpp +++ b/dart/collision/fcl/FCLCollisionObject.cpp @@ -63,7 +63,11 @@ const fcl::CollisionObject* FCLCollisionObject::getFCLCollisionObject() const FCLCollisionObject::FCLCollisionObject( CollisionDetector* collisionDetector, const dynamics::ShapeFrame* shapeFrame, +#if FCL_VERSION_AT_LEAST(0,5,0) + const std::shared_ptr& fclCollGeom) +#else const boost::shared_ptr& fclCollGeom) +#endif : CollisionObject(collisionDetector, shapeFrame), mFCLCollisionObjectUserData(new UserData(this)), mFCLCollisionObject(new fcl::CollisionObject(fclCollGeom)) diff --git a/dart/collision/fcl/FCLCollisionObject.hpp b/dart/collision/fcl/FCLCollisionObject.hpp index ff011cf75e074..f0567af83999a 100644 --- a/dart/collision/fcl/FCLCollisionObject.hpp +++ b/dart/collision/fcl/FCLCollisionObject.hpp @@ -34,6 +34,7 @@ #include #include "dart/collision/CollisionObject.hpp" +#include "dart/collision/fcl/FCLTypes.hpp" namespace dart { namespace collision { @@ -64,7 +65,11 @@ class FCLCollisionObject : public CollisionObject /// Constructor FCLCollisionObject(CollisionDetector* collisionDetector, const dynamics::ShapeFrame* shapeFrame, +#if FCL_VERSION_AT_LEAST(0,5,0) + const std::shared_ptr& fclCollGeom); +#else const boost::shared_ptr& fclCollGeom); +#endif // Documentation inherited void updateEngineData() override; From 05c59450992788c173f87f4015acefef2238024f Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 21 Jul 2016 07:09:11 -0400 Subject: [PATCH 07/16] Update changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index c520be5bbf0e4..24b86cd4909fc 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -4,6 +4,7 @@ * Collision detection + * Added support of FCL 0.5.0: [#749](https://github.com/dartsim/dart/pull/749) * Added warnings for unsupported shape pairs of DARTCollisionDetector: [#722](https://github.com/dartsim/dart/pull/722) * Misc improvements and bug fixes From 742a5ef80b26902fb513f039565281c15939e8fc Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 21 Jul 2016 07:23:19 -0400 Subject: [PATCH 08/16] Remove unnecessary SphereShapeNode --- dart/gui/osg/render/SphereShapeNode.cpp | 19 +++---------------- 1 file changed, 3 insertions(+), 16 deletions(-) diff --git a/dart/gui/osg/render/SphereShapeNode.cpp b/dart/gui/osg/render/SphereShapeNode.cpp index 49f0c6feaad9f..596ab86abcfc6 100644 --- a/dart/gui/osg/render/SphereShapeNode.cpp +++ b/dart/gui/osg/render/SphereShapeNode.cpp @@ -50,8 +50,8 @@ class SphereShapeGeode : public ShapeNode, public ::osg::Geode public: SphereShapeGeode(dart::dynamics::SphereShape* shape, - ShapeFrameNode* parentShapeFrame, - SphereShapeNode* parentNode); + ShapeFrameNode* parentShapeFrame, + SphereShapeNode* parentNode); void refresh(); void extractData(); @@ -113,21 +113,8 @@ void SphereShapeNode::refresh() } //============================================================================== -void SphereShapeNode::extractData(bool firstTime) +void SphereShapeNode::extractData(bool /*firstTime*/) { - if( mShape->checkDataVariance(dart::dynamics::Shape::DYNAMIC_TRANSFORM) - || mShape->checkDataVariance(dart::dynamics::Shape::DYNAMIC_PRIMITIVE) - || firstTime ) - { - Eigen::Matrix4d S(Eigen::Matrix4d::Zero()); - const double radius = mSphereShape->getRadius(); - S(0,0) = radius; - S(1,1) = radius; - S(2,2) = radius; - S(3,3) = 1.0; - setMatrix(eigToOsgMatrix(S)); - } - if(nullptr == mGeode) { mGeode = new SphereShapeGeode(mSphereShape.get(), mParentShapeFrameNode, this); From 2523d37fcd712ae76c17d5756bc8cdf125972d26 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 21 Jul 2016 13:44:03 -0400 Subject: [PATCH 09/16] Use type aliasing rather than boilerplate preprocessors --- dart/collision/fcl/FCLCollisionDetector.cpp | 24 ++++----------------- dart/collision/fcl/FCLCollisionDetector.hpp | 18 +++------------- dart/collision/fcl/FCLCollisionObject.cpp | 6 +----- dart/collision/fcl/FCLCollisionObject.hpp | 6 +----- dart/collision/fcl/FCLTypes.hpp | 8 +++++++ 5 files changed, 17 insertions(+), 45 deletions(-) diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index 0e3c60f303c40..cf0ee008f1a99 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -778,11 +778,7 @@ std::unique_ptr FCLCollisionDetector::createCollisionObject( } //============================================================================== -#if FCL_VERSION_AT_LEAST(0,5,0) -std::shared_ptr -#else -boost::shared_ptr -#endif +fcl_shared_ptr FCLCollisionDetector::claimFCLCollisionGeometry( const dynamics::ConstShapePtr& shape) { @@ -805,11 +801,7 @@ FCLCollisionDetector::claimFCLCollisionGeometry( } //============================================================================== -#if FCL_VERSION_AT_LEAST(0,5,0) -std::shared_ptr -#else -boost::shared_ptr -#endif +fcl_shared_ptr FCLCollisionDetector::createFCLCollisionGeometry( const dynamics::ConstShapePtr& shape, FCLCollisionDetector::PrimitiveShape type, @@ -947,17 +939,9 @@ FCLCollisionDetector::createFCLCollisionGeometry( } if (geom) -#if FCL_VERSION_AT_LEAST(0,5,0) - return std::shared_ptr(geom, deleter); -#else - return boost::shared_ptr(geom, deleter); -#endif + return fcl_shared_ptr(geom, deleter); else -#if FCL_VERSION_AT_LEAST(0,5,0) - return std::shared_ptr(); -#else - return boost::shared_ptr(); -#endif + return fcl_shared_ptr(); } //============================================================================== diff --git a/dart/collision/fcl/FCLCollisionDetector.hpp b/dart/collision/fcl/FCLCollisionDetector.hpp index 4e7c55bee98d5..d37c0e58ccb8f 100644 --- a/dart/collision/fcl/FCLCollisionDetector.hpp +++ b/dart/collision/fcl/FCLCollisionDetector.hpp @@ -131,11 +131,7 @@ class FCLCollisionDetector : public CollisionDetector /// Return fcl::CollisionGeometry associated with give Shape. New /// fcl::CollisionGeome will be created if it hasn't created yet. -#if FCL_VERSION_AT_LEAST(0,5,0) - std::shared_ptr claimFCLCollisionGeometry( -#else - boost::shared_ptr claimFCLCollisionGeometry( -#endif + fcl_shared_ptr claimFCLCollisionGeometry( const dynamics::ConstShapePtr& shape); protected: @@ -167,11 +163,7 @@ class FCLCollisionDetector : public CollisionDetector /// Create fcl::CollisionGeometry with the custom deleter /// FCLCollisionGeometryDeleter -#if FCL_VERSION_AT_LEAST(0,5,0) - std::shared_ptr createFCLCollisionGeometry( -#else - boost::shared_ptr createFCLCollisionGeometry( -#endif + fcl_shared_ptr createFCLCollisionGeometry( const dynamics::ConstShapePtr& shape, FCLCollisionDetector::PrimitiveShape type, const FCLCollisionGeometryDeleter& deleter); @@ -179,11 +171,7 @@ class FCLCollisionDetector : public CollisionDetector private: using ShapeMap = std::map>; -#else - boost::weak_ptr>; -#endif + fcl_weak_ptr>; // TODO(JS): FCL replaced all the use of boost in version 0.5. Once we migrate // to 0.5 or greater, this also should be changed to // std::weak_ptr diff --git a/dart/collision/fcl/FCLCollisionObject.cpp b/dart/collision/fcl/FCLCollisionObject.cpp index 11e6dfce9c57d..ec1cf15f56002 100644 --- a/dart/collision/fcl/FCLCollisionObject.cpp +++ b/dart/collision/fcl/FCLCollisionObject.cpp @@ -63,11 +63,7 @@ const fcl::CollisionObject* FCLCollisionObject::getFCLCollisionObject() const FCLCollisionObject::FCLCollisionObject( CollisionDetector* collisionDetector, const dynamics::ShapeFrame* shapeFrame, -#if FCL_VERSION_AT_LEAST(0,5,0) - const std::shared_ptr& fclCollGeom) -#else - const boost::shared_ptr& fclCollGeom) -#endif + const fcl_shared_ptr& fclCollGeom) : CollisionObject(collisionDetector, shapeFrame), mFCLCollisionObjectUserData(new UserData(this)), mFCLCollisionObject(new fcl::CollisionObject(fclCollGeom)) diff --git a/dart/collision/fcl/FCLCollisionObject.hpp b/dart/collision/fcl/FCLCollisionObject.hpp index f0567af83999a..544c36eea1fbb 100644 --- a/dart/collision/fcl/FCLCollisionObject.hpp +++ b/dart/collision/fcl/FCLCollisionObject.hpp @@ -65,11 +65,7 @@ class FCLCollisionObject : public CollisionObject /// Constructor FCLCollisionObject(CollisionDetector* collisionDetector, const dynamics::ShapeFrame* shapeFrame, -#if FCL_VERSION_AT_LEAST(0,5,0) - const std::shared_ptr& fclCollGeom); -#else - const boost::shared_ptr& fclCollGeom); -#endif + const fcl_shared_ptr& fclCollGeom); // Documentation inherited void updateEngineData() override; diff --git a/dart/collision/fcl/FCLTypes.hpp b/dart/collision/fcl/FCLTypes.hpp index 8fa6e88108a81..8fbf15c2c130f 100644 --- a/dart/collision/fcl/FCLTypes.hpp +++ b/dart/collision/fcl/FCLTypes.hpp @@ -46,6 +46,14 @@ (FCL_MAJOR_VERSION < x || (FCL_MAJOR_VERSION <= x && \ (FCL_MINOR_VERSION < y || (FCL_MINOR_VERSION <= y)))) +#if FCL_VERSION_AT_LEAST(0,5,0) +template using fcl_shared_ptr = std::shared_ptr; +template using fcl_weak_ptr = std::weak_ptr; +#else +template using fcl_shared_ptr = boost::shared_ptr; +template using fcl_weak_ptr = boost::weak_ptr; +#endif + namespace dart { namespace collision { From eaa10ae2b4e0d29de003921d868f0c62e12ed0dc Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 21 Jul 2016 13:56:04 -0400 Subject: [PATCH 10/16] Switch to use XML_SUCCESS rather than XML_NO_ERROR --- dart/utils/XmlHelpers.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/dart/utils/XmlHelpers.cpp b/dart/utils/XmlHelpers.cpp index adaf7de17647f..bb0fcc7ca329e 100644 --- a/dart/utils/XmlHelpers.cpp +++ b/dart/utils/XmlHelpers.cpp @@ -726,7 +726,7 @@ bool getAttributeBool(const tinyxml2::XMLElement* element, const int result = element->QueryBoolAttribute(attributeName.c_str(), &val); - if (result != tinyxml2::XML_NO_ERROR) + if (result != tinyxml2::XML_SUCCESS) { dtwarn << "[getAttribute] Error in parsing bool type attribute [" << attributeName << "] of an element [" << element->Name() @@ -744,7 +744,7 @@ int getAttributeInt(const tinyxml2::XMLElement* element, int val = 0; const int result = element->QueryIntAttribute(attributeName.c_str(), &val); - if (result != tinyxml2::XML_NO_ERROR) + if (result != tinyxml2::XML_SUCCESS) { dtwarn << "[getAttribute] Error in parsing int type attribute [" << attributeName << "] of an element [" << element->Name() @@ -763,7 +763,7 @@ unsigned int getAttributeUInt(const tinyxml2::XMLElement* element, const int result = element->QueryUnsignedAttribute(attributeName.c_str(), &val); - if (result != tinyxml2::XML_NO_ERROR) + if (result != tinyxml2::XML_SUCCESS) { dtwarn << "[getAttribute] Error in parsing unsiged int type attribute [" << attributeName << "] of an element [" << element->Name() @@ -782,7 +782,7 @@ float getAttributeFloat(const tinyxml2::XMLElement* element, const int result = element->QueryFloatAttribute(attributeName.c_str(), &val); - if (result != tinyxml2::XML_NO_ERROR) + if (result != tinyxml2::XML_SUCCESS) { dtwarn << "[getAttribute] Error in parsing float type attribute [" << attributeName << "] of an element [" << element->Name() @@ -801,7 +801,7 @@ double getAttributeDouble(const tinyxml2::XMLElement* element, const int result = element->QueryDoubleAttribute(attributeName.c_str(), &val); - if (result != tinyxml2::XML_NO_ERROR) + if (result != tinyxml2::XML_SUCCESS) { dtwarn << "[getAttribute] Error in parsing double type attribute [" << attributeName << "] of an element [" << element->Name() From 64ac82cb66061066359c00c32c2d10604fa7584f Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 21 Jul 2016 23:35:05 -0400 Subject: [PATCH 11/16] Add simple test for sphere-sphere --- unittests/testCollision.cpp | 87 +++++++++++++++++++++++++++++++++++++ 1 file changed, 87 insertions(+) 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) From 7b886b7d04186e0131bd464fa49a42d9a529401a Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 26 Jul 2016 07:19:15 -0400 Subject: [PATCH 12/16] Fix typo: grount --> ground (#755) * Fix typo: grount --> ground * Fix typo: grount --> ground * Update changelog --- CHANGELOG.md | 1 + data/skel/bullet_collision.skel | 2 +- data/skel/cube.skel | 2 +- data/skel/cubes.skel | 2 +- data/skel/fullbody1.skel | 2 +- data/skel/ground.skel | 2 +- data/skel/joint_limit.skel | 2 +- data/skel/mesh_collision.skel | 2 +- data/skel/shapes.skel | 2 +- data/skel/softBodies.skel | 2 +- data/skel/soft_cubes.skel | 2 +- data/skel/soft_open_chain.skel | 2 +- data/skel/sphere.skel | 2 +- data/skel/spheres.skel | 2 +- data/skel/test/box_stacking.skel | 2 +- data/skel/test/double_pendulum_with_base.skel | 2 +- data/skel/test/drop.skel | 2 +- data/skel/test/drop_unrotated_box.skel | 2 +- data/skel/test/file_info_world_test.skel | 2 +- data/skel/test/spheres.skel | 2 +- data/skel/test/test_adaptive_deformable.skel | 2 +- data/skel/test/test_drop_box.skel | 2 +- data/skel/test/test_drop_box_offset.skel | 2 +- data/skel/test/test_drop_low_stiffness.skel | 2 +- data/skel/test/test_drop_sphere.skel | 2 +- data/skel/two_cubes.skel | 2 +- 26 files changed, 26 insertions(+), 25 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 5b1016f714448..f1d5c9ed368c1 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,6 +9,7 @@ * Misc improvements and bug fixes * Added `virtual Shape::getType()` and deprecated `ShapeType Shpae::getShapeType()`: [#724](https://github.com/dartsim/dart/pull/724) + * Fixed typo: grount --> ground: [#755](https://github.com/dartsim/dart/pull/755) ### DART 6.0.1 (2016-XX-XX) 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 From cca9ff0015893bb5119b6431cdc18bf5367791cd Mon Sep 17 00:00:00 2001 From: Jose Luis Rivero Date: Thu, 4 Aug 2016 01:21:51 +0200 Subject: [PATCH 13/16] Fix spelling errors detected by Debian QA checker --- dart/gui/GlutWindow.cpp | 2 +- dart/utils/CompositeResourceRetriever.cpp | 4 ++-- dart/utils/VskParser.cpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) 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/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/VskParser.cpp b/dart/utils/VskParser.cpp index cf6121fe1dee9..2e7ec92694b0c 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; } From 5e7d1f4016a8791b30f375d37019e7fe5c2c3ca5 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 3 Aug 2016 21:22:49 -0400 Subject: [PATCH 14/16] Fix typos --- dart/math/Helpers.hpp | 4 ++-- dart/utils/VskParser.cpp | 2 +- tutorials/tutorialCollisions-Finished.cpp | 2 +- tutorials/tutorialCollisions.cpp | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) 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/VskParser.cpp b/dart/utils/VskParser.cpp index cf6121fe1dee9..d08f51f373b92 100644 --- a/dart/utils/VskParser.cpp +++ b/dart/utils/VskParser.cpp @@ -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/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; } From 5cf6bfbc15f599a19013d4899d31a7f6b665ce6d Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 3 Aug 2016 21:30:01 -0400 Subject: [PATCH 15/16] Update changelog --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index f1d5c9ed368c1..113aea5e42b1a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,7 +9,7 @@ * Misc improvements and bug fixes * Added `virtual Shape::getType()` and deprecated `ShapeType Shpae::getShapeType()`: [#724](https://github.com/dartsim/dart/pull/724) - * Fixed typo: grount --> ground: [#755](https://github.com/dartsim/dart/pull/755) + * 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) From 1e35c39862d584b667352cc8f0e62ef500b05177 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sat, 6 Aug 2016 06:54:11 -0400 Subject: [PATCH 16/16] Update changelog --- CHANGELOG.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 945e1b8606cc9..e017a1058f250 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,6 +2,10 @@ ### DART 6.1.0 (2016-XX-XX) +* Dynamics + + * 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)