/**
* \note also points the outboard link to this joint
*/
void Joint::set_outboard_link(RigidBodyPtr outboard)
{
_outboard_link = outboard;
if (!outboard)
return;
//update spatial axes if both links are set
if (!_outboard_link.expired() && !_inboard_link.expired())
update_spatial_axes();
// use one articulated body pointer to set the other, if possible
if (!outboard->get_articulated_body() && !_abody.expired())
outboard->set_articulated_body(ArticulatedBodyPtr(_abody));
else if (outboard->get_articulated_body() && _abody.expired())
set_articulated_body(ArticulatedBodyPtr(outboard->get_articulated_body()));
// the articulated body pointers must now be equal; it is
// conceivable that the user is updating the art. body pointers in an
// unorthodox manner, but we'll look for this anwyway...
if (!_abody.expired())
{
ArticulatedBodyPtr abody1(outboard->get_articulated_body());
ArticulatedBodyPtr abody2(_abody);
assert(abody1 == abody2);
}
}
/// Sets s bar from si
void Joint::calc_s_bar_from_si()
{
const unsigned SPATIAL_DIM = 6;
const unsigned NDOF = num_dof();
SAFESTATIC MatrixN ns;
SAFESTATIC SMatrix6N sx;
// transform sx to frame located at joint
RigidBodyPtr outboard = get_outboard_link();
if (!outboard)
return;
const Matrix4& To = outboard->get_transform();
Vector3 x = get_position_global();
SpatialTransform(To, IDENTITY_3x3, x).transform(_si, sx);
// setup ns - it's the standard (i.e., non-spatial) transpose of sx
assert(sx.columns() == NDOF);
ns.resize(NDOF, SPATIAL_DIM);
for (unsigned i=0; i< NDOF; i++)
for (unsigned j=0; j< SPATIAL_DIM; j++)
ns(i,j) = sx(j,i);
// compute the nullspace
LinAlg::nullspace(ns, _s_bar);
}
TEFUNC void computeCorrectValuesForOne() {
simulationTime += interval;
double scalar = exp(lambda * simulationTime);
correctPosition = scalar * positionInit;
Vec3 axis;
float angle;
orientationInit.getAxisAngle(axis, angle);
correctOrientation = Quaternion(scalar * angle, axis);
correctAngularVelocity = scalar * angularVelocityInit;
referenceBody->setPosition(correctPosition);
referenceBody->setVelocity(correctVelocity);
referenceBody->setOrientation(correctOrientation);
referenceBody->setAngularVelocity(correctAngularVelocity);
referenceBody->getOrientation().getAxisAngle(axis, angle);
correctRotationAxis = axis;
correctRotationAngle = angle;
cout << "Correct Values:" << endl
<< " Position: " << correctPosition << endl
<< " Velocity: " << correctVelocity << endl
<< " Orientation: " << correctOrientation << endl
<< " AngularVelocity: " << correctAngularVelocity << endl
<< " RotationAngle: " << correctRotationAngle << endl
<< " RotationAxis: " << correctRotationAxis
<< endl << endl;
}
/**
* The primitive is not cloned, nor is it unaltered; <b>this</b> points to
* the pointer returned by this method (typically <b>primitive</b>).
* \return primitive <i>unless the geometry of the underlying primitive is
* inconsistent, degenerate, or non-convex<i>; in that case,
* a corrected primitive will be returned.
*/
PrimitivePtr CollisionGeometry::set_geometry(PrimitivePtr primitive)
{
Quatd EYE;
if (_single_body.expired())
throw std::runtime_error("CollisionGeometry::set_geometry() called before single body set!");
SingleBodyPtr sb(_single_body);
RigidBodyPtr rb = dynamic_pointer_cast<RigidBody>(sb);
if (rb && !Quatd::rel_equal(rb->get_pose()->q, EYE))
{
std::cerr << "CollisionGeometry::set_primitive() warning - rigid body's orientation is not identity." << std::endl;
std::cerr << " At the rigid body's current orientation (" << AAngled(rb->get_pose()->q) << ")" << std::endl;
std::cerr << " the primitive will have the orientation (" << AAngled(primitive->get_pose()->q) << ")" << std::endl;
}
// save the primitive
_geometry = primitive;
// add this to the primitive
CollisionGeometryPtr cg = dynamic_pointer_cast<CollisionGeometry>(shared_from_this());
primitive->add_collision_geometry(cg);
return primitive;
}
/// Updates the spatial axis for this joint
void ScrewJoint::update_spatial_axes()
{
const unsigned X = 0, Y = 1, Z = 2;
// call parent method
Joint::update_spatial_axes();
// make sure the outboard link exists
RigidBodyPtr inboard = get_inboard_link();
RigidBodyPtr outboard = get_outboard_link();
if (!inboard || !outboard)
return;
try
{
// get the joint to com vector in outer link coordinates
const Vector3d& di = outboard->get_inner_joint_data(inboard).joint_to_com_vec_of;
// get the joint axis in outer link coordinates
Vector3d u0 = inboard->get_transform().mult_vector(_u);
Vector3d ui = outboard->get_transform().transpose_mult_vector(u0);
// update the spatial axis in link coordinates
Vector3d x = Vector3d::cross(ui, di);
_s[0].set_angular(ui);
_s[0].set_lower(ui*_pitch + x);
// setup s_bar
calc_s_bar_from_s();
}
catch (std::runtime_error e)
{
// do nothing -- joint data has not yet been set in the link
}
}
/// (Relatively slow) method for determining the joint velocity from current link velocities
void Joint::determine_q_dot()
{
MatrixNd m, U, V;
VectorNd S;
// get the spatial axes
const vector<SVelocityd>& s = get_spatial_axes();
// convert to a matrix
to_matrix(s, m);
// compute the SVD
_LA.svd(m, U, S, V);
// get the velocities in computation frames
RigidBodyPtr inboard = get_inboard_link();
RigidBodyPtr outboard = get_outboard_link();
const SVelocityd& vi = inboard->get_velocity();
const SVelocityd& vo = outboard->get_velocity();
// get velocities in s's frame
shared_ptr<const Pose3d> spose = get_pose();
SVelocityd svi = Pose3d::transform(spose, vi);
SVelocityd svo = Pose3d::transform(spose, vo);
// compute the change in velocity
m.mult(svo - svi, this->qd);
}
/// Determines (and sets) the value of Q from the axes and the inboard link and outboard link transforms
void UniversalJoint::determine_q(VectorNd& q)
{
const unsigned X = 0, Y = 1, Z = 2;
// get the outboard link
RigidBodyPtr outboard = get_outboard_link();
// verify that the outboard link is set
if (!outboard)
throw std::runtime_error("determine_q() called on NULL outboard link!");
// set proper size for q
this->q.resize(num_dof());
// get the poses of the joint and outboard link
shared_ptr<const Pose3d> Fj = get_pose();
shared_ptr<const Pose3d> Fo = outboard->get_pose();
// compute transforms
Transform3d wTo = Pose3d::calc_relative_pose(Fo, GLOBAL);
Transform3d jTw = Pose3d::calc_relative_pose(GLOBAL, Fj);
Transform3d jTo = jTw * wTo;
// determine the joint transformation
Matrix3d R = jTo.q;
// determine q1 and q2 -- they are uniquely determined by examining the rotation matrix
// (see get_rotation())
q.resize(num_dof());
q[DOF_1] = std::atan2(R(Z,Y), R(Y,Y));
q[DOF_2] = std::atan2(R(X,Z), R(X,X));
}
/// Determines (and sets) the value of Q from the axis and the inboard link and outboard link transforms
void ScrewJoint::determine_q(VectorN& q)
{
// get the inboard and outboard link pointers
RigidBodyPtr inboard = get_inboard_link();
RigidBodyPtr outboard = get_outboard_link();
// verify that the inboard and outboard links are set
if (!inboard || !outboard)
{
std::cerr << "ScrewJoint::determine_Q() called on NULL inboard and/or outboard links!" << std::endl;
assert(false);
return;
}
// if axis is not defined, can't use this method
if (std::fabs(_u.norm() - 1.0) > NEAR_ZERO)
{
std::cerr << "ScrewJoint::determine_Q() warning: some axes undefined; aborting..." << std::endl;
return;
}
// get the attachment points on the link (global coords)
Vector3d p1 = get_position_global(false);
Vector3d p2 = get_position_global(true);
// get the joint axis in the global frame
Vector3d ug = inboard->get_transform().mult_vector(_u);
// now, we'll project p2 onto the axis ug; points will be setup so that
// ug passes through origin on inboard
q.resize(num_dof());
q[DOF_1] = ug.dot(p2-p1)/_pitch;
}
TEFUNC void computeCorrectValuesForThree() {
simulationTime += interval;
double scalar = exp(lambda * simulationTime);
Vec3 correctRotation(orientationInit.getEulerRotation() +
lambda * simulationTime *
scalar * angularVelocityInit);
correctPosition = positionInit +
lambda * simulationTime *
scalar * velocityInit;
correctOrientation = Quaternion(correctRotation[0],
correctRotation[1],
correctRotation[2]);
Vec3 axis;
float angle;
referenceBody->getOrientation().getAxisAngle(axis, angle);
correctRotationAxis = axis;
correctRotationAngle = angle;
referenceBody->setPosition(correctPosition);
referenceBody->setVelocity(correctVelocity);
referenceBody->setOrientation(correctOrientation);
referenceBody->setAngularVelocity(correctAngularVelocity);
cout << "Correct Values:" << endl
<< " Position: " << correctPosition << endl
<< " Orientation: " << correctOrientation << endl
<< " RotationAngle: " << correctRotationAngle << endl
<< " RotationAxis: " << correctRotationAxis
<< endl << endl;
}
//! Do update for the RB by useing the second test case
TEFUNC void updateRigidBodyForThree(RigidBodyPtr body) {
body->setAcceleration(lambda * body->getVelocity());
body->setAngularAcceleration(lambda * body->getAngularVelocity());
cout << "Accel: " << body->getAcceleration() << endl;
}
//! Do update for the RB by useing the second test case
TEFUNC void updateRigidBodyForTwo(RigidBodyPtr body) {
body->setVelocity(lambda * body->getPosition());
body->setAngularVelocity(lambda * body->getOrientation().getEulerRotation());
body->setAngularAcceleration(Vec3(0,0,0));
}
/**
* \note these spatial axes are not constant, unlike many joints.
*/
const SMatrix6N& SphericalJoint::get_spatial_axes(ReferenceFrameType rftype)
{
const unsigned X = 0, Y = 1, Z = 2;
RigidBodyPtr inboard = get_inboard_link();
RigidBodyPtr outboard = get_outboard_link();
if (!inboard)
throw std::runtime_error("SphericalJoint::get_spatial_axes() called with NULL inboard link");
if (!outboard)
throw std::runtime_error("SphericalJoint::get_spatial_axes() called with NULL outboard link");
// get current values of q
const VectorN& q = this->q;
const VectorN& q_tare = this->_q_tare;
// get the outboard link's joint to com vector in outer link coordinates
const Vector3& p = outboard->get_inner_joint_data(inboard).joint_to_com_vec_of;
// get the set of spatial axes
Real c1 = std::cos(q[DOF_1]+q_tare[DOF_1]);
Real c2 = std::cos(q[DOF_2]+q_tare[DOF_2]);
Real s1 = std::sin(q[DOF_1]+q_tare[DOF_1]);
Real s2 = std::sin(q[DOF_2]+q_tare[DOF_2]);
// form untransformed spatial axes -- this are the vectors describing each axis, after
// rotation by preceding axis/axes; note that first axis always points toward 1,0,0
Vector3 uu2(0, c1, s1);
Vector3 uu3(s2, -c2*s1, c1*c2);
// transform the spatial axes into the outer link frame
Vector3 u1;
_R.get_column(X, u1.begin());
Vector3 u2 = _R * uu2;
Vector3 u3 = _R * uu3;
// update the spatial axis in link coordinates
SVector6 si1, si2, si3;
si1.set_upper(u1);
si2.set_upper(u2);
si3.set_upper(u3);
si1.set_lower(Vector3::cross(u1, p));
si2.set_lower(Vector3::cross(u2, p));
si3.set_lower(Vector3::cross(u3, p));
_si.set_column(eAxis1, si1);
_si.set_column(eAxis2, si2);
_si.set_column(eAxis3, si3);
// setup the complement of the spatial axes in link coordinates
calc_s_bar_from_si();
// use the Joint function to do the rest
return Joint::get_spatial_axes(rftype);
}
/**
* \note these spatial axes are not constant, unlike many joints.
*/
const SMatrix6N& SphericalJoint::get_spatial_axes_dot(ReferenceFrameType rftype)
{
RigidBodyPtr inboard = get_inboard_link();
RigidBodyPtr outboard = get_outboard_link();
if (!inboard)
throw std::runtime_error("SphericalJoint::get_spatial_axes_dot() called with NULL inboard link");
if (!outboard)
throw std::runtime_error("SphericalJoint::get_spatial_axes_dot() called with NULL outboard link");
// get q, _q_tare, and qd
const VectorN& q = this->q;
const VectorN& q_tare = this->_q_tare;
const VectorN& qd = this->qd;
// get the two transformed axes
Real c1 = std::cos(q[DOF_1]+q_tare[DOF_1]);
Real c2 = std::cos(q[DOF_2]+q_tare[DOF_2]);
Real s1 = std::sin(q[DOF_1]+q_tare[DOF_1]);
Real s2 = std::sin(q[DOF_2]+q_tare[DOF_2]);
Real qd1 = qd[DOF_1];
Real qd2 = qd[DOF_2];
// form the time derivatives of the non-constant spatial axes (untransformed)
Vector3 uu2(0, -s1*qd1, c1*qd1);
Vector3 uu3(c2*qd2, -c2*c1*qd1 + s2*s1*qd2, -c1*s2*qd2 - s1*c2*qd1);
// transform the axes into outer link coordinates
Vector3 u2 = _R * uu2;
Vector3 u3 = _R * uu3;
// get the outboard link's joint to com vector in outer link coordinates
const Vector3& p = outboard->get_inner_joint_data(inboard).joint_to_com_vec_of;
// update the spatial axis in link coordinates; note that third column of spatial axis
// derivative set to zero in constructor and is never modified
SVector6 si2, si3;
si2.set_upper(u2);
si3.set_upper(u3);
si2.set_lower(Vector3::cross(u2, p));
si3.set_lower(Vector3::cross(u3, p));
_si_dot.set_column(eAxis2, si2);
_si_dot.set_column(eAxis3, si3);
// transform to global coordinates
SpatialTransform X_0_i = outboard->get_spatial_transform_link_to_global();
X_0_i.transform(_si_dot, _s0_dot);
return (rftype == eLink) ? _si_dot : _s0_dot;
}
/// Gets the spatial constraints for this joint
SMatrix6N& Joint::get_spatial_constraints(ReferenceFrameType rftype, SMatrix6N& s)
{
const unsigned X = 0, Y = 1, Z = 2;
Real Cq[7];
// get the outboard link and its orientation quaternion
RigidBodyPtr outboard = get_outboard_link();
assert(outboard);
const Quat& q = outboard->get_orientation();
// resize the spatial constraint matrix
s.resize(6, num_constraint_eqns());
// calculate the constraint Jacobian in relation to the outboard link
for (unsigned i=0; i< num_constraint_eqns(); i++)
{
// calculate the constraint Jacobian
calc_constraint_jacobian_rodrigues(outboard, i, Cq);
// convert the differential quaternion constraints to an angular velocity
// representation
Vector3 omega = q.G_mult(Cq[3], Cq[4], Cq[5], Cq[6]) * (Real) 0.5;
// setup the column of the constraint Jacobian
s(0,i) = omega[X];
s(1,i) = omega[Y];
s(2,i) = omega[Z];
s(3,i) = Cq[X];
s(4,i) = Cq[Y];
s(5,i) = Cq[Z];
}
// TODO: this should be in link-global frame -- fix this!
assert(false);
// convert to link frame if necessary (constraints computed in global frame)
if (rftype == eLink)
{
SpatialTransform Xi0 = outboard->get_spatial_transform_global_to_link();
for (unsigned i=0; i< num_constraint_eqns(); i++)
{
SVector6 scol = s.get_column(i);
scol = Xi0.transform(scol);
s.set_column(i, scol);
}
}
return s;
}
/// Sets the location of this joint with specified inboard and outboard links
void Joint::set_location(const Point3d& point, RigidBodyPtr inboard, RigidBodyPtr outboard)
{
assert(inboard && outboard);
// convert p to the inboard and outboard links' frames
Point3d pi = Pose3d::transform_point(inboard->get_pose(), point);
Point3d po = Pose3d::transform_point(outboard->get_pose(), point);
// set _F's and Fb's origins
_F->x = Origin3d(pi);
_Fb->x = Origin3d(po);
// invalidate all outboard pose vectors
outboard->invalidate_pose_vectors();
// set inboard and outboard links
set_inboard_link(inboard, false);
set_outboard_link(outboard, false);
}
//! Do update for the RB by useing the first test case
TEFUNC void updateRigidBodyForOne(RigidBodyPtr body) {
body->setVelocity(lambda * body->getPosition());
body->setAcceleration(lambda * body->getVelocity());
Vec3 axis;
float angle;
body->getOrientation().getAxisAngle(axis, angle);
Quaternion tmp(lambda * angle, axis);
body->setAngularVelocity(tmp.getEulerRotation());
body->setAngularAcceleration(lambda * body->getAngularVelocity());
}
/**
* \brief Start-Funktion der Testumgebung
*
* Diese Funktion wird direkt von display() aufgerufen. Hier
* sollte alles reingeschrieben werden, was für die einzelnen
* Tests nötig ist.
*/
TEFUNC void displayLoop() {
if (loop) {
//! gravitation aufrechnen
bodySystem.addGravity();
geometrySystem.resolveCollisions(10);
constraintSystem.step();
if (postStabilization)
constraintSystem.computePostStabilization ();
//! Integriere so weit, wie der letzte simulations Schritt brauchte
//bodySystem.integrateRungeKutta(getLastTime());
//bodySystem.integrateEuler(getLastTime());
bodySystem.integrateEulerVelocities(10);
geometrySystem.resolveContacts(10);
bodySystem.integrateEulerPositions(10);
// für schweben : -1.170
sph1acc->addForce(-1.10 * SimonState::exemplar()->getGravityVector()* sph1acc->getMass());
if (move) {
sph6acc->addForce(Vec3(5.0,5.0,0.0));
sph7acc->addForce(Vec3(-5.0,5.0,0.0));
sph8acc->addForce(Vec3(0.0,5.0,5.0));
sph9acc->addForce(Vec3(0.0,5.0,-5.0));
}
//sph->addForce(50 * Vec3(1.0,0.0,0.0));
}
geometrySystem.drawGeometries();
//cout << getLastTime() << endl;
}
/// Sets the pointer to the inboard link for this joint (and updates the spatial axes, if the outboard link has been set)
void Joint::set_inboard_link(RigidBodyPtr inboard, bool update_pose)
{
_inboard_link = inboard;
if (!inboard)
return;
// add this joint to the outer joints
inboard->_outer_joints.insert(get_this());
// setup F's pose relative to the inboard
set_inboard_pose(inboard->get_pose(), update_pose);
// update articulated body pointers, if possible
if (!inboard->get_articulated_body() && !_abody.expired())
inboard->set_articulated_body(ArticulatedBodyPtr(_abody));
else if (inboard->get_articulated_body() && _abody.expired())
set_articulated_body(ArticulatedBodyPtr(inboard->get_articulated_body()));
// the articulated body pointers must now be equal; it is
// conceivable that the user is updating the art. body pointers in an
// unorthodox manner, but we'll look for this anwyway...
if (!_abody.expired())
{
ArticulatedBodyPtr abody1(inboard->get_articulated_body());
ArticulatedBodyPtr abody2(_abody);
assert(abody1 == abody2);
}
}
/// Evaluates the time derivative of the constraint
void Joint::evaluate_constraints_dot(Real C[6])
{
Real Cx[6];
// get the inboard and outboard links
RigidBodyPtr in = get_inboard_link();
RigidBodyPtr out = get_outboard_link();
// get the linear angular velocities
const Vector3& lvi = in->get_lvel();
const Vector3& lvo = out->get_lvel();
const Vector3& avi = in->get_avel();
const Vector3& avo = out->get_avel();
// compute
const unsigned NEQ = num_constraint_eqns();
for (unsigned i=0; i< NEQ; i++)
{
calc_constraint_jacobian(DynamicBody::eAxisAngle, in, i, Cx);
Vector3 lv(Cx[0], Cx[1], Cx[2]);
Vector3 av(Cx[3], Cx[4], Cx[5]);
C[i] = lv.dot(lvi) + av.dot(avi);
calc_constraint_jacobian(DynamicBody::eAxisAngle, out, i, Cx);
lv = Vector3(Cx[0], Cx[1], Cx[2]);
av = Vector3(Cx[3], Cx[4], Cx[5]);
C[i] += -lv.dot(lvo) - av.dot(avo);
}
}
/// Gets the farthest point from this geometry
double CollisionGeometry::get_farthest_point_distance() const
{
// get the primitive from this
PrimitivePtr primitive = get_geometry();
// get the vertices
vector<Point3d> verts;
get_vertices(verts);
if (verts.empty())
return 0.0;
// get the rigid body pose in P's frame
RigidBodyPtr rb = dynamic_pointer_cast<RigidBody>(get_single_body());
Point3d rbX = Pose3d::transform_point(verts.front().pose, Point3d(0,0,0,rb->get_pose()));
// find which point is closest
double max_dist = 0.0;
for (unsigned i=0; i< verts.size(); i++)
max_dist = std::max(max_dist, (verts[i] - rbX).norm());
return max_dist;
}
/// The main control loop
void controller(DynamicBodyPtr robot, Real time, void* data)
{
// determine coordinates of ball in gripper coordinate frames
if (first)
{
_ball_grip_left = Matrix4::inverse_transform(left_gripper->get_transform()) .mult_point(ball->get_position());
_ball_grip_right = Matrix4::inverse_transform(right_gripper->get_transform()).mult_point(ball->get_position());
first = false;
}
else
{
// output the combined error from the starting position w.r.t. both grippers
std::ofstream out("error.ball", std::ios::app);
Vector3 ball_grip_left = Matrix4::inverse_transform(left_gripper->get_transform()).mult_point(ball->get_position());
Vector3 ball_grip_right = Matrix4::inverse_transform(right_gripper->get_transform()).mult_point(ball->get_position());
Real err = std::sqrt((ball_grip_left - _ball_grip_left).norm_sq() + (ball_grip_right - _ball_grip_right).norm_sq());
out << time << " " << err << std::endl;
out.close();
}
control_PID(dynamic_pointer_cast<RCArticulatedBody>(robot), time);
}
void SetIgnoreCollisionCheck(RigidBodyPtr rigidBody, bool ignore){
auto colObj = dynamic_cast<btCollisionObject*>(rigidBody.get());
assert(colObj);
if (ignore){
int i = mSelf->m_objectsWithoutCollisionCheck.findLinearSearch(colObj);
if (i == mSelf->m_objectsWithoutCollisionCheck.size()){
mSelf->setIgnoreCollisionCheck(colObj, ignore);
}
}
else{
mSelf->setIgnoreCollisionCheck(colObj, ignore);
}
}
/// (Relatively slow) method for determining the joint velocity from current link velocities
void Joint::determine_q_dot()
{
// get the pseudo-inverse of the spatial axes
MatrixN s;
s.copy_from(get_spatial_axes(eGlobal));
try
{
LinAlg::pseudo_inverse(s, LinAlg::svd1);
}
catch (NumericalException e)
{
s.copy_from(get_spatial_axes(eGlobal));
LinAlg::pseudo_inverse(s, LinAlg::svd2);
}
// get the change in velocity
RigidBodyPtr inboard = get_inboard_link();
RigidBodyPtr outboard = get_outboard_link();
SVector6 vi = inboard->get_spatial_velocity(eGlobal);
SVector6 vo = outboard->get_spatial_velocity(eGlobal);
s.mult(vo - vi, this->qd);
}
/**
* \note also points the outboard link to this joint
*/
void Joint::set_outboard_link(RigidBodyPtr outboard, bool update_pose)
{
_outboard_link = outboard;
if (!outboard)
return;
// add this joint to the outer joints
outboard->_inner_joints.insert(get_this());
// get the outboard pose
if (outboard->_F->rpose)
throw std::runtime_error("Joint::set_inboard_link() - relative pose on inboard link already set");
// setup Fb's pose relative to the outboard
set_outboard_pose(outboard->_F, update_pose);
// setup the frame
outboard->_xdj.pose = get_pose();
outboard->_xddj.pose = get_pose();
outboard->_Jj.pose = get_pose();
outboard->_forcej.pose = get_pose();
// use one articulated body pointer to set the other, if possible
if (!outboard->get_articulated_body() && !_abody.expired())
outboard->set_articulated_body(ArticulatedBodyPtr(_abody));
else if (outboard->get_articulated_body() && _abody.expired())
set_articulated_body(ArticulatedBodyPtr(outboard->get_articulated_body()));
// the articulated body pointers must now be equal; it is
// conceivable that the user is updating the art. body pointers in an
// unorthodox manner, but we'll look for this anwyway...
if (!_abody.expired())
{
ArticulatedBodyPtr abody1(outboard->get_articulated_body());
ArticulatedBodyPtr abody2(_abody);
assert(abody1 == abody2);
}
}
TEFUNC void printRigidBody(RigidBodyPtr body) {
Vec3 axis;
float angle;
body->getOrientation().getAxisAngle(axis, angle);
cout << "Differences from " << body->getId() << " to reference:" << endl
<< " Position: " << body->getPosition() - correctPosition << endl
<< " Velocity: " << body->getVelocity() - correctVelocity << endl
//! \todo komponentenweise subtraktion für quaternionen
<< " Orientation: " << body->getOrientation() + (-1 * correctOrientation) << endl
<< " AngularVelocity: " << body->getAngularVelocity() - correctAngularVelocity << endl
<< " RotationAngle: " << angle - correctRotationAngle << endl
<< " RotationAxis: " << axis - correctRotationAxis
<< endl << endl;
}
/**
* \pre Uses joint accelerations computed by forward dynamics, so the
* appropriate forward dynamics function must be run first.
*/
void RNEAlgorithm::calc_constraint_forces(RCArticulatedBodyPtr body)
{
queue<RigidBodyPtr> link_queue;
SMatrix6N s;
vector<SpatialRBInertia> Iiso;
FILE_LOG(LOG_DYNAMICS) << "RNEAlgorithm::calc_constraint_forces() entered" << endl;
// get the reference frame for computation
ReferenceFrameType rftype = body->computation_frame_type;
// ** STEP 0: compute isolated inertias
// get the set of links
const vector<RigidBodyPtr>& links = body->get_links();
// get the isolated inertiae
Iiso.resize(links.size());
for (unsigned i=1; i< links.size(); i++)
{
unsigned idx = links[i]->get_index();
Iiso[idx] = links[i]->get_spatial_iso_inertia(rftype);
}
// ** STEP 1: compute velocities and accelerations
// get the base link
RigidBodyPtr base = links.front();
// setup the vector of link accelerations
vector<SVector6> accels(links.size(), ZEROS_6);
// add all child links of the base to the processing queue
list<RigidBodyPtr> child_links;
base->get_child_links(std::back_inserter(child_links));
BOOST_FOREACH(RigidBodyPtr rb, child_links)
link_queue.push(rb);
// ** STEP 1: compute link forces -- backward recursion
vector<bool> processed(links.size(), false);
vector<SVector6> forces(links.size(), ZEROS_6);
// add all leaf links to the queue
for (unsigned i=1; i< links.size(); i++)
if (links[i]->num_child_links() == 0)
link_queue.push(links[i]);
// process all links up to, but not including, the base
while (!link_queue.empty())
{
// get the link off of the front of the queue and push all children of the link onto the queue
RigidBodyPtr link = link_queue.front();
link_queue.pop();
unsigned idx = link->get_index();
// if this link has already been processed, do not process it again
if (processed[idx])
continue;
// if the link is the base, continue the loop
if (link->is_base())
continue;
// link is not the base; add the parent to the queue for processing
RigidBodyPtr parent(link->get_parent_link());
link_queue.push(parent);
unsigned pidx = parent->get_index();
// get the forces for this link and this link's parent
SVector6& fi = forces[idx];
SVector6& fim1 = forces[pidx];
// get this link's acceleration
SVector6 a = link->get_spatial_accel(rftype);
FILE_LOG(LOG_DYNAMICS) << " computing necessary force; processing link " << link->id << endl;
FILE_LOG(LOG_DYNAMICS) << " currently determined link force: " << fi << endl;
FILE_LOG(LOG_DYNAMICS) << " I * a = " << (Iiso[idx] * a) << endl;
// get the spatial velocity for this link
const SVector6& v = link->get_spatial_velocity(rftype);
// add I*a to the link force
fi += Iiso[idx] * a;
// update the determined force to this link w/Coriolis + centrifugal terms
fi += SVector6::spatial_cross(v, Iiso[idx] * v);
FILE_LOG(LOG_DYNAMICS) << " force (+ I*a): " << fi << endl;
// determine external forces in link frame
const Vector3& fext = link->sum_forces();
const Vector3& text = link->sum_torques();
const Matrix4& T = link->get_transform();
SVector6 fx(T.transpose_mult_vector(fext), T.transpose_mult_vector(text));
// subtract external forces in the appropriate frame
if (rftype == eGlobal)
{
SpatialTransform X_0_i = link->get_spatial_transform_link_to_global();
fi -= X_0_i.transform(fx);
}
else
fi -= fx;
FILE_LOG(LOG_DYNAMICS) << " force on link after subtracting external force: " << fi << endl;
// indicate that this link has been processed
processed[idx] = true;
// update the parent force, if the parent is not the base
if (parent->is_base())
continue;
else
{
if (rftype == eGlobal)
fim1 += fi;
else
fim1 += link->get_spatial_transform_backward().transform(fi);
}
}
// ** STEP 2: compute constraint forces
// compute actuator forces
for (unsigned i=1; i< links.size(); i++)
{
RigidBodyPtr link = links[i];
JointPtr joint(link->get_inner_joint_implicit());
joint->get_spatial_constraints(rftype, s);
s.transpose_mult(forces[link->get_index()], joint->lambda);
FILE_LOG(LOG_DYNAMICS) << "joint " << joint->id << " constraint force: " << joint->lambda << endl;
}
FILE_LOG(LOG_DYNAMICS) << "RNEAlgorithm::calc_constraint_forces() exited" << endl;
}
/**
* Computed joint actuator forces are stored in inv_dyn_data.
*/
map<JointPtr, VectorN> RNEAlgorithm::calc_inv_dyn_fixed_base(RCArticulatedBodyPtr body, const map<RigidBodyPtr, RCArticulatedBodyInvDynData>& inv_dyn_data) const
{
queue<RigidBodyPtr> link_queue;
map<RigidBodyPtr, RCArticulatedBodyInvDynData>::const_iterator idd_iter;
vector<SpatialRBInertia> Iiso;
FILE_LOG(LOG_DYNAMICS) << "RNEAlgorithm::calc_inv_dyn_fixed_base() entered" << endl;
// get the reference frame for computation
ReferenceFrameType rftype = body->computation_frame_type;
// ** STEP 0: compute isolated inertias
// get the set of links
const vector<RigidBodyPtr>& links = body->get_links();
// get the isolated inertiae
Iiso.resize(links.size());
for (unsigned i=1; i< links.size(); i++)
{
unsigned idx = links[i]->get_index();
Iiso[idx] = links[i]->get_spatial_iso_inertia(rftype);
}
// ** STEP 1: compute velocities and accelerations
// get the base link
RigidBodyPtr base = links.front();
// setup the vector of link accelerations
vector<SVector6> accels(links.size(), ZEROS_6);
// add all child links of the base to the processing queue
list<RigidBodyPtr> child_links;
base->get_child_links(std::back_inserter(child_links));
BOOST_FOREACH(RigidBodyPtr rb, child_links)
link_queue.push(rb);
// process all links
while (!link_queue.empty())
{
// get the link off of the front of the queue
RigidBodyPtr link = link_queue.front();
link_queue.pop();
unsigned idx = link->get_index();
// push all children of the link onto the queue
child_links.clear();
link->get_child_links(std::back_inserter(child_links));
BOOST_FOREACH(RigidBodyPtr rb, child_links)
link_queue.push(rb);
// get the link's parent
RigidBodyPtr parent(link->get_parent_link());
unsigned pidx = parent->get_index();
// get the joint for this link
JointPtr joint(link->get_inner_joint_implicit());
// get the spatial link velocity
const SVector6& v = link->get_spatial_velocity(rftype);
// get the reference to the spatial link acceleration
SVector6& a = accels[idx];
// get spatial axes for this link's inner joint
const SMatrix6N& s = joint->get_spatial_axes(rftype);
// get derivative of the spatial axes for this link's inner joint
const SMatrix6N& s_dot = joint->get_spatial_axes_dot(rftype);
// get the current joint velocity
const VectorN& qd = joint->qd;
// **** compute acceleration
// get the desired joint acceleration
idd_iter = inv_dyn_data.find(link);
assert(idd_iter != inv_dyn_data.end());
const VectorN& qdd_des = idd_iter->second.qdd;
// add this link's contribution
a += SVector6::spatial_cross(v, s.mult(qd)) + s.mult(qdd_des) + s_dot.mult(qd);
// now add parent's contribution
if (rftype == eGlobal)
a += accels[pidx];
else
{
SpatialTransform X_i_im1 = link->get_spatial_transform_forward();
a += X_i_im1.transform(accels[pidx]);
}
FILE_LOG(LOG_DYNAMICS) << " computing link velocity / acceleration; processing link " << link->id << endl;
FILE_LOG(LOG_DYNAMICS) << " spatial axis: " << s << endl;
FILE_LOG(LOG_DYNAMICS) << " spatial joint velocity: " << s.mult(qd) << endl;
FILE_LOG(LOG_DYNAMICS) << " link velocity: " << v << endl;
FILE_LOG(LOG_DYNAMICS) << " link accel: " << a << endl;
}
// ** STEP 2: compute link forces -- backward recursion
vector<bool> processed(links.size(), false);
vector<SVector6> forces(links.size(), SVector6(0,0,0,0,0,0));
// add all leaf links to the queue
for (unsigned i=1; i< links.size(); i++)
if (links[i]->num_child_links() == 0)
link_queue.push(links[i]);
// process all links up to, but not including, the base
while (!link_queue.empty())
{
// get the link off of the front of the queue
RigidBodyPtr link = link_queue.front();
link_queue.pop();
unsigned idx = link->get_index();
// if this link has already been processed, do not process it again
if (processed[idx])
continue;
// if the link is the base, continue the loop
if (link->is_base())
continue;
// link is not the base; add the parent to the queue for processing
RigidBodyPtr parent(link->get_parent_link());
link_queue.push(parent);
unsigned pidx = parent->get_index();
// get the forces for this link and this link's parent
SVector6& fi = forces[idx];
SVector6& fim1 = forces[pidx];
FILE_LOG(LOG_DYNAMICS) << " computing necessary force; processing link " << link->id << endl;
FILE_LOG(LOG_DYNAMICS) << " currently determined link force: " << fi << endl;
FILE_LOG(LOG_DYNAMICS) << " I * a = " << (Iiso[idx] * accels[idx]) << endl;
// get the spatial velocity for this link
const SVector6& v = link->get_spatial_velocity(rftype);
// add I*a to the link force
fi += Iiso[idx] * accels[idx];
// update the determined force to this link w/Coriolis + centrifugal terms
fi += SVector6::spatial_cross(v, Iiso[idx] * v);
FILE_LOG(LOG_DYNAMICS) << " force (+ I*a): " << fi << endl;
// determine external forces in link frame
idd_iter = inv_dyn_data.find(link);
assert(idd_iter != inv_dyn_data.end());
const Vector3& fext = idd_iter->second.fext;
const Vector3& text = idd_iter->second.text;
const Matrix4& T = link->get_transform();
SVector6 fx(T.transpose_mult_vector(fext), T.transpose_mult_vector(text));
// subtract external forces in the appropriate frame
if (rftype == eGlobal)
{
SpatialTransform X_0_i = link->get_spatial_transform_link_to_global();
fi -= X_0_i.transform(fx);
}
else
fi -= fx;
FILE_LOG(LOG_DYNAMICS) << " force on link after subtracting external force: " << fi << endl;
// indicate that this link has been processed
processed[idx] = true;
// update the parent force, if the parent is not the base
if (parent->is_base())
continue;
else
if (rftype == eGlobal)
fim1 += fi;
else
fim1 += link->get_spatial_transform_backward().transform(fi);
}
// ** STEP 3: compute joint forces
// setup a map from joints to actuator forces
map<JointPtr, VectorN> actuator_forces;
// compute actuator forces
for (unsigned i=1; i< links.size(); i++)
{
RigidBodyPtr link = links[i];
JointPtr joint(link->get_inner_joint_implicit());
const SMatrix6N& s = joint->get_spatial_axes(rftype);
VectorN& Q = actuator_forces[joint];
s.transpose_mult(forces[link->get_index()], Q);
FILE_LOG(LOG_DYNAMICS) << "joint " << joint->id << " inner joint force: " << actuator_forces[joint] << endl;
}
FILE_LOG(LOG_DYNAMICS) << "RNEAlgorithm::calc_inv_dyn_fixed_base() exited" << endl;
return actuator_forces;
}
/**
* \param inv_dyn_data a mapping from links to the external forces (and
* torques) applied to them and to the desired inner joint
* accelerations; note that all links in the robot should be included
* in this map (even the base link, although inner joint acceleration
* is not applicable in that case and will be ignored for it)
* \return a mapping from joints to actuator forces
*/
map<JointPtr, VectorN> RNEAlgorithm::calc_inv_dyn_floating_base(RCArticulatedBodyPtr body, const map<RigidBodyPtr, RCArticulatedBodyInvDynData>& inv_dyn_data) const
{
queue<RigidBodyPtr> link_queue;
map<RigidBodyPtr, RCArticulatedBodyInvDynData>::const_iterator idd_iter;
vector<SpatialRBInertia> Iiso, I;
vector<SVector6> Z, v, a;
FILE_LOG(LOG_DYNAMICS) << "RNEAlgorithm::calc_inv_dyn_floating_base() entered" << endl;
// get the reference frame type
ReferenceFrameType rftype = body->computation_frame_type;
// get the set of links
const vector<RigidBodyPtr>& links = body->get_links();
// ** STEP 0: compute isolated inertias
// get the isolated inertiae
Iiso.resize(links.size());
for (unsigned i=0; i< links.size(); i++)
{
unsigned idx = links[i]->get_index();
Iiso[idx] = links[i]->get_spatial_iso_inertia(rftype);
}
// ** STEP 1: compute velocities and relative accelerations
// set all desired velocities and accelerations (latter relative to the base)
// to zero initially
v.resize(links.size());
a.resize(links.size());
for (unsigned i=0; i< links.size(); i++)
v[i] = a[i] = ZEROS_6;
// get the base link
RigidBodyPtr base = links.front();
// set velocity for the base
v.front() = base->get_spatial_velocity(rftype);
// add all child links of the base to the processing queue
list<RigidBodyPtr> child_links;
base->get_child_links(std::back_inserter(child_links));
BOOST_FOREACH(RigidBodyPtr rb, child_links)
link_queue.push(rb);
// process all links
while (!link_queue.empty())
{
// get the link off of the front of the queue
RigidBodyPtr link = link_queue.front();
link_queue.pop();
// add all child links of the link to the processing queue
child_links.clear();
link->get_child_links(std::back_inserter(child_links));
BOOST_FOREACH(RigidBodyPtr rb, child_links)
link_queue.push(rb);
// get the parent link
RigidBodyPtr parent(link->get_parent_link());
// get the index of this link and its parent
unsigned i = link->get_index();
unsigned im1 = parent->get_index();
// get the spatial axes (and derivative) of this link's inner joint
JointPtr joint(link->get_inner_joint_implicit());
const SMatrix6N& s = joint->get_spatial_axes(rftype);
const SMatrix6N& s_dot = joint->get_spatial_axes_dot(rftype);
// compute s * qdot
SVector6 sqd = s.mult(joint->qd);
// get the desired acceleration for the current link
idd_iter = inv_dyn_data.find(link);
assert(idd_iter != inv_dyn_data.end());
const VectorN& qdd_des = idd_iter->second.qdd;
// compute velocity and relative acceleration
v[i] = v[im1] + sqd;
a[i] = a[im1] + s.mult(qdd_des) + s_dot.mult(joint->qd) + SVector6::spatial_cross(v[i], sqd);
FILE_LOG(LOG_DYNAMICS) << " s: " << s << endl;
FILE_LOG(LOG_DYNAMICS) << " velocity for link " << links[i]->id << ": " << v[i] << endl;
FILE_LOG(LOG_DYNAMICS) << " s * qdd: " << s.mult(qdd_des) << endl;
FILE_LOG(LOG_DYNAMICS) << " v x s * qd: " << SVector6::spatial_cross(v[i], sqd) << endl;
FILE_LOG(LOG_DYNAMICS) << " relative accel for link " << links[i]->id << ": " << a[i] << endl;
}
// ** STEP 2: compute composite inertias and Z.A. forces
// resize vectors of spatial inertias and Z.A. vectors
I.resize(links.size());
Z.resize(links.size());
// zero out I and Z
for (unsigned i=0; i< links.size(); i++)
{
I[i].set_zero();
Z[i] = ZEROS_6;
}
// set all spatial isolated inertias and Z.A. forces
for (unsigned i=0; i< links.size(); i++)
{
// get the i'th link
RigidBodyPtr link = links[i];
unsigned idx = link->get_index();
// add the spatial isolated inertia for this link to the composite inertia
I[idx] += Iiso[idx];
// setup forces due to (relative) acceleration on link
Z[idx] = Iiso[idx] * a[idx];
// add coriolis and centrifugal forces on link
Z[idx] += SVector6::spatial_cross(v[i], Iiso[idx] * v[idx]);
// determine external forces on the link in link frame
idd_iter = inv_dyn_data.find(link);
assert(idd_iter != inv_dyn_data.end());
const Vector3& fext = idd_iter->second.fext;
const Vector3& text = idd_iter->second.text;
const Matrix4& T = link->get_transform();
SVector6 fx(T.transpose_mult_vector(fext), T.transpose_mult_vector(text));
// transform external forces and subtract from Z.A. vector
SpatialTransform X_0_i = link->get_spatial_transform_link_to_global();
Z[idx] -= X_0_i.transform(fx);
FILE_LOG(LOG_DYNAMICS) << " -- processing link " << link->id << endl;
FILE_LOG(LOG_DYNAMICS) << " external force / torque: " << fext << " / " << text << endl;
FILE_LOG(LOG_DYNAMICS) << " ZA vector: " << Z[idx] << endl;
FILE_LOG(LOG_DYNAMICS) << " isolated spatial-inertia: " << endl << Iiso[idx];
}
// *** compute composite inertias and zero acceleration vectors
// setup vector that indicates when links have been processed
vector<bool> processed(links.size(), false);
// put all leaf links into the queue
for (unsigned i=0; i< links.size(); i++)
if (links[i]->num_child_links() == 0)
link_queue.push(links[i]);
// process all links
while (!link_queue.empty())
{
// get the link off of the front of the queue
RigidBodyPtr link = link_queue.front();
link_queue.pop();
// get the index for this link
unsigned i = link->get_index();
// see whether this link has already been processed
if (processed[i])
continue;
// process the parent link, if possible
RigidBodyPtr parent(link->get_parent_link());
if (parent)
{
// put the parent on the queue
link_queue.push(parent);
// get the parent index
unsigned im1 = parent->get_index();
// add this inertia and Z.A. force to its parent
I[im1] += I[i];
Z[im1] += Z[i];
// indicate that the link has been processed
processed[i] = true;
}
}
// ** STEP 3: compute base acceleration
a.front() = I.front().inverse_mult(-Z.front());
SpatialTransform X_i_0 = base->get_spatial_transform_global_to_link();
FILE_LOG(LOG_DYNAMICS) << " Composite inertia for the base: " << endl << I.front();
FILE_LOG(LOG_DYNAMICS) << " ZA vector for the base (link frame): " << X_i_0.transform(Z.front()) << endl;
FILE_LOG(LOG_DYNAMICS) << " Determined base acceleration (link frame): " << X_i_0.transform(a.front()) << endl;
// ** STEP 4: compute joint forces
// setup the map of actuator forces
map<JointPtr, VectorN> actuator_forces;
// compute the forces
for (unsigned i=1; i< links.size(); i++)
{
unsigned idx = links[i]->get_index();
JointPtr joint(links[i]->get_inner_joint_implicit());
const SMatrix6N& s = joint->get_spatial_axes(rftype);
VectorN& Q = actuator_forces[joint];
s.transpose_mult((I[idx] * a.front()) + Z[idx], Q);
FILE_LOG(LOG_DYNAMICS) << " processing link: " << links[i]->id << endl;
FILE_LOG(LOG_DYNAMICS) << " spatial axis: " << endl << s;
FILE_LOG(LOG_DYNAMICS) << " I: " << endl << I[idx];
FILE_LOG(LOG_DYNAMICS) << " Z: " << endl << Z[idx];
FILE_LOG(LOG_DYNAMICS) << " actuator force: " << actuator_forces[joint] << endl;
}
FILE_LOG(LOG_DYNAMICS) << "RNEAlgorithm::calc_inv_dyn_floating_base() exited" << endl;
return actuator_forces;
}
/// Computes the time derivative of the constraint jacobian with respect to a body
void SphericalJoint::calc_constraint_jacobian_dot_rodrigues(RigidBodyPtr body, unsigned index, Real Cq[7])
{
const unsigned X = 0, Y = 1, Z = 2, SPATIAL_DIM = 7;
// get the two links
RigidBodyPtr inner = get_inboard_link();
RigidBodyPtr outer = get_outboard_link();
// mke sure that body is one of the links
if (inner != body && outer != body)
{
for (unsigned i=0; i< SPATIAL_DIM; i++)
Cq[i] = (Real) 0.0;
return;
}
// setup the constraint equations (from Shabana, p. 432)
if (body == inner)
{
// get the information necessary to compute the constraint equations
const Quat& q = inner->get_orientation();
const Vector3& p = inner->get_outer_joint_data(outer).com_to_joint_vec;
const Real px = p[X];
const Real py = p[Y];
const Real pz = p[Z];
Quat qd = Quat::deriv(q, inner->get_avel());
const Real dqw = qd.w;
const Real dqx = qd.x;
const Real dqy = qd.y;
const Real dqz = qd.z;
switch (index)
{
case 0:
Cq[0] = 0.0;
Cq[1] = 0.0;
Cq[2] = 0.0;
Cq[3] = 4*px*dqw + 2*pz*dqy - 2*py*dqz;
Cq[4] = 4*dqx*px + 2*dqy*py + 2*dqz*pz;
Cq[5] = 2*pz*dqw + 2*py*dqx;
Cq[6] = 2*pz*dqx - 2*py*dqw;
break;
case 1:
Cq[0] = 0.0;
Cq[1] = 0.0;
Cq[2] = 0.0;
Cq[3] = 4*py*dqw - 2*pz*dqx + 2*px*dqz;
Cq[4] = 2*dqy*px - 2*dqw*pz;
Cq[5] = 2*px*dqx + 4*py*dqy + 2*pz*dqz;
Cq[6] = 2*px*dqw + 2*pz*dqy;
break;
case 2:
Cq[0] = 0.0;
Cq[1] = 0.0;
Cq[2] = 0.0;
Cq[3] = 4*pz*dqw + 2*py*dqx - 2*px*dqy;
Cq[4] = 2*dqz*px + 2*dqw*py;
Cq[5] = 2*py*dqz - 2*px*dqw;
Cq[6] = 4*pz*dqz + 2*py*dqy + 2*px*dqx;
break;
default:
throw std::runtime_error("Invalid joint constraint index!");
}
}
else
{
// get the information necessary to compute the constraint equations
const Quat& q = outer->get_orientation();
const Vector3& p = body->get_inner_joint_data(inner).joint_to_com_vec_of;
const Real px = -p[X];
const Real py = -p[Y];
const Real pz = -p[Z];
Quat qd = Quat::deriv(q, outer->get_avel());
const Real dqw = qd.w;
const Real dqx = qd.x;
const Real dqy = qd.y;
const Real dqz = qd.z;
switch (index)
{
case 0:
Cq[0] = 0.0;
Cq[1] = 0.0;
Cq[2] = 0.0;
Cq[3] = -(4*px*dqw + 2*pz*dqy - 2*py*dqz);
Cq[4] = -(4*dqx*px + 2*dqy*py + 2*dqz*pz);
Cq[5] = -(2*pz*dqw + 2*py*dqx);
Cq[6] = -(2*pz*dqx - 2*py*dqw);
break;
case 1:
Cq[0] = 0.0;
Cq[1] = 0.0;
Cq[2] = 0.0;
Cq[3] = -(4*py*dqw - 2*pz*dqx + 2*px*dqz);
Cq[4] = -(2*dqy*px - 2*dqw*pz);
Cq[5] = -(2*px*dqx + 4*py*dqy + 2*pz*dqz);
Cq[6] = -(2*px*dqw + 2*pz*dqy);
break;
case 2:
Cq[0] = 0.0;
Cq[1] = 0.0;
Cq[2] = 0.0;
Cq[3] = -(4*pz*dqw + 2*py*dqx - 2*px*dqy);
Cq[4] = -(2*dqz*px + 2*dqw*py);
Cq[5] = -(2*py*dqz - 2*px*dqw);
Cq[6] = -(4*pz*dqz + 2*py*dqy + 2*px*dqx);
break;
default:
throw std::runtime_error("Invalid joint constraint index!");
}
}
}
/// Determines (and sets) the value of Q from the axes and the inboard link and outboard link transforms
void SphericalJoint::determine_q(VectorN& q)
{
const unsigned X = 0, Y = 1, Z = 2;
// get the inboard and outboard links
RigidBodyPtr inboard = get_inboard_link();
RigidBodyPtr outboard = get_outboard_link();
// verify that the inboard and outboard links are set
if (!inboard || !outboard)
throw NullPointerException("SphericalJoint::determine_q() called on NULL inboard and/or outboard links!");
// if any of the axes are not defined, can't use this method
if (std::fabs(_u[0].norm_sq() - 1.0) > NEAR_ZERO ||
std::fabs(_u[1].norm_sq() - 1.0) > NEAR_ZERO ||
std::fabs(_u[2].norm_sq() - 1.0) > NEAR_ZERO)
return;
// set proper size for q
q.resize(num_dof());
// get the link transforms
Matrix3 R_inboard, R_outboard;
inboard->get_transform().get_rotation(&R_inboard);
outboard->get_transform().get_rotation(&R_outboard);
// determine the joint transformation
Matrix3 R_local = R_inboard.transpose_mult(R_outboard);
// back out the transformation to z-axis
Matrix3 RU = _R.transpose_mult(R_local * _R);
// determine cos and sin values for q1, q2, and q3
Real s2 = RU(X,Z);
Real c2 = std::cos(std::asin(s2));
Real s1, c1, s3, c3;
if (std::fabs(c2) > NEAR_ZERO)
{
s1 = -RU(Y,Z)/c2;
c1 = RU(Z,Z)/c2;
s3 = -RU(X,Y)/c2;
c3 = RU(X,X)/c2;
assert(!std::isnan(s1));
assert(!std::isnan(c1));
assert(!std::isnan(s3));
assert(!std::isnan(c3));
}
else
{
// singular, we can pick any value for s1, c1, s3, c3 as long as the
// following conditions are satisfied
// c1*s3 + s1*c3*s2 = RU(Y,X)
// c1*c3 - s1*s3*s2 = RU(Y,Y)
// s1*s3 - c1*c3*s2 = RU(Z,X)
// s1*c3 + c1*s3*s2 = RU(Z,Y)
// so, we'll set q1 to zero (arbitrarily) and obtain
s1 = 0;
c1 = 1;
s3 = RU(Y,X);
c3 = RU(Y,Y);
}
// now determine q; only q2 can be determined without ambiguity
if (std::fabs(s1) < NEAR_ZERO)
q[DOF_2] = std::atan2(RU(X,Z), RU(Z,Z)/c1);
else
q[DOF_2] = std::atan2(RU(X,Z), -RU(Y,Z)/s1);
assert(!std::isnan(q[DOF_2]));
// if cos(q2) is not singular, proceed easily from here..
if (std::fabs(c2) > NEAR_ZERO)
{
q[DOF_1] = std::atan2(-RU(Y,Z)/c2, RU(Z,Z)/c2);
q[DOF_3] = std::atan2(-RU(X,Y)/c2, RU(X,X)/c2);
assert(!std::isnan(q[DOF_1]));
assert(!std::isnan(q[DOF_3]));
}
else
{
if (std::fabs(c1) > NEAR_ZERO)
q[DOF_3] = std::atan2((RU(Y,X) - s1*s2*c3)/c1, (RU(Y,Y) + s1*s2*s3)/c1);
else
q[DOF_3] = std::atan2((RU(Z,X) + c1*s2*c3)/s1, (RU(Z,Y) - c1*s2*s3)/s1);
if (std::fabs(c3) > NEAR_ZERO)
q[DOF_1] = std::atan2((RU(Y,X) - c1*s3)/(s2*c3), (-RU(Y,X) + s1*s3)/(s2*c3));
else
q[DOF_1] = std::atan2((-RU(Y,Y) + c1*c3)/(s2*s3), (RU(Z,Y) - s1*c3)/(s2*s3));
assert(!std::isnan(q[DOF_1]));
assert(!std::isnan(q[DOF_3]));
}
}
