From 29769639df057ef30bc812df8fbc3d9675de64a5 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sun, 24 Apr 2016 11:46:37 -0400 Subject: [PATCH 1/9] Add `std::` to size_t and find() --- dart/constraint/BalanceConstraint.cpp | 4 ++-- dart/dynamics/BodyNode.cpp | 14 +++++++------- dart/dynamics/InverseKinematics.cpp | 16 ++++++++-------- dart/dynamics/InverseKinematics.h | 2 +- unittests/testForwardKinematics.cpp | 4 ++-- 5 files changed, 20 insertions(+), 20 deletions(-) diff --git a/dart/constraint/BalanceConstraint.cpp b/dart/constraint/BalanceConstraint.cpp index 382d35ae712d7..ae340c611e372 100644 --- a/dart/constraint/BalanceConstraint.cpp +++ b/dart/constraint/BalanceConstraint.cpp @@ -411,11 +411,11 @@ void BalanceConstraint::convertJacobianMethodOutputToGradient( mInitialPositionsCache = skel->getPositions(); - for(size_t i=0; i < skel->getNumJoints(); ++i) + for(std::size_t i=0; i < skel->getNumJoints(); ++i) skel->getJoint(i)->integratePositions(1.0); // Clear out the velocities so we don't interfere with other Jacobian methods - for(size_t i=0; i < skel->getNumDofs(); ++i) + for(std::size_t i=0; i < skel->getNumDofs(); ++i) skel->setVelocity(i, 0.0); grad = skel->getPositions(); diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index aa4442ec6fba7..f77b7c442d725 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/dart/dynamics/BodyNode.cpp @@ -1440,8 +1440,8 @@ void BodyNode::processNewEntity(Entity* _newChildEntity) // Check if it's a child BodyNode (if not, then it's just some other arbitrary // type of Entity) - if(find(mChildBodyNodes.begin(), mChildBodyNodes.end(), _newChildEntity) != - mChildBodyNodes.end()) + if(std::find(mChildBodyNodes.begin(), mChildBodyNodes.end(), + _newChildEntity) != mChildBodyNodes.end()) return; // Check if it's already accounted for in our Non-BodyNode Entities @@ -1460,17 +1460,17 @@ void BodyNode::processNewEntity(Entity* _newChildEntity) //============================================================================== void BodyNode::processRemovedEntity(Entity* _oldChildEntity) { - std::vector::iterator it = find(mChildBodyNodes.begin(), - mChildBodyNodes.end(), - _oldChildEntity); + std::vector::iterator it = std::find(mChildBodyNodes.begin(), + mChildBodyNodes.end(), + _oldChildEntity); if(it != mChildBodyNodes.end()) mChildBodyNodes.erase(it); if(JacobianNode* node = dynamic_cast(_oldChildEntity)) mChildJacobianNodes.erase(node); - if(find(mNonBodyNodeEntities.begin(), mNonBodyNodeEntities.end(), - _oldChildEntity) != mNonBodyNodeEntities.end()) + if(std::find(mNonBodyNodeEntities.begin(), mNonBodyNodeEntities.end(), + _oldChildEntity) != mNonBodyNodeEntities.end()) mNonBodyNodeEntities.erase(_oldChildEntity); } diff --git a/dart/dynamics/InverseKinematics.cpp b/dart/dynamics/InverseKinematics.cpp index 1bb65bf7454fa..b6f64df013d9f 100644 --- a/dart/dynamics/InverseKinematics.cpp +++ b/dart/dynamics/InverseKinematics.cpp @@ -97,7 +97,7 @@ bool InverseKinematics::solve(bool _applySolution) // need to clear out any velocities that might be in the Skeleton and then // reset those velocities later. Eigen::VectorXd originalVelocities = skel->getVelocities(); - for(size_t i=0; i < skel->getNumDofs(); ++i) + for(std::size_t i=0; i < skel->getNumDofs(); ++i) skel->getDof(i)->setVelocity(0.0); if(_applySolution) @@ -709,23 +709,23 @@ InverseKinematics::GradientMethod::getComponentWeights() const //============================================================================== void InverseKinematics::GradientMethod::convertJacobianMethodOutputToGradient( - Eigen::VectorXd& grad, const std::vector& dofs) + Eigen::VectorXd& grad, const std::vector& dofs) { const SkeletonPtr& skel = mIK->getNode()->getSkeleton(); mInitialPositionsCache = skel->getPositions(dofs); - for(size_t i=0; i < dofs.size(); ++i) + for(std::size_t i=0; i < dofs.size(); ++i) skel->getDof(dofs[i])->setVelocity(grad[i]); // Velocities of unused DOFs should already be set to zero. - for(size_t i=0; i < dofs.size(); ++i) + for(std::size_t i=0; i < dofs.size(); ++i) { Joint* joint = skel->getDof(dofs[i])->getJoint(); joint->integratePositions(1.0); // Reset this joint's velocities to zero to avoid double-integrating - const size_t numJointDofs = joint->getNumDofs(); - for(size_t j=0; j < numJointDofs; ++j) + const std::size_t numJointDofs = joint->getNumDofs(); + for(std::size_t j=0; j < numJointDofs; ++j) joint->setVelocity(j, 0.0); } @@ -1049,9 +1049,9 @@ void InverseKinematics::Analytical::addExtraDofGradient( convertJacobianMethodOutputToGradient(mExtraDofGradCache, mExtraDofs); - for(size_t i=0; i < mExtraDofs.size(); ++i) + for(std::size_t i=0; i < mExtraDofs.size(); ++i) { - size_t depIndex = mExtraDofs[i]; + std::size_t depIndex = mExtraDofs[i]; int gradIndex = gradMap[depIndex]; if(gradIndex == -1) continue; diff --git a/dart/dynamics/InverseKinematics.h b/dart/dynamics/InverseKinematics.h index 5e47ae1c6437a..ed05e7a74e8fb 100644 --- a/dart/dynamics/InverseKinematics.h +++ b/dart/dynamics/InverseKinematics.h @@ -508,7 +508,7 @@ class InverseKinematics : public common::Subject /// through this function before returning it will make the gradient /// suitable for a standard solver. void convertJacobianMethodOutputToGradient( - Eigen::VectorXd& grad, const std::vector& dofs); + Eigen::VectorXd& grad, const std::vector& dofs); /// Get the Properties of this GradientMethod Properties getGradientMethodProperties() const; diff --git a/unittests/testForwardKinematics.cpp b/unittests/testForwardKinematics.cpp index 698508b80f8ab..32029b7f26259 100644 --- a/unittests/testForwardKinematics.cpp +++ b/unittests/testForwardKinematics.cpp @@ -233,8 +233,8 @@ TEST(FORWARD_KINEMATICS, JACOBIAN_END_EFFECTOR_CHANGE) BodyNode* last_bn2 = skeleton2->getBodyNode(skeleton2->getNumBodyNodes()-1); EndEffector* ee2 = last_bn2->createEndEffector(); - std::vector active_indices; - for(size_t i=0; i < 3; ++i) + std::vector active_indices; + for(std::size_t i=0; i < 3; ++i) active_indices.push_back(i); Eigen::VectorXd q = Eigen::VectorXd::Random(active_indices.size()); From a1c10331a56e1683932a99b8afe3e2391991875d Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sun, 24 Apr 2016 16:00:03 -0400 Subject: [PATCH 2/9] Fix BULLET_INCLUDE_DIRS in DARTConfig.cmake The fix from #361 doesn't work if bullet packages was failed to be found by `find_package()`. When bullet is not found, `BULLET_INCLUDE_DIRS` will be set to `BULLET_INCLUDE_DIRS-NOT-FOUND` rather than blank as we assumed in #361. Instead, we set `DART_INCLUDE_DIRS` in the top level CMakeList.txt by adding `BULLET_INCLUDE_DIRS` to `DART_INCLUDE_DIRS` only when bullet is found. `DARTConfig.cmake` is then configured using `DART_INCLUDE_DIRS`. --- CMakeLists.txt | 7 +++++++ cmake/DARTConfig.cmake.in | 5 +---- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c75132bb51821..5f5b1861340f7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -123,6 +123,13 @@ if(HAVE_BULLET_COLLISION) endif() include_directories("${CMAKE_BINARY_DIR}") +set(DART_INCLUDE_DIRS + "${CMAKE_INSTALL_PREFIX}/include" + "${EIGEN3_INCLUDE_DIRS}") +if(HAVE_BULLET_COLLISION) + list(APPEND DART_INCLUDE_DIRS "${BULLET_INCLUDE_DIRS}") +endif() + #=============================================================================== # Check for non-case-sensitive filesystems #=============================================================================== diff --git a/cmake/DARTConfig.cmake.in b/cmake/DARTConfig.cmake.in index 20b1298de7c42..55745082188fd 100644 --- a/cmake/DARTConfig.cmake.in +++ b/cmake/DARTConfig.cmake.in @@ -9,10 +9,7 @@ #=============================================================================== # DART_INCLUDE_DIRS #=============================================================================== -set(DART_INCLUDE_DIRS - "@CMAKE_INSTALL_PREFIX@/include" - "@EIGEN3_INCLUDE_DIRS@" - "@BULLET_INCLUDE_DIRS@") +set(DART_INCLUDE_DIRS "@DART_INCLUDE_DIRS@") #=============================================================================== # DART_LIBRARIES From e6b8ff93158da120939ab5c68678a9a5f6baa89d Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Mon, 25 Apr 2016 12:51:50 -0400 Subject: [PATCH 3/9] Made changes based on Reviewable comments --- dart/dynamics/InverseKinematics.cpp | 12 +- dart/dynamics/InverseKinematics.h | 1680 ++++++++++++++------------- unittests/testForwardKinematics.cpp | 2 +- 3 files changed, 860 insertions(+), 834 deletions(-) diff --git a/dart/dynamics/InverseKinematics.cpp b/dart/dynamics/InverseKinematics.cpp index b6f64df013d9f..d2679e3840649 100644 --- a/dart/dynamics/InverseKinematics.cpp +++ b/dart/dynamics/InverseKinematics.cpp @@ -95,7 +95,7 @@ bool InverseKinematics::solve(bool _applySolution) // Many GradientMethod implementations use Joint::integratePositions, so we // need to clear out any velocities that might be in the Skeleton and then - // reset those velocities later. + // reset those velocities later. This has been opened as issue #699. Eigen::VectorXd originalVelocities = skel->getVelocities(); for(std::size_t i=0; i < skel->getNumDofs(); ++i) skel->getDof(i)->setVelocity(0.0); @@ -866,7 +866,7 @@ InverseKinematics::Analytical::Solution::Solution( //============================================================================== InverseKinematics::Analytical::UniqueProperties::UniqueProperties( - ExtraDofUtilization_t extraDofUtilization, double extraErrorLengthClamp) + ExtraDofUtilization extraDofUtilization, double extraErrorLengthClamp) : mExtraDofUtilization(extraDofUtilization), mExtraErrorLengthClamp(extraErrorLengthClamp) { @@ -875,7 +875,7 @@ InverseKinematics::Analytical::UniqueProperties::UniqueProperties( //============================================================================== InverseKinematics::Analytical::UniqueProperties::UniqueProperties( - ExtraDofUtilization_t extraDofUtilization, + ExtraDofUtilization extraDofUtilization, double extraErrorLengthClamp, QualityComparison qualityComparator) : mExtraDofUtilization(extraDofUtilization), @@ -1031,7 +1031,7 @@ const std::vector& IK::Analytical::getSolutions( void InverseKinematics::Analytical::addExtraDofGradient( Eigen::VectorXd& grad, const Eigen::Vector6d& error, - ExtraDofUtilization_t /*utilization*/) + ExtraDofUtilization /*utilization*/) { mExtraDofGradCache.resize(mExtraDofs.size()); const math::Jacobian& J = mIK->computeJacobian(); @@ -1160,13 +1160,13 @@ Eigen::VectorXd InverseKinematics::Analytical::getPositions() const //============================================================================== void InverseKinematics::Analytical::setExtraDofUtilization( - ExtraDofUtilization_t _utilization) + ExtraDofUtilization _utilization) { mAnalyticalP.mExtraDofUtilization = _utilization; } //============================================================================== -IK::Analytical::ExtraDofUtilization_t +IK::Analytical::ExtraDofUtilization IK::Analytical::getExtraDofUtilization() const { return mAnalyticalP.mExtraDofUtilization; diff --git a/dart/dynamics/InverseKinematics.h b/dart/dynamics/InverseKinematics.h index ed05e7a74e8fb..bd2f47307bb10 100644 --- a/dart/dynamics/InverseKinematics.h +++ b/dart/dynamics/InverseKinematics.h @@ -146,758 +146,18 @@ class InverseKinematics : public common::Subject /// by pointer instead of being cloned. InverseKinematicsPtr clone(JacobianNode* _newNode) const; - /// This class should be inherited by optimizer::Function classes that have a - /// dependency on the InverseKinematics module that they belong to. If you - /// pass an InverseKinematics::Function into the Problem of an - /// InverseKinematics module, then it will be properly cloned whenever the - /// InverseKinematics module that it belongs to gets cloned. Any Function - /// classes in the Problem that do not inherit InverseKinematics::Function - /// will just be copied over by reference. - class Function - { - public: - - /// Enable this function to be cloned to a new IK module. - virtual optimizer::FunctionPtr clone(InverseKinematics* _newIK) const = 0; - - /// Virtual destructor - virtual ~Function() = default; - }; - - /// ErrorMethod is a base class for different ways of computing the error of - /// an InverseKinematics module. - class ErrorMethod : public common::Subject - { - public: - - typedef std::pair Bounds; - - /// The Properties struct contains settings that are commonly used by - /// methods that compute error for inverse kinematics. - struct Properties - { - /// Default constructor - Properties(const Bounds& _bounds = - Bounds(Eigen::Vector6d::Constant(-DefaultIKTolerance), - Eigen::Vector6d::Constant( DefaultIKTolerance)), - - double _errorClamp = DefaultIKErrorClamp, - - const Eigen::Vector6d& _errorWeights = Eigen::compose( - Eigen::Vector3d::Constant(DefaultIKAngularWeight), - Eigen::Vector3d::Constant(DefaultIKLinearWeight))); - - /// Bounds that define the acceptable range of the Node's transform - /// relative to its target frame. - std::pair mBounds; - - /// The error vector will be clamped to this length with each iteration. - /// This is used to enforce sane behavior, even when there are extremely - /// large error vectors. - double mErrorLengthClamp; - - /// These weights will be applied to the error vector component-wise. This - /// allows you to set some components of error as more important than - /// others, or to scale their coordinate spaces. For example, you will - /// often want the first three components (orientation error) to have - /// smaller weights than the last three components (translation error). - Eigen::Vector6d mErrorWeights; - - // To get byte-aligned Eigen vectors - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; - - /// Constructor - ErrorMethod(InverseKinematics* _ik, - const std::string& _methodName, - const Properties& _properties = Properties()); - - /// Virtual destructor - virtual ~ErrorMethod() = default; - - /// Enable this ErrorMethod to be cloned to a new IK module. - virtual std::unique_ptr clone( - InverseKinematics* _newIK) const = 0; - - /// Override this function with your implementation of the error vector - /// computation. The expectation is that the first three components of the - /// vector will correspond to orientation error (in an angle-axis format) - /// while the last three components correspond to translational error. - /// - /// When implementing this function, you should assume that the Skeleton's - /// current joint positions corresponds to the positions that you - /// must use to compute the error. This function will only get called when - /// an update is needed. - virtual Eigen::Vector6d computeError() = 0; - - /// Override this function with your implementation of computing the desired - /// given the current transform and error vector. If you want the desired - /// transform to always be equal to the Target's transform, you can simply - /// call ErrorMethod::computeDesiredTransform to implement this function. - virtual Eigen::Isometry3d computeDesiredTransform( - const Eigen::Isometry3d& _currentTf, const Eigen::Vector6d& _error) = 0; - - /// This function is used to handle caching the error vector. - const Eigen::Vector6d& evalError(const Eigen::VectorXd& _q); - - /// Get the name of this ErrorMethod. - const std::string& getMethodName() const; - - /// Set all the error bounds. - void setBounds( - const Eigen::Vector6d& _lower = - Eigen::Vector6d::Constant(-DefaultIKTolerance), - const Eigen::Vector6d& _upper = - Eigen::Vector6d::Constant( DefaultIKTolerance)); - - /// Set all the error bounds. - void setBounds(const std::pair& _bounds); - - /// Get all the error bounds. - const std::pair& getBounds() const; - - /// Set the error bounds for orientation. - void setAngularBounds( - const Eigen::Vector3d& _lower = - Eigen::Vector3d::Constant(-DefaultIKTolerance), - const Eigen::Vector3d& _upper = - Eigen::Vector3d::Constant( DefaultIKTolerance)); - - /// Set the error bounds for orientation. - void setAngularBounds( - const std::pair& _bounds); - - /// Get the error bounds for orientation. - std::pair getAngularBounds() const; - - /// Set the error bounds for translation. - void setLinearBounds( - const Eigen::Vector3d& _lower = - Eigen::Vector3d::Constant(-DefaultIKTolerance), - const Eigen::Vector3d& _upper = - Eigen::Vector3d::Constant( DefaultIKTolerance)); - - /// Set the error bounds for translation. - void setLinearBounds( - const std::pair& _bounds); - - /// Get the error bounds for translation. - std::pair getLinearBounds() const; - - /// Set the clamp that will be applied to the length of the error vector - /// each iteration. - void setErrorLengthClamp(double _clampSize = DefaultIKErrorClamp); - - /// Set the clamp that will be applied to the length of the error vector - /// each iteration. - double getErrorLengthClamp() const; - - /// Set the weights that will be applied to each component of the error - /// vector. - void setErrorWeights(const Eigen::Vector6d& _weights); - - /// Get the weights that will be applied to each component of the error - /// vector. - const Eigen::Vector6d& getErrorWeights() const; - - /// Set the weights that will be applied to each angular component of the - /// error vector. - void setAngularErrorWeights( - const Eigen::Vector3d& _weights = - Eigen::Vector3d::Constant(DefaultIKAngularWeight)); - - /// Get the weights that will be applied to each angular component of the - /// error vector. - Eigen::Vector3d getAngularErrorWeights() const; - - /// Set the weights that will be applied to each linear component of the - /// error vector. - void setLinearErrorWeights( - const Eigen::Vector3d& _weights = - Eigen::Vector3d::Constant(DefaultIKLinearWeight)); - - /// Get the weights that will be applied to each linear component of the - /// error vector. - Eigen::Vector3d getLinearErrorWeights() const; - - /// Get the Properties of this ErrorMethod - Properties getErrorMethodProperties() const; - - /// Clear the cache to force the error to be recomputed. It should generally - /// not be necessary to call this function. - void clearCache(); - - protected: - - /// Pointer to the IK module of this ErrorMethod - common::sub_ptr mIK; - - /// Name of this error method - std::string mMethodName; - - /// The last joint positions passed into this ErrorMethod - Eigen::VectorXd mLastPositions; - - /// The last error vector computed by this ErrorMethod - Eigen::Vector6d mLastError; - - /// The properties of this ErrorMethod - Properties mErrorP; - - public: - // To get byte-aligned Eigen vectors - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - }; - - /// The TaskSpaceRegion is a nicely generalized method for computing the error - /// of an IK Problem. - class TaskSpaceRegion : public ErrorMethod - { - public: - - struct UniqueProperties - { - /// Setting this to true (which is default) will tell it to compute the - /// error based on the center of the Task Space Region instead of the edge - /// of the Task Space Region. This often results in faster convergence, as - /// the Node will enter the Task Space Region more aggressively. - /// - /// Once the Node is inside the Task Space Region, the error vector will - /// drop to zero, regardless of whether this flag is true or false. - bool mComputeErrorFromCenter; - - /// Default constructor - UniqueProperties(bool computeErrorFromCenter = true); - }; - - struct Properties : ErrorMethod::Properties, UniqueProperties - { - /// Default constructor - Properties( - const ErrorMethod::Properties& errorProperties = - ErrorMethod::Properties(), - const UniqueProperties& taskSpaceProperties = UniqueProperties()); - }; - - /// Default Constructor - explicit TaskSpaceRegion(InverseKinematics* _ik, - const Properties& _properties = Properties()); - - /// Virtual destructor - virtual ~TaskSpaceRegion() = default; - - // Documentation inherited - std::unique_ptr clone(InverseKinematics* _newIK) const override; - - // Documentation inherited - Eigen::Isometry3d computeDesiredTransform( - const Eigen::Isometry3d& _currentTf, - const Eigen::Vector6d& _error) override; - - // Documentation inherited - Eigen::Vector6d computeError() override; - - /// Set whether this TaskSpaceRegion should compute its error vector from - /// the center of the region. - void setComputeFromCenter(bool computeFromCenter); - - /// Get whether this TaskSpaceRegion is set to compute its error vector from - /// the center of the region. - bool isComputingFromCenter() const; - - /// Get the Properties of this TaskSpaceRegion - Properties getTaskSpaceRegionProperties() const; - - protected: - - /// Properties of this TaskSpaceRegion - UniqueProperties mTaskSpaceP; + // For the definitions of these classes, see the bottom of this header + class Function; - }; - - /// GradientMethod is a base class for different ways of computing the - /// gradient of an InverseKinematics module. - class GradientMethod : public common::Subject - { - public: - - struct Properties - { - /// The component-wise clamp for this GradientMethod - double mComponentWiseClamp; - - /// The weights for this GradientMethod - Eigen::VectorXd mComponentWeights; - - /// Default constructor - Properties(double clamp = DefaultIKGradientComponentClamp, - const Eigen::VectorXd& weights = Eigen::VectorXd()); - }; - - /// Constructor - GradientMethod(InverseKinematics* _ik, - const std::string& _methodName, - const Properties& _properties); - - /// Virtual destructor - virtual ~GradientMethod() = default; - - /// Enable this GradientMethod to be cloned to a new IK module - virtual std::unique_ptr clone( - InverseKinematics* _newIK) const = 0; - - /// Override this function with your implementation of the gradient - /// computation. The direction that this gradient points in should make the - /// error **worse** if applied to the joint positions, because the - /// Problem is configured as a gradient **descent** error minimization - /// Problem. - /// - /// The error vector that is passed in will be determined by the IK module's - /// ErrorMethod. The expectation is that the first three components of the - /// vector correspond to orientation error (in an angle-axis format) while - /// the last three components correspond to translational error. - /// - /// When implementing this function, you should assume that the Skeleton's - /// current joint positions corresponds to the positions that you - /// must use to compute the error. This function will only get called when - /// an update is needed. - virtual void computeGradient(const Eigen::Vector6d& _error, - Eigen::VectorXd& _grad) = 0; - - /// This function is used to handle caching the gradient vector and - /// interfacing with the solver. - void evalGradient(const Eigen::VectorXd& _q, - Eigen::Map _grad); - - /// Get the name of this GradientMethod. - const std::string& getMethodName() const; - - /// Clamp the gradient based on the clamp settings of this GradientMethod. - void clampGradient(Eigen::VectorXd& _grad) const; - - /// Set the component-wise clamp for this GradientMethod. Each component - /// of the gradient will be individually clamped to this size. - void setComponentWiseClamp(double _clamp = DefaultIKGradientComponentClamp); - - /// Get the component-wise clamp for this GradientMethod. - double getComponentWiseClamp() const; - - /// Apply weights to the gradient based on the weight settings of this - /// GradientMethod. - void applyWeights(Eigen::VectorXd& _grad) const; - - /// Set the weights that will be applied to each component of the gradient. - /// If the number of components in _weights is smaller than the number of - /// components in the gradient, then a weight of 1.0 will be applied to all - /// components that are out of the range of _weights. Passing in an empty - /// vector for _weights will effectively make all the gradient components - /// unweighted. - void setComponentWeights(const Eigen::VectorXd& _weights); - - /// Get the weights of this GradientMethod. - const Eigen::VectorXd& getComponentWeights() const; - - /// Convert the gradient that gets generated by Jacobian methods into a - /// gradient that can be used by a GradientDescentSolver. - /// - /// Not all Joint types can be integrated using standard addition (e.g. - /// FreeJoint and BallJoint), therefore Jacobian-based differential methods - /// will tend to generate gradients that cannot be correctly used by a - /// simple addition-based GradientDescentSolver. Running your gradient - /// through this function before returning it will make the gradient - /// suitable for a standard solver. - void convertJacobianMethodOutputToGradient( - Eigen::VectorXd& grad, const std::vector& dofs); - - /// Get the Properties of this GradientMethod - Properties getGradientMethodProperties() const; - - /// Clear the cache to force the gradient to be recomputed. It should - /// generally not be necessary to call this function. - void clearCache(); - - protected: - - /// The IK module that this GradientMethod belongs to. - common::sub_ptr mIK; - - /// The name of this method - std::string mMethodName; - - /// The last positions that was passed to this GradientMethod - Eigen::VectorXd mLastPositions; - - /// The last gradient that was computed by this GradientMethod - Eigen::VectorXd mLastGradient; - - /// Properties for this GradientMethod - Properties mGradientP; - - private: - - /// Cache used by convertJacobianMethodOutputToGradient to avoid - /// reallocating this vector on each iteration. - Eigen::VectorXd mInitialPositionsCache; - }; - - /// JacobianDLS refers to the Damped Least Squares Jacobian Pseudoinverse - /// (specifically, Moore-Penrose Pseudoinverse). This is a very precise method - /// for computing the gradient and is especially suitable for performing IK on - /// industrial manipulators that need to follow very exact workspace paths. - /// However, it is vulnerable to be jittery around singularities (though the - /// damping helps with this), and each cycle might take more time to compute - /// than the JacobianTranspose method (although the JacobianDLS method will - /// usually converge in fewer cycles than JacobianTranspose). - class JacobianDLS : public GradientMethod - { - public: + // Methods for computing IK error + class ErrorMethod; + class TaskSpaceRegion; - struct UniqueProperties - { - /// Damping coefficient - double mDamping; - - /// Default constructor - UniqueProperties(double damping = DefaultIKDLSCoefficient); - }; - - struct Properties : GradientMethod::Properties, UniqueProperties - { - /// Default constructor - Properties( - const GradientMethod::Properties& gradientProperties = - GradientMethod::Properties(), - const UniqueProperties& dlsProperties = UniqueProperties()); - }; - - /// Constructor - explicit JacobianDLS(InverseKinematics* _ik, - const Properties& properties = Properties()); - - /// Virtual destructor - virtual ~JacobianDLS() = default; - - // Documentation inherited - std::unique_ptr clone( - InverseKinematics* _newIK) const override; - - // Documentation inherited - void computeGradient(const Eigen::Vector6d& _error, - Eigen::VectorXd& _grad) override; - - /// Set the damping coefficient. A higher damping coefficient will smooth - /// out behavior around singularities but will also result in less precision - /// in general. The default value is appropriate for most use cases. - void setDampingCoefficient(double _damping = DefaultIKDLSCoefficient); - - /// Get the damping coefficient. - double getDampingCoefficient() const; - - /// Get the Properties of this JacobianDLS - Properties getJacobianDLSProperties() const; - - protected: - - /// Properties of this Damped Least Squares method - UniqueProperties mDLSProperties; - - }; - - /// JacobianTranspose will simply apply the transpose of the Jacobian to the - /// error vector in order to compute the gradient. This method tends to be - /// very smooth but imprecise, requiring more iterations before converging - /// and being less precise in general. This method is suitable for animations - /// where smoothness is prefered over precision. - class JacobianTranspose : public GradientMethod - { - public: - - /// Constructor - explicit JacobianTranspose(InverseKinematics* _ik, - const Properties& properties = Properties()); - - /// Virtual destructor - virtual ~JacobianTranspose() = default; - - // Documentation inherited - std::unique_ptr clone( - InverseKinematics* _newIK) const override; - - // Documentation inherited - void computeGradient(const Eigen::Vector6d& _error, - Eigen::VectorXd& _grad) override; - }; - - /// Analytical is a base class that should be inherited by methods that are - /// made to solve the IK analytically instead of iteratively. This provides an - /// extended API that is relevant to Analytical solvers but not iterative - /// solvers. - /// - /// Creating an Analytical solver will have the side effect of removing the - /// error clamp and error weights from your ErrorMethod. If you still want - /// your error computations to be clamped and weighted, you should set it - /// again after creating the Analytical solver. Clamping and weighting the - /// error vector often helps iterative methods to converge smoothly, but it is - /// counter-productive for analytical methods which do not typically rely on - /// convergence; analytical methods can usually solve the entire error vector - /// directly. - class Analytical : public GradientMethod - { - public: - - /// Bitwise enumerations that are used to describe some properties of each - /// solution produced by the analytical IK. - enum Validity_t - { - VALID = 0, ///< The solution is completely valid and reaches the target - OUT_OF_REACH = 1 << 0, ///< The solution does not reach the target - LIMIT_VIOLATED = 1 << 1 ///< The solution has one or more joint positions that violate the joint limits - }; - - /// If there are extra DOFs in the IK module which your Analytical solver - /// implementation does not make use of, those DOFs can be used to - /// supplement the analytical solver using Jacobian transpose iteration. - /// This enumeration is used to indicate whether you want those DOFs to be - /// used before applying the analytical solution, after applying the - /// analytical solution, or not be used at all. - /// - /// Jacobian transpose is used for the extra DOFs because it is inexpensive - /// and robust to degenerate Jacobians which are common in low dimensional - /// joint spaces. The primary advantage of pseudoinverse methods over - /// Jacobian transpose methods is their precision, but analytical methods - /// are even more precise than pseudoinverse methods, so that precision is - /// not needed in this case. - /// - /// If you want the extra DOFs to use a different method than Jacobian - /// transpose, you can create two seperate IK modules (one which is - /// analytical and one with the iterative method of your choice) and combine - /// them in a HierarchicalIK. - enum ExtraDofUtilization_t - { - UNUSED = 0, - PRE_ANALYTICAL, - POST_ANALYTICAL, - PRE_AND_POST_ANALYTICAL - }; - - struct Solution - { - /// Default constructor - Solution(const Eigen::VectorXd& _config = Eigen::VectorXd(), - int _validity = VALID); - - /// Configuration computed by the Analytical solver - Eigen::VectorXd mConfig; - - /// Bitmap for whether this configuration is valid. Bitwise-compare it to - /// the enumerations in Validity_t to whether this configuration is valid. - int mValidity; - }; - - // std::function template for comparing the quality of configurations - typedef std::function QualityComparison; - - struct UniqueProperties - { - /// Flag for how to use the extra DOFs in the IK module. - ExtraDofUtilization_t mExtraDofUtilization; - - /// How much to clamp the extra error that gets applied to DOFs - double mExtraErrorLengthClamp; - - /// Function for comparing the qualities of solutions - QualityComparison mQualityComparator; - - /// Default constructor. Uses a default quality comparison function. - UniqueProperties(ExtraDofUtilization_t extraDofUtilization = UNUSED, - double extraErrorLengthClamp = DefaultIKErrorClamp); - - /// Constructor that allows you to set the quality comparison function. - UniqueProperties(ExtraDofUtilization_t extraDofUtilization, - double extraErrorLengthClamp, - QualityComparison qualityComparator); - - /// Reset the quality comparison function to its default behavior. - void resetQualityComparisonFunction(); - }; - - struct Properties : GradientMethod::Properties, UniqueProperties - { - // Default constructor - Properties( - const GradientMethod::Properties& gradientProperties = - GradientMethod::Properties(), - const UniqueProperties& analyticalProperties = UniqueProperties()); - - // Construct Properties by specifying the UniqueProperties. The - // GradientMethod::Properties components will be set to defaults. - Properties(const UniqueProperties& analyticalProperties); - }; - - /// Constructor - Analytical(InverseKinematics* _ik, const std::string& _methodName, - const Properties& _properties); - - /// Virtual destructor - virtual ~Analytical() = default; - - /// Get the solutions for this IK module, along with a tag indicating - /// whether each solution is valid. This function will assume that you want - /// to use the desired transform given by the IK module's current - /// ErrorMethod. - const std::vector& getSolutions(); - - /// Get the solutions for this IK module, along with a tag indicating - /// whether each solution is valid. This function will compute the - /// configurations using the given desired transform instead of using the - /// IK module's current ErrorMethod. - const std::vector& getSolutions( - const Eigen::Isometry3d& _desiredTf); - - /// You should not need to override this function. Instead, you should - /// override computeSolutions. - void computeGradient(const Eigen::Vector6d& _error, - Eigen::VectorXd& _grad) override; - - /// Use this function to fill the entries of the mSolutions variable. Be - /// sure to clear the mSolutions vector at the start, and to also return the - /// mSolutions vector at the end. Note that you are not expected to evaluate - /// any of the solutions for their quality. However, you should set the - /// Solution::mValidity flag to OUT_OF_REACH for each solution that does not - /// actually reach the desired transform, and you should call - /// checkSolutionJointLimits() and the end of the function, which will set - /// the LIMIT_VIOLATED flags of any configurations that are outside of the - /// position limits. - virtual const std::vector& computeSolutions( - const Eigen::Isometry3d& _desiredBodyTf) = 0; - - /// Get a list of the DOFs that will be included in the entries of the - /// solutions returned by getSolutions(). Ideally, this should match up with - /// the DOFs being used by the InverseKinematics module, but this might not - /// always be possible, so this function ensures that solutions can be - /// interpreted correctly. - virtual const std::vector& getDofs() const = 0; - - /// Set the configuration of the DOFs. The components of _config must - /// correspond to the DOFs provided by getDofs(). - void setPositions(const Eigen::VectorXd& _config); - - /// Get the configuration of the DOFs. The components of this vector will - /// correspond to the DOFs provided by getDofs(). - Eigen::VectorXd getPositions() const; - - /// Set how you want extra DOFs to be utilized by the IK module - void setExtraDofUtilization(ExtraDofUtilization_t _utilization); - - /// Get how extra DOFs are being utilized by the IK module - ExtraDofUtilization_t getExtraDofUtilization() const; - - /// Set how much to clamp the error vector that gets applied to extra DOFs - void setExtraErrorLengthClamp(double _clamp); - - /// Get how much we will clamp the error vector that gets applied to extra - /// DOFs - double getExtraErrorLengthClamp() const; - - /// Set the function that will be used to compare the qualities of two - /// solutions. This function should return true if the first argument is a - /// better solution than the second argument. - /// - /// By default, it will prefer the solution which has the smallest size for - /// its largest change in joint angle. In other words, for each - /// configuration that it is given, it will compare the largest change in - /// joint angle for each configuration and pick the one that is smallest. - /// - /// Note that outside of this comparison function, the Solutions will be - /// split between which are valid, which are out-of-reach, and which are - /// in violation of joint limits. Valid solutions will always be ranked - /// above invalid solutions, and joint limit violations will always be - /// ranked last. - void setQualityComparisonFunction(const QualityComparison& _func); - - /// Reset the quality comparison function to the default method. - void resetQualityComparisonFunction(); - - /// Get the Properties for this Analytical class - Properties getAnalyticalProperties() const; - - /// Construct a mapping from the DOFs of getDofs() to their indices within - /// the Node's list of dependent DOFs. This will be called immediately after - /// the Analytical is constructed; this one call is sufficient as long as - /// the DOFs of Analytical::getDofs() is not changed. However, if your - /// Analytical is able to change the DOFs that it operates on, then you will - /// need to call this function each time the DOFs have changed. - void constructDofMap(); - - protected: - - /// This function will compute a gradient which utilizes the extra DOFs - /// that go unused by the Analytical solution and then it will add the - /// components of that gradient to the output parameter: grad. - /// - /// You can override this function to customize how the extra DOFs are used. - /// The default behavior is to use a simple Jacobian Transpose method. - /// - /// The utilization flag will be PRE_ANALYTICAL if the function is being - /// called before the Analytical solution is computed; it will be - /// POST_ANALYTICAL if the function is being called after the Analytical - /// solution is computed. - virtual void addExtraDofGradient( - Eigen::VectorXd& grad, - const Eigen::Vector6d& error, - ExtraDofUtilization_t utilization); - - /// Go through the mSolutions vector and tag entries with LIMIT_VIOLATED if - /// any components of their configuration are outside of their position - /// limits. This will NOT clear the LIMIT_VIOLATED flag from entries of - /// mSolutions which are already tagged with it, even if they do not violate - /// any limits. - void checkSolutionJointLimits(); - - /// Vector of solutions - std::vector mSolutions; - - /// Properties for this Analytical IK solver - UniqueProperties mAnalyticalP; - - private: - - /// This maps the DOFs provided by getDofs() to their index in the Node's - /// list of dependent DOFs. This map is constructed by constructDofMap(). - std::vector mDofMap; - - /// List of extra DOFs in the module which are not covered by the Analytical - /// IK implementation. The index of each DOF is its dependency index in the - /// JacobianNode (i.e. the column it applies to in the Node's Jacobian). - std::vector mExtraDofs; - - /// A cache for the valid solutions. The valid and invalid solution caches - /// are kept separate so that they can each be sorted by quality - /// individually. Valid solutions will always be at the top of mFinalResults - /// even if their quality is scored below the invalid solutions. - std::vector mValidSolutionsCache; - - /// A cache for the out of reach solutions. - std::vector mOutOfReachCache; - - /// A cache for solutions that violate joint limits - std::vector mLimitViolationCache; - - /// A cache for storing the current configuration - Eigen::VectorXd mConfigCache; - - /// A cache for storing the current configuration so that it can be restored - /// later. This is different from mConfigCache which will not be used to - /// restore the configuration. - Eigen::VectorXd mRestoreConfigCache; - - /// A cache for storing the gradient for the extra DOFs - Eigen::VectorXd mExtraDofGradCache; - }; + // Methods for computing IK gradient + class GradientMethod; + class JacobianDLS; + class JacobianTranspose; + class Analytical; /// If this IK module is set to active, then it will be utilized by any /// HierarchicalIK that has it in its list. If it is set to inactive, then it @@ -1116,84 +376,10 @@ class InverseKinematics : public common::Subject protected: - /// The InverseKinematics::Objective Function is simply used to merge the - /// objective and null space objective functions that are being held by an - /// InverseKinematics module. This class is not meant to be extended or - /// instantiated by a user. Call InverseKinematics::resetProblem() to set - /// the objective of the module's Problem to an InverseKinematics::Objective. - class Objective final : public Function, public optimizer::Function - { - public: - - /// Constructor - Objective(InverseKinematics* _ik); - - /// Virtual destructor - virtual ~Objective() = default; - - // Documentation inherited - optimizer::FunctionPtr clone(InverseKinematics* _newIK) const override; - - // Documentation inherited - double eval(const Eigen::VectorXd& _x) override; - - // Documentation inherited - void evalGradient(const Eigen::VectorXd& _x, - Eigen::Map _grad) override; - - protected: - - /// Pointer to this Objective's IK module - sub_ptr mIK; - - /// Cache for the gradient of the Objective - Eigen::VectorXd mGradCache; - - /// Cache for the null space SVD - Eigen::JacobiSVD mSVDCache; - - /// Cache for the null space - Eigen::MatrixXd mNullSpaceCache; - - public: - // To get byte-aligned Eigen vectors - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; - + // For the definitions of these functions, see the bottom of this header + class Objective; friend class Objective; - - /// The InverseKinematics::Constraint Function is simply meant to be used to - /// merge the ErrorMethod and GradientMethod that are being held by an - /// InverseKinematics module. This class is not meant to be extended or - /// instantiated by a user. Call InverseKinematics::resetProblem() to set the - /// first equality constraint of the module's Problem to an - /// InverseKinematics::Constraint. - class Constraint final : public Function, public optimizer::Function - { - public: - - /// Constructor - Constraint(InverseKinematics* _ik); - - /// Virtual constructor - virtual ~Constraint() = default; - - // Documentation inherited - optimizer::FunctionPtr clone(InverseKinematics* _newIK) const override; - - // Documentation inherited - double eval(const Eigen::VectorXd& _x) override; - - // Documentation inherited - void evalGradient(const Eigen::VectorXd& _x, - Eigen::Map _grad) override; - - protected: - - /// Pointer to this Constraint's IK module - sub_ptr mIK; - }; - + class Constraint; friend class Constraint; /// Constructor that accepts a JacobianNode @@ -1270,6 +456,846 @@ class InverseKinematics : public common::Subject typedef InverseKinematics IK; +//============================================================================== +/// This class should be inherited by optimizer::Function classes that have a +/// dependency on the InverseKinematics module that they belong to. If you +/// pass an InverseKinematics::Function into the Problem of an +/// InverseKinematics module, then it will be properly cloned whenever the +/// InverseKinematics module that it belongs to gets cloned. Any Function +/// classes in the Problem that do not inherit InverseKinematics::Function +/// will just be copied over by reference. +class InverseKinematics::Function +{ +public: + + /// Enable this function to be cloned to a new IK module. + virtual optimizer::FunctionPtr clone(InverseKinematics* _newIK) const = 0; + + /// Virtual destructor + virtual ~Function() = default; +}; + +//============================================================================== +/// ErrorMethod is a base class for different ways of computing the error of +/// an InverseKinematics module. +class InverseKinematics::ErrorMethod : public common::Subject +{ +public: + + typedef std::pair Bounds; + + /// The Properties struct contains settings that are commonly used by + /// methods that compute error for inverse kinematics. + struct Properties + { + /// Default constructor + Properties(const Bounds& _bounds = + Bounds(Eigen::Vector6d::Constant(-DefaultIKTolerance), + Eigen::Vector6d::Constant( DefaultIKTolerance)), + + double _errorClamp = DefaultIKErrorClamp, + + const Eigen::Vector6d& _errorWeights = Eigen::compose( + Eigen::Vector3d::Constant(DefaultIKAngularWeight), + Eigen::Vector3d::Constant(DefaultIKLinearWeight))); + + /// Bounds that define the acceptable range of the Node's transform + /// relative to its target frame. + std::pair mBounds; + + /// The error vector will be clamped to this length with each iteration. + /// This is used to enforce sane behavior, even when there are extremely + /// large error vectors. + double mErrorLengthClamp; + + /// These weights will be applied to the error vector component-wise. This + /// allows you to set some components of error as more important than + /// others, or to scale their coordinate spaces. For example, you will + /// often want the first three components (orientation error) to have + /// smaller weights than the last three components (translation error). + Eigen::Vector6d mErrorWeights; + + // To get byte-aligned Eigen vectors + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; + + /// Constructor + ErrorMethod(InverseKinematics* _ik, + const std::string& _methodName, + const Properties& _properties = Properties()); + + /// Virtual destructor + virtual ~ErrorMethod() = default; + + /// Enable this ErrorMethod to be cloned to a new IK module. + virtual std::unique_ptr clone( + InverseKinematics* _newIK) const = 0; + + /// Override this function with your implementation of the error vector + /// computation. The expectation is that the first three components of the + /// vector will correspond to orientation error (in an angle-axis format) + /// while the last three components correspond to translational error. + /// + /// When implementing this function, you should assume that the Skeleton's + /// current joint positions corresponds to the positions that you + /// must use to compute the error. This function will only get called when + /// an update is needed. + virtual Eigen::Vector6d computeError() = 0; + + /// Override this function with your implementation of computing the desired + /// given the current transform and error vector. If you want the desired + /// transform to always be equal to the Target's transform, you can simply + /// call ErrorMethod::computeDesiredTransform to implement this function. + virtual Eigen::Isometry3d computeDesiredTransform( + const Eigen::Isometry3d& _currentTf, const Eigen::Vector6d& _error) = 0; + + /// This function is used to handle caching the error vector. + const Eigen::Vector6d& evalError(const Eigen::VectorXd& _q); + + /// Get the name of this ErrorMethod. + const std::string& getMethodName() const; + + /// Set all the error bounds. + void setBounds( + const Eigen::Vector6d& _lower = + Eigen::Vector6d::Constant(-DefaultIKTolerance), + const Eigen::Vector6d& _upper = + Eigen::Vector6d::Constant( DefaultIKTolerance)); + + /// Set all the error bounds. + void setBounds(const std::pair& _bounds); + + /// Get all the error bounds. + const std::pair& getBounds() const; + + /// Set the error bounds for orientation. + void setAngularBounds( + const Eigen::Vector3d& _lower = + Eigen::Vector3d::Constant(-DefaultIKTolerance), + const Eigen::Vector3d& _upper = + Eigen::Vector3d::Constant( DefaultIKTolerance)); + + /// Set the error bounds for orientation. + void setAngularBounds( + const std::pair& _bounds); + + /// Get the error bounds for orientation. + std::pair getAngularBounds() const; + + /// Set the error bounds for translation. + void setLinearBounds( + const Eigen::Vector3d& _lower = + Eigen::Vector3d::Constant(-DefaultIKTolerance), + const Eigen::Vector3d& _upper = + Eigen::Vector3d::Constant( DefaultIKTolerance)); + + /// Set the error bounds for translation. + void setLinearBounds( + const std::pair& _bounds); + + /// Get the error bounds for translation. + std::pair getLinearBounds() const; + + /// Set the clamp that will be applied to the length of the error vector + /// each iteration. + void setErrorLengthClamp(double _clampSize = DefaultIKErrorClamp); + + /// Set the clamp that will be applied to the length of the error vector + /// each iteration. + double getErrorLengthClamp() const; + + /// Set the weights that will be applied to each component of the error + /// vector. + void setErrorWeights(const Eigen::Vector6d& _weights); + + /// Get the weights that will be applied to each component of the error + /// vector. + const Eigen::Vector6d& getErrorWeights() const; + + /// Set the weights that will be applied to each angular component of the + /// error vector. + void setAngularErrorWeights( + const Eigen::Vector3d& _weights = + Eigen::Vector3d::Constant(DefaultIKAngularWeight)); + + /// Get the weights that will be applied to each angular component of the + /// error vector. + Eigen::Vector3d getAngularErrorWeights() const; + + /// Set the weights that will be applied to each linear component of the + /// error vector. + void setLinearErrorWeights( + const Eigen::Vector3d& _weights = + Eigen::Vector3d::Constant(DefaultIKLinearWeight)); + + /// Get the weights that will be applied to each linear component of the + /// error vector. + Eigen::Vector3d getLinearErrorWeights() const; + + /// Get the Properties of this ErrorMethod + Properties getErrorMethodProperties() const; + + /// Clear the cache to force the error to be recomputed. It should generally + /// not be necessary to call this function. + void clearCache(); + +protected: + + /// Pointer to the IK module of this ErrorMethod + common::sub_ptr mIK; + + /// Name of this error method + std::string mMethodName; + + /// The last joint positions passed into this ErrorMethod + Eigen::VectorXd mLastPositions; + + /// The last error vector computed by this ErrorMethod + Eigen::Vector6d mLastError; + + /// The properties of this ErrorMethod + Properties mErrorP; + +public: + // To get byte-aligned Eigen vectors + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +}; + +//============================================================================== +/// The TaskSpaceRegion is a nicely generalized method for computing the error +/// of an IK Problem. +class InverseKinematics::TaskSpaceRegion : public ErrorMethod +{ +public: + + struct UniqueProperties + { + /// Setting this to true (which is default) will tell it to compute the + /// error based on the center of the Task Space Region instead of the edge + /// of the Task Space Region. This often results in faster convergence, as + /// the Node will enter the Task Space Region more aggressively. + /// + /// Once the Node is inside the Task Space Region, the error vector will + /// drop to zero, regardless of whether this flag is true or false. + bool mComputeErrorFromCenter; + + /// Default constructor + UniqueProperties(bool computeErrorFromCenter = true); + }; + + struct Properties : ErrorMethod::Properties, UniqueProperties + { + /// Default constructor + Properties( + const ErrorMethod::Properties& errorProperties = + ErrorMethod::Properties(), + const UniqueProperties& taskSpaceProperties = UniqueProperties()); + }; + + /// Default Constructor + explicit TaskSpaceRegion(InverseKinematics* _ik, + const Properties& _properties = Properties()); + + /// Virtual destructor + virtual ~TaskSpaceRegion() = default; + + // Documentation inherited + std::unique_ptr clone(InverseKinematics* _newIK) const override; + + // Documentation inherited + Eigen::Isometry3d computeDesiredTransform( + const Eigen::Isometry3d& _currentTf, + const Eigen::Vector6d& _error) override; + + // Documentation inherited + Eigen::Vector6d computeError() override; + + /// Set whether this TaskSpaceRegion should compute its error vector from + /// the center of the region. + void setComputeFromCenter(bool computeFromCenter); + + /// Get whether this TaskSpaceRegion is set to compute its error vector from + /// the center of the region. + bool isComputingFromCenter() const; + + /// Get the Properties of this TaskSpaceRegion + Properties getTaskSpaceRegionProperties() const; + +protected: + + /// Properties of this TaskSpaceRegion + UniqueProperties mTaskSpaceP; + +}; + +//============================================================================== +/// GradientMethod is a base class for different ways of computing the +/// gradient of an InverseKinematics module. +class InverseKinematics::GradientMethod : public common::Subject +{ +public: + + struct Properties + { + /// The component-wise clamp for this GradientMethod + double mComponentWiseClamp; + + /// The weights for this GradientMethod + Eigen::VectorXd mComponentWeights; + + /// Default constructor + Properties(double clamp = DefaultIKGradientComponentClamp, + const Eigen::VectorXd& weights = Eigen::VectorXd()); + }; + + /// Constructor + GradientMethod(InverseKinematics* _ik, + const std::string& _methodName, + const Properties& _properties); + + /// Virtual destructor + virtual ~GradientMethod() = default; + + /// Enable this GradientMethod to be cloned to a new IK module + virtual std::unique_ptr clone( + InverseKinematics* _newIK) const = 0; + + /// Override this function with your implementation of the gradient + /// computation. The direction that this gradient points in should make the + /// error **worse** if applied to the joint positions, because the + /// Problem is configured as a gradient **descent** error minimization + /// Problem. + /// + /// The error vector that is passed in will be determined by the IK module's + /// ErrorMethod. The expectation is that the first three components of the + /// vector correspond to orientation error (in an angle-axis format) while + /// the last three components correspond to translational error. + /// + /// When implementing this function, you should assume that the Skeleton's + /// current joint positions corresponds to the positions that you + /// must use to compute the error. This function will only get called when + /// an update is needed. + virtual void computeGradient(const Eigen::Vector6d& _error, + Eigen::VectorXd& _grad) = 0; + + /// This function is used to handle caching the gradient vector and + /// interfacing with the solver. + void evalGradient(const Eigen::VectorXd& _q, + Eigen::Map _grad); + + /// Get the name of this GradientMethod. + const std::string& getMethodName() const; + + /// Clamp the gradient based on the clamp settings of this GradientMethod. + void clampGradient(Eigen::VectorXd& _grad) const; + + /// Set the component-wise clamp for this GradientMethod. Each component + /// of the gradient will be individually clamped to this size. + void setComponentWiseClamp(double _clamp = DefaultIKGradientComponentClamp); + + /// Get the component-wise clamp for this GradientMethod. + double getComponentWiseClamp() const; + + /// Apply weights to the gradient based on the weight settings of this + /// GradientMethod. + void applyWeights(Eigen::VectorXd& _grad) const; + + /// Set the weights that will be applied to each component of the gradient. + /// If the number of components in _weights is smaller than the number of + /// components in the gradient, then a weight of 1.0 will be applied to all + /// components that are out of the range of _weights. Passing in an empty + /// vector for _weights will effectively make all the gradient components + /// unweighted. + void setComponentWeights(const Eigen::VectorXd& _weights); + + /// Get the weights of this GradientMethod. + const Eigen::VectorXd& getComponentWeights() const; + + /// Convert the gradient that gets generated by Jacobian methods into a + /// gradient that can be used by a GradientDescentSolver. + /// + /// Not all Joint types can be integrated using standard addition (e.g. + /// FreeJoint and BallJoint), therefore Jacobian-based differential methods + /// will tend to generate gradients that cannot be correctly used by a + /// simple addition-based GradientDescentSolver. Running your gradient + /// through this function before returning it will make the gradient + /// suitable for a standard solver. + void convertJacobianMethodOutputToGradient( + Eigen::VectorXd& grad, const std::vector& dofs); + + /// Get the Properties of this GradientMethod + Properties getGradientMethodProperties() const; + + /// Clear the cache to force the gradient to be recomputed. It should + /// generally not be necessary to call this function. + void clearCache(); + +protected: + + /// The IK module that this GradientMethod belongs to. + common::sub_ptr mIK; + + /// The name of this method + std::string mMethodName; + + /// The last positions that was passed to this GradientMethod + Eigen::VectorXd mLastPositions; + + /// The last gradient that was computed by this GradientMethod + Eigen::VectorXd mLastGradient; + + /// Properties for this GradientMethod + Properties mGradientP; + +private: + + /// Cache used by convertJacobianMethodOutputToGradient to avoid + /// reallocating this vector on each iteration. + Eigen::VectorXd mInitialPositionsCache; +}; + +//============================================================================== +/// JacobianDLS refers to the Damped Least Squares Jacobian Pseudoinverse +/// (specifically, Moore-Penrose Pseudoinverse). This is a very precise method +/// for computing the gradient and is especially suitable for performing IK on +/// industrial manipulators that need to follow very exact workspace paths. +/// However, it is vulnerable to be jittery around singularities (though the +/// damping helps with this), and each cycle might take more time to compute +/// than the JacobianTranspose method (although the JacobianDLS method will +/// usually converge in fewer cycles than JacobianTranspose). +class InverseKinematics::JacobianDLS : public GradientMethod +{ +public: + + struct UniqueProperties + { + /// Damping coefficient + double mDamping; + + /// Default constructor + UniqueProperties(double damping = DefaultIKDLSCoefficient); + }; + + struct Properties : GradientMethod::Properties, UniqueProperties + { + /// Default constructor + Properties( + const GradientMethod::Properties& gradientProperties = + GradientMethod::Properties(), + const UniqueProperties& dlsProperties = UniqueProperties()); + }; + + /// Constructor + explicit JacobianDLS(InverseKinematics* _ik, + const Properties& properties = Properties()); + + /// Virtual destructor + virtual ~JacobianDLS() = default; + + // Documentation inherited + std::unique_ptr clone( + InverseKinematics* _newIK) const override; + + // Documentation inherited + void computeGradient(const Eigen::Vector6d& _error, + Eigen::VectorXd& _grad) override; + + /// Set the damping coefficient. A higher damping coefficient will smooth + /// out behavior around singularities but will also result in less precision + /// in general. The default value is appropriate for most use cases. + void setDampingCoefficient(double _damping = DefaultIKDLSCoefficient); + + /// Get the damping coefficient. + double getDampingCoefficient() const; + + /// Get the Properties of this JacobianDLS + Properties getJacobianDLSProperties() const; + +protected: + + /// Properties of this Damped Least Squares method + UniqueProperties mDLSProperties; + +}; + +//============================================================================== +/// JacobianTranspose will simply apply the transpose of the Jacobian to the +/// error vector in order to compute the gradient. This method tends to be +/// very smooth but imprecise, requiring more iterations before converging +/// and being less precise in general. This method is suitable for animations +/// where smoothness is prefered over precision. +class InverseKinematics::JacobianTranspose : public GradientMethod +{ +public: + + /// Constructor + explicit JacobianTranspose(InverseKinematics* _ik, + const Properties& properties = Properties()); + + /// Virtual destructor + virtual ~JacobianTranspose() = default; + + // Documentation inherited + std::unique_ptr clone( + InverseKinematics* _newIK) const override; + + // Documentation inherited + void computeGradient(const Eigen::Vector6d& _error, + Eigen::VectorXd& _grad) override; +}; + +//============================================================================== +/// Analytical is a base class that should be inherited by methods that are +/// made to solve the IK analytically instead of iteratively. This provides an +/// extended API that is relevant to Analytical solvers but not iterative +/// solvers. +/// +/// Creating an Analytical solver will have the side effect of removing the +/// error clamp and error weights from your ErrorMethod. If you still want +/// your error computations to be clamped and weighted, you should set it +/// again after creating the Analytical solver. Clamping and weighting the +/// error vector often helps iterative methods to converge smoothly, but it is +/// counter-productive for analytical methods which do not typically rely on +/// convergence; analytical methods can usually solve the entire error vector +/// directly. +class InverseKinematics::Analytical : public GradientMethod +{ +public: + + /// Bitwise enumerations that are used to describe some properties of each + /// solution produced by the analytical IK. + enum Validity_t + { + VALID = 0, ///< The solution is completely valid and reaches the target + OUT_OF_REACH = 1 << 0, ///< The solution does not reach the target + LIMIT_VIOLATED = 1 << 1 ///< The solution has one or more joint positions that violate the joint limits + }; + + /// If there are extra DOFs in the IK module which your Analytical solver + /// implementation does not make use of, those DOFs can be used to + /// supplement the analytical solver using Jacobian transpose iteration. + /// This enumeration is used to indicate whether you want those DOFs to be + /// used before applying the analytical solution, after applying the + /// analytical solution, or not be used at all. + /// + /// Jacobian transpose is used for the extra DOFs because it is inexpensive + /// and robust to degenerate Jacobians which are common in low dimensional + /// joint spaces. The primary advantage of pseudoinverse methods over + /// Jacobian transpose methods is their precision, but analytical methods + /// are even more precise than pseudoinverse methods, so that precision is + /// not needed in this case. + /// + /// If you want the extra DOFs to use a different method than Jacobian + /// transpose, you can create two seperate IK modules (one which is + /// analytical and one with the iterative method of your choice) and combine + /// them in a HierarchicalIK. + enum ExtraDofUtilization + { + UNUSED = 0, + PRE_ANALYTICAL, + POST_ANALYTICAL, + PRE_AND_POST_ANALYTICAL + }; + + struct Solution + { + /// Default constructor + Solution(const Eigen::VectorXd& _config = Eigen::VectorXd(), + int _validity = VALID); + + /// Configuration computed by the Analytical solver + Eigen::VectorXd mConfig; + + /// Bitmap for whether this configuration is valid. Bitwise-compare it to + /// the enumerations in Validity_t to whether this configuration is valid. + int mValidity; + }; + + // std::function template for comparing the quality of configurations + typedef std::function QualityComparison; + + struct UniqueProperties + { + /// Flag for how to use the extra DOFs in the IK module. + ExtraDofUtilization mExtraDofUtilization; + + /// How much to clamp the extra error that gets applied to DOFs + double mExtraErrorLengthClamp; + + /// Function for comparing the qualities of solutions + QualityComparison mQualityComparator; + + /// Default constructor. Uses a default quality comparison function. + UniqueProperties(ExtraDofUtilization extraDofUtilization = UNUSED, + double extraErrorLengthClamp = DefaultIKErrorClamp); + + /// Constructor that allows you to set the quality comparison function. + UniqueProperties(ExtraDofUtilization extraDofUtilization, + double extraErrorLengthClamp, + QualityComparison qualityComparator); + + /// Reset the quality comparison function to its default behavior. + void resetQualityComparisonFunction(); + }; + + struct Properties : GradientMethod::Properties, UniqueProperties + { + // Default constructor + Properties( + const GradientMethod::Properties& gradientProperties = + GradientMethod::Properties(), + const UniqueProperties& analyticalProperties = UniqueProperties()); + + // Construct Properties by specifying the UniqueProperties. The + // GradientMethod::Properties components will be set to defaults. + Properties(const UniqueProperties& analyticalProperties); + }; + + /// Constructor + Analytical(InverseKinematics* _ik, const std::string& _methodName, + const Properties& _properties); + + /// Virtual destructor + virtual ~Analytical() = default; + + /// Get the solutions for this IK module, along with a tag indicating + /// whether each solution is valid. This function will assume that you want + /// to use the desired transform given by the IK module's current + /// ErrorMethod. + const std::vector& getSolutions(); + + /// Get the solutions for this IK module, along with a tag indicating + /// whether each solution is valid. This function will compute the + /// configurations using the given desired transform instead of using the + /// IK module's current ErrorMethod. + const std::vector& getSolutions( + const Eigen::Isometry3d& _desiredTf); + + /// You should not need to override this function. Instead, you should + /// override computeSolutions. + void computeGradient(const Eigen::Vector6d& _error, + Eigen::VectorXd& _grad) override; + + /// Use this function to fill the entries of the mSolutions variable. Be + /// sure to clear the mSolutions vector at the start, and to also return the + /// mSolutions vector at the end. Note that you are not expected to evaluate + /// any of the solutions for their quality. However, you should set the + /// Solution::mValidity flag to OUT_OF_REACH for each solution that does not + /// actually reach the desired transform, and you should call + /// checkSolutionJointLimits() and the end of the function, which will set + /// the LIMIT_VIOLATED flags of any configurations that are outside of the + /// position limits. + virtual const std::vector& computeSolutions( + const Eigen::Isometry3d& _desiredBodyTf) = 0; + + /// Get a list of the DOFs that will be included in the entries of the + /// solutions returned by getSolutions(). Ideally, this should match up with + /// the DOFs being used by the InverseKinematics module, but this might not + /// always be possible, so this function ensures that solutions can be + /// interpreted correctly. + virtual const std::vector& getDofs() const = 0; + + /// Set the configuration of the DOFs. The components of _config must + /// correspond to the DOFs provided by getDofs(). + void setPositions(const Eigen::VectorXd& _config); + + /// Get the configuration of the DOFs. The components of this vector will + /// correspond to the DOFs provided by getDofs(). + Eigen::VectorXd getPositions() const; + + /// Set how you want extra DOFs to be utilized by the IK module + void setExtraDofUtilization(ExtraDofUtilization _utilization); + + /// Get how extra DOFs are being utilized by the IK module + ExtraDofUtilization getExtraDofUtilization() const; + + /// Set how much to clamp the error vector that gets applied to extra DOFs + void setExtraErrorLengthClamp(double _clamp); + + /// Get how much we will clamp the error vector that gets applied to extra + /// DOFs + double getExtraErrorLengthClamp() const; + + /// Set the function that will be used to compare the qualities of two + /// solutions. This function should return true if the first argument is a + /// better solution than the second argument. + /// + /// By default, it will prefer the solution which has the smallest size for + /// its largest change in joint angle. In other words, for each + /// configuration that it is given, it will compare the largest change in + /// joint angle for each configuration and pick the one that is smallest. + /// + /// Note that outside of this comparison function, the Solutions will be + /// split between which are valid, which are out-of-reach, and which are + /// in violation of joint limits. Valid solutions will always be ranked + /// above invalid solutions, and joint limit violations will always be + /// ranked last. + void setQualityComparisonFunction(const QualityComparison& _func); + + /// Reset the quality comparison function to the default method. + void resetQualityComparisonFunction(); + + /// Get the Properties for this Analytical class + Properties getAnalyticalProperties() const; + + /// Construct a mapping from the DOFs of getDofs() to their indices within + /// the Node's list of dependent DOFs. This will be called immediately after + /// the Analytical is constructed; this one call is sufficient as long as + /// the DOFs of Analytical::getDofs() is not changed. However, if your + /// Analytical is able to change the DOFs that it operates on, then you will + /// need to call this function each time the DOFs have changed. + void constructDofMap(); + +protected: + + /// This function will compute a gradient which utilizes the extra DOFs + /// that go unused by the Analytical solution and then it will add the + /// components of that gradient to the output parameter: grad. + /// + /// You can override this function to customize how the extra DOFs are used. + /// The default behavior is to use a simple Jacobian Transpose method. + /// + /// The utilization flag will be PRE_ANALYTICAL if the function is being + /// called before the Analytical solution is computed; it will be + /// POST_ANALYTICAL if the function is being called after the Analytical + /// solution is computed. + virtual void addExtraDofGradient( + Eigen::VectorXd& grad, + const Eigen::Vector6d& error, + ExtraDofUtilization utilization); + + /// Go through the mSolutions vector and tag entries with LIMIT_VIOLATED if + /// any components of their configuration are outside of their position + /// limits. This will NOT clear the LIMIT_VIOLATED flag from entries of + /// mSolutions which are already tagged with it, even if they do not violate + /// any limits. + void checkSolutionJointLimits(); + + /// Vector of solutions + std::vector mSolutions; + + /// Properties for this Analytical IK solver + UniqueProperties mAnalyticalP; + +private: + + /// This maps the DOFs provided by getDofs() to their index in the Node's + /// list of dependent DOFs. This map is constructed by constructDofMap(). + std::vector mDofMap; + + /// List of extra DOFs in the module which are not covered by the Analytical + /// IK implementation. The index of each DOF is its dependency index in the + /// JacobianNode (i.e. the column it applies to in the Node's Jacobian). + std::vector mExtraDofs; + + /// A cache for the valid solutions. The valid and invalid solution caches + /// are kept separate so that they can each be sorted by quality + /// individually. Valid solutions will always be at the top of mFinalResults + /// even if their quality is scored below the invalid solutions. + std::vector mValidSolutionsCache; + + /// A cache for the out of reach solutions. + std::vector mOutOfReachCache; + + /// A cache for solutions that violate joint limits + std::vector mLimitViolationCache; + + /// A cache for storing the current configuration + Eigen::VectorXd mConfigCache; + + /// A cache for storing the current configuration so that it can be restored + /// later. This is different from mConfigCache which will not be used to + /// restore the configuration. + Eigen::VectorXd mRestoreConfigCache; + + /// A cache for storing the gradient for the extra DOFs + Eigen::VectorXd mExtraDofGradCache; +}; + +//============================================================================== +/// The InverseKinematics::Objective Function is simply used to merge the +/// objective and null space objective functions that are being held by an +/// InverseKinematics module. This class is not meant to be extended or +/// instantiated by a user. Call InverseKinematics::resetProblem() to set +/// the objective of the module's Problem to an InverseKinematics::Objective. +class InverseKinematics::Objective final : + public Function, public optimizer::Function +{ +public: + + /// Constructor + Objective(InverseKinematics* _ik); + + /// Virtual destructor + virtual ~Objective() = default; + + // Documentation inherited + optimizer::FunctionPtr clone(InverseKinematics* _newIK) const override; + + // Documentation inherited + double eval(const Eigen::VectorXd& _x) override; + + // Documentation inherited + void evalGradient(const Eigen::VectorXd& _x, + Eigen::Map _grad) override; + +protected: + + /// Pointer to this Objective's IK module + sub_ptr mIK; + + /// Cache for the gradient of the Objective + Eigen::VectorXd mGradCache; + + /// Cache for the null space SVD + Eigen::JacobiSVD mSVDCache; + + /// Cache for the null space + Eigen::MatrixXd mNullSpaceCache; + +public: + // To get byte-aligned Eigen vectors + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +//============================================================================== +/// The InverseKinematics::Constraint Function is simply meant to be used to +/// merge the ErrorMethod and GradientMethod that are being held by an +/// InverseKinematics module. This class is not meant to be extended or +/// instantiated by a user. Call InverseKinematics::resetProblem() to set the +/// first equality constraint of the module's Problem to an +/// InverseKinematics::Constraint. +class InverseKinematics::Constraint final : + public Function, public optimizer::Function +{ +public: + + /// Constructor + Constraint(InverseKinematics* _ik); + + /// Virtual constructor + virtual ~Constraint() = default; + + // Documentation inherited + optimizer::FunctionPtr clone(InverseKinematics* _newIK) const override; + + // Documentation inherited + double eval(const Eigen::VectorXd& _x) override; + + // Documentation inherited + void evalGradient(const Eigen::VectorXd& _x, + Eigen::Map _grad) override; + +protected: + + /// Pointer to this Constraint's IK module + sub_ptr mIK; +}; + } // namespace dynamics } // namespace dart diff --git a/unittests/testForwardKinematics.cpp b/unittests/testForwardKinematics.cpp index 32029b7f26259..4a1be2687ebe5 100644 --- a/unittests/testForwardKinematics.cpp +++ b/unittests/testForwardKinematics.cpp @@ -219,7 +219,7 @@ TEST(FORWARD_KINEMATICS, JACOBIAN_PARTIAL_CHANGE) //============================================================================== TEST(FORWARD_KINEMATICS, JACOBIAN_END_EFFECTOR_CHANGE) { - // This is a regression test for issue #499 + // This is a regression test for pull request #683 const double tolerance = 1e-8; dart::utils::DartLoader loader; From 9d9a8a0366384d667008dcc54db2b7237666329a Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Mon, 25 Apr 2016 13:08:03 -0400 Subject: [PATCH 4/9] Standardizing the create functions for built-in Node types --- dart/dynamics/BodyNode.cpp | 13 +++++++++++++ dart/dynamics/BodyNode.h | 9 ++++++--- dart/dynamics/Skeleton.h | 3 ++- dart/dynamics/detail/BodyNode.h | 8 -------- 4 files changed, 21 insertions(+), 12 deletions(-) diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index 444511a7e68de..b7c3720c2ffc5 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/dart/dynamics/BodyNode.cpp @@ -947,6 +947,13 @@ void BodyNode::removeAllShapeNodes() //============================================================================== DART_BAKE_SPECIALIZED_NODE_DEFINITIONS( BodyNode, EndEffector ) +//============================================================================== +EndEffector* BodyNode::createEndEffector( + const EndEffector::BasicProperties& _properties) +{ + return createNode(_properties); +} + //============================================================================== EndEffector* BodyNode::createEndEffector(const std::string& _name) { @@ -978,6 +985,12 @@ Marker* BodyNode::createMarker(const std::string& name, return createNode(properties); } +//============================================================================== +Marker* BodyNode::createMarker(const Marker::BasicProperties& properties) +{ + return createNode(properties); +} + //============================================================================== bool BodyNode::dependsOn(size_t _genCoordIndex) const { diff --git a/dart/dynamics/BodyNode.h b/dart/dynamics/BodyNode.h index 1a01b02887704..1689e524313ee 100644 --- a/dart/dynamics/BodyNode.h +++ b/dart/dynamics/BodyNode.h @@ -76,7 +76,7 @@ class Marker; /// BodyNode of the BodyNode. class BodyNode : public detail::BodyNodeCompositeBase, - public virtual BodyNodeSpecializedFor, + public virtual BodyNodeSpecializedFor, public SkeletonRefCountingBase, public TemplatedJacobianNode { @@ -596,8 +596,8 @@ class BodyNode : /// Create an EndEffector attached to this BodyNode. Pass an /// EndEffector::Properties argument into this function. - template - EndEffector* createEndEffector(const EndEffectorProperties& _properties); + EndEffector* createEndEffector( + const EndEffector::BasicProperties& _properties); /// Create an EndEffector with the specified name EndEffector* createEndEffector(const std::string& _name = "EndEffector"); @@ -613,6 +613,9 @@ class BodyNode : const Eigen::Vector3d& position = Eigen::Vector3d::Zero(), const Eigen::Vector4d& color = Eigen::Vector4d::Constant(1.0)); + /// Create a Marker given its basic properties + Marker* createMarker(const Marker::BasicProperties& properties); + // Documentation inherited bool dependsOn(size_t _genCoordIndex) const override; diff --git a/dart/dynamics/Skeleton.h b/dart/dynamics/Skeleton.h index 7c250617577d5..689c6360fda37 100644 --- a/dart/dynamics/Skeleton.h +++ b/dart/dynamics/Skeleton.h @@ -47,6 +47,7 @@ #include "dart/dynamics/Joint.h" #include "dart/dynamics/ShapeNode.h" #include "dart/dynamics/EndEffector.h" +#include "dart/dynamics/Marker.h" #include "dart/dynamics/detail/BodyNodeAspect.h" #include "dart/dynamics/SpecializedNodeManager.h" #include "dart/dynamics/detail/SkeletonAspect.h" @@ -58,7 +59,7 @@ namespace dynamics { class Skeleton : public virtual common::VersionCounter, public MetaSkeleton, - public SkeletonSpecializedFor, + public SkeletonSpecializedFor, public detail::SkeletonAspectBase { public: diff --git a/dart/dynamics/detail/BodyNode.h b/dart/dynamics/detail/BodyNode.h index b39789d8a6da5..9974631c84218 100644 --- a/dart/dynamics/detail/BodyNode.h +++ b/dart/dynamics/detail/BodyNode.h @@ -240,14 +240,6 @@ void BodyNode::removeAllShapeNodesWith() shapeNode->remove(); } -//============================================================================== -template -EndEffector* BodyNode::createEndEffector( - const EndEffectorProperties& _properties) -{ - return createNode(_properties); -} - } // namespace dynamics } // namespace dart From c8d6d7d00d66c36a46dea59edef12ca45248cc1e Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 25 Apr 2016 14:07:21 -0400 Subject: [PATCH 5/9] Run full build tests only for pull request builds --- .travis.yml | 33 +++++++++++++++++++-------------- ci/script.sh | 7 +++++++ ci/script_pull_request.sh | 27 +++++++++++++++++++++++++++ 3 files changed, 53 insertions(+), 14 deletions(-) create mode 100644 ci/script.sh create mode 100644 ci/script_pull_request.sh diff --git a/.travis.yml b/.travis.yml index e1f0ecdfa832e..42b5605e2d10a 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,35 +1,40 @@ language: cpp -os: - - linux - - osx - sudo: required dist: trusty -compiler: - - gcc - - clang - env: - BUILD_TYPE=Debug TREAT_WARNINGS_AS_ERRORS=ON COVERALLS=ON - BUILD_TYPE=Release TREAT_WARNINGS_AS_ERRORS=ON COVERALLS=OFF matrix: - exclude: - - os: osx + include: + - os: linux compiler: gcc + env: BUILD_TYPE=Debug COVERALLS=ON COMPILER=GCC + - os: linux + compiler: gcc + env: BUILD_TYPE=Release COVERALLS=OFF COMPILER=GCC + - os: linux + compiler: clang + env: BUILD_TYPE=Debug COVERALLS=OFF COMPILER=CLANG + - os: linux + compiler: clang + env: BUILD_TYPE=Release COVERALLS=OFF COMPILER=CLANG + - os: osx + compiler: clang + env: BUILD_TYPE=Debug COVERALLS=OFF COMPILER=CLANG + - os: osx + compiler: clang + env: BUILD_TYPE=Release COVERALLS=OFF COMPILER=CLANG install: - if [ "$TRAVIS_OS_NAME" = "linux" ]; then 'ci/install_linux.sh' ; fi - if [ "$TRAVIS_OS_NAME" = "osx" ]; then 'ci/install_osx.sh' ; fi script: - - mkdir build && cd build - - cmake -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DDART_TREAT_WARNINGS_AS_ERRORS=$TREAT_WARNINGS_AS_ERRORS -DDART_COVERALLS=$COVERALLS .. - - make -j4 all tutorials examples tests test - - if [ $COVERALLS = ON ] && [ "$TRAVIS_OS_NAME" = "linux" ]; then make -j4 coveralls; fi + - 'ci/scrip.sh' after_failure: - cat Testing/Temporary/LastTest.log diff --git a/ci/script.sh b/ci/script.sh new file mode 100644 index 0000000000000..b5723f958fe9a --- /dev/null +++ b/ci/script.sh @@ -0,0 +1,7 @@ +if [ "$TRAVIS_PULL_REQUEST" = "false" ] && [ "$COMPILER" = "CLANG" ]; then exit; fi + +mkdir build && cd build +cmake -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DDART_TREAT_WARNINGS_AS_ERRORS=ON -DDART_COVERALLS=$COVERALLS .. +make -j4 tutorials examples tests +if [ $COVERALLS = ON ]; then make -j4 coveralls; else make -j4 test; fi + diff --git a/ci/script_pull_request.sh b/ci/script_pull_request.sh new file mode 100644 index 0000000000000..b948ce5e6585f --- /dev/null +++ b/ci/script_pull_request.sh @@ -0,0 +1,27 @@ +sudo apt-add-repository --yes ppa:libccd-debs/ppa +sudo apt-add-repository --yes ppa:fcl-debs/ppa +sudo apt-add-repository --yes ppa:dartsim/ppa +sudo apt-get -qq update + +APT=' +cmake +libassimp-dev +libboost-all-dev +libccd-dev +libeigen3-dev +libfcl-dev +freeglut3-dev +libxi-dev +libxmu-dev +libbullet-dev +libflann-dev +libnlopt-dev +coinor-libipopt-dev +libtinyxml-dev +libtinyxml2-dev +liburdfdom-dev +liburdfdom-headers-dev +libopenscenegraph-dev +' + +sudo apt-get -qq --yes --force-yes install $APT From 93fefdb74a381bfa3025ac08f8450b258b7898dc Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 25 Apr 2016 14:10:32 -0400 Subject: [PATCH 6/9] Run full build tests only for pull request builds -- completing the previous commit --- .travis.yml | 7 +------ ci/install.sh | 5 +++++ ci/script_pull_request.sh | 27 --------------------------- 3 files changed, 6 insertions(+), 33 deletions(-) create mode 100644 ci/install.sh delete mode 100644 ci/script_pull_request.sh diff --git a/.travis.yml b/.travis.yml index 42b5605e2d10a..ae579d80959e5 100644 --- a/.travis.yml +++ b/.travis.yml @@ -4,10 +4,6 @@ sudo: required dist: trusty -env: - - BUILD_TYPE=Debug TREAT_WARNINGS_AS_ERRORS=ON COVERALLS=ON - - BUILD_TYPE=Release TREAT_WARNINGS_AS_ERRORS=ON COVERALLS=OFF - matrix: include: - os: linux @@ -30,8 +26,7 @@ matrix: env: BUILD_TYPE=Release COVERALLS=OFF COMPILER=CLANG install: - - if [ "$TRAVIS_OS_NAME" = "linux" ]; then 'ci/install_linux.sh' ; fi - - if [ "$TRAVIS_OS_NAME" = "osx" ]; then 'ci/install_osx.sh' ; fi + - 'ci/install.sh' script: - 'ci/scrip.sh' diff --git a/ci/install.sh b/ci/install.sh new file mode 100644 index 0000000000000..bbad659f0374c --- /dev/null +++ b/ci/install.sh @@ -0,0 +1,5 @@ +if [ "$TRAVIS_PULL_REQUEST" = "false" ] && [ "$COMPILER" = "CLANG" ]; then exit; fi + +if [ "$TRAVIS_OS_NAME" = "linux" ]; then 'ci/install_linux.sh' ; fi +if [ "$TRAVIS_OS_NAME" = "osx" ]; then 'ci/install_osx.sh' ; fi + diff --git a/ci/script_pull_request.sh b/ci/script_pull_request.sh deleted file mode 100644 index b948ce5e6585f..0000000000000 --- a/ci/script_pull_request.sh +++ /dev/null @@ -1,27 +0,0 @@ -sudo apt-add-repository --yes ppa:libccd-debs/ppa -sudo apt-add-repository --yes ppa:fcl-debs/ppa -sudo apt-add-repository --yes ppa:dartsim/ppa -sudo apt-get -qq update - -APT=' -cmake -libassimp-dev -libboost-all-dev -libccd-dev -libeigen3-dev -libfcl-dev -freeglut3-dev -libxi-dev -libxmu-dev -libbullet-dev -libflann-dev -libnlopt-dev -coinor-libipopt-dev -libtinyxml-dev -libtinyxml2-dev -liburdfdom-dev -liburdfdom-headers-dev -libopenscenegraph-dev -' - -sudo apt-get -qq --yes --force-yes install $APT From a4e06543804fdb54429a890e06b62151eede2387 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 25 Apr 2016 14:14:34 -0400 Subject: [PATCH 7/9] Add missing return statement --- dart/constraint/ConstraintSolver.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index e35b4a26cfad3..dfa8514619b0e 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -228,6 +228,7 @@ void ConstraintSolver::setCollisionDetector( dtwarn << "[ConstraintSolver::setCollisionDetector] Attempting to assign " << "nullptr as the new collision detector to the constraint solver, " << "which is not allowed. Ignoring.\n"; + return; } if (mCollisionDetector == collisionDetector) From c9639335480007f9711734c74434052c80dacdda Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 25 Apr 2016 16:24:40 -0400 Subject: [PATCH 8/9] Change filemode executable --- ci/install.sh | 0 ci/script.sh | 0 2 files changed, 0 insertions(+), 0 deletions(-) mode change 100644 => 100755 ci/install.sh mode change 100644 => 100755 ci/script.sh diff --git a/ci/install.sh b/ci/install.sh old mode 100644 new mode 100755 diff --git a/ci/script.sh b/ci/script.sh old mode 100644 new mode 100755 From 7db3fbd56c8c9aebd15ec78e7f6dbe38508d7f1d Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 25 Apr 2016 16:28:58 -0400 Subject: [PATCH 9/9] Fix typo in .travis.yml --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index ae579d80959e5..d8ccaa637727e 100644 --- a/.travis.yml +++ b/.travis.yml @@ -29,7 +29,7 @@ install: - 'ci/install.sh' script: - - 'ci/scrip.sh' + - 'ci/script.sh' after_failure: - cat Testing/Temporary/LastTest.log