12 dart::dynamics::Joint *joint)
13 :
Joint(space, joint, 4)
15 space_->addDimension(-1., 1.);
16 space_->addDimension(-1., 1.);
17 space_->addDimension(-1., 1.);
18 space_->addDimension(-1., 1.);
23 return Eigen::Quaterniond(a[0], a[1], a[2], a[3]);
36 const Eigen::Ref<const Eigen::VectorXd> &b)
const
47 const Eigen::Ref<const Eigen::VectorXd> &b,
49 Eigen::Ref<Eigen::VectorXd> c)
const
57 auto q =
toQuat(a).normalized();
78 const Eigen::Ref<const Eigen::VectorXd> &near,
86 double d =
rng_.uniform01();
90 Eigen::Vector3d{
rng_.gaussian01(),
rng_.gaussian01(),
rng_.gaussian01()})
100 auto *j =
static_cast<dart::dynamics::FreeJoint *
>(joint);
102 Eigen::Isometry3d tf;
103 tf.translation() = j->getPositions();
104 tf.linear() =
toQuat(a).toRotationMatrix();
106 j->setRelativeTransform(tf);
113 Eigen::Isometry3d tf = joint->getRelativeTransform();
114 Eigen::Quaterniond q(tf.linear());
Abstract controllable joint for robowflex::darts::StateSpace.
StateSpace * space_
State space this joint is for.
dart::dynamics::Joint * getJoint(WorldPtr world) const
Get the Dart joint from a world.
ompl::RNG & rng_
Random number generator.
void interpolate(const Eigen::Ref< const Eigen::VectorXd > &a, const Eigen::Ref< const Eigen::VectorXd > &b, double t, Eigen::Ref< Eigen::VectorXd > c) const override
Interpolate to a new configuration c which is t from a to b.
SO3Joint(StateSpace *space, dart::dynamics::Joint *joint)
Constructor.
void setQuat(Eigen::Ref< Eigen::VectorXd > a, const Eigen::Quaterniond &q) const
Set the joint configuration to a quaternion.
void enforceBounds(Eigen::Ref< Eigen::VectorXd > a) const override
Enforce bounds on a joint configuration.
double getMaximumExtent() const override
Get the maximum extent of this joint (maximum distance).
double distance(const Eigen::Ref< const Eigen::VectorXd > &a, const Eigen::Ref< const Eigen::VectorXd > &b) const override
Compute the distance between two joint configurations.
void sample(Eigen::Ref< Eigen::VectorXd > a) const override
Sample a configuration for this joint.
void setJointState(WorldPtr world, const Eigen::Ref< const Eigen::VectorXd > &a) const override
Set the state of the joint in world.
void getJointState(WorldPtr world, Eigen::Ref< Eigen::VectorXd > a) const override
Get the state of the joint in world.
Eigen::Quaterniond toQuat(const Eigen::Ref< const Eigen::VectorXd > &a) const
Get the quaternion corresponding to the joint configuration.
void sampleNear(Eigen::Ref< Eigen::VectorXd > a, const Eigen::Ref< const Eigen::VectorXd > &near, double r) const override
Sample a configuration for this joint near another configuration near.
bool satisfiesBounds(const Eigen::Ref< const Eigen::VectorXd > &a) const override
Check if a joint configuration satisfies bounds.
An OMPL state space for robowflex::darts::World. Can do motion planning for any of the robots or stru...
A shared pointer wrapper for robowflex::darts::World.
static const double half_pi
static const double quarter_pi
static const double third
DART-based robot modeling and planning.