From 1c6642665e8fb6f66546e70c3722d7ba84674378 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 2 Apr 2014 17:51:36 -0400 Subject: [PATCH] Testing ContactConstraint for frictionless contacts --- Migration.md | 6 + apps/CMakeLists.txt | 1 + apps/cube/CMakeLists.txt | 7 + apps/cube/Main.cpp | 71 +++ apps/cube/MyWindow.cpp | 161 ++++++ apps/cube/MyWindow.h | 75 +++ dart/collision/CollisionDetector.cpp | 7 +- dart/constraint/BallJointConstraint.cpp | 55 ++ dart/constraint/BallJointConstraint.h | 58 ++ dart/constraint/ClosedLoopConstraint.cpp | 55 ++ dart/constraint/ClosedLoopConstraint.h | 58 ++ dart/constraint/ConstrainedGroup.cpp | 268 +++++++++ dart/constraint/ConstrainedGroup.h | 135 +++++ dart/constraint/Constraint.cpp | 153 ++++++ dart/constraint/Constraint.h | 152 +++++ dart/constraint/ConstraintSolver.cpp | 515 +++++++++++++++++ dart/constraint/ConstraintSolver.h | 209 +++++++ dart/constraint/ContactConstraint.cpp | 519 ++++++++++++++++++ dart/constraint/ContactConstraint.h | 158 ++++++ dart/constraint/JointConstraint.cpp | 62 +++ dart/constraint/JointConstraint.h | 65 +++ dart/constraint/JointLimitConstraint.cpp | 65 +++ dart/constraint/JointLimitConstraint.h | 76 +++ dart/constraint/RevoluteJointConstraint.cpp | 55 ++ dart/constraint/RevoluteJointConstraint.h | 58 ++ dart/constraint/WeldJointConstraint.cpp | 55 ++ dart/constraint/WeldJointConstraint.h | 58 ++ dart/dynamics/BodyNode.cpp | 10 +- dart/dynamics/BodyNode.h | 5 +- dart/dynamics/EulerJoint.h | 2 + dart/dynamics/Joint.h | 3 + dart/dynamics/Skeleton.cpp | 47 +- dart/dynamics/Skeleton.h | 10 +- dart/gui/SimWindow.cpp | 5 +- dart/integration/Integrator.h | 8 +- .../SemiImplicitEulerIntegrator.cpp | 14 + .../integration/SemiImplicitEulerIntegrator.h | 6 + dart/simulation/World.cpp | 126 ++++- dart/simulation/World.h | 11 +- dart/utils/SkelParser.cpp | 9 +- dart/utils/SoftParser.cpp | 9 +- data/skel/cube.skel | 71 +++ 42 files changed, 3446 insertions(+), 47 deletions(-) create mode 100644 apps/cube/CMakeLists.txt create mode 100644 apps/cube/Main.cpp create mode 100644 apps/cube/MyWindow.cpp create mode 100644 apps/cube/MyWindow.h create mode 100644 dart/constraint/BallJointConstraint.cpp create mode 100644 dart/constraint/BallJointConstraint.h create mode 100644 dart/constraint/ClosedLoopConstraint.cpp create mode 100644 dart/constraint/ClosedLoopConstraint.h create mode 100644 dart/constraint/ConstrainedGroup.cpp create mode 100644 dart/constraint/ConstrainedGroup.h create mode 100644 dart/constraint/Constraint.cpp create mode 100644 dart/constraint/Constraint.h create mode 100644 dart/constraint/ConstraintSolver.cpp create mode 100644 dart/constraint/ConstraintSolver.h create mode 100644 dart/constraint/ContactConstraint.cpp create mode 100644 dart/constraint/ContactConstraint.h create mode 100644 dart/constraint/JointConstraint.cpp create mode 100644 dart/constraint/JointConstraint.h create mode 100644 dart/constraint/JointLimitConstraint.cpp create mode 100644 dart/constraint/JointLimitConstraint.h create mode 100644 dart/constraint/RevoluteJointConstraint.cpp create mode 100644 dart/constraint/RevoluteJointConstraint.h create mode 100644 dart/constraint/WeldJointConstraint.cpp create mode 100644 dart/constraint/WeldJointConstraint.h create mode 100644 data/skel/cube.skel diff --git a/Migration.md b/Migration.md index c117972e62969..4a003313ac8e8 100644 --- a/Migration.md +++ b/Migration.md @@ -93,6 +93,12 @@ + virtual void integrateConfigs(const Eigen::VectorXd& _genVels, double _dt) = 0 + virtual void integrateGenVels(const Eigen::VectorXd& _genAccs, double _dt) = 0 +1. **dart/utils/SkelParser.h** + + static dynamics::Skeleton* readSkeleton(const std::string& _filename) + +1. **dart/utils/SoftSkelParser.h** + + static dynamics::SoftSkeleton* readSoftSkeleton(const std::string& _filename) + ### Deletions 1. **dart/dynamics/BodyNode.h** diff --git a/apps/CMakeLists.txt b/apps/CMakeLists.txt index 7bea86f250c17..08f171ae44f9b 100644 --- a/apps/CMakeLists.txt +++ b/apps/CMakeLists.txt @@ -7,6 +7,7 @@ foreach(APPDIR balance ballJointConstraintTest closedLoop + cube cubes doublePendulumWithBase forwardSim diff --git a/apps/cube/CMakeLists.txt b/apps/cube/CMakeLists.txt new file mode 100644 index 0000000000000..e2b77f7c685e9 --- /dev/null +++ b/apps/cube/CMakeLists.txt @@ -0,0 +1,7 @@ +############################################### +# apps/cube +file(GLOB cube_srcs "*.cpp") +file(GLOB cube_hdrs "*.h") +add_executable(cube ${cube_srcs} ${cube_hdrs}) +target_link_libraries(cube dart) +set_target_properties(cube PROPERTIES RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/bin") diff --git a/apps/cube/Main.cpp b/apps/cube/Main.cpp new file mode 100644 index 0000000000000..a1bd3486a51ad --- /dev/null +++ b/apps/cube/Main.cpp @@ -0,0 +1,71 @@ +/* + * Copyright (c) 2011-2013, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Karen Liu , + * Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include + +#include "dart/simulation/World.h" +#include "dart/utils/Paths.h" +#include "dart/utils/SkelParser.h" +#include "dart/constraint/ConstraintSolver.h" +#include "apps/cubes/MyWindow.h" + +int main(int argc, char* argv[]) { + // create and initialize the world + dart::simulation::World *myWorld + = dart::utils::SkelParser::readSkelFile(DART_DATA_PATH"/skel/cube.skel"); + assert(myWorld != NULL); + Eigen::Vector3d gravity(0.0, -9.81, 0.0); + myWorld->setGravity(gravity); + + // create a window and link it to the world + MyWindow window; + window.setWorld(myWorld); + + std::cout << "space bar: simulation on/off" << std::endl; + std::cout << "'p': playback/stop" << std::endl; + std::cout << "'[' and ']': play one frame backward and forward" << std::endl; + std::cout << "'v': visualization on/off" << std::endl; + std::cout << "'1'--'4': programmed interaction" << std::endl; + std::cout << "'q': spawn a random cube" << std::endl; + std::cout << "'w': delete a spawned cube" << std::endl; + + glutInit(&argc, argv); + window.initWindow(640, 480, "Boxes"); + glutMainLoop(); + + return 0; +} diff --git a/apps/cube/MyWindow.cpp b/apps/cube/MyWindow.cpp new file mode 100644 index 0000000000000..94c42444d1771 --- /dev/null +++ b/apps/cube/MyWindow.cpp @@ -0,0 +1,161 @@ +/* + * Copyright (c) 2011-2013, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Karen Liu , + * Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "apps/cubes/MyWindow.h" + +#include "dart/math/Helpers.h" +#include "dart/simulation/World.h" +#include "dart/dynamics/BodyNode.h" +#include "dart/dynamics/Skeleton.h" +#include "dart/dynamics/FreeJoint.h" +#include "dart/dynamics/BoxShape.h" + +MyWindow::MyWindow() + : SimWindow() { + mForce = Eigen::Vector3d::Zero(); +} + +MyWindow::~MyWindow() { +} + +void MyWindow::timeStepping() { + mWorld->getSkeleton(1)->getBodyNode(0)->addExtForce(mForce); + mWorld->step(); + mForce /= 2.0; +} + +void MyWindow::drawSkels() { + glEnable(GL_LIGHTING); + glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); + + SimWindow::drawSkels(); +} + +void MyWindow::keyboard(unsigned char _key, int _x, int _y) { + switch (_key) { + case ' ': // use space key to play or stop the motion + mSimulating = !mSimulating; + if (mSimulating) { + mPlay = false; + glutTimerFunc(mDisplayTimeout, refreshTimer, 0); + } + break; + case 'p': // playBack + mPlay = !mPlay; + if (mPlay) { + mSimulating = false; + glutTimerFunc(mDisplayTimeout, refreshTimer, 0); + } + break; + case '[': // step backward + if (!mSimulating) { + mPlayFrame--; + if (mPlayFrame < 0) + mPlayFrame = 0; + glutPostRedisplay(); + } + break; + case ']': // step forwardward + if (!mSimulating) { + mPlayFrame++; + if (mPlayFrame >= mBakedStates.size()) + mPlayFrame = 0; + glutPostRedisplay(); + } + break; + case 'v': // show or hide markers + mShowMarkers = !mShowMarkers; + break; + case '1': // upper right force + mForce[0] = -500; + break; + case '2': // upper right force + mForce[0] = 500; + break; + case '3': // upper right force + mForce[2] = -500; + break; + case '4': // upper right force + mForce[2] = 500; + break; + case 'q': // Spawn a cube + case 'Q': { // Spawn a cube + Eigen::Vector3d position = Eigen::Vector3d(dart::math::random(-1.0, 1.0), + dart::math::random(-1.0, 1.0), + dart::math::random(0.5, 1.0)); + Eigen::Vector3d size = Eigen::Vector3d(dart::math::random(0.01, 0.2), + dart::math::random(0.01, 0.2), + dart::math::random(0.01, 0.2)); + spawnCube(position, size); + break; + } + case 'w': // Spawn a cube + case 'W': { // Spawn a cube + if (mWorld->getNumSkeletons() > 4) + mWorld->removeSkeleton(mWorld->getSkeleton(4)); + break; + } + default: + Win3D::keyboard(_key, _x, _y); + } + glutPostRedisplay(); +} + +void MyWindow::spawnCube(const Eigen::Vector3d& _position, + const Eigen::Vector3d& _size, + double _mass) { + dart::dynamics::Skeleton* newCubeSkeleton = + new dart::dynamics::Skeleton(); + dart::dynamics::BodyNode* newBodyNode = + new dart::dynamics::BodyNode("cube_link"); + dart::dynamics::FreeJoint* newFreeJoint = + new dart::dynamics::FreeJoint("cube_joint"); + dart::dynamics::BoxShape* newBoxShape = + new dart::dynamics::BoxShape(_size); + + newBodyNode->addVisualizationShape(newBoxShape); + newBodyNode->addCollisionShape(newBoxShape); + newBodyNode->setMass(_mass); + newBodyNode->setParentJoint(newFreeJoint); + newFreeJoint->setTransformFromParentBodyNode( + Eigen::Isometry3d(Eigen::Translation3d(_position))); + newBoxShape->setColor(Eigen::Vector3d(dart::math::random(0.0, 1.0), + dart::math::random(0.0, 1.0), + dart::math::random(0.0, 1.0))); + newCubeSkeleton->addBodyNode(newBodyNode); + mWorld->addSkeleton(newCubeSkeleton); +} diff --git a/apps/cube/MyWindow.h b/apps/cube/MyWindow.h new file mode 100644 index 0000000000000..15789716a2679 --- /dev/null +++ b/apps/cube/MyWindow.h @@ -0,0 +1,75 @@ +/* + * Copyright (c) 2011-2013, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Karen Liu , + * Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef APPS_CUBES_MYWINDOW_H_ +#define APPS_CUBES_MYWINDOW_H_ + +#include "dart/gui/SimWindow.h" + +/// \brief +class MyWindow : public dart::gui::SimWindow { +public: + /// \brief + MyWindow(); + + /// \brief + virtual ~MyWindow(); + + /// \brief + virtual void timeStepping(); + + /// \brief + virtual void drawSkels(); + + /// \brief + virtual void keyboard(unsigned char _key, int _x, int _y); + + /// \brief + void spawnCube( + const Eigen::Vector3d& _position = Eigen::Vector3d(0.0, 1.0, 0.0), + const Eigen::Vector3d& _size = Eigen::Vector3d(0.1, 0.1, 0.1), + double _mass = 1.0); + +private: + /// \brief + Eigen::Vector3d mForce; + + /// \brief Number of frames for applying external force + int mImpulseDuration; +}; + +#endif // APPS_CUBES_MYWINDOW_H_ diff --git a/dart/collision/CollisionDetector.cpp b/dart/collision/CollisionDetector.cpp index b3d93afd5f5f0..df0b01092fe0d 100644 --- a/dart/collision/CollisionDetector.cpp +++ b/dart/collision/CollisionDetector.cpp @@ -100,9 +100,10 @@ void CollisionDetector::removeSkeleton(dynamics::Skeleton* _skeleton) //============================================================================== void CollisionDetector::removeAllSkeletons() { - std::cout << "CollisionDetector::removeAllSkeletons(): " - << "Not implemented yet." - << std::endl; + for (size_t i = 0; i < mSkeletons.size(); ++i) + removeSkeleton(mSkeletons[i]); + + mSkeletons.clear(); } void CollisionDetector::addCollisionSkeletonNode(dynamics::BodyNode* _bodyNode, diff --git a/dart/constraint/BallJointConstraint.cpp b/dart/constraint/BallJointConstraint.cpp new file mode 100644 index 0000000000000..94f1a174ce06b --- /dev/null +++ b/dart/constraint/BallJointConstraint.cpp @@ -0,0 +1,55 @@ +/* + * Copyright (c) 2014, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Karen Liu , + * Jeongseok Lee + * + * Geoorgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/constraint/BallJointConstraint.h" + +namespace dart { +namespace constraint { + +BallJointConstraint::BallJointConstraint() + : JointConstraint() +{ + +} + +BallJointConstraint::~BallJointConstraint() +{ + +} + +} // namespace constraint +} // namespace dart diff --git a/dart/constraint/BallJointConstraint.h b/dart/constraint/BallJointConstraint.h new file mode 100644 index 0000000000000..a889f1b9b7af7 --- /dev/null +++ b/dart/constraint/BallJointConstraint.h @@ -0,0 +1,58 @@ +/* + * Copyright (c) 2014, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Karen Liu , + * Jeongseok Lee + * + * Geoorgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_CONSTRAINT_BALLJOINTCONSTRAINT_H_ +#define DART_CONSTRAINT_BALLJOINTCONSTRAINT_H_ + +#include "dart/constraint/JointConstraint.h" + +namespace dart { +namespace constraint { + +class BallJointConstraint : public JointConstraint +{ +public: + BallJointConstraint(); + + ~BallJointConstraint(); +}; + +} // namespace constraint +} // namespace dart + +#endif // DART_CONSTRAINT_BALLJOINTCONSTRAINT_H_ + diff --git a/dart/constraint/ClosedLoopConstraint.cpp b/dart/constraint/ClosedLoopConstraint.cpp new file mode 100644 index 0000000000000..99fd08d8d2d3e --- /dev/null +++ b/dart/constraint/ClosedLoopConstraint.cpp @@ -0,0 +1,55 @@ +/* + * Copyright (c) 2014, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Karen Liu , + * Jeongseok Lee + * + * Geoorgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/constraint/ClosedLoopConstraint.h" + +namespace dart { +namespace constraint { + +ClosedLoopConstraint::ClosedLoopConstraint() + : Constraint(CT_STATIC) +{ + +} + +ClosedLoopConstraint::~ClosedLoopConstraint() +{ + +} + +} // namespace constraint +} // namespace dart diff --git a/dart/constraint/ClosedLoopConstraint.h b/dart/constraint/ClosedLoopConstraint.h new file mode 100644 index 0000000000000..93942e8031fef --- /dev/null +++ b/dart/constraint/ClosedLoopConstraint.h @@ -0,0 +1,58 @@ +/* + * Copyright (c) 2014, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Karen Liu , + * Jeongseok Lee + * + * Geoorgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_CONSTRAINT_CLOSEDLOOPCONSTRAINT_H_ +#define DART_CONSTRAINT_CLOSEDLOOPCONSTRAINT_H_ + +#include "dart/constraint/Constraint.h" + +namespace dart { +namespace constraint { + +class ClosedLoopConstraint : public Constraint +{ +public: + ClosedLoopConstraint(); + + ~ClosedLoopConstraint(); +}; + +} // namespace constraint +} // namespace dart + +#endif // DART_CONSTRAINT_CLOSEDLOOPCONSTRAINT_H_ + diff --git a/dart/constraint/ConstrainedGroup.cpp b/dart/constraint/ConstrainedGroup.cpp new file mode 100644 index 0000000000000..094b8da325d77 --- /dev/null +++ b/dart/constraint/ConstrainedGroup.cpp @@ -0,0 +1,268 @@ +/* + * Copyright (c) 2014, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Karen Liu , + * Jeongseok Lee + * + * Geoorgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/constraint/ConstrainedGroup.h" + +#include +#include + +#include "dart/common/Console.h" +#include "dart/lcpsolver/LCPSolver.h" +#include "dart/lcpsolver/Lemke.h" +#include "dart/lcpsolver/lcp.h" +#include "dart/constraint/Constraint.h" + +namespace dart { +namespace constraint { + +using namespace lcpsolver; + +//============================================================================== +ConstrainedGroup::ConstrainedGroup() +{ +} + +//============================================================================== +ConstrainedGroup::~ConstrainedGroup() +{ +} + +//============================================================================== +void ConstrainedGroup::addConstraint(Constraint* _constraint) +{ + assert(_constraint != NULL && "Null constraint pointer is now allowed."); + assert(_containConstraint(_constraint) == false + && "Don't try to add same constraint multiple times into Community."); + + mConstraints.push_back(_constraint); +} + +//============================================================================== +void ConstrainedGroup::removeConstraint(Constraint* _constraint) +{ + assert(_constraint != NULL && "Null constraint pointer is now allowed."); + assert(_containConstraint(_constraint) == true + && "Don't try to remove a constraint not contained in Community."); + + mConstraints.erase( + remove(mConstraints.begin(), mConstraints.end(), _constraint), + mConstraints.end()); +} + +//============================================================================== +void ConstrainedGroup::removeAllConstraints() +{ +// dtwarn << "ConstrainedGroup::removeAllConstraints(): " +// << "Not implemented." << std::endl; + + // TODO(JS): Temporary implementation +// for (int i = 0; i < mConstraints.size(); ++i) +// { +// delete mConstraints[i]; +// } + + mConstraints.clear(); +} + +//============================================================================== +bool ConstrainedGroup::solve() +{ + // If there is no constraint, then just return true. + if (mConstraints.size() == 0) + return true; + +// for (std::vector::iterator it = mConstraints) + + // Build LCP terms by aggregating them from constraints + ODELcp lcp(getTotalDimension()); + + // Fill LCP terms + _fillLCPTermsODE(&lcp); + + ////////////////////////////////////////////////////////////////////////////// +// dtmsg << "Before solve" << std::endl; +// lcp.print(); + ////////////////////////////////////////////////////////////////////////////// + + // Solve LCP + bool result = _solveODE(&lcp); + +// dtmsg << "After solve" << std::endl; +// lcp.print(); + + // Apply impulse + _applyODE(&lcp); + + return result; +} + +//============================================================================== +bool ConstrainedGroup::_containConstraint(Constraint* _constraint) const +{ +// std::cout << "CommunityTEST::_containConstraint(): Not implemented." +// << std::endl; + + return false; +} + +//============================================================================== +bool ConstrainedGroup::_checkAndAddConstraint(Constraint* _constraint) +{ + std::cout << "CommunityTEST::_checkAndAddConstraint(): Not implemented." + << std::endl; + + return false; +} + +//============================================================================== +void ConstrainedGroup::_fillLCPTermsODE(ODELcp* _lcp) +{ + // Compute offset indices + int* offsetIndex = new int[_lcp->dim]; + offsetIndex[0] = 0; + for (int i = 1; i < mConstraints.size(); ++i) + { + assert(mConstraints[i - 1]->getDimension() > 0); + offsetIndex[i] = offsetIndex[i - 1] + mConstraints[i - 1]->getDimension(); +// std::cout << "offsetIndex[" << i << "]: " << offsetIndex[i] << std::endl; + } + + // For each constraint + Constraint* constraint; + for (int i = 0; i < mConstraints.size(); ++i) + { + constraint = mConstraints[i]; + + // Update constraint + constraint->update(); + + // Fill vectors: lo, hi, b, w + constraint->fillLcpOde(_lcp, offsetIndex[i]); + + // Fill a matrix by impulse tests: A + constraint->excite(); + for (int j = 0; j < constraint->getDimension(); ++j) + { + // Apply impulse for mipulse test + constraint->applyUnitImpulse(j); + + // Fill upper triangle blocks of A matrix + for (int k = i; k < mConstraints.size(); ++k) + { + int index = _lcp->nSkip * (offsetIndex[i] + j) + offsetIndex[k]; + mConstraints[k]->getVelocityChange(_lcp->A, index); + } + +// std::cout << "idx: " << _lcp->nSkip * (offsetIndex[i] + j) + offsetIndex[i] << std::endl; + + _lcp->A[_lcp->nSkip * (offsetIndex[i] + j) + offsetIndex[i]] + = _lcp->A[_lcp->nSkip * (offsetIndex[i] + j) + offsetIndex[i]] * 1.001; + + // Filling symmetric part of A matrix + for (int k = 0; k < i; ++k) + { + for (int l = 0; l < mConstraints[k]->getDimension(); ++l) + { + int index1 = _lcp->nSkip * (offsetIndex[i] + j) + offsetIndex[k] + l; + int index2 = _lcp->nSkip * (offsetIndex[k] + l) + offsetIndex[i] + j; + + _lcp->A[index1] = _lcp->A[index2]; + } + } + } + constraint->unexcite(); + } + + delete[] offsetIndex; +} + +//============================================================================== +bool ConstrainedGroup::_solveODE(ODELcp* _lcp) +{ + // Solve LCP using ODE's Dantzig algorithm + dSolveLCP(_lcp->dim, + _lcp->A, + _lcp->x, + _lcp->b, + _lcp->w, + 0, + _lcp->lb, + _lcp->ub, + _lcp->frictionIndex); + + // TODO(JS): Do we need to return boolean? + return true; +} + +//============================================================================== +void ConstrainedGroup::_applyODE(ODELcp* _lcp) +{ + // Compute offset indices + int* offsetIndex = new int[_lcp->dim]; + offsetIndex[0] = 0; + for (int i = 1; i < mConstraints.size(); ++i) + offsetIndex[i] = offsetIndex[i - 1] + mConstraints[i - 1]->getDimension(); + +// // TODO(JS): Clear constraint impulses +// for (int i = 0; i < mConstraints.size(); ++i) +// { +// mConstraints[i]->get +// } + + // Apply constraint impulses + for (int i = 0; i < mConstraints.size(); ++i) + { + mConstraints[i]->applyConstraintImpulse(_lcp->x, offsetIndex[i]); + mConstraints[i]->excite(); + } + + delete[] offsetIndex; +} + +//============================================================================== +int ConstrainedGroup::getTotalDimension() const +{ + int totalDim = 0; + + for (int i = 0; i < mConstraints.size(); ++i) + totalDim += mConstraints[i]->getDimension(); + + return totalDim; +} + +} // namespace constraint +} // namespace dart diff --git a/dart/constraint/ConstrainedGroup.h b/dart/constraint/ConstrainedGroup.h new file mode 100644 index 0000000000000..d7d8ae3ea0503 --- /dev/null +++ b/dart/constraint/ConstrainedGroup.h @@ -0,0 +1,135 @@ +/* + * Copyright (c) 2014, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Karen Liu , + * Jeongseok Lee + * + * Geoorgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_CONSTRAINT_COMMUNITY_H_ +#define DART_CONSTRAINT_COMMUNITY_H_ + +#include +#include + +namespace dart { +namespace constraint { +class ODELcp; +class Constraint; +} // namespace constraint +} // namespace dart + +namespace dart { +namespace constraint { + +//============================================================================== +/// \brief class ConstrainedGroup is a set of skeletons interacting each +/// other by various kinds of constraints. +/// \sa class ConstraintSolver +class ConstrainedGroup +{ +public: + //---------------------------------------------------------------------------- + // Constructor / Desctructor + //---------------------------------------------------------------------------- + /// \brief Default contructor + ConstrainedGroup(); + + /// \brief Default destructor + virtual ~ConstrainedGroup(); + + //---------------------------------------------------------------------------- + // Setting + //---------------------------------------------------------------------------- + /// \brief Add constraint + void addConstraint(Constraint* _constraint); + + /// \brief Remove constraint + void removeConstraint(Constraint* _constraint); + + /// \brief Remove all constraints + void removeAllConstraints(); + + /// \brief Get total dimension of contraints in this group + int getTotalDimension() const; + + //---------------------------------------------------------------------------- + // Solving + //---------------------------------------------------------------------------- + // TODO(JS): Pass option + /// \brief Solve constraints and store the results of constraint impulse to + /// each skeleton. + bool solve(); + +protected: + /// \brief List of constraints + std::vector mConstraints; + +private: + /// \brief Check if _constraint is contained + bool _containConstraint(Constraint* _constraint) const; + + /// \brief Check if _constraint is contained and, if so, add the constraint + bool _checkAndAddConstraint(Constraint* _constraint); + + /// \brief + void _fillLCPTermsODE(ODELcp* _lcp); + +// /// \brief +// void fillLCPTermsLemke(const LCPTermsODE& _lcp); + +// /// \brief +// void fillLCPTermsPGS(const LCPTermsODE& _lcp); + + // TODO(JS): more solvers + /// \brief + bool _solveODE(ODELcp* _lcp); + +// bool _solveLemke(); + +// bool _solvePGS(); + + void _applyODE(ODELcp* _lcp); + +// // Matrices to pass to solver +// Eigen::MatrixXd mA; +// Eigen::VectorXd mQBar; +// Eigen::VectorXd mX; + + +}; + +} // namespace constraint +} // namespace dart + +#endif // DART_CONSTRAINT_COMMUNITY_H_ + diff --git a/dart/constraint/Constraint.cpp b/dart/constraint/Constraint.cpp new file mode 100644 index 0000000000000..2929b3c1c011a --- /dev/null +++ b/dart/constraint/Constraint.cpp @@ -0,0 +1,153 @@ +/* + * Copyright (c) 2014, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Karen Liu , + * Jeongseok Lee + * + * Geoorgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/constraint/Constraint.h" + +#include +#include + +namespace dart { +namespace constraint { + +#define dPAD(a) (((a) > 1) ? ((((a)-1)|3)+1) : (a)) + +//============================================================================== +ODELcp::ODELcp(int _n) +{ + nSkip = dPAD(_n); + + A = new double[_n * nSkip]; + b = new double[_n]; + w = new double[_n]; + x = new double[_n]; + lb = new double[_n]; + ub = new double[_n]; + frictionIndex = new int[_n]; + dim = _n; + +// std::memset(A, 0, _n * nSkip * sizeof(double)); + std::memset(frictionIndex, -1, _n * sizeof(int)); +} + +//============================================================================== +ODELcp::~ODELcp() +{ + delete[] A; + delete[] b; + delete[] w; + delete[] x; + delete[] lb; + delete[] ub; + delete[] frictionIndex; +} + +//============================================================================== +void ODELcp::print() +{ + std::cout << "A: " << std::endl; + for (int i = 0; i < dim; ++i) + { + for (int j = 0; j < nSkip; ++j) + { + std::cout << A[i * nSkip + j] << " "; + } + std::cout << std::endl; + } + std::cout << std::endl; + + std::cout << "b: " << std::endl; + for (int i = 0; i < dim; ++i) + { + std::cout << b[i] << " "; + } + std::cout << std::endl; + + std::cout << "w: " << std::endl; + for (int i = 0; i < dim; ++i) + { + std::cout << w[i] << " "; + } + std::cout << std::endl; + + std::cout << "x: " << std::endl; + for (int i = 0; i < dim; ++i) + { + std::cout << x[i] << " "; + } + std::cout << std::endl; + + std::cout << "lb: " << std::endl; + for (int i = 0; i < dim; ++i) + { + std::cout << lb[i] << " "; + } + std::cout << std::endl; + + std::cout << "ub: " << std::endl; + for (int i = 0; i < dim; ++i) + { + std::cout << ub[i] << " "; + } + std::cout << std::endl; + + std::cout << "frictionIndex: " << std::endl; + for (int i = 0; i < dim; ++i) + { + std::cout << frictionIndex[i] << " "; + } + std::cout << std::endl; +} + +//============================================================================== +Constraint::Constraint(ConstraintType _type) + : mDim(0) +{ +} + +//============================================================================== +Constraint::~Constraint() +{ +} + +//============================================================================== +int Constraint::getDimension() const +{ + return mDim; +} + +} // namespace constraint +} // namespace dart diff --git a/dart/constraint/Constraint.h b/dart/constraint/Constraint.h new file mode 100644 index 0000000000000..e9d7e6a94c0d0 --- /dev/null +++ b/dart/constraint/Constraint.h @@ -0,0 +1,152 @@ +/* + * Copyright (c) 2014, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Karen Liu , + * Jeongseok Lee + * + * Geoorgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_CONSTRAINT_CONSTRAINT_H_TEST +#define DART_CONSTRAINT_CONSTRAINT_H_TEST + +namespace dart { +namespace constraint { + +//============================================================================== +// TODO(JS): Restricted to ODE LCP solver. Generalize this class for various +// types of LCP solvers. +/// \brief LCPTerms class +class ODELcp +{ +public: + /// \brief Constructor + explicit ODELcp(int _n); + + /// \brief Destructor + ~ODELcp(); + + //-------------------------- TEST CODE --------------------------------------- + void print(); + + //---------------------------------------------------------------------------- + /// \brief + double* A; + + /// \brief + double* b; + + /// \brief + double* x; + + /// \brief + double* w; + + /// \brief Lower bound of x + double* lb; + + /// \brief Upper bound of x + double* ub; + + /// \brief Friction index + int* frictionIndex; + + /// \brief Total dimension of contraints + int dim; + + /// \brief + int nSkip; +}; + +//============================================================================== +/// \brief ConstraintTEST class +class Constraint +{ +public: + enum ConstraintType + { + CT_STATIC, + CT_DYNAMIC + }; + + /// \brief Default contructor + explicit Constraint(ConstraintType _type); + + /// \brief Default destructor + virtual ~Constraint(); + + //---------------------------------------------------------------------------- + // Pure virtual functions for solving + //---------------------------------------------------------------------------- + /// \brief + virtual void preprocess() = 0; + + /// \brief + virtual void update() = 0; + + /// \brief + virtual void fillLcpOde(ODELcp* _lcp, int _idx) = 0; + + /// \brief Apply unit impulse to constraint space of _idx + virtual void applyUnitImpulse(int _idx) = 0; + + /// \brief + virtual void getVelocityChange(double* _delVel, int _idx) = 0; + + /// \brief + virtual void excite() {} + + /// \brief + virtual void unexcite() {} + + /// \brief Apply computed constraint impulse to constrained skeletons + virtual void applyConstraintImpulse(double* _lambda, int _idx) = 0; + + //---------------------------------------------------------------------------- + /// \brief + int getDimension() const; + +protected: +// /// \brief +// void impulseTest(); + + /// \brief + int mDim; + + /// \brief + ConstraintType mConstraintType; +}; + +} // namespace constraint +} // namespace dart + +#endif // DART_CONSTRAINT_CONSTRAINT_H_TEST + diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp new file mode 100644 index 0000000000000..1cdf3a4bf8e9d --- /dev/null +++ b/dart/constraint/ConstraintSolver.cpp @@ -0,0 +1,515 @@ +/* + * Copyright (c) 2014, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Karen Liu , + * Jeongseok Lee + * + * Geoorgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/constraint/ConstraintSolver.h" + +#include "dart/common/Console.h" +#include "dart/dynamics/Skeleton.h" +#include "dart/collision/fcl_mesh/FCLMeshCollisionDetector.h" +#include "dart/constraint/ConstrainedGroup.h" +//#include "dart/constraint/BallJointConstraint.h" +//#include "dart/constraint/ClosedLoopConstraint.h" +#include "dart/constraint/ContactConstraint.h" +//#include "dart/constraint/JointLimitConstraint.h" +//#include "dart/constraint/RevoluteJointConstraint.h" +//#include "dart/constraint/WeldJointConstraint.h" + +namespace dart { +namespace constraint { + +using namespace dynamics; + +//============================================================================== +ConstraintSolver::ConstraintSolver(const std::vector& _skeletons, + double _timeStep, + bool _useODE) + : mTimeStep(_timeStep), + mUseODE(_useODE), + mCollisionDetector(new collision::FCLMeshCollisionDetector()) +{ + _init(); +} + +//============================================================================== +ConstraintSolver::~ConstraintSolver() +{ + delete mCollisionDetector; +} + +//============================================================================== +void ConstraintSolver::addSkeleton(Skeleton* _skeleton) +{ + assert(_skeleton != NULL + && "Null pointer skeleton is now allowed to add to ConstraintSover."); + + if (_containSkeleton(_skeleton) == false) + { + mSkeletons.push_back(_skeleton); + + // TODO(JS): Dont't initialize this solver but just add _skeleton. + _init(); + } + else + { + dtwarn << "Skeleton [" << _skeleton->getName() + << "] is already in ConstraintSolver." << std::endl; + } +} + +//============================================================================== +void ConstraintSolver::addSkeletons(const std::vector& _skeletons) +{ + int numAddedSkeletons = 0; + + for (std::vector::const_iterator it = _skeletons.begin(); + it != _skeletons.end(); ++it) + { + assert(*it != NULL + && "Null pointer skeleton is now allowed to add to ConstraintSover."); + + if (_containSkeleton(*it) == false) + { + mSkeletons.push_back(*it); + ++numAddedSkeletons; + } + else + { + dtwarn << "Skeleton [" << (*it)->getName() + << "] is already in ConstraintSolver." << std::endl; + } + } + +// if (numAddedSkeletons > 0) + // TODO(JS): Dont't initialize this solver but just add _skeletons. + _init(); +} + +//============================================================================== +void ConstraintSolver::removeSkeleton(Skeleton* _skeleton) +{ + assert(_skeleton != NULL + && "Null pointer skeleton is now allowed to add to ConstraintSover."); + + if (_containSkeleton(_skeleton)) + { + mSkeletons.erase(remove(mSkeletons.begin(), mSkeletons.end(), _skeleton), + mSkeletons.end()); + + // TODO(JS): Dont't initialize this solver but just remove _skeleton. + _init(); + } + else + { + dtwarn << "Skeleton [" << _skeleton->getName() + << "] is not in ConstraintSolver." << std::endl; + } +} + +//============================================================================== +void ConstraintSolver::removeSkeletons( + const std::vector& _skeletons) +{ + int numRemovedSkeletons = 0; + + for (std::vector::const_iterator it = _skeletons.begin(); + it != _skeletons.end(); ++it) + { + assert(*it != NULL + && "Null pointer skeleton is now allowed to add to ConstraintSover."); + + if (_containSkeleton(*it)) + { + mSkeletons.erase(remove(mSkeletons.begin(), mSkeletons.end(), *it), + mSkeletons.end()); + ++numRemovedSkeletons; + } + else + { + dtwarn << "Skeleton [" << (*it)->getName() + << "] is not in ConstraintSolver." << std::endl; + } + } + +// if (numRemovedSkeletons > 0) + // TODO(JS): Dont't initialize this solver but just remove _skeletons. + _init(); +} + +//============================================================================== +void ConstraintSolver::removeAllSkeletons() +{ + std::cout << "ConstraintSolver::removeAllSkeletons(): " + << "Not implemented yet." + << std::endl; +} + +//============================================================================== +void ConstraintSolver::addConstraint(Constraint* _constraint) +{ + std::cout << "ConstraintSolver::addConstraint(): " + << "Not implemented yet." + << std::endl; +} + +//============================================================================== +void ConstraintSolver::addConstraints(const std::vector& _constraints) +{ + std::cout << "ConstraintSolver::addConstraints(): " + << "Not implemented yet." + << std::endl; +} + +//============================================================================== +void ConstraintSolver::removeConstraint(Constraint* _constraint) +{ + std::cout << "ConstraintSolver::removeConstraint(): " + << "Not implemented yet." + << std::endl; +} + +//============================================================================== +void ConstraintSolver::removeConstraints(const std::vector& _constraints) +{ + std::cout << "ConstraintSolver::removeConstraints(): " + << "Not implemented yet." + << std::endl; +} + +//============================================================================== +void ConstraintSolver::removeAllConstraints() +{ + std::cout << "ConstraintSolver::removeAllConstraints(): " + << "Not implemented yet." + << std::endl; +} + +//============================================================================== +void ConstraintSolver::setTimeStep(double _timeStep) +{ + assert(_timeStep > 0.0 && "Time step should be positive value."); + mTimeStep = _timeStep; +} + +//============================================================================== +double ConstraintSolver::getTimeStep() const +{ + return mTimeStep; +} + +//============================================================================== +void ConstraintSolver::setCollisionDetector( + collision::CollisionDetector* _collisionDetector) +{ + assert(_collisionDetector && "Invalid collision detector."); + mCollisionDetector = _collisionDetector; +} + +//============================================================================== +collision::CollisionDetector* ConstraintSolver::getCollisionDetector() const +{ + return mCollisionDetector; +} + +//============================================================================== +void ConstraintSolver::solve() +{ + for (int i = 0; i < mSkeletons.size(); ++i) + mSkeletons[i]->clearConstraintImpulses(); + + _updateDynamicConstraints(); + _buildConstrainedGroups(); + _solveConstrainedGroups(); +} + +//============================================================================== +void ConstraintSolver::_init() +{ + _bakeConstraints(); + + //---------------------------------------------------------------------------- + // Static constraints + //---------------------------------------------------------------------------- + mStaticConstraints.clear(); + + // Closed loop constraints +// for (std::vector::const_iterator it +// = mBakedClosedLoopConstraints.begin(); +// it != mBakedClosedLoopConstraints.end(); ++it) +// { +// mStaticConstraints.push_back(*it); +// } + + //---------------------------------------------------------------------------- + // Dynamic constraints + //---------------------------------------------------------------------------- + mDynamicConstraints.clear(); + + int maxNumDynamicConstraints = 0; + maxNumDynamicConstraints += mBakedContactConstraints.size(); + maxNumDynamicConstraints += mBakedJointLimitContraints.size(); + maxNumDynamicConstraints += mBakedJointConstraints.size(); + + mDynamicConstraints.reserve(maxNumDynamicConstraints); + + //---------------------------------------------------------------------------- + // Communities + //---------------------------------------------------------------------------- + // TODO(JS): Create one community for test + for (std::vector::iterator it = mConstrainedGroups.begin(); + it != mConstrainedGroups.end(); ++it) + { + delete *it; + } + mConstrainedGroups.clear(); + mConstrainedGroups.resize(1); + mConstrainedGroups[0] = new ConstrainedGroup; +} + +//============================================================================== +void ConstraintSolver::_bakeConstraints() +{ + // Contact constraints + __bakeContactConstraints(); + + // Joint limit constraints + __bakeJointLimitConstraints(); + + // closed loop constraints + __bakeClosedLoopConstraints(); + + // Joint constraints + __bakeJointConstraints(); +} + +//============================================================================== +void ConstraintSolver::__bakeContactConstraints() +{ +// dtdbg << "ConstraintSolver::__bakeContactConstraints(): " +// << "Not implemented yet." +// << std::endl; + + // Reset backed contact constraints + for (std::vector::iterator it + = mBakedContactConstraints.begin(); + it != mBakedContactConstraints.end(); ++it) + { + delete *it; + } + mBakedContactConstraints.clear(); + + // TODO(JS): + mCollisionDetector->removeAllSkeletons(); + for (std::vector::iterator it = mSkeletons.begin(); + it != mSkeletons.end(); ++it) + { + mCollisionDetector->addSkeleton(*it); + } +} + +//============================================================================== +void ConstraintSolver::__bakeJointLimitConstraints() +{ +// std::cout << "ConstraintSolver::__bakeJointLimitConstraints(): " +// << "Not implemented yet." +// << std::endl; +} + +//============================================================================== +void ConstraintSolver::__bakeClosedLoopConstraints() +{ +// std::cout << "ConstraintSolver::__bakeClosedLoopConstraints(): " +// << "Not implemented yet." +// << std::endl; +} + +//============================================================================== +void ConstraintSolver::__bakeJointConstraints() +{ +// std::cout << "ConstraintSolver::__bakeJointConstraints(): " +// << "Not implemented yet." +// << std::endl; +} + +//============================================================================== +bool ConstraintSolver::_containSkeleton(const Skeleton* _skeleton) const +{ + assert(_skeleton != NULL && "Now allowed to insert null pointer skeleton."); + + for (std::vector::const_iterator it = mSkeletons.begin(); + it != mSkeletons.end(); ++it) + { + if ((*it) == _skeleton) + return true; + } + + return false; +} + +//============================================================================== +bool ConstraintSolver::_checkAndAddSkeleton(Skeleton* _skeleton) +{ + if (!_containSkeleton(_skeleton)) + { + mSkeletons.push_back(_skeleton); + return true; + } + else + { + dtwarn << "Skeleton [" << _skeleton->getName() + << "] is already in ConstraintSolver." << std::endl; + return false; + } +} + +//============================================================================== +bool ConstraintSolver::_containConstraint( + const Constraint* _constraint) const +{ + std::cout << "ConstraintSolverTEST::_containConstraint(): " + << "Not implemented." + << std::endl; + + return false; +} + +//============================================================================== +bool ConstraintSolver::_checkAndAddConstraint(Constraint* _constraint) +{ + std::cout << "ConstraintSolverTEST::_checkAndAddConstraint(): " + << "Not implemented." + << std::endl; + + if (!_containConstraint(_constraint)) + { +// mConstraints.push_back(_constraint); + } + else + { +// dtwarn << "Constraint [" << _constraint->getName() +// << "] is already in ConstraintSolver." << std::endl; + } + + return false; +} + +//============================================================================== +void ConstraintSolver::_updateDynamicConstraints() +{ + // TODO(JS): Use warn starting + mCollisionDetector->clearAllContacts(); + mCollisionDetector->detectCollision(true, true); + + mDynamicConstraints.clear(); + + // Contact constraints + for (std::vector::const_iterator it + = mBakedContactConstraints.begin(); + it != mBakedContactConstraints.end(); ++it) + { + delete *it; + } + mBakedContactConstraints.clear(); + for (int i = 0; i < mCollisionDetector->getNumContacts(); ++i) + { + const collision::Contact& ct = mCollisionDetector->getContact(i); + ContactConstraint* cc = new ContactConstraint(ct); + mBakedContactConstraints.push_back(cc); + } + for (std::vector::const_iterator it + = mBakedContactConstraints.begin(); + it != mBakedContactConstraints.end(); ++it) + { + if ((*it)->isActive()) + mDynamicConstraints.push_back(*it); + } + + // Joint limit constraints +// for (std::vector::const_iterator it +// = mBakedJointLimitContraints.begin(); +// it != mBakedJointLimitContraints.end(); ++it) +// { +// if ((*it)->isActive()) +// mDynamicConstraints.push_back(*it); +// } + + // Joint constraints +// for (std::vector::const_iterator it +// = mBakedJointConstraints.begin(); +// it != mBakedJointConstraints.end(); ++it) +// { +// if ((*it)->isActive()) +// mDynamicConstraints.push_back(*it); +// } +} + +//============================================================================== +void ConstraintSolver::_buildConstrainedGroups() +{ + // TODO(JS): + mConstrainedGroups[0]->removeAllConstraints(); + + //-------------- Add Constraints to constrained groups ----------------------- + // Static constraints + for (std::vector::iterator it = mStaticConstraints.begin(); + it != mStaticConstraints.end(); ++it) + { + // TODO(JS): + mConstrainedGroups[0]->addConstraint(*it); + } + + // Dynamics constraints + for (std::vector::iterator it = mDynamicConstraints.begin(); + it != mDynamicConstraints.end(); ++it) + { + // TODO(JS): + mConstrainedGroups[0]->addConstraint(*it); + } +} + +//============================================================================== +void ConstraintSolver::_solveConstrainedGroups() +{ + // TODO(JS): Parallel computing is possible here. + for (std::vector::iterator it = mConstrainedGroups.begin(); + it != mConstrainedGroups.end(); ++it) + { + (*it)->solve(); + } +} + +} // namespace constraint +} // namespace dart diff --git a/dart/constraint/ConstraintSolver.h b/dart/constraint/ConstraintSolver.h new file mode 100644 index 0000000000000..4d9271bae208c --- /dev/null +++ b/dart/constraint/ConstraintSolver.h @@ -0,0 +1,209 @@ +/* + * Copyright (c) 2014, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Karen Liu , + * Jeongseok Lee + * + * Geoorgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_CONSTRAINT_CONSTRAINTSOVER_H_ +#define DART_CONSTRAINT_CONSTRAINTSOVER_H_ + +#include + +#include + +#include "dart/constraint/Constraint.h" +#include "dart/collision/CollisionDetector.h" + +namespace dart { +namespace dynamics { +class Skeleton; +} +namespace constraint { +class ConstrainedGroup; +class Constraint; +class BallJointContraintTEST; +class ClosedLoopConstraint; +class ContactConstraint; +class JointLimitConstraint; +class RevoluteJointContraintTEST; +class WeldJointContraintTEST; +class JointConstraint; +} // namespace constraint +} // namespace dart + +namespace dart { +namespace constraint { + +class ConstraintSolver +{ +public: + /// \brief Constructor + ConstraintSolver(const std::vector& _skeletons, + double _timeStep, + bool _useODE = true); + + /// \brief Destructor + virtual ~ConstraintSolver(); + + //---------------------------------------------------------------------------- + /// \brief Add single skeleton + void addSkeleton(dynamics::Skeleton* _skeleton); + + /// \brief Add skeletons + void addSkeletons(const std::vector& _skeletons); + + /// \brief Remove single skeleton. + void removeSkeleton(dynamics::Skeleton* _skeleton); + + /// \brief Remove skeletons. + void removeSkeletons(const std::vector& _skeletons); + + /// \brief Remove all skeletons. + void removeAllSkeletons(); + + //---------------------------------------------------------------------------- + /// \brief Add single constraint. + void addConstraint(Constraint* _constraint); + + /// \brief Add constraints. + void addConstraints(const std::vector& _constraints); + + /// \brief Remove single constraint. + void removeConstraint(Constraint* _constraint); + + /// \brief Remove constraints. + void removeConstraints(const std::vector& _constraints); + + /// \brief Remove all constraints. + void removeAllConstraints(); + + //---------------------------------------------------------------------------- + /// \brief + void setTimeStep(double _timeStep); + + /// \brief + double getTimeStep() const; + + //---------------------------------------------------------------------------- + /// \brief Set collision detector + void setCollisionDetector(collision::CollisionDetector* _collisionDetector); + + /// \brief Get collision detector + collision::CollisionDetector* getCollisionDetector() const; + + //---------------------------------------------------------------------------- + // Solving + //---------------------------------------------------------------------------- + /// \brief Solve constraint impulses and apply them to the skeletons + virtual void solve(); + +protected: + //---------------------------------------------------------------------------- + /// \brief + std::vector mSkeletons; + +// /// \brief +// std::vector mBakedConstraints; + + /// \brief + std::vector mBakedContactConstraints; + + /// \brief + std::vector mBakedJointLimitContraints; + + /// \brief + std::vector mBakedClosedLoopConstraints; + + /// \brief + std::vector mBakedJointConstraints; + + //---------------------------------------------------------------------------- + /// \brief + std::vector mStaticConstraints; + + /// \brief + std::vector mDynamicConstraints; + + /// \brief List of communities + std::vector mConstrainedGroups; + + /// \brief +// std::vector mBakedClosedLoopConstraints; + +private: + /// \brief + void _init(); + + /// \brief + void _bakeConstraints(); + void __bakeContactConstraints(); + void __bakeJointLimitConstraints(); + void __bakeClosedLoopConstraints(); + void __bakeJointConstraints(); + + /// \brief Check if the skeleton is contained in this solver + bool _containSkeleton(const dynamics::Skeleton* _skeleton) const; + + /// \brief Add skeleton if the constraint is not contained in this solver + bool _checkAndAddSkeleton(dynamics::Skeleton* _skeleton); + + /// \brief Check if the constraint is contained in this solver + bool _containConstraint(const Constraint* _constraint) const; + + /// \brief Add constraint if the constraint is not contained in this solver + bool _checkAndAddConstraint(Constraint* _constraint); + + /// \brief Update dynamic constraints + void _updateDynamicConstraints(); + + /// \brief Build constrained groups + void _buildConstrainedGroups(); + + /// \brief Solve constrained groups + void _solveConstrainedGroups(); + + /// \brief Collision detector + collision::CollisionDetector* mCollisionDetector; + + /// \brief Time step + double mTimeStep; + + /// \brief Flag for using ODE + bool mUseODE; +}; + +} // namespace constraint +} // namespace dart + +#endif // DART_CONSTRAINT_CONSTRAINTSOVER_H_ diff --git a/dart/constraint/ContactConstraint.cpp b/dart/constraint/ContactConstraint.cpp new file mode 100644 index 0000000000000..77ddc9563bf19 --- /dev/null +++ b/dart/constraint/ContactConstraint.cpp @@ -0,0 +1,519 @@ +/* + * Copyright (c) 2014, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Karen Liu , + * Jeongseok Lee + * + * Geoorgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/constraint/ContactConstraint.h" + +#include + +#include "dart/dynamics/BodyNode.h" +#include "dart/dynamics/Skeleton.h" +#include "dart/lcpsolver/lcp.h" + +#define DART_CONTACT_CONSTRAIN_EPSILON 1e-6 + +namespace dart { +namespace constraint { + +//============================================================================== +ContactConstraint::ContactConstraint(const collision::Contact& _contact) + : Constraint(CT_DYNAMIC), + mFirstFrictionalDirection(Eigen::Vector3d::UnitZ()), + _IsFrictionOn(true) +{ + mContacts.push_back(_contact); + + // TODO(JS): + mBodyNode1 = _contact.collisionNode1->getBodyNode(); + mBodyNode2 = _contact.collisionNode2->getBodyNode(); + + // TODO(JS): Assume the frictional coefficient can be changed during + // simulation steps. + // Update mFrictionalCoff + _frictionalCoff = std::min(mBodyNode1->getFrictionCoeff(), + mBodyNode2->getFrictionCoeff()); + if (_frictionalCoff > DART_FRICTION_THRESHOLD) + { + _IsFrictionOn = true; + + // Update frictional direction + _updateFirstFrictionalDirection(); + } + else + { + _IsFrictionOn = false; + } + + // Compute local contact Jacobians expressed in body frame + if (_IsFrictionOn) + { + // Set the dimension of this constraint. 1 is for Normal direction constraint. + // TODO(JS): Assumed that the number of contact is not static. + // TODO(JS): Adjust following code once use of mNumFrictionConeBases is + // implemented. + // mDim = mContacts.size() * (1 + mNumFrictionConeBases); + mDim = mContacts.size() * 3; + + mJacobians1.resize(mDim); + mJacobians2.resize(mDim); + + // Intermediate variables + int idx = 0; + + Eigen::Vector3d bodyDirection1; + Eigen::Vector3d bodyDirection2; + + Eigen::Vector3d bodyPoint1; + Eigen::Vector3d bodyPoint2; + + for (int i = 0; i < mContacts.size(); ++i) + { + const collision::Contact& ct = mContacts[i]; + + // TODO(JS): Assumed that the number of tangent basis is 2. + Eigen::MatrixXd D = _getTangentBasisMatrixODE(ct.normal); + + // Jacobian for normal contact + bodyDirection1.noalias() + = mBodyNode1->getWorldTransform().linear().transpose() * ct.normal; + bodyDirection2.noalias() + = mBodyNode2->getWorldTransform().linear().transpose() * -ct.normal; + + bodyPoint1.noalias() + = mBodyNode1->getWorldTransform().inverse() * ct.point; + bodyPoint2.noalias() + = mBodyNode2->getWorldTransform().inverse() * ct.point; + + mJacobians1[idx].head<3>().noalias() = bodyPoint1.cross(bodyDirection1); + mJacobians2[idx].head<3>().noalias() = bodyPoint2.cross(bodyDirection2); + + mJacobians1[idx].tail<3>().noalias() = bodyDirection1; + mJacobians2[idx].tail<3>().noalias() = bodyDirection2; + + ++idx; + + // Jacobian for directional friction 1 + bodyDirection1.noalias() + = mBodyNode1->getWorldTransform().linear().transpose() * D.col(0); + bodyDirection2.noalias() + = mBodyNode2->getWorldTransform().linear().transpose() * -D.col(0); + + bodyPoint1.noalias() + = mBodyNode1->getWorldTransform().inverse() * ct.point; + bodyPoint2.noalias() + = mBodyNode2->getWorldTransform().inverse() * ct.point; + + mJacobians1[idx].head<3>().noalias() = bodyPoint1.cross(bodyDirection1); + mJacobians2[idx].head<3>().noalias() = bodyPoint2.cross(bodyDirection2); + + mJacobians1[idx].tail<3>().noalias() = bodyDirection1; + mJacobians2[idx].tail<3>().noalias() = bodyDirection2; + + ++idx; + + // Jacobian for directional friction 2 + bodyDirection1.noalias() + = mBodyNode1->getWorldTransform().linear().transpose() * D.col(1); + bodyDirection2.noalias() + = mBodyNode2->getWorldTransform().linear().transpose() * -D.col(1); + + bodyPoint1.noalias() + = mBodyNode1->getWorldTransform().inverse() * ct.point; + bodyPoint2.noalias() + = mBodyNode2->getWorldTransform().inverse() * ct.point; + + mJacobians1[idx].head<3>().noalias() = bodyPoint1.cross(bodyDirection1); + mJacobians2[idx].head<3>().noalias() = bodyPoint2.cross(bodyDirection2); + + mJacobians1[idx].tail<3>().noalias() = bodyDirection1; + mJacobians2[idx].tail<3>().noalias() = bodyDirection2; + + ++idx; + } + } + else + { + // Set the dimension of this constraint. + mDim = mContacts.size(); + + mJacobians1.resize(mDim); + mJacobians2.resize(mDim); + + Eigen::Vector3d bodyDirection1; + Eigen::Vector3d bodyDirection2; + + Eigen::Vector3d bodyPoint1; + Eigen::Vector3d bodyPoint2; + + for (int i = 0; i < mContacts.size(); ++i) + { + const collision::Contact& ct = mContacts[i]; + + bodyDirection1.noalias() + = mBodyNode1->getWorldTransform().linear().transpose() * ct.normal; + bodyDirection2.noalias() + = mBodyNode2->getWorldTransform().linear().transpose() * -ct.normal; + + bodyPoint1.noalias() + = mBodyNode1->getWorldTransform().inverse() * ct.point; + bodyPoint2.noalias() + = mBodyNode2->getWorldTransform().inverse() * ct.point; + + mJacobians1[i].head<3>().noalias() = bodyPoint1.cross(bodyDirection1); + mJacobians2[i].head<3>().noalias() = bodyPoint2.cross(bodyDirection2); + + mJacobians1[i].tail<3>().noalias() = bodyDirection1; + mJacobians2[i].tail<3>().noalias() = bodyDirection2; + } + } +} + +//============================================================================== +ContactConstraint::~ContactConstraint() +{ +} + +//============================================================================== +void ContactConstraint::setFirstFrictionDir( + const Eigen::Vector3d& _dir) +{ + mFirstFrictionalDirection = _dir.normalized(); +} + +//============================================================================== +const Eigen::Vector3d&ContactConstraint::getFirstFrictionlDir() const +{ + return mFirstFrictionalDirection; +} + +//============================================================================== +void ContactConstraint::preprocess() +{ + std::cout << "ContactConstraintTEST::preprocess(): " + << "Not implemented." + << std::endl; +} + +//============================================================================== +void ContactConstraint::update() +{ +// std::cout << "ContactConstraintTEST::update(): " +// << "Not implemented." +// << std::endl; +} + +//============================================================================== +void ContactConstraint::fillLcpOde(ODELcp* _lcp, int _idx) +{ + // Fill w, where the LCP form is Ax = b + w (x >= 0, w >= 0, x^T w = 0) + _getRelVelocity(_lcp->b, _idx); + + //---------------------------------------------------------------------------- + // Friction case + //---------------------------------------------------------------------------- + if (_IsFrictionOn) + { + for (int i = 0; i < mContacts.size(); ++i) + { + // Bias term, w, should be zero + _lcp->w[_idx] = 0.0; + _lcp->w[_idx + 1] = 0.0; + _lcp->w[_idx + 2] = 0.0; + + // Upper and lower bounds of normal impulsive force + _lcp->lb[_idx] = 0.0; + _lcp->ub[_idx] = dInfinity; + + // Upper and lower bounds of tangential direction-1 impulsive force + _lcp->lb[_idx + 1] = -_frictionalCoff; + _lcp->ub[_idx + 1] = _frictionalCoff; + _lcp->frictionIndex[_idx + 1] = _idx; + + // Upper and lower bounds of tangential direction-2 impulsive force + _lcp->lb[_idx + 2] = -_frictionalCoff; + _lcp->ub[_idx + 2] = _frictionalCoff; + _lcp->frictionIndex[_idx + 2] = _idx; + + // TODO(JS): Penetration correction should be here + + // TODO(JS): Bounce condition should be here + + // TODO(JS): Initial guess + + // Increase index + _idx += 3; + } + } + //---------------------------------------------------------------------------- + // Frictionless case + //---------------------------------------------------------------------------- + else + { + for (int i = 0; i < mContacts.size(); ++i) + { + // Bias term, w, should be zero + _lcp->w[_idx] = 0.0; + + // Upper and lower bounds of normal impulsive force + _lcp->lb[_idx] = 0.0; + _lcp->ub[_idx] = dInfinity; + + // TODO(JS): Penetration correction should be here + + // TODO(JS): Bounce condition should be here + + // TODO(JS): Initial guess + + // Increase index + _idx++; + } + } +} + +//============================================================================== +void ContactConstraint::applyUnitImpulse(int _idx) +{ + assert(0 <= _idx && _idx < mDim && "Invalid Index."); + + // Self collision case + if (mBodyNode1->getSkeleton() == mBodyNode2->getSkeleton() + && (mBodyNode1->isImpulseReponsible() + || mBodyNode2->isImpulseReponsible())) + { + mBodyNode1->getSkeleton()->clearImpulseTest(); + + if (mBodyNode1->isImpulseReponsible()) + mBodyNode1->getSkeleton()->updateBiasImpulse(mBodyNode1, + mJacobians1[_idx]); + + if (mBodyNode2->isImpulseReponsible()) + mBodyNode2->getSkeleton()->updateBiasImpulse(mBodyNode2, + mJacobians2[_idx]); + + mBodyNode1->getSkeleton()->updateVelocityChange(); + return; + } + + if (mBodyNode1->isImpulseReponsible()) + { + mBodyNode1->getSkeleton()->clearImpulseTest(); + mBodyNode1->getSkeleton()->updateBiasImpulse(mBodyNode1, mJacobians1[_idx]); + mBodyNode1->getSkeleton()->updateVelocityChange(); + } + + if (mBodyNode2->isImpulseReponsible()) + { + mBodyNode2->getSkeleton()->clearImpulseTest(); + mBodyNode2->getSkeleton()->updateBiasImpulse(mBodyNode2, mJacobians2[_idx]); + mBodyNode2->getSkeleton()->updateVelocityChange(); + } +} + +//============================================================================== +void ContactConstraint::getVelocityChange(double* _delVel, int _idx) +{ + assert(_delVel != NULL && "Null pointer is not allowed."); + + for (int i = 0; i < mDim; ++i) + { + _delVel[i + _idx] = 0.0; + + if (mBodyNode1->getSkeleton()->isImpulseApplied() + && mBodyNode1->isImpulseReponsible()) + { + _delVel[i + _idx] + += mJacobians1[i].dot(mBodyNode1->getBodyVelocityChange()); + } + + if (mBodyNode2->getSkeleton()->isImpulseApplied() + && mBodyNode2->isImpulseReponsible()) + { + _delVel[i + _idx] + += mJacobians2[i].dot(mBodyNode2->getBodyVelocityChange()); + } + } +} + +//============================================================================== +void ContactConstraint::excite() +{ + if (mBodyNode1->isImpulseReponsible()) + mBodyNode1->getSkeleton()->setImpulseApplied(true); + + if (mBodyNode2->isImpulseReponsible()) + mBodyNode2->getSkeleton()->setImpulseApplied(true); +} + +//============================================================================== +void ContactConstraint::unexcite() +{ + if (mBodyNode1->isImpulseReponsible()) + mBodyNode1->getSkeleton()->setImpulseApplied(false); + + if (mBodyNode2->isImpulseReponsible()) + mBodyNode2->getSkeleton()->setImpulseApplied(false); +} + +//============================================================================== +void ContactConstraint::applyConstraintImpulse(double* _lambda, int _idx) +{ + //---------------------------------------------------------------------------- + // Friction case + //---------------------------------------------------------------------------- + if (_IsFrictionOn) + { + for (int i = 0; i < mContacts.size(); ++i) + { + // Normal impulsive force +// mContacts[i]->lambda[0] = _lambda[_idx]; + mBodyNode1->addConstraintImpulse(mJacobians1[i] * _lambda[_idx]); + mBodyNode2->addConstraintImpulse(mJacobians2[i] * _lambda[_idx]); + _idx++; + + // Tangential direction-1 impulsive force +// mContacts[i]->lambda[1] = _lambda[_idx]; + mBodyNode1->addConstraintImpulse(mJacobians1[i] * _lambda[_idx]); + mBodyNode2->addConstraintImpulse(mJacobians2[i] * _lambda[_idx]); + _idx++; + + // Tangential direction-2 impulsive force +// mContacts[i]->lambda[2] = _lambda[_idx]; + mBodyNode1->addConstraintImpulse(mJacobians1[i] * _lambda[_idx]); + mBodyNode2->addConstraintImpulse(mJacobians2[i] * _lambda[_idx]); + _idx++; + } + } + //---------------------------------------------------------------------------- + // Frictionless case + //---------------------------------------------------------------------------- + else + { + for (int i = 0; i < mContacts.size(); ++i) + { + // Normal impulsive force +// pContactPts[i]->lambda[0] = _lambda[i]; + if (mBodyNode1->isImpulseReponsible()) + mBodyNode1->addConstraintImpulse(mJacobians1[i] * _lambda[_idx]); + + if (mBodyNode2->isImpulseReponsible()) + mBodyNode2->addConstraintImpulse(mJacobians2[i] * _lambda[_idx]); + _idx++; + } + } +} + +//============================================================================== +void ContactConstraint::_getRelVelocity(double* _relVel, int _idx) +{ + assert(_relVel != NULL && "Null pointer is not allowed."); + + for (int i = 0; i < mDim; ++i) + { + _relVel[i + _idx] = 0.0; + + if (mBodyNode1->isImpulseReponsible()) + _relVel[i + _idx] -= mJacobians1[i].dot(mBodyNode1->getBodyVelocity()); + + if (mBodyNode2->isImpulseReponsible()) + _relVel[i + _idx] -= mJacobians2[i].dot(mBodyNode2->getBodyVelocity()); + } +} + +//============================================================================== +bool ContactConstraint::isActive() +{ +// std::cout << "ContactConstraintTEST::isActive(): " +// << "Not implemented." +// << std::endl; + + return true; +} + +//============================================================================== +void ContactConstraint::_updateVelocityChange(int _idx) +{ + std::cout << "ContactConstraintTEST::_exciteSystem1And2(): " + << "Not implemented." + << std::endl; +} + +//============================================================================== +void ContactConstraint::_updateFirstFrictionalDirection() +{ + std::cout << "ContactConstraintTEST::_updateFirstFrictionalDirection(): " + << "Not finished implementation." + << std::endl; + + // TODO(JS): Not implemented + // Refer to: + // https://github.com/erwincoumans/bullet3/blob/master/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp#L910 +// mFirstFrictionalDirection; +} + +//============================================================================== +Eigen::MatrixXd ContactConstraint::_getTangentBasisMatrixODE( + const Eigen::Vector3d& _n) +{ + // TODO(JS): Use mNumFrictionConeBases + // Check if the number of bases is even number. +// bool isEvenNumBases = mNumFrictionConeBases % 2 ? true : false; + + Eigen::MatrixXd T(Eigen::MatrixXd::Zero(3, 2)); + + // Pick an arbitrary vector to take the cross product of (in this case, + // Z-axis) + Eigen::Vector3d tangent = mFirstFrictionalDirection.cross(_n); + + // TODO(JS): Modify following lines once _updateFirstFrictionalDirection() is + // implemented. + // If they're too close, pick another tangent (use X-axis as arbitrary vector) + if (tangent.norm() < DART_CONTACT_CONSTRAIN_EPSILON) + tangent = Eigen::Vector3d::UnitX().cross(_n); + + tangent.normalize(); + + // Rotate the tangent around the normal to compute bases. + // Note: a possible speedup is in place for mNumDir % 2 = 0 + // Each basis and its opposite belong in the matrix, so we iterate half as + // many times + double angle = 0.5 * DART_PI; + T.col(0) = tangent; + T.col(1) = Eigen::Quaterniond(Eigen::AngleAxisd(angle, _n)) * tangent; + return T; +} + +} // namespace constraint +} // namespace dart diff --git a/dart/constraint/ContactConstraint.h b/dart/constraint/ContactConstraint.h new file mode 100644 index 0000000000000..70184ceaa0552 --- /dev/null +++ b/dart/constraint/ContactConstraint.h @@ -0,0 +1,158 @@ +/* + * Copyright (c) 2014, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Karen Liu , + * Jeongseok Lee + * + * Geoorgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_CONSTRAINT_CONTACTCONSTRAINT_H_ +#define DART_CONSTRAINT_CONTACTCONSTRAINT_H_ + +#include "dart/constraint/Constraint.h" + +#include "dart/math/MathTypes.h" +#include "dart/collision/CollisionDetector.h" + +// Note: ODE's dSolve uses fixed number of friction cone bases as 4 +//#define DART_MIN_NUM_FRICTION_CONE_BASES 4 +//#define DART_MAX_NUM_FRICTION_CONE_BASES 16 +//#define DART_DEFAULT_NUM_FRICTION_CONE_BASES 2 + +#define DART_FRICTION_THRESHOLD 1e-4 + +namespace dart { +namespace constraint { + +//============================================================================== +/// \brief The ContactConstraintTEST class +class ContactConstraint : public Constraint +{ +public: + //---------------------------------------------------------------------------- + // TODO(JS): One contact constraint is allowed now. + /// \brief Constructor + ContactConstraint(const collision::Contact& _contact); + + /// \brief Default destructor + virtual ~ContactConstraint(); + + //---------------------------------------------------------------------------- + // Settings + //---------------------------------------------------------------------------- + /// \brief Set first frictional direction + void setFirstFrictionDir(const Eigen::Vector3d& _dir); + + /// \brief Get first frictional direction + const Eigen::Vector3d& getFirstFrictionlDir() const; + + //---------------------------------------------------------------------------- + // Constraint virtual function + //---------------------------------------------------------------------------- + // Documentaion inherited. + virtual void preprocess(); + + // Documentaion inherited. + virtual void update(); + + // Documentaion inherited. + virtual void fillLcpOde(ODELcp* _lcp, int _idx); + + // Documentaion inherited. + virtual void applyUnitImpulse(int _idx); + + // Documentaion inherited. + virtual void getVelocityChange(double* _delVel, int _idx); + + // Documentaion inherited. + virtual void excite(); + + // Documentaion inherited. + virtual void unexcite(); + + // Documentaion inherited. + virtual void applyConstraintImpulse(double* _lambda, int _idx); + + //---------------------------------------------------------------------------- + // Solving + //---------------------------------------------------------------------------- + /// \brief + bool isActive(); + +protected: + /// \brief Fircst body node + dynamics::BodyNode* mBodyNode1; + + /// \brief Second body node + dynamics::BodyNode* mBodyNode2; + + /// \brief Contacts between mBodyNode1 and mBodyNode2 + std::vector mContacts; + + /// \brief First frictional direction + Eigen::Vector3d mFirstFrictionalDirection; + + /// \brief Frictional coefficient + double _frictionalCoff; + +private: + /// \brief Get change in relative velocity at contact point due to external + /// impulse + /// \param[out] _relVel Change in relative velocity at contact point of the + /// two colliding bodies + /// \param[in] _idx Index the relative velocity change will be stored + void _getRelVelocity(double* _relVel, int _idx); + + /// \brief Compute change in velocity due to _idx-th impulse. + void _updateVelocityChange(int _idx); + + /// \brief + void _updateFirstFrictionalDirection(); + + /// \brief + Eigen::MatrixXd _getTangentBasisMatrixODE(const Eigen::Vector3d& _n); + + /// \brief Local body jacobians for mBodyNode1 + std::vector mJacobians1; + + /// \brief Local body jacobians for mBodyNode2 + std::vector mJacobians2; + + /// \brief + bool _IsFrictionOn; +}; + +} // namespace constraint +} // namespace dart + +#endif // DART_CONSTRAINT_IBCONTACTCONSTRAINT_H_ + diff --git a/dart/constraint/JointConstraint.cpp b/dart/constraint/JointConstraint.cpp new file mode 100644 index 0000000000000..b8d455079b95f --- /dev/null +++ b/dart/constraint/JointConstraint.cpp @@ -0,0 +1,62 @@ +/* + * Copyright (c) 2014, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Karen Liu , + * Jeongseok Lee + * + * Geoorgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/constraint/JointConstraint.h" + +#include + +namespace dart { +namespace constraint { + +JointConstraint::JointConstraint() + : Constraint(CT_DYNAMIC) +{ +} + +JointConstraint::~JointConstraint() +{ +} + +bool JointConstraint::isActive() +{ + std::cout << "JointConstraintTEST::isActive(): Not implemented." + << std::endl; + return false; +} + +} // namespace constraint +} // namespace dart diff --git a/dart/constraint/JointConstraint.h b/dart/constraint/JointConstraint.h new file mode 100644 index 0000000000000..6c877799db079 --- /dev/null +++ b/dart/constraint/JointConstraint.h @@ -0,0 +1,65 @@ +/* + * Copyright (c) 2014, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Karen Liu , + * Jeongseok Lee + * + * Geoorgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_CONSTRAINT_JOINTCONSTRAINT_H_ +#define DART_CONSTRAINT_JOINTCONSTRAINT_H_ + +#include "dart/constraint/Constraint.h" + +namespace dart { +namespace constraint { + +class JointConstraint : public Constraint +{ +public: + /// \brief Default contructor + JointConstraint(); + + /// \brief Default destructor + virtual ~JointConstraint(); + + /// \brief + bool isActive(); + +protected: +}; + +} // namespace constraint +} // namespace dart + +#endif // DART_CONSTRAINT_CONSTRAINT_H_ + diff --git a/dart/constraint/JointLimitConstraint.cpp b/dart/constraint/JointLimitConstraint.cpp new file mode 100644 index 0000000000000..e473ba119af18 --- /dev/null +++ b/dart/constraint/JointLimitConstraint.cpp @@ -0,0 +1,65 @@ +/* + * Copyright (c) 2014, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Karen Liu , + * Jeongseok Lee + * + * Geoorgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/constraint/JointLimitConstraint.h" + +#include + +namespace dart { +namespace constraint { + +JointLimitConstraint::JointLimitConstraint() + : Constraint(CT_DYNAMIC) +{ + +} + +JointLimitConstraint::~JointLimitConstraint() +{ + +} + +bool JointLimitConstraint::isActive() +{ + std::cout << "JointLimitConstraintTEST::isActive(): Not implemented." + << std::endl; + + return false; +} + +} // namespace constraint +} // namespace dart diff --git a/dart/constraint/JointLimitConstraint.h b/dart/constraint/JointLimitConstraint.h new file mode 100644 index 0000000000000..6b8b12cc1a503 --- /dev/null +++ b/dart/constraint/JointLimitConstraint.h @@ -0,0 +1,76 @@ +/* + * Copyright (c) 2014, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Karen Liu , + * Jeongseok Lee + * + * Geoorgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_CONSTRAINT_JOINTLIMITCONSTRAINT_H_ +#define DART_CONSTRAINT_JOINTLIMITCONSTRAINT_H_ + +#include "dart/constraint/Constraint.h" + +namespace dart { +namespace dynamics { +class Joint; +} // namespace dynamics +} // namespace dart + +namespace dart { +namespace constraint { + +/// \brief JointLimitConstraint handles joint position or velocity limits +class JointLimitConstraint : public Constraint +{ +public: + /// \brief Default constructor + JointLimitConstraint(); + + /// \brief Default destructor + ~JointLimitConstraint(); + + /// \brief + bool isActive(); + +protected: + /// \brief + dynamics::Joint* mJoint; + +private: +}; + +} // namespace constraint +} // namespace dart + +#endif // DART_CONSTRAINT_JOINTLIMITCONSTRAINT_H_ + diff --git a/dart/constraint/RevoluteJointConstraint.cpp b/dart/constraint/RevoluteJointConstraint.cpp new file mode 100644 index 0000000000000..5b2f9cdf36c38 --- /dev/null +++ b/dart/constraint/RevoluteJointConstraint.cpp @@ -0,0 +1,55 @@ +/* + * Copyright (c) 2014, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Karen Liu , + * Jeongseok Lee + * + * Geoorgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/constraint/RevoluteJointConstraint.h" + +namespace dart { +namespace constraint { + +RevoluteJointConstraint::RevoluteJointConstraint() + : JointConstraint() +{ + +} + +RevoluteJointConstraint::~RevoluteJointConstraint() +{ + +} + +} // namespace constraint +} // namespace dart diff --git a/dart/constraint/RevoluteJointConstraint.h b/dart/constraint/RevoluteJointConstraint.h new file mode 100644 index 0000000000000..7711fff026253 --- /dev/null +++ b/dart/constraint/RevoluteJointConstraint.h @@ -0,0 +1,58 @@ +/* + * Copyright (c) 2014, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Karen Liu , + * Jeongseok Lee + * + * Geoorgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_CONSTRAINT_REVOLUTEJOINTCONSTRAINT_H_ +#define DART_CONSTRAINT_REVOLUTEJOINTCONSTRAINT_H_ + +#include "dart/constraint/JointConstraint.h" + +namespace dart { +namespace constraint { + +class RevoluteJointConstraint : public JointConstraint +{ +public: + RevoluteJointConstraint(); + + ~RevoluteJointConstraint(); +}; + +} // namespace constraint +} // namespace dart + +#endif // DART_CONSTRAINT_REVOLUTEJOINTCONSTRAINT_H_ + diff --git a/dart/constraint/WeldJointConstraint.cpp b/dart/constraint/WeldJointConstraint.cpp new file mode 100644 index 0000000000000..0662cfb87e92d --- /dev/null +++ b/dart/constraint/WeldJointConstraint.cpp @@ -0,0 +1,55 @@ +/* + * Copyright (c) 2014, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Karen Liu , + * Jeongseok Lee + * + * Geoorgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/constraint/WeldJointConstraint.h" + +namespace dart { +namespace constraint { + +WeldJointConstraint::WeldJointConstraint() + : JointConstraint() +{ + +} + +WeldJointConstraint::~WeldJointConstraint() +{ + +} + +} // namespace constraint +} // namespace dart diff --git a/dart/constraint/WeldJointConstraint.h b/dart/constraint/WeldJointConstraint.h new file mode 100644 index 0000000000000..73bed11028321 --- /dev/null +++ b/dart/constraint/WeldJointConstraint.h @@ -0,0 +1,58 @@ +/* + * Copyright (c) 2014, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Karen Liu , + * Jeongseok Lee + * + * Geoorgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_CONSTRAINT_WELDJOINTCONSTRAINT_H_ +#define DART_CONSTRAINT_WELDJOINTCONSTRAINT_H_ + +#include "dart/constraint/JointConstraint.h" + +namespace dart { +namespace constraint { + +class WeldJointConstraint : public JointConstraint +{ +public: + WeldJointConstraint(); + + ~WeldJointConstraint(); +}; + +} // namespace constraint +} // namespace dart + +#endif // DART_CONSTRAINT_WELDJOINTCONSTRAINT_H_ + diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index 0a14120738b62..c60a90a5b124f 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/dart/dynamics/BodyNode.cpp @@ -52,7 +52,7 @@ #include "dart/dynamics/Skeleton.h" #include "dart/dynamics/Marker.h" -#define DART_DEFAULT_FRICTION_COEFF 0.4 +#define DART_DEFAULT_FRICTION_COEFF 0.0 namespace dart { namespace dynamics { @@ -94,9 +94,7 @@ BodyNode::BodyNode(const std::string& _name) mIsBodyJacobianDirty(true), mIsBodyJacobianTimeDerivDirty(true), mDelV(Eigen::Vector6d::Zero()), - mImpFext(Eigen::Vector6d::Zero()), mImpB(Eigen::Vector6d::Zero()), - mImpAlpha(Eigen::Vector6d::Zero()), mImpBeta(Eigen::Vector6d::Zero()), mConstraintImpulse(Eigen::Vector6d::Zero()), mImpF(Eigen::Vector6d::Zero()) @@ -442,6 +440,8 @@ void BodyNode::init(Skeleton* _skeleton, int _skeletonIndex) mPsi.setZero(dof, dof); mImplicitPsi.setZero(dof, dof); mAlpha.setZero(dof); + + mImpAlpha.setZero(dof); } void BodyNode::aggregateGenCoords(std::vector* _genCoords) { @@ -979,7 +979,7 @@ bool BodyNode::isImpulseReponsible() const // Should be called at BodyNode::init() // TODO(JS): Once hybrid dynamics is implemented, we should consider joint // type of parent joint. - if (mParentJoint->getNumGenCoords() > 0) + if (mSkeleton->isMobile() && getNumDependentGenCoords() > 0) return true; else return false; @@ -989,7 +989,7 @@ bool BodyNode::isImpulseReponsible() const void BodyNode::updateImpBiasForce() { // Update impulsive bias force - mImpB = -mConstraintImpulse - mImpFext; + mImpB = -mConstraintImpulse; // assert(mImpFext == Eigen::Vector6d::Zero()); for (std::vector::const_iterator it = mChildBodyNodes.begin(); diff --git a/dart/dynamics/BodyNode.h b/dart/dynamics/BodyNode.h index c6f91d79b5eb0..d6e43e6fdb0dc 100644 --- a/dart/dynamics/BodyNode.h +++ b/dart/dynamics/BodyNode.h @@ -416,7 +416,7 @@ class BodyNode /// \brief Add constraint impulse void addConstraintImpulse( const Eigen::Vector3d& _constImp, - const Eigen::Vector3d& _offset = Eigen::Vector3d::Zero(), + const Eigen::Vector3d& _offset, bool _isImpulseLocal = false, bool _isOffsetLocal = true); @@ -802,9 +802,6 @@ class BodyNode /// bodies of the parent skeleton. Eigen::Vector6d mDelV; - /// \brief Impulsive external force - Eigen::Vector6d mImpFext; - /// \brief Impulsive bias force due to external impulsive force exerted on /// bodies of the parent skeleton. Eigen::Vector6d mImpB; diff --git a/dart/dynamics/EulerJoint.h b/dart/dynamics/EulerJoint.h index e11dfc63600f2..13928bd8fd050 100644 --- a/dart/dynamics/EulerJoint.h +++ b/dart/dynamics/EulerJoint.h @@ -68,6 +68,8 @@ class EulerJoint : public Joint { /// \brief AxisOrder getAxisOrder() const; + Eigen::Isometry3d getTransform(size_t _index) const; + // Documentation inherited. virtual void updateTransform(); diff --git a/dart/dynamics/Joint.h b/dart/dynamics/Joint.h index 870e20797ae7b..c00fd61e154cf 100644 --- a/dart/dynamics/Joint.h +++ b/dart/dynamics/Joint.h @@ -177,6 +177,9 @@ class Joint : public GenCoordSystem bool _updateAccs); //---------------------------------------------------------------------------- +// void updateVel + + //---------------------------------------------------------------------------- /// \brief Get potential energy. double getPotentialEnergy() const; diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index bd956a2564621..d23a3e4d3d30b 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -818,8 +818,7 @@ void Skeleton::clearImpulseTest() it != mBodyNodes.end(); ++it) { (*it)->mImpB.setZero(); - (*it)->mImpFext.setZero(); -// (*it)->mConstImp.setZero(); + (*it)->mConstraintImpulse.setZero(); } // Clear velocity change @@ -828,17 +827,26 @@ void Skeleton::clearImpulseTest() } //============================================================================== -void Skeleton::updateImpBiasForce(BodyNode* _bodyNode, +void Skeleton::updateBiasImpulse(BodyNode* _bodyNode, const Eigen::Vector6d& _imp) { - assert(0); - // Assertions assert(_bodyNode != NULL); assert(getNumGenCoords() > 0); + // This skeleton should contains _bodyNode + assert(std::find(mBodyNodes.begin(), mBodyNodes.end(), _bodyNode) + != mBodyNodes.end()); + +#ifndef NDEBUG + // All the constraint impulse should be zero + for (int i = 0; i < mBodyNodes.size(); ++i) + assert(mBodyNodes[i]->mConstraintImpulse == Eigen::Vector6d::Zero()); +#endif + // Set impulse to _bodyNode - _bodyNode->mImpFext = _imp; + Eigen::Vector6d oldConstraintImpulse =_bodyNode->mConstraintImpulse; + _bodyNode->mConstraintImpulse = _imp; // Prepare cache data BodyNode* it = _bodyNode; @@ -849,7 +857,7 @@ void Skeleton::updateImpBiasForce(BodyNode* _bodyNode, } // TODO(JS): Do we need to backup and restore the original value? - _bodyNode->mImpFext.setZero(); + _bodyNode->mConstraintImpulse = oldConstraintImpulse; } //============================================================================== @@ -915,6 +923,31 @@ void Skeleton::computeImpulseForwardDynamics() //DEBUG_CODE////////////////////////////////////////////////////////////////// // dtdbg << "Velocity change: " << getVelsChange().transpose() << std::endl; ////////////////////////////////////////////////////////////////////////////// + + + // 1. dq = dq + del_dq + // 2. ddq = ddq + del_dq / dt + // 3. tau = tau + imp / dt + +// dtdbg << "getGenVelsgetGenVels: " << getGenVels().transpose() << std::endl; + + GenCoordSystem::setGenVels(GenCoordSystem::getGenVels() + + GenCoordSystem::getVelsChange()); + +// dtdbg << "getGenVelsgetGenVels: " << getGenVels().transpose() << std::endl; + +// GenCoordSystem::setGenAccs(GenCoordSystem::getGenAccs() +// + GenCoordSystem::getVelsChange() / mTimeStep); + +// GenCoordSystem::setGenForces( +// GenCoordSystem::getGenForces() +// + GenCoordSystem::getConstraintImpulses() / mTimeStep); + + // 4. F = F + impF / dt + + // Integration +// integrateConfigs(mTimeStep); +// computeForwardKinematics(false, true, false); } void Skeleton::setInternalForceVector(const Eigen::VectorXd& _forces) { diff --git a/dart/dynamics/Skeleton.h b/dart/dynamics/Skeleton.h index b153d8085b0f4..4a3b9f2fcbc5e 100644 --- a/dart/dynamics/Skeleton.h +++ b/dart/dynamics/Skeleton.h @@ -239,12 +239,10 @@ class Skeleton : public GenCoordSystem // Impulse-based dynamics //---------------------------------------------------------------------------- /// \brief Clear velocity change and external impulses - void clearImpulseTest(); + virtual void clearImpulseTest(); - // TODO(JS): Not implemented yet - /// \brief Compute changes in generalized coordinate velocities due to - /// impulse, _imp, exerted on a body node, _bodyNode - void updateImpBiasForce(BodyNode* _bodyNode, const Eigen::Vector6d& _imp); + /// \brief Update bias impulses due to impulse[_imp] on body node [_bodyNode] + void updateBiasImpulse(BodyNode* _bodyNode, const Eigen::Vector6d& _imp); /// \brief Update velocity changes in body nodes and joints due to applied /// impulse @@ -335,7 +333,7 @@ class Skeleton : public GenCoordSystem //---------------------------------------------------------------------------- /// \brief Clear constraint impulses: (a) spatial constraints on BodyNode and /// (b) generalized constraints on Joint - void clearConstraintImpulses(); + virtual void clearConstraintImpulses(); // TODO(JS): To be deprecated /// \brief Set constraint force vector. diff --git a/dart/gui/SimWindow.cpp b/dart/gui/SimWindow.cpp index 3ef3647bf492d..cfd7309e3f525 100644 --- a/dart/gui/SimWindow.cpp +++ b/dart/gui/SimWindow.cpp @@ -47,6 +47,7 @@ #include "dart/simulation/World.h" #include "dart/dynamics/Skeleton.h" #include "dart/constraint/OldConstraintDynamics.h" +#include "dart/constraint/ConstraintSolver.h" #include "dart/collision/CollisionDetector.h" #include "dart/gui/GLFuncs.h" @@ -131,7 +132,7 @@ void SimWindow::draw() { } else { if (mShowMarkers) { collision::CollisionDetector* cd = - mWorld->getConstraintHandler()->getCollisionDetector(); + mWorld->getConstraintSolver()->getCollisionDetector(); for (int k = 0; k < cd->getNumContacts(); k++) { Eigen::Vector3d v = cd->getContact(k).point; Eigen::Vector3d f = cd->getContact(k).force / 10.0; @@ -216,7 +217,7 @@ void SimWindow::setWorld(simulation::World* _world) { void SimWindow::bake() { collision::CollisionDetector* cd = - mWorld->getConstraintHandler()->getCollisionDetector(); + mWorld->getConstraintSolver()->getCollisionDetector(); int nContacts = cd->getNumContacts(); int nSkeletons = mWorld->getNumSkeletons(); Eigen::VectorXd state(mWorld->getIndex(nSkeletons) + 6 * nContacts); diff --git a/dart/integration/Integrator.h b/dart/integration/Integrator.h index 4e05a2a060589..8f341a3816b31 100644 --- a/dart/integration/Integrator.h +++ b/dart/integration/Integrator.h @@ -94,7 +94,13 @@ class Integrator public: /// \brief Integrate the system with time step dt - virtual void integrate(IntegrableSystem* system, double dt) = 0; + virtual void integrate(IntegrableSystem* _system, double _dt) = 0; + + /// \brief Integrate velocity of the system with time step dt + virtual void integratePos(IntegrableSystem* _system, double _dt) {} + + /// \brief Integrate velocity of the system with time step dt + virtual void integrateVel(IntegrableSystem* _system, double _dt) {} }; } // namespace integration diff --git a/dart/integration/SemiImplicitEulerIntegrator.cpp b/dart/integration/SemiImplicitEulerIntegrator.cpp index 52aec4d7ae81f..d422a842edad8 100644 --- a/dart/integration/SemiImplicitEulerIntegrator.cpp +++ b/dart/integration/SemiImplicitEulerIntegrator.cpp @@ -59,5 +59,19 @@ void SemiImplicitEulerIntegrator::integrate(IntegrableSystem* _system, _system->integrateConfigs(_system->getGenVels(), _dt); } +//============================================================================== +void SemiImplicitEulerIntegrator::integratePos(IntegrableSystem* _system, + double _dt) +{ + _system->integrateConfigs(_system->getGenVels(), _dt); +} + +//============================================================================== +void SemiImplicitEulerIntegrator::integrateVel(IntegrableSystem* _system, + double _dt) +{ + _system->integrateGenVels(_system->evalGenAccs(), _dt); +} + } // namespace integration } // namespace dart diff --git a/dart/integration/SemiImplicitEulerIntegrator.h b/dart/integration/SemiImplicitEulerIntegrator.h index 1e99b255806ed..28e506ba950d2 100644 --- a/dart/integration/SemiImplicitEulerIntegrator.h +++ b/dart/integration/SemiImplicitEulerIntegrator.h @@ -54,6 +54,12 @@ class SemiImplicitEulerIntegrator : public Integrator // Documentation inherited virtual void integrate(IntegrableSystem* _system, double _dt); + + // Documentation inherited + virtual void integratePos(IntegrableSystem* _system, double _dt); + + // Documentation inherited + virtual void integrateVel(IntegrableSystem* _system, double _dt); }; } // namespace integration diff --git a/dart/simulation/World.cpp b/dart/simulation/World.cpp index 780543091e6c5..09cd154981133 100644 --- a/dart/simulation/World.cpp +++ b/dart/simulation/World.cpp @@ -50,10 +50,12 @@ #include "dart/dynamics/GenCoord.h" #include "dart/dynamics/Skeleton.h" #include "dart/constraint/OldConstraintDynamics.h" +#include "dart/constraint/ConstraintSolver.h" namespace dart { namespace simulation { +//============================================================================== World::World() : integration::IntegrableSystem(), mGravity(0.0, 0.0, -9.81), @@ -62,17 +64,23 @@ World::World() mFrame(0), mIntegrator(new integration::SemiImplicitEulerIntegrator()), mConstraintHandler( - new constraint::OldConstraintDynamics(mSkeletons, mTimeStep)) { + new constraint::OldConstraintDynamics(mSkeletons, mTimeStep)), + mConstraintSolver(new constraint::ConstraintSolver(mSkeletons, mTimeStep)) +{ mIndices.push_back(0); } -World::~World() { +//============================================================================== +World::~World() +{ delete mIntegrator; delete mConstraintHandler; for (std::vector::const_iterator it = mSkeletons.begin(); it != mSkeletons.end(); ++it) + { delete (*it); + } } //============================================================================== @@ -146,6 +154,13 @@ Eigen::VectorXd World::getGenVels() const //============================================================================== Eigen::VectorXd World::evalGenAccs() +{ +// return _evalGenAccsOld(); + return _evalGenAccsNew(); +} + +//============================================================================== +Eigen::VectorXd World::_evalGenAccsOld() { // compute constraint (contact/contact, joint limit) forces mConstraintHandler->computeConstraintForces(); @@ -186,6 +201,62 @@ Eigen::VectorXd World::evalGenAccs() return genAccs; } +//============================================================================== +Eigen::VectorXd World::_evalGenAccsNew() +{ + // Compute unconstrained acceleration. + for (std::vector::iterator it = mSkeletons.begin(); + it != mSkeletons.end(); ++it) + { + // Transmitted body force doesn't need to be computed here since it will be + // computed at below. + (*it)->computeForwardDynamics(); + + // TODO(JS): Just do Euler integration for test +// (*it)->integrateConfigs(mTimeStep); +// (*it)->integrateGenVels(mTimeStep); + } + +// // compute constraint (contact/contact, joint limit) forces +// mConstraintHandler->computeConstraintForces(); + +// // set constraint force +// for (int i = 0; i < getNumSkeletons(); i++) +// { +// // skip immobile objects in forward simulation +// if (!mSkeletons[i]->isMobile() || mSkeletons[i]->getNumGenCoords() == 0) +// continue; + +// mSkeletons[i]->setConstraintForceVector( +// mConstraintHandler->getTotalConstraintForce(i) - +// mConstraintHandler->getContactForce(i)); +// } + +// // compute forward dynamics +// for (std::vector::iterator it = mSkeletons.begin(); +// it != mSkeletons.end(); ++it) +// { +// (*it)->computeForwardDynamics(); +// } + + // compute derivatives for integration + Eigen::VectorXd genAccs = Eigen::VectorXd::Zero(mIndices.back()); + for (unsigned int i = 0; i < getNumSkeletons(); i++) + { + // skip immobile objects in forward simulation + if (!mSkeletons[i]->isMobile() || mSkeletons[i]->getNumGenCoords() == 0) + continue; + + int start = mIndices[i]; + int size = getSkeleton(i)->getNumGenCoords(); + + // set accelerations + genAccs.segment(start, size) = getSkeleton(i)->getGenAccs(); + } + + return genAccs; +} + //============================================================================== void World::integrateConfigs(const Eigen::VectorXd& _genVels, double _dt) { @@ -233,13 +304,38 @@ double World::getTimeStep() const { return mTimeStep; } -void World::step() { - mIntegrator->integrate(this, mTimeStep); +//============================================================================== +void World::step() +{ + // Integrate velocity unconstrained skeletons + mIntegrator->integrateVel(this, mTimeStep); + + // Detect active constraints and compute constraint impulses + mConstraintSolver->solve(); + + // Integrate skeletons with constraint impulses +// mIntegrator->integrate(this, mTimeStep); + + // Compute velocity changes given constraint impulses + for (std::vector::iterator it = mSkeletons.begin(); + it != mSkeletons.end(); ++it) + { + if ((*it)->isImpulseApplied()) + { + (*it)->computeImpulseForwardDynamics(); +// (*it)->updateForwardKinematicsWithVelocityChanges(); + (*it)->setImpulseApplied(false); + } + } + + mIntegrator->integratePos(this, mTimeStep); for (std::vector::iterator itr = mSkeletons.begin(); - itr != mSkeletons.end(); ++itr) { + itr != mSkeletons.end(); ++itr) + { (*itr)->clearInternalForces(); (*itr)->clearExternalForces(); + (*itr)->clearConstraintImpulses(); } mTime += mTimeStep; @@ -288,12 +384,14 @@ int World::getNumSkeletons() const { return mSkeletons.size(); } -void World::addSkeleton(dynamics::Skeleton* _skeleton) { +//============================================================================== +void World::addSkeleton(dynamics::Skeleton* _skeleton) +{ assert(_skeleton != NULL && "Invalid skeleton."); // If mSkeletons already has _skeleton, then we do nothing. - if (find(mSkeletons.begin(), mSkeletons.end(), _skeleton) != - mSkeletons.end()) { + if (find(mSkeletons.begin(), mSkeletons.end(), _skeleton) != mSkeletons.end()) + { std::cout << "Skeleton [" << _skeleton->getName() << "] is already in the world." << std::endl; return; @@ -302,7 +400,8 @@ void World::addSkeleton(dynamics::Skeleton* _skeleton) { mSkeletons.push_back(_skeleton); _skeleton->init(mTimeStep, mGravity); mIndices.push_back(mIndices.back() + _skeleton->getNumGenCoords()); - mConstraintHandler->addSkeleton(_skeleton); +// mConstraintHandler->addSkeleton(_skeleton); + mConstraintSolver->addSkeleton(_skeleton); } void World::removeSkeleton(dynamics::Skeleton* _skeleton) { @@ -350,9 +449,16 @@ bool World::checkCollision(bool _checkAllCollisions) { _checkAllCollisions, false); } -constraint::OldConstraintDynamics* World::getConstraintHandler() const { +constraint::OldConstraintDynamics* World::getConstraintHandler() const +{ return mConstraintHandler; } +//============================================================================== +constraint::ConstraintSolver* World::getConstraintSolver() const +{ + return mConstraintSolver; +} + } // namespace simulation } // namespace dart diff --git a/dart/simulation/World.h b/dart/simulation/World.h index a5b495d33ec9b..eac7802d5b52e 100644 --- a/dart/simulation/World.h +++ b/dart/simulation/World.h @@ -60,6 +60,7 @@ class Skeleton; } // namespace dynamics namespace constraint { class OldConstraintDynamics; +class ConstraintSolver; } // namespace constraint } // namespace dart @@ -96,6 +97,8 @@ class World : public integration::IntegrableSystem // Documentation inherited virtual Eigen::VectorXd evalGenAccs(); + virtual Eigen::VectorXd _evalGenAccsOld(); + virtual Eigen::VectorXd _evalGenAccsNew(); // Documentation inherited virtual void integrateConfigs(const Eigen::VectorXd& _genVels, double _dt); @@ -169,9 +172,13 @@ class World : public integration::IntegrableSystem //-------------------------------------------------------------------------- // Constraint //-------------------------------------------------------------------------- - /// \brief Get the constraint handler + // TODO(JS): To be deprecated (DART 4.0) + /// \brief Get the constraint solver constraint::OldConstraintDynamics* getConstraintHandler() const; + /// \brief Get the constraint solver + constraint::ConstraintSolver* getConstraintSolver() const; + protected: /// \brief Skeletones in this world std::vector mSkeletons; @@ -200,6 +207,8 @@ class World : public integration::IntegrableSystem /// \brief The constraint handler constraint::OldConstraintDynamics* mConstraintHandler; + /// \brief Constraint solver + constraint::ConstraintSolver* mConstraintSolver; }; } // namespace simulation diff --git a/dart/utils/SkelParser.cpp b/dart/utils/SkelParser.cpp index 5ef23de1849cb..0b4ad465ec713 100644 --- a/dart/utils/SkelParser.cpp +++ b/dart/utils/SkelParser.cpp @@ -49,6 +49,7 @@ #include "dart/collision/fcl/FCLCollisionDetector.h" #include "dart/collision/fcl_mesh/FCLMeshCollisionDetector.h" #include "dart/constraint/OldConstraintDynamics.h" +#include "dart/constraint/ConstraintSolver.h" #include "dart/dynamics/BodyNode.h" #include "dart/dynamics/BoxShape.h" #include "dart/dynamics/CylinderShape.h" @@ -135,17 +136,17 @@ simulation::World* SkelParser::readWorld(tinyxml2::XMLElement* _worldElement) { if (hasElement(physicsElement, "collision_detector")) { std::string strCD = getValueString(physicsElement, "collision_detector"); if (strCD == "fcl_mesh") { - newWorld->getConstraintHandler()->setCollisionDetector( + newWorld->getConstraintSolver()->setCollisionDetector( new collision::FCLMeshCollisionDetector()); } else if (strCD == "fcl") { - newWorld->getConstraintHandler()->setCollisionDetector( + newWorld->getConstraintSolver()->setCollisionDetector( new collision::FCLCollisionDetector()); } else if (strCD == "dart") { - newWorld->getConstraintHandler()->setCollisionDetector( + newWorld->getConstraintSolver()->setCollisionDetector( new collision::DARTCollisionDetector()); #ifdef HAVE_BULLET_COLLISION } else if (strCD == "bullet") { - newWorld->getConstraintHandler()->setCollisionDetector( + newWorld->getConstraintSolver()->setCollisionDetector( new collision::BulletCollisionDetector()); #endif } else { diff --git a/dart/utils/SoftParser.cpp b/dart/utils/SoftParser.cpp index a26af2a980742..ed4d76d5688f1 100644 --- a/dart/utils/SoftParser.cpp +++ b/dart/utils/SoftParser.cpp @@ -49,6 +49,7 @@ #include "dart/collision/fcl_mesh/SoftFCLMeshCollisionDetector.h" #include "dart/collision/fcl_mesh/FCLMeshCollisionDetector.h" #include "dart/constraint/OldConstraintDynamics.h" +#include "dart/constraint/ConstraintSolver.h" #include "dart/dynamics/BoxShape.h" #include "dart/dynamics/CylinderShape.h" #include "dart/dynamics/EllipsoidShape.h" @@ -154,17 +155,17 @@ simulation::SoftWorld* SoftSkelParser::readSoftWorld( std::string strCD = getValueString(physicsElement, "collision_detector"); if (strCD == "fcl_mesh") { - newSoftWorld->getConstraintHandler()->setCollisionDetector( + newSoftWorld->getConstraintSolver()->setCollisionDetector( new collision::SoftFCLMeshCollisionDetector()); } else if (strCD == "fcl") { - newSoftWorld->getConstraintHandler()->setCollisionDetector( + newSoftWorld->getConstraintSolver()->setCollisionDetector( new collision::FCLCollisionDetector()); } else if (strCD == "dart") { - newSoftWorld->getConstraintHandler()->setCollisionDetector( + newSoftWorld->getConstraintSolver()->setCollisionDetector( new collision::DARTCollisionDetector()); } else @@ -176,7 +177,7 @@ simulation::SoftWorld* SoftSkelParser::readSoftWorld( } else { - newSoftWorld->getConstraintHandler()->setCollisionDetector( + newSoftWorld->getConstraintSolver()->setCollisionDetector( new collision::SoftFCLMeshCollisionDetector()); } } diff --git a/data/skel/cube.skel b/data/skel/cube.skel new file mode 100644 index 0000000000000..ab7651aad3307 --- /dev/null +++ b/data/skel/cube.skel @@ -0,0 +1,71 @@ + + + + + 0.001 + 0 -9.81 0 + fcl_mesh + + + + + 0 -0.375 0 0 0 0 + + 0 0 0 0 0 0 + + + 2.5 0.05 2.5 + + + 0.95 0.95 0.95 + + + 0 0 0 0 0 0 + + + 2.5 0.05 2.5 + + + + + + world + ground + + + + + + 1 + 0 -0.025 0 1 2 3 + + 1 + 0 0 0 + + + 0 0 0 0 0 0 + + + 0.1 0.05 0.1 + + + 0.8 0.3 0.3 + + + 0 0 0 0 0 0 + + + 0.1 0.05 0.1 + + + + + + + world + box + + + + +