Skip to content

Commit

Permalink
Renaming constraint files
Browse files Browse the repository at this point in the history
  • Loading branch information
jeking04 committed Apr 22, 2016
1 parent e978729 commit 47a764b
Show file tree
Hide file tree
Showing 23 changed files with 240 additions and 240 deletions.
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
#ifndef AIKIDO_CONSTRAINT_FRAMECONSTRAINTADAPTOR_HPP_
#define AIKIDO_CONSTRAINT_FRAMECONSTRAINTADAPTOR_HPP_
#ifndef AIKIDO_CONSTRAINT_FRAMEDIFFERENTIABLE_HPP_
#define AIKIDO_CONSTRAINT_FRAMEDIFFERENTIABLE_HPP_

#include <dart/dynamics/dynamics.h>
#include "Differentiable.hpp"
Expand All @@ -16,7 +16,7 @@ namespace constraint{
/// 1) Differentiable
/// 2) in SE3.
/// 2) constrains _jacobianNode's pose in World Frame.
class FrameConstraintAdaptor: public Differentiable
class FrameDifferentiable: public Differentiable
{
public:

Expand All @@ -26,7 +26,7 @@ class FrameConstraintAdaptor: public Differentiable
/// \param _jacobianNode The frame being constrained.
/// \param _poseConstraint Constraint on _jacobian. This should be
/// in SE3.
FrameConstraintAdaptor(
FrameDifferentiable(
statespace::dart::MetaSkeletonStateSpacePtr _metaSkeletonStateSpace,
dart::dynamics::ConstJacobianNodePtr _jacobianNode,
DifferentiablePtr _poseConstraint);
Expand Down
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
#ifndef AIKIDO_CONSTRAINT_FRAMEPAIRCONSTRAINTADAPTOR_HPP_
#define AIKIDO_CONSTRAINT_FRAMEPAIRCONSTRAINTADAPTOR_HPP_
#ifndef AIKIDO_CONSTRAINT_FRAMEPAIRDIFFERENTIABLE_HPP_
#define AIKIDO_CONSTRAINT_FRAMEPAIRDIFFERENTIABLE_HPP_

#include <dart/dynamics/dynamics.h>
#include "Differentiable.hpp"
Expand All @@ -15,7 +15,7 @@ namespace constraint{
/// 1) Differentiable
/// 2) in SE3.
/// 2) constrains _jacobianNodeTarget's pose in jacobianNodeBase's frame.
class FramePairConstraintAdaptor: public Differentiable
class FramePairDifferentiable: public Differentiable
{
public:

Expand All @@ -27,7 +27,7 @@ class FramePairConstraintAdaptor: public Differentiable
/// \param _jacobianNodeBase The base frame for this constraint.
/// \param _relPoseConstraint Relative pose constraint on _jacobianNodeTarget
/// w.r.t. _jacobianNodeBase.
FramePairConstraintAdaptor(
FramePairDifferentiable(
statespace::dart::MetaSkeletonStateSpacePtr _metaSkeletonStateSpace,
dart::dynamics::ConstJacobianNodePtr _jacobianNodeTarget,
dart::dynamics::ConstJacobianNodePtr _jacobianNodeBase,
Expand Down
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
#ifndef AIKIDO_CONSTRAINT_FKTESTABLE_HPP_
#define AIKIDO_CONSTRAINT_FKTESTABLE_HPP_
#ifndef AIKIDO_CONSTRAINT_FRAMETESTABLE_HPP_
#define AIKIDO_CONSTRAINT_FRAMETESTABLE_HPP_

#include "Testable.hpp"
#include "../statespace/SE3.hpp"
Expand All @@ -12,14 +12,14 @@ namespace constraint {
/// Transforms a SE(3) Testable into a MetaSkeleton-Testable by
/// performing forward kinematics on a configuration (metaskeleton state)
/// and checking the resulting SE(3) pose of the asked frame.
class FkTestable : public Testable
class FrameTestable : public Testable
{
public:
/// Creat a Testable for the MetaSkeleton.
/// \param _stateSpace Configuration space of the metaskeleton.
/// \param _frame Frame constrained by _poseConstraint.
/// \param _poseConstraint A testable constraint on _frame.
FkTestable(statespace::dart::MetaSkeletonStateSpacePtr _stateSpace,
FrameTestable(statespace::dart::MetaSkeletonStateSpacePtr _stateSpace,
dart::dynamics::ConstJacobianNodePtr _frame,
TestablePtr _poseConstraint);

Expand Down
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
#ifndef AIKIDO_CONSTRAINT_DIFFERENTIABLEPROJECTOR_HPP_
#define AIKIDO_CONSTRAINT_DIFFERENTIABLEPROJECTOR_HPP_
#ifndef AIKIDO_CONSTRAINT_NEWTONSMETHODPROJECTABLE_HPP_
#define AIKIDO_CONSTRAINT_NEWTONSMETHODPROJECTABLE_HPP_

#include <Eigen/Dense>
#include "Projectable.hpp"
Expand All @@ -9,7 +9,7 @@ namespace aikido {
namespace constraint{

/// Uses Newton's method to project state.
class DifferentiableProjector : public Projectable
class NewtonsMethodProjectable : public Projectable
{
public:

Expand All @@ -22,7 +22,7 @@ class DifferentiableProjector : public Projectable
/// dimension.
/// \param _maxIteration Max iteration for Newton's method.
/// \param _minStepSize Minimum step size to be taken in Newton's method.
DifferentiableProjector(
NewtonsMethodProjectable(
DifferentiablePtr _differentiable,
std::vector<double> _tolerance,
int _maxIteration=1000,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@ namespace constraint {
/// Testable for CompoundStates.
/// It takes in a set of Testables and test i-th substate on
/// the i-th Testable.
class TestableSubSpace : public Testable
class TestableSubspace : public Testable
{
public:

Expand All @@ -19,7 +19,7 @@ class TestableSubSpace : public Testable
/// \param _constraints Set of testables. The size of _constraints
/// should match the number of subspaces in _stateSpace.
/// i-th constraint applies to i-th subspace.
TestableSubSpace(
TestableSubspace(
std::shared_ptr<statespace::CartesianProduct> _stateSpace,
std::vector<TestablePtr> _constraints);

Expand Down
6 changes: 3 additions & 3 deletions aikido/include/aikido/constraint/detail/dart.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@
#include "../../statespace/dart/SE2Joint.hpp"
#include "../../statespace/dart/SE3Joint.hpp"
#include "../../util/metaprogramming.hpp"
#include "../uniform/RealVectorBoxConstraint.hpp"
#include "../uniform/RnBoxConstraint.hpp"
#include "../uniform/SO2UniformSampler.hpp"
#include "../uniform/SO3UniformSampler.hpp"
#include "../Satisfied.hpp"
Expand Down Expand Up @@ -80,7 +80,7 @@ std::unique_ptr<OutputConstraint> createBoxConstraint(
const auto joint = _stateSpace->getJoint();

if (isLimited(joint))
return dart::common::make_unique<statespace::RealVectorBoxConstraint>(
return dart::common::make_unique<statespace::RnBoxConstraint>(
std::move(_stateSpace), std::move(_rng),
getPositionLowerLimits(joint), getPositionUpperLimits(joint));
else
Expand Down Expand Up @@ -138,7 +138,7 @@ struct createSampleableFor_impl<statespace::dart::RnJoint>
const auto joint = _stateSpace->getJoint();

if (isLimited(joint))
return dart::common::make_unique<statespace::RealVectorBoxConstraint>(
return dart::common::make_unique<statespace::RnBoxConstraint>(
std::move(_stateSpace), std::move(_rng),
getPositionLowerLimits(joint), getPositionUpperLimits(joint));
else
Expand Down
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
#ifndef AIKIDO_STATESPACE_REALVECTORSTATESPACESAMPLEABLECONSTRAINT_H_
#define AIKIDO_STATESPACE_REALVECTORSTATESPACESAMPLEABLECONSTRAINT_H_
#ifndef AIKIDO_STATESPACE_RNBOXCONSTRAINT_H_
#define AIKIDO_STATESPACE_RNBOXCONSTRAINT_H_
#include "../../statespace/Rn.hpp"
#include "../Differentiable.hpp"
#include "../Projectable.hpp"
Expand All @@ -11,7 +11,7 @@ namespace statespace {

/// A BoxConstraint on RealVectorStates.
/// For each dimension, this constraint has lowerLimit and upperLimit.
class RealVectorBoxConstraint
class RnBoxConstraint
: public constraint::Differentiable
, public constraint::Projectable
, public constraint::Sampleable
Expand All @@ -25,7 +25,7 @@ class RealVectorBoxConstraint
/// The length of this vector should match the dimension of _space.
/// \param _upperLimits Upper limits.
/// The length of this vector should match the dimension of _space.
RealVectorBoxConstraint(
RnBoxConstraint(
std::shared_ptr<statespace::Rn> _space,
std::unique_ptr<util::RNG> _rng,
const Eigen::VectorXd& _lowerLimits,
Expand Down
12 changes: 6 additions & 6 deletions aikido/src/constraint/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -1,15 +1,15 @@
aikido_add_library("${PROJECT_NAME}_constraint"
uniform/RealVectorBoxConstraint.cpp
uniform/RnBoxConstraint.cpp
uniform/SO2UniformSampler.cpp
uniform/SO3UniformSampler.cpp
Differentiable.cpp
DifferentiableProjector.cpp
NewtonsMethodProjectable.cpp
DifferentiableSubSpace.cpp
FiniteSampleable.cpp
CyclicSampleable.cpp
FkTestable.cpp
FrameConstraintAdaptor.cpp
FramePairConstraintAdaptor.cpp
FrameTestable.cpp
FrameDifferentiable.cpp
FramePairDifferentiable.cpp
InverseKinematicsSampleable.cpp
NonColliding.cpp
Projectable.cpp
Expand All @@ -20,7 +20,7 @@ aikido_add_library("${PROJECT_NAME}_constraint"
DifferentiableIntersection.cpp
TSR.cpp
TestableIntersection.cpp
TestableSubSpace.cpp
TestableSubspace.cpp
dart.cpp
)
target_link_libraries("${PROJECT_NAME}_constraint"
Expand Down
Original file line number Diff line number Diff line change
@@ -1,12 +1,12 @@
#include <aikido/constraint/FrameConstraintAdaptor.hpp>
#include <aikido/constraint/FrameDifferentiable.hpp>
#include <aikido/statespace/SE3.hpp>

namespace aikido {
namespace constraint {


//=============================================================================
FrameConstraintAdaptor::FrameConstraintAdaptor(
FrameDifferentiable::FrameDifferentiable(
statespace::dart::MetaSkeletonStateSpacePtr _metaSkeletonStateSpace,
dart::dynamics::ConstJacobianNodePtr _jacobianNode,
DifferentiablePtr _poseConstraint)
Expand Down Expand Up @@ -41,13 +41,13 @@ FrameConstraintAdaptor::FrameConstraintAdaptor(
}

//=============================================================================
size_t FrameConstraintAdaptor::getConstraintDimension() const
size_t FrameDifferentiable::getConstraintDimension() const
{
return mPoseConstraint->getConstraintDimension();
}

//=============================================================================
Eigen::VectorXd FrameConstraintAdaptor::getValue(
Eigen::VectorXd FrameDifferentiable::getValue(
const statespace::StateSpace::State* _s) const
{
using State = statespace::CartesianProduct::State;
Expand All @@ -64,7 +64,7 @@ Eigen::VectorXd FrameConstraintAdaptor::getValue(
}

//=============================================================================
Eigen::MatrixXd FrameConstraintAdaptor::getJacobian(
Eigen::MatrixXd FrameDifferentiable::getJacobian(
const statespace::StateSpace::State* _s) const
{
using State = statespace::CartesianProduct::State;
Expand All @@ -90,7 +90,7 @@ Eigen::MatrixXd FrameConstraintAdaptor::getJacobian(
}

//=============================================================================
std::pair<Eigen::VectorXd, Eigen::MatrixXd> FrameConstraintAdaptor::getValueAndJacobian(
std::pair<Eigen::VectorXd, Eigen::MatrixXd> FrameDifferentiable::getValueAndJacobian(
const statespace::StateSpace::State* _s) const
{
using State = statespace::CartesianProduct::State;
Expand Down Expand Up @@ -119,14 +119,14 @@ std::pair<Eigen::VectorXd, Eigen::MatrixXd> FrameConstraintAdaptor::getValueAndJ
}

//=============================================================================
std::vector<ConstraintType> FrameConstraintAdaptor::getConstraintTypes() const
std::vector<ConstraintType> FrameDifferentiable::getConstraintTypes() const
{
return mPoseConstraint->getConstraintTypes();
}


//=============================================================================
statespace::StateSpacePtr FrameConstraintAdaptor::getStateSpace() const
statespace::StateSpacePtr FrameDifferentiable::getStateSpace() const
{
return mMetaSkeletonStateSpace;
}
Expand Down
Original file line number Diff line number Diff line change
@@ -1,12 +1,12 @@
#include <aikido/constraint/FramePairConstraintAdaptor.hpp>
#include <aikido/constraint/FramePairDifferentiable.hpp>
#include <aikido/statespace/SE3.hpp>

namespace aikido {
namespace constraint {


//=============================================================================
FramePairConstraintAdaptor::FramePairConstraintAdaptor(
FramePairDifferentiable::FramePairDifferentiable(
statespace::dart::MetaSkeletonStateSpacePtr _metaSkeletonStateSpace,
dart::dynamics::ConstJacobianNodePtr _jacobianNode1,
dart::dynamics::ConstJacobianNodePtr _jacobianNode2,
Expand Down Expand Up @@ -47,13 +47,13 @@ FramePairConstraintAdaptor::FramePairConstraintAdaptor(
}

//=============================================================================
size_t FramePairConstraintAdaptor::getConstraintDimension() const
size_t FramePairDifferentiable::getConstraintDimension() const
{
return mRelPoseConstraint->getConstraintDimension();
}

//=============================================================================
Eigen::VectorXd FramePairConstraintAdaptor::getValue(
Eigen::VectorXd FramePairDifferentiable::getValue(
const statespace::StateSpace::State* _s) const
{
using State = statespace::CartesianProduct::State;
Expand All @@ -73,7 +73,7 @@ Eigen::VectorXd FramePairConstraintAdaptor::getValue(


//=============================================================================
Eigen::MatrixXd FramePairConstraintAdaptor::getJacobian(
Eigen::MatrixXd FramePairDifferentiable::getJacobian(
const statespace::StateSpace::State* _s) const
{
using State = statespace::CartesianProduct::State;
Expand Down Expand Up @@ -106,7 +106,7 @@ Eigen::MatrixXd FramePairConstraintAdaptor::getJacobian(


//=============================================================================
std::pair<Eigen::VectorXd, Eigen::MatrixXd> FramePairConstraintAdaptor::getValueAndJacobian(
std::pair<Eigen::VectorXd, Eigen::MatrixXd> FramePairDifferentiable::getValueAndJacobian(
const statespace::StateSpace::State* _s) const
{
using State = statespace::CartesianProduct::State;
Expand Down Expand Up @@ -144,14 +144,14 @@ std::pair<Eigen::VectorXd, Eigen::MatrixXd> FramePairConstraintAdaptor::getValue


//=============================================================================
std::vector<ConstraintType> FramePairConstraintAdaptor::getConstraintTypes() const
std::vector<ConstraintType> FramePairDifferentiable::getConstraintTypes() const
{
return mRelPoseConstraint->getConstraintTypes();
}


//=============================================================================
statespace::StateSpacePtr FramePairConstraintAdaptor::getStateSpace() const
statespace::StateSpacePtr FramePairDifferentiable::getStateSpace() const
{
return mMetaSkeletonStateSpace;
}
Expand Down
Original file line number Diff line number Diff line change
@@ -1,10 +1,10 @@
#include <aikido/constraint/FkTestable.hpp>
#include <aikido/constraint/FrameTestable.hpp>

namespace aikido {
namespace constraint {

//=============================================================================
FkTestable::FkTestable(statespace::dart::MetaSkeletonStateSpacePtr _stateSpace,
FrameTestable::FrameTestable(statespace::dart::MetaSkeletonStateSpacePtr _stateSpace,
dart::dynamics::ConstJacobianNodePtr _frame,
TestablePtr _poseConstraint)
: mStateSpace(std::move(_stateSpace))
Expand Down Expand Up @@ -33,7 +33,7 @@ FkTestable::FkTestable(statespace::dart::MetaSkeletonStateSpacePtr _stateSpace,
}

//=============================================================================
bool FkTestable::isSatisfied(const statespace::StateSpace::State* _state) const
bool FrameTestable::isSatisfied(const statespace::StateSpace::State* _state) const
{
// Set the state
auto state
Expand All @@ -48,7 +48,7 @@ bool FkTestable::isSatisfied(const statespace::StateSpace::State* _state) const
}

//=============================================================================
std::shared_ptr<statespace::StateSpace> FkTestable::getStateSpace() const
std::shared_ptr<statespace::StateSpace> FrameTestable::getStateSpace() const
{
return mStateSpace;
}
Expand Down
Loading

0 comments on commit 47a764b

Please sign in to comment.