Robowflex  v0.1
Making MoveIt Easy
so3joint.cpp
Go to the documentation of this file.
1 /* Author: Zachary Kingston */
2 
4 
6 #include <robowflex_dart/space.h>
7 
9 using namespace robowflex::darts;
10 
12  dart::dynamics::Joint *joint)
13  : Joint(space, joint, 4)
14 {
15  space_->addDimension(-1., 1.); // q w
16  space_->addDimension(-1., 1.); // q x
17  space_->addDimension(-1., 1.); // q y
18  space_->addDimension(-1., 1.); // q z
19 }
20 
21 Eigen::Quaterniond SO3Joint::toQuat(const Eigen::Ref<const Eigen::VectorXd> &a) const
22 {
23  return Eigen::Quaterniond(a[0], a[1], a[2], a[3]);
24 }
25 
26 void SO3Joint::setQuat(Eigen::Ref<Eigen::VectorXd> a, const Eigen::Quaterniond &q) const
27 {
28  a[0] = q.w();
29  a[1] = q.x();
30  a[2] = q.y();
31  a[3] = q.z();
32 }
33 
34 // L1
35 double SO3Joint::distance(const Eigen::Ref<const Eigen::VectorXd> &a,
36  const Eigen::Ref<const Eigen::VectorXd> &b) const
37 {
38  return toQuat(a).angularDistance(toQuat(b));
39 }
40 
42 {
43  return constants::half_pi;
44 }
45 
46 void SO3Joint::interpolate(const Eigen::Ref<const Eigen::VectorXd> &a, //
47  const Eigen::Ref<const Eigen::VectorXd> &b, //
48  double t, //
49  Eigen::Ref<Eigen::VectorXd> c) const
50 {
51  auto slerp = toQuat(a).slerp(t, toQuat(b));
52  setQuat(c, slerp);
53 }
54 
55 void SO3Joint::enforceBounds(Eigen::Ref<Eigen::VectorXd> a) const
56 {
57  auto q = toQuat(a).normalized();
58  setQuat(a, q);
59 }
60 
61 bool SO3Joint::satisfiesBounds(const Eigen::Ref<const Eigen::VectorXd> &a) const
62 {
63  double n = a.norm();
64  return (n - 1.) < constants::eps;
65 }
66 
67 void SO3Joint::sample(Eigen::Ref<Eigen::VectorXd> a) const
68 {
69  double q[4];
70  rng_.quaternion(q);
71  a[0] = q[3];
72  a[1] = q[0];
73  a[2] = q[1];
74  a[3] = q[2];
75 }
76 
77 void SO3Joint::sampleNear(Eigen::Ref<Eigen::VectorXd> a, //
78  const Eigen::Ref<const Eigen::VectorXd> &near, //
79  double r) const
80 {
81  if (r >= constants::quarter_pi)
82  sample(a);
83 
84  else
85  {
86  double d = rng_.uniform01();
87  auto qnear = toQuat(near);
88  Eigen::Quaterniond q( //
89  Eigen::AngleAxisd(2. * std::pow(d, constants::third) * r, //
90  Eigen::Vector3d{rng_.gaussian01(), rng_.gaussian01(), rng_.gaussian01()}) //
91  );
92  auto qs = qnear * q;
93  setQuat(a, qs);
94  }
95 }
96 
97 void SO3Joint::setJointState(WorldPtr world, const Eigen::Ref<const Eigen::VectorXd> &a) const
98 {
99  auto *joint = getJoint(world);
100  auto *j = static_cast<dart::dynamics::FreeJoint *>(joint);
101 
102  Eigen::Isometry3d tf;
103  tf.translation() = j->getPositions();
104  tf.linear() = toQuat(a).toRotationMatrix();
105 
106  j->setRelativeTransform(tf);
107 }
108 
109 void SO3Joint::getJointState(WorldPtr world, Eigen::Ref<Eigen::VectorXd> a) const
110 {
111  auto *joint = getJoint(world);
112 
113  Eigen::Isometry3d tf = joint->getRelativeTransform();
114  Eigen::Quaterniond q(tf.linear());
115 
116  setQuat(a, q);
117 }
Abstract controllable joint for robowflex::darts::StateSpace.
Definition: joints.h:38
StateSpace * space_
State space this joint is for.
Definition: joints.h:205
dart::dynamics::Joint * getJoint(WorldPtr world) const
Get the Dart joint from a world.
Definition: joint.cpp:54
ompl::RNG & rng_
Random number generator.
Definition: joints.h:206
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.
Definition: so3joint.cpp:46
SO3Joint(StateSpace *space, dart::dynamics::Joint *joint)
Constructor.
Definition: so3joint.cpp:11
void setQuat(Eigen::Ref< Eigen::VectorXd > a, const Eigen::Quaterniond &q) const
Set the joint configuration to a quaternion.
Definition: so3joint.cpp:26
void enforceBounds(Eigen::Ref< Eigen::VectorXd > a) const override
Enforce bounds on a joint configuration.
Definition: so3joint.cpp:55
double getMaximumExtent() const override
Get the maximum extent of this joint (maximum distance).
Definition: so3joint.cpp:41
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.
Definition: so3joint.cpp:35
void sample(Eigen::Ref< Eigen::VectorXd > a) const override
Sample a configuration for this joint.
Definition: so3joint.cpp:67
void setJointState(WorldPtr world, const Eigen::Ref< const Eigen::VectorXd > &a) const override
Set the state of the joint in world.
Definition: so3joint.cpp:97
void getJointState(WorldPtr world, Eigen::Ref< Eigen::VectorXd > a) const override
Get the state of the joint in world.
Definition: so3joint.cpp:109
Eigen::Quaterniond toQuat(const Eigen::Ref< const Eigen::VectorXd > &a) const
Get the quaternion corresponding to the joint configuration.
Definition: so3joint.cpp:21
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.
Definition: so3joint.cpp:77
bool satisfiesBounds(const Eigen::Ref< const Eigen::VectorXd > &a) const override
Check if a joint configuration satisfies bounds.
Definition: so3joint.cpp:61
An OMPL state space for robowflex::darts::World. Can do motion planning for any of the robots or stru...
Definition: space.h:31
A shared pointer wrapper for robowflex::darts::World.
static const double half_pi
Definition: constants.h:22
static const double quarter_pi
Definition: constants.h:23
static const double third
Definition: constants.h:15
static const double eps
Definition: constants.h:16
DART-based robot modeling and planning.
Definition: acm.h:16
T pow(T... args)