Skip to content

Commit

Permalink
Renamed RealVectorJointStateSpace -> RnJoint
Browse files Browse the repository at this point in the history
  • Loading branch information
Michael Koval committed Apr 22, 2016
1 parent 2ef4992 commit 4faac5a
Show file tree
Hide file tree
Showing 6 changed files with 38 additions and 38 deletions.
22 changes: 11 additions & 11 deletions aikido/include/aikido/constraint/detail/dart.hpp
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
#include <sstream>
#include <dart/common/StlHelpers.h>
#include "../../statespace/dart/RealVectorJointStateSpace.hpp"
#include "../../statespace/dart/RnJoint.hpp"
#include "../../statespace/dart/SO2JointStateSpace.hpp"
#include "../../statespace/dart/SO3JointStateSpace.hpp"
#include "../../statespace/dart/SE2JointStateSpace.hpp"
Expand Down Expand Up @@ -52,7 +52,7 @@ inline Eigen::VectorXd getPositionUpperLimits(

//=============================================================================
using JointStateSpaceTypeList = util::type_list<
statespace::dart::RealVectorJointStateSpace,
statespace::dart::RnJoint,
statespace::dart::SO2JointStateSpace,
statespace::dart::SO3JointStateSpace,
statespace::dart::SE2JointStateSpace,
Expand All @@ -74,7 +74,7 @@ struct createSampleableFor_impl { };
//=============================================================================
template <class OutputConstraint>
std::unique_ptr<OutputConstraint> createBoxConstraint(
std::shared_ptr<statespace::dart::RealVectorJointStateSpace> _stateSpace,
std::shared_ptr<statespace::dart::RnJoint> _stateSpace,
std::unique_ptr<util::RNG> _rng)
{
const auto joint = _stateSpace->getJoint();
Expand All @@ -89,9 +89,9 @@ std::unique_ptr<OutputConstraint> createBoxConstraint(
}

template <>
struct createDifferentiableFor_impl<statespace::dart::RealVectorJointStateSpace>
struct createDifferentiableFor_impl<statespace::dart::RnJoint>
{
using StateSpace = statespace::dart::RealVectorJointStateSpace;
using StateSpace = statespace::dart::RnJoint;
using StateSpacePtr = std::shared_ptr<StateSpace>;

static std::unique_ptr<Differentiable> create(StateSpacePtr _stateSpace)
Expand All @@ -101,9 +101,9 @@ struct createDifferentiableFor_impl<statespace::dart::RealVectorJointStateSpace>
};

template <>
struct createTestableFor_impl<statespace::dart::RealVectorJointStateSpace>
struct createTestableFor_impl<statespace::dart::RnJoint>
{
using StateSpace = statespace::dart::RealVectorJointStateSpace;
using StateSpace = statespace::dart::RnJoint;
using StateSpacePtr = std::shared_ptr<StateSpace>;

static std::unique_ptr<TestableConstraint> create(StateSpacePtr _stateSpace)
Expand All @@ -114,9 +114,9 @@ struct createTestableFor_impl<statespace::dart::RealVectorJointStateSpace>
};

template <>
struct createProjectableFor_impl<statespace::dart::RealVectorJointStateSpace>
struct createProjectableFor_impl<statespace::dart::RnJoint>
{
using StateSpace = statespace::dart::RealVectorJointStateSpace;
using StateSpace = statespace::dart::RnJoint;
using StateSpacePtr = std::shared_ptr<StateSpace>;

static std::unique_ptr<Projectable> create(StateSpacePtr _stateSpace)
Expand All @@ -127,9 +127,9 @@ struct createProjectableFor_impl<statespace::dart::RealVectorJointStateSpace>
};

template <>
struct createSampleableFor_impl<statespace::dart::RealVectorJointStateSpace>
struct createSampleableFor_impl<statespace::dart::RnJoint>
{
using StateSpace = statespace::dart::RealVectorJointStateSpace;
using StateSpace = statespace::dart::RnJoint;
using StateSpacePtr = std::shared_ptr<StateSpace>;

static std::unique_ptr<SampleableConstraint> create(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,18 +15,18 @@ namespace dart {
/// best modelled as having an SE(3) state space. If you are not sure what type
/// of \c JointStateSpace to for a \c Joint you most likely should use
/// the \c createJointStateSpace helper function.
class RealVectorJointStateSpace
class RnJoint
: public Rn
, public JointStateSpace
, public std::enable_shared_from_this<RealVectorJointStateSpace>
, public std::enable_shared_from_this<RnJoint>
{
public:
using Rn::State;

/// Create a real vector state space for \c _joint.
///
/// \param _joint joint to create a state space for
explicit RealVectorJointStateSpace(::dart::dynamics::Joint* _joint);
explicit RnJoint(::dart::dynamics::Joint* _joint);

// Documentation inherited.
void convertPositionsToState(
Expand Down
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
#include <memory>
#include <dart/common/StlHelpers.h>
#include "../RealVectorJointStateSpace.hpp"
#include "../RnJoint.hpp"
#include "../SO2JointStateSpace.hpp"
#include "../SO3JointStateSpace.hpp"
#include "../SE2JointStateSpace.hpp"
Expand Down Expand Up @@ -28,7 +28,7 @@ struct createJointStateSpaceFor_impl<::dart::dynamics::RevoluteJoint>
if (_joint->isCyclic(0))
return make_unique<SO2JointStateSpace>(_joint);
else
return make_unique<RealVectorJointStateSpace>(_joint);
return make_unique<RnJoint>(_joint);
}
};

Expand All @@ -38,7 +38,7 @@ struct createJointStateSpaceFor_impl<::dart::dynamics::PrismaticJoint>
{
static Ptr create(::dart::dynamics::PrismaticJoint* _joint)
{
return make_unique<RealVectorJointStateSpace>(_joint);
return make_unique<RnJoint>(_joint);
}
};

Expand All @@ -48,7 +48,7 @@ struct createJointStateSpaceFor_impl<::dart::dynamics::TranslationalJoint>
{
static Ptr create(::dart::dynamics::TranslationalJoint* _joint)
{
return make_unique<RealVectorJointStateSpace>(_joint);
return make_unique<RnJoint>(_joint);
}
};

Expand Down
2 changes: 1 addition & 1 deletion aikido/src/statespace/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@ aikido_add_library("${PROJECT_NAME}_statespace"
dart/SE3JointStateSpace.cpp
dart/SO2JointStateSpace.cpp
dart/SO3JointStateSpace.cpp
dart/RealVectorJointStateSpace.cpp
dart/RnJoint.cpp
)
target_link_libraries("${PROJECT_NAME}_statespace"
${DART_LIBRARIES}
Expand Down
Original file line number Diff line number Diff line change
@@ -1,26 +1,26 @@
#include <aikido/statespace/dart/RealVectorJointStateSpace.hpp>
#include <aikido/statespace/dart/RnJoint.hpp>

namespace aikido {
namespace statespace {
namespace dart {

//=============================================================================
RealVectorJointStateSpace::RealVectorJointStateSpace(
RnJoint::RnJoint(
::dart::dynamics::Joint* _joint)
: Rn(_joint->getNumDofs())
, JointStateSpace(_joint)
{
}

//=============================================================================
void RealVectorJointStateSpace::convertPositionsToState(
void RnJoint::convertPositionsToState(
const Eigen::VectorXd& _positions, StateSpace::State* _state) const
{
setValue(static_cast<State*>(_state), _positions);
}

//=============================================================================
void RealVectorJointStateSpace::convertStateToPositions(
void RnJoint::convertStateToPositions(
const StateSpace::State* _state, Eigen::VectorXd& _positions) const
{
_positions = getValue(static_cast<const State*>(_state));
Expand Down
30 changes: 15 additions & 15 deletions aikido/tests/constraint/test_DartHelpers.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@
#include <dart/common/StlHelpers.h>
#include <aikido/constraint/dart.hpp>
#include <aikido/constraint/SatisfiedConstraint.hpp>
#include <aikido/statespace/dart/RealVectorJointStateSpace.hpp>
#include <aikido/statespace/dart/RnJoint.hpp>
#include <aikido/statespace/dart/SO2JointStateSpace.hpp>
#include <aikido/statespace/dart/SO3JointStateSpace.hpp>

Expand All @@ -19,7 +19,7 @@ using dart::dynamics::Skeleton;
using dart::dynamics::SkeletonPtr;
using aikido::constraint::SampleGenerator;
using aikido::constraint::SatisfiedConstraint;
using aikido::statespace::dart::RealVectorJointStateSpace;
using aikido::statespace::dart::RnJoint;
using aikido::statespace::dart::SO2JointStateSpace;
using aikido::statespace::dart::SO3JointStateSpace;
using aikido::util::RNGWrapper;
Expand All @@ -37,7 +37,7 @@ static Vector1d make_vector(double _x)
}

//=============================================================================
class RealVectorJointStateSpaceHelpersTests : public ::testing::Test
class RnJointHelpersTests : public ::testing::Test
{
protected:
static constexpr int NUM_SAMPLES { 1000 };
Expand All @@ -50,18 +50,18 @@ class RealVectorJointStateSpaceHelpersTests : public ::testing::Test
mJoint->setPositionLowerLimit(0, -1.);
mJoint->setPositionUpperLimit(0, 2.);

mStateSpace = std::make_shared<RealVectorJointStateSpace>(mJoint);
mStateSpace = std::make_shared<RnJoint>(mJoint);
}

SkeletonPtr mSkeleton;
RevoluteJoint* mJoint;
std::shared_ptr<RealVectorJointStateSpace> mStateSpace;
std::shared_ptr<RnJoint> mStateSpace;
};

//=============================================================================
TEST_F(RealVectorJointStateSpaceHelpersTests, createTestableBoundsFor)
TEST_F(RnJointHelpersTests, createTestableBoundsFor)
{
auto constraint = createTestableBoundsFor<RealVectorJointStateSpace>(
auto constraint = createTestableBoundsFor<RnJoint>(
mStateSpace);
auto state = mStateSpace->createState();

Expand All @@ -85,12 +85,12 @@ TEST_F(RealVectorJointStateSpaceHelpersTests, createTestableBoundsFor)
}

//=============================================================================
TEST_F(RealVectorJointStateSpaceHelpersTests, createProjectableBounds)
TEST_F(RnJointHelpersTests, createProjectableBounds)
{
auto testableConstraint
= createTestableBoundsFor<RealVectorJointStateSpace>(mStateSpace);
= createTestableBoundsFor<RnJoint>(mStateSpace);
auto projectableConstraint
= createProjectableBoundsFor<RealVectorJointStateSpace>(mStateSpace);
= createProjectableBoundsFor<RnJoint>(mStateSpace);

auto inState = mStateSpace->createState();
auto outState = mStateSpace->createState();
Expand Down Expand Up @@ -121,10 +121,10 @@ TEST_F(RealVectorJointStateSpaceHelpersTests, createProjectableBounds)
}

//=============================================================================
TEST_F(RealVectorJointStateSpaceHelpersTests, createDifferentiableBounds)
TEST_F(RnJointHelpersTests, createDifferentiableBounds)
{
const auto differentiableConstraint
= createDifferentiableBoundsFor<RealVectorJointStateSpace>(mStateSpace);
= createDifferentiableBoundsFor<RnJoint>(mStateSpace);

EXPECT_EQ(mStateSpace, differentiableConstraint->getStateSpace());

Expand Down Expand Up @@ -154,13 +154,13 @@ TEST_F(RealVectorJointStateSpaceHelpersTests, createDifferentiableBounds)
}

//=============================================================================
TEST_F(RealVectorJointStateSpaceHelpersTests, createSampleableBounds)
TEST_F(RnJointHelpersTests, createSampleableBounds)
{
auto rng = make_unique<RNGWrapper<std::default_random_engine>>(0);
const auto testableConstraint
= createTestableBoundsFor<RealVectorJointStateSpace>(mStateSpace);
= createTestableBoundsFor<RnJoint>(mStateSpace);
const auto sampleableConstraint
= createSampleableBoundsFor<RealVectorJointStateSpace>(
= createSampleableBoundsFor<RnJoint>(
mStateSpace, std::move(rng));
ASSERT_TRUE(!!sampleableConstraint);
EXPECT_EQ(mStateSpace, sampleableConstraint->getStateSpace());
Expand Down

0 comments on commit 4faac5a

Please sign in to comment.