void pubOdometry()
{
nav_msgs::Odometry odom;
{
odom.header.stamp = _last_imu_stamp;
odom.header.frame_id = "map";
odom.pose.pose.position.x = x(0);
odom.pose.pose.position.y = x(1);
odom.pose.pose.position.z = x(2);
Quaterniond q;
q = RPYtoR(x(3), x(4), x(5)).block<3,3>(0, 0);
odom.pose.pose.orientation.x = q.x();
odom.pose.pose.orientation.y = q.y();
odom.pose.pose.orientation.z = q.z();
odom.pose.pose.orientation.w = q.w();
}
//ROS_WARN("[update] publication done");
ROS_WARN_STREAM("[final] b_g = " << x.segment(_B_G_BEG, _B_G_LEN).transpose());
ROS_WARN_STREAM("[final] b_a = " << x.segment(_B_A_BEG, _B_A_LEN).transpose() << endl);
///ROS_WARN_STREAM("[final] cov_x = " << endl << cov_x << endl);
odom_pub.publish(odom);
}
// SLERP interpolation between two quaternions
Quaterniond Quaterniond::slerp( const Quaterniond &b,
H3DDouble frac ) const {
Quaterniond a = *this;
H3DDouble alpha = a.dotProduct(b);
if ( alpha < 0 ) {
a = -a;
alpha = -alpha;
}
H3DDouble scale;
H3DDouble invscale;
if ( ( 1 - alpha ) >= Constants::f_epsilon) {
// spherical interpolation
H3DDouble theta = acos( alpha );
H3DDouble sintheta = 1 / sin( theta );
scale = sin( theta * (1-frac) ) * sintheta;
invscale = sin( theta * frac ) * sintheta;
}
else {
// linear interploation
scale = 1 - frac;
invscale = frac;
}
return ( a * scale) + ( b * invscale);
}
bool VertexCam::read(std::istream& is)
{
// first the position and orientation (vector3 and quaternion)
Vector3d t;
for (int i=0; i<3; i++){
is >> t[i];
}
Vector4d rc;
for (int i=0; i<4; i++) {
is >> rc[i];
}
Quaterniond r;
r.coeffs() = rc;
r.normalize();
// form the camera object
SBACam cam(r,t);
// now fx, fy, cx, cy, baseline
double fx, fy, cx, cy, tx;
// try to read one value
is >> fx;
if (is.good()) {
is >> fy >> cx >> cy >> tx;
cam.setKcam(fx,fy,cx,cy,tx);
} else{
void doCapsuleSphereTest(double capsuleHeight, double capsuleRadius,
const Vector3d& capsulePosition, const Quaterniond& capsuleQuat,
double sphereRadius, const Vector3d& spherePosition, const Quaterniond& sphereQuat,
bool hasContacts, double depth,
const Vector3d& sphereProjection = Vector3d::Zero(),
const Vector3d& expectedNorm = Vector3d::Zero())
{
std::shared_ptr<CollisionPair> pair = std::make_shared<CollisionPair>(
makeCapsuleRepresentation(capsuleHeight, capsuleRadius, capsuleQuat, capsulePosition),
makeSphereRepresentation(sphereRadius, sphereQuat, spherePosition));
CapsuleSphereDcdContact calc;
calc.calculateContact(pair);
EXPECT_EQ(hasContacts, pair->hasContacts());
if (pair->hasContacts())
{
std::shared_ptr<Contact> contact(pair->getContacts().front());
EXPECT_TRUE(eigenEqual(expectedNorm, contact->normal));
EXPECT_NEAR(depth, contact->depth, SurgSim::Math::Geometry::DistanceEpsilon);
EXPECT_TRUE(contact->penetrationPoints.first.rigidLocalPosition.hasValue());
EXPECT_TRUE(contact->penetrationPoints.second.rigidLocalPosition.hasValue());
Vector3d capsuleLocalNormal = capsuleQuat.inverse() * expectedNorm;
Vector3d penetrationPoint0(sphereProjection - capsuleLocalNormal * capsuleRadius);
Vector3d sphereLocalNormal = sphereQuat.inverse() * expectedNorm;
Vector3d penetrationPoint1(sphereLocalNormal * sphereRadius);
EXPECT_TRUE(eigenEqual(penetrationPoint0, contact->penetrationPoints.first.rigidLocalPosition.getValue()));
EXPECT_TRUE(eigenEqual(penetrationPoint1, contact->penetrationPoints.second.rigidLocalPosition.getValue()));
}
}
void KeyFrame::updateLoopConnection(Vector3d relative_t, Quaterniond relative_q, double relative_yaw)
{
Eigen::Matrix<double, 8, 1> connected_info;
connected_info <<relative_t.x(), relative_t.y(), relative_t.z(),
relative_q.w(), relative_q.x(), relative_q.y(), relative_q.z(),
relative_yaw;
loop_info = connected_info;
}
void KeyFrame::addConnection(int index, KeyFrame* connected_kf, Vector3d relative_t, Quaterniond relative_q, double relative_yaw)
{
Eigen::Matrix<double, 8, 1> connected_info;
connected_info <<relative_t.x(), relative_t.y(), relative_t.z(),
relative_q.w(), relative_q.x(), relative_q.y(), relative_q.z(),
relative_yaw;
connection_list.push_back(make_pair(index, connected_info));
}
Matrix3d quaternion2mat(Quaterniond q)
{
Matrix3d m;
double a = q.w(), b = q.x(), c = q.y(), d = q.z();
m << a*a + b*b - c*c - d*d, 2*(b*c - a*d), 2*(b*d+a*c),
2*(b*c+a*d), a*a - b*b + c*c - d*d, 2*(c*d - a*b),
2*(b*d - a*c), 2*(c*d+a*b), a*a-b*b - c*c + d*d;
return m;
}
Matrix4d virtuose::getPose(float f[7]) {
Matrix4d m;
Vec3d pos(f[1], f[2], f[0]);
Quaterniond q;
q.setValue(f[4], f[5], f[3]);
m.setRotate(q);
m.setTranslate(pos);
return m;
}
void BenchmarkNode::runFromFolder(int start)
{
ofstream outfile;
outfile.open ("/home/worxli/Datasets/data/associate_unscaled.txt");
// outfile.open ("/home/worxli/data/test/associate_unscaled.txt");
for(int img_id = start;;++img_id)
{
// load image
std::stringstream ss;
ss << "/home/worxli/Datasets/data/img/color" << img_id << ".png";
// ss << "/home/worxli/data/test/img/color" << img_id << ".png";
std::cout << "reading image " << ss.str() << std::endl;
cv::Mat img(cv::imread(ss.str().c_str(), 0));
// end loop if no images left
if(img.empty())
break;
assert(!img.empty());
// process frame
vo_->addImage(img, img_id);
// display tracking quality
if(vo_->lastFrame() != NULL)
{
int id = vo_->lastFrame()->id_;
std::cout << "Frame-Id: " << id << " \t"
<< "#Features: " << vo_->lastNumObservations() << " \t"
<< "Proc. Time: " << vo_->lastProcessingTime()*1000 << "ms \n";
// access the pose of the camera via vo_->lastFrame()->T_f_w_.
//std::cout << vo_->lastFrame()->T_f_w_ << endl;
//std::count << vo_->lastFrame()->pos() << endl;
Quaterniond quat = vo_->lastFrame()->T_f_w_.unit_quaternion();
Vector3d trans = vo_->lastFrame()->T_f_w_.translation();
outfile << trans.x() << " "
<< trans.y() << " "
<< trans.z() << " "
<< quat.x() << " "
<< quat.y() << " "
<< quat.z() << " "
<< quat.w() << " "
<< "depth/mapped" << img_id << ".png "
<< "img/color" << img_id << ".png\n";
}
}
outfile.close();
}
void Visualization::transformPose(geometry_msgs::Pose& pose,
const Vector3d& trans, const Quaterniond& rot)
{
Vector3d pos;
pos.x() = pose.position.x;
pos.y() = pose.position.y;
pos.z() = pose.position.z;
pos = rot * pos + trans;
pose.position.x = pos.x();
pose.position.y = pos.y();
pose.position.z = pos.z();
Quaterniond orientation;
orientation.x() = pose.orientation.x;
orientation.y() = pose.orientation.y;
orientation.z() = pose.orientation.z;
orientation.w() = pose.orientation.w;
orientation = rot * orientation;
pose.orientation.x = orientation.x();
pose.orientation.y = orientation.y();
pose.orientation.z = orientation.z();
pose.orientation.w = orientation.w();
}
void mouse_drag(int mouse_x, int mouse_y)
{
using namespace igl;
using namespace std;
using namespace Eigen;
bool tw_using = TwMouseMotion(mouse_x,mouse_y);
if(is_rotating)
{
glutSetCursor(GLUT_CURSOR_CYCLE);
Quaterniond q;
auto & camera = s.camera;
switch(rotation_type)
{
case ROTATION_TYPE_IGL_TRACKBALL:
{
// Rotate according to trackball
igl::trackball<double>(
width,
height,
2.0,
down_camera.m_rotation_conj.coeffs().data(),
down_x,
down_y,
mouse_x,
mouse_y,
q.coeffs().data());
break;
}
case ROTATION_TYPE_TWO_AXIS_VALUATOR_FIXED_UP:
{
// Rotate according to two axis valuator with fixed up vector
two_axis_valuator_fixed_up(
width, height,
2.0,
down_camera.m_rotation_conj,
down_x, down_y, mouse_x, mouse_y,
q);
break;
}
default:
break;
}
switch(center_type)
{
default:
case CENTER_TYPE_ORBIT:
camera.orbit(q.conjugate());
break;
case CENTER_TYPE_FPS:
camera.turn_eye(q.conjugate());
break;
}
}
}
void
ArTrackOrientation::setKeyFrame(unsigned frame, const Quaterniond &orientation)
{
VectorN<double,4> base;
base[0]=orientation.q0();
base[1]=orientation.q1();
base[2]=orientation.q2();
base[3]=orientation.q3();
((ArTrackOrientationInterpolator *)_track)->setKeyFrame(frame,base);
}
inline Pose::Pose(const Quaterniond& orientation,
const Vector3d& position)
: m_orientation(orientation)
, m_position(position)
{
CHECK(std::fabs(orientation.norm() - 1) < 1e-15)
<< "Your quaternion is not normalized:"
<< orientation.norm();
// Or, one may prefer
m_orientation.normalize();
}
/*! Get the position of the location relative to its body in
* the J2000 ecliptic coordinate system.
*/
Vector3d Location::getPlanetocentricPosition(double t) const
{
if (parent == NULL)
{
return position.cast<double>();
}
else
{
Quaterniond q = parent->getEclipticToBodyFixed(t);
return q.conjugate() * position.cast<double>();
}
}
geometry_msgs::Quaternion eigenQuaterniondToTfQuaternion( Quaterniond q ){
geometry_msgs::Quaternion tfq;
tfq.x = q.x();
tfq.y = q.y();
tfq.z = q.z();
tfq.w = q.w();
return tfq;
}
void doSphereDoubleSidedPlaneTest(std::shared_ptr<SphereShape> sphere,
const Quaterniond& sphereQuat,
const Vector3d& sphereTrans,
std::shared_ptr<DoubleSidedPlaneShape> plane,
const Quaterniond& planeQuat,
const Vector3d& planeTrans,
bool expectedIntersect,
const double& expectedDepth = 0 ,
const Vector3d& expectedNorm = Vector3d::Zero())
{
using SurgSim::Math::Geometry::ScalarEpsilon;
using SurgSim::Math::Geometry::DistanceEpsilon;
std::shared_ptr<ShapeCollisionRepresentation> planeRep =
std::make_shared<ShapeCollisionRepresentation>("Collision Plane");
planeRep->setShape(plane);
planeRep->setLocalPose(SurgSim::Math::makeRigidTransform(planeQuat, planeTrans));
std::shared_ptr<ShapeCollisionRepresentation> sphereRep =
std::make_shared<ShapeCollisionRepresentation>("Collision Sphere");
sphereRep->setShape(sphere);
sphereRep->setLocalPose(SurgSim::Math::makeRigidTransform(sphereQuat, sphereTrans));
SphereDoubleSidedPlaneContact calcNormal;
std::shared_ptr<CollisionPair> pair = std::make_shared<CollisionPair>(sphereRep, planeRep);
// Again this replicates the way this is calculated in the contact calculation just with different
// starting values
Vector3d sphereLocalNormal = sphereQuat.inverse() * expectedNorm;
Vector3d spherePenetration = -sphereLocalNormal * sphere->getRadius();
Vector3d planePenetration = -sphereLocalNormal * (sphere->getRadius() - expectedDepth);
planePenetration = (sphereQuat * planePenetration) + sphereTrans;
planePenetration = planeQuat.inverse() * (planePenetration - planeTrans);
calcNormal.calculateContact(pair);
if (expectedIntersect)
{
ASSERT_TRUE(pair->hasContacts());
std::shared_ptr<Contact> contact = pair->getContacts().front();
EXPECT_NEAR(expectedDepth, contact->depth, 1e-10);
EXPECT_TRUE(eigenEqual(expectedNorm, contact->normal));
EXPECT_TRUE(contact->penetrationPoints.first.rigidLocalPosition.hasValue());
EXPECT_TRUE(contact->penetrationPoints.second.rigidLocalPosition.hasValue());
EXPECT_TRUE(eigenEqual(spherePenetration,
contact->penetrationPoints.first.rigidLocalPosition.getValue()));
EXPECT_TRUE(eigenEqual(planePenetration,
contact->penetrationPoints.second.rigidLocalPosition.getValue()));
}
else
{
EXPECT_FALSE(pair->hasContacts());
}
}
bool addPlaneToCollisionModel(const std::string &armName, double sx, const Quaterniond &q)
{
std::string arm2Name;
ros::NodeHandle nh("~") ;
ros::service::waitForService("/environment_server/set_planning_scene_diff");
ros::ServiceClient get_planning_scene_client =
nh.serviceClient<arm_navigation_msgs::GetPlanningScene>("/environment_server/set_planning_scene_diff");
arm_navigation_msgs::GetPlanningScene::Request planning_scene_req;
arm_navigation_msgs::GetPlanningScene::Response planning_scene_res;
arm_navigation_msgs::AttachedCollisionObject att_object;
att_object.link_name = armName + "_gripper";
att_object.object.id = "attached_plane";
att_object.object.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD;
att_object.object.header.frame_id = armName + "_ee" ;
att_object.object.header.stamp = ros::Time::now();
arm_navigation_msgs::Shape object;
object.type = arm_navigation_msgs::Shape::BOX;
object.dimensions.resize(3);
object.dimensions[0] = sx;
object.dimensions[1] = sx;
object.dimensions[2] = 0.01;
geometry_msgs::Pose pose;
pose.position.x = 0 ;
pose.position.y = 0 ;
pose.position.z = sx/2 ;
pose.orientation.x = q.x();
pose.orientation.y = q.y();
pose.orientation.z = q.z();
pose.orientation.w = q.w();
att_object.object.shapes.push_back(object);
att_object.object.poses.push_back(pose);
planning_scene_req.planning_scene_diff.attached_collision_objects.push_back(att_object);
if(!get_planning_scene_client.call(planning_scene_req, planning_scene_res)) return false;
return true ;
}
Quaterniond mat2quaternion(Matrix3d m)
{
//return euler2quaternion(mat2euler(m));
Quaterniond q;
double a, b, c, d;
a = sqrt(1 + m(0, 0) + m(1, 1) + m(2, 2))/2;
b = (m(2, 1) - m(1, 2))/(4*a);
c = (m(0, 2) - m(2, 0))/(4*a);
d = (m(1, 0) - m(0, 1))/(4*a);
q.w() = a; q.x() = b; q.y() = c; q.z() = d;
return q;
}
void SlamSystem::processIMU(double dt, const Vector3d&linear_acceleration, const Vector3d &angular_velocity)
{
Quaterniond dq;
dq.x() = angular_velocity(0)*dt*0.5;
dq.y() = angular_velocity(1)*dt*0.5;
dq.z() = angular_velocity(2)*dt*0.5;
dq.w() = sqrt(1 - SQ(dq.x()) * SQ(dq.y()) * SQ(dq.z()));
Matrix3d deltaR(dq);
//R_c_0 = R_c_0 * deltaR;
//T_c_0 = ;
Frame *current = slidingWindow[tail].get();
Matrix<double, 9, 9> F = Matrix<double, 9, 9>::Zero();
F.block<3, 3>(0, 3) = Matrix3d::Identity();
F.block<3, 3>(3, 6) = -current->R_k1_k* vectorToSkewMatrix(linear_acceleration);
F.block<3, 3>(6, 6) = -vectorToSkewMatrix(angular_velocity);
Matrix<double, 6, 6> Q = Matrix<double, 6, 6>::Zero();
Q.block<3, 3>(0, 0) = acc_cov;
Q.block<3, 3>(3, 3) = gyr_cov;
Matrix<double, 9, 6> G = Matrix<double, 9, 6>::Zero();
G.block<3, 3>(3, 0) = -current->R_k1_k;
G.block<3, 3>(6, 3) = -Matrix3d::Identity();
current->P_k = (Matrix<double, 9, 9>::Identity() + dt * F) * current->P_k * (Matrix<double, 9, 9>::Identity() + dt * F).transpose() + (dt * G) * Q * (dt * G).transpose();
//current->R_k1_k = current->R_k1_k*deltaR;
current->alpha_c_k += current->beta_c_k*dt + current->R_k1_k*linear_acceleration * dt * dt * 0.5 ;
current->beta_c_k += current->R_k1_k*linear_acceleration*dt;
current->R_k1_k = current->R_k1_k*deltaR;
current->timeIntegral += dt;
}
Quaterniond
ArrowVisualizer::orientation(const Entity* parent, double t) const
{
// The subclass computes the direction
Vector3d targetDirection = direction(parent, t);
Quaterniond rotation = Quaterniond::Identity();
// The arrow geometry points in the +x direction, so calculate the rotation
// required to make the arrow point in target direction
rotation.setFromTwoVectors(Vector3d::UnitX(), targetDirection);
return rotation;
}
void TransformerTF2::convertTransformToTf(const Transform &t,
geometry_msgs::TransformStamped &tOut) {
Quaterniond eigenQuat = t.getRotationQuat();
tOut.header.frame_id = t.getFrameParent();
tOut.header.stamp = ros::Time::fromBoost(t.getTime());
tOut.child_frame_id = t.getFrameChild();
tOut.transform.rotation.w = eigenQuat.w();
tOut.transform.rotation.x = eigenQuat.x();
tOut.transform.rotation.y = eigenQuat.y();
tOut.transform.rotation.z = eigenQuat.z();
tOut.transform.translation.x = t.getTranslation()(0);
tOut.transform.translation.y = t.getTranslation()(1);
tOut.transform.translation.z = t.getTranslation()(2);
}
/**
Euler angle defination: zyx
Rotation matrix: C_body2ned
**/
Quaterniond euler2quaternion(Vector3d euler)
{
double cr = cos(euler(0)/2);
double sr = sin(euler(0)/2);
double cp = cos(euler(1)/2);
double sp = sin(euler(1)/2);
double cy = cos(euler(2)/2);
double sy = sin(euler(2)/2);
Quaterniond q;
q.w() = cr*cp*cy + sr*sp*sy;
q.x() = sr*cp*cy - cr*sp*sy;
q.y() = cr*sp*cy + sr*cp*sy;
q.z() = cr*cp*sy - sr*sp*cy;
return q;
}
TEST(OsgRigidTransformConversionsTests, RigidTransform3dTest)
{
Quaterniond rotation = Quaterniond(Vector4d::Random());
rotation.normalize();
Vector3d translation = Vector3d::Random();
RigidTransform3d transform = makeRigidTransform(rotation, translation);
/// Convert to OSG
std::pair<osg::Quat, osg::Vec3d> osgTransform = toOsg(transform);
/// Convert back to Eigen and compare with original
RigidTransform3d resultTransform = fromOsg(osgTransform);
EXPECT_TRUE(transform.isApprox(resultTransform));
}
void mouse_drag(int mouse_x, int mouse_y)
{
using namespace igl;
using namespace Eigen;
if(is_rotating)
{
glutSetCursor(GLUT_CURSOR_CYCLE);
Quaterniond q;
auto & camera = s.camera;
switch(rotation_type)
{
case ROTATION_TYPE_IGL_TRACKBALL:
{
// Rotate according to trackball
igl::trackball<double>(
width,
height,
2.0,
down_camera.m_rotation_conj.coeffs().data(),
down_x,
down_y,
mouse_x,
mouse_y,
q.coeffs().data());
break;
}
case ROTATION_TYPE_TWO_AXIS_VALUATOR_FIXED_UP:
{
// Rotate according to two axis valuator with fixed up vector
two_axis_valuator_fixed_up(
width, height,
2.0,
down_camera.m_rotation_conj,
down_x, down_y, mouse_x, mouse_y,
q);
break;
}
default:
break;
}
camera.orbit(q.conjugate());
}else
{
TwEventMouseMotionGLUT(mouse_x, mouse_y);
}
glutPostRedisplay();
}
geometry_msgs::Pose eigenPoseToROS(const Vector3d &pos, const Quaterniond &orient)
{
geometry_msgs::Pose pose ;
pose.position.x = pos.x() ;
pose.position.y = pos.y() ;
pose.position.z = pos.z() ;
pose.orientation.x = orient.x() ;
pose.orientation.y = orient.y() ;
pose.orientation.z = orient.z() ;
pose.orientation.w = orient.w() ;
return pose ;
}
void
EditorIkSolver::_checkPivotXZ(ArRef<Joint> joint, Vector3d /* requestedTranslation */, Quaterniond requestedRotation) {
double prx, pry, prz;
joint->getDeltaRotation().getEulerAngles(prx, pry, prz);
requestedRotation = joint->getDeltaRotation() * requestedRotation;
double rx, ry, rz;
requestedRotation.getEulerAngles(rx, ry, rz);
double* max[2];
double* min[2];
ar_down_cast<JointConstraint2DOF>(joint->getConstraint())->getLimitValues(min, max);
double xmin = *min[0], xmax = *max[0], zmin = *min[1], zmax = *max[1];
rx = _clampWithPreviousValue(xmin, xmax, prx, rx);
rz = _clampWithPreviousValue(zmin, zmax, prz, rz);
Quaterniond qx(Vector3d(1.0, 0.0, 0.0), rx);
Quaterniond qz(Vector3d(0.0, 0.0, 1.0), rz);
joint->setDeltaRotation(qx * qz);
}
void SO3::
setQuaternion(const Quaterniond& quaternion)
{
assert(quaternion.norm()!=0);
unit_quaternion_ = quaternion;
unit_quaternion_.normalize();
}
void ComputeSmdTlsphShape::compute_peratom() {
double *contact_radius = atom->contact_radius;
invoked_peratom = update->ntimestep;
// grow vector array if necessary
if (atom->nlocal > nmax) {
memory->destroy(strainVector);
nmax = atom->nmax;
memory->create(strainVector, nmax, size_peratom_cols, "strainVector");
array_atom = strainVector;
}
int *mask = atom->mask;
double **smd_data_9 = atom->smd_data_9;
int nlocal = atom->nlocal;
Matrix3d E, eye, R, U, F;
eye.setIdentity();
Quaterniond q;
bool status;
for (int i = 0; i < nlocal; i++) {
if (mask[i] & groupbit) {
F = Map<Matrix3d>(smd_data_9[i]);
status = PolDec(F, R, U, false); // polar decomposition of the deformation gradient, F = R * U
if (!status) {
error->message(FLERR, "Polar decomposition of deformation gradient failed.\n");
}
E = 0.5 * (F.transpose() * F - eye); // Green-Lagrange strain
strainVector[i][0] = contact_radius[i] * (1.0 + E(0, 0));
strainVector[i][1] = contact_radius[i] * (1.0 + E(1, 1));
strainVector[i][2] = contact_radius[i] * (1.0 + E(2, 2));
q = R; // convert pure rotation matrix to quaternion
strainVector[i][3] = q.w();
strainVector[i][4] = q.x();
strainVector[i][5] = q.y();
strainVector[i][6] = q.z();
} else {
for (int j = 0; j < size_peratom_cols; j++) {
strainVector[i][j] = 0.0;
}
}
}
}
Vector3d CSkeleton::fitToLocation(Joint_handle hJoint, Vector3d target_loc)
{
Joint_handle hRoot = rootOf(hJoint);
updateGlobalPostures(hRoot);
if(hRoot == parents[hJoint])
{
joints[hJoint].setOffset(target_loc);
updateGlobalPostures(hRoot);
return globals[hJoint].getOffset();
}
//updateGlobalPostures(0);
Joint_handle hParent = parents[hJoint];
Vector3d dJoint, dTarget;
Vector3d pJoint, pParent;
pJoint = getGlobalPosition(hJoint);
pParent = getGlobalPosition(hParent);
dJoint = (pJoint - pParent).Normalize();
dTarget = (target_loc - pParent).Normalize();
Quaterniond qParent = getGlobalRotation(hParent);
// dJoint = cast_to_vector(!qParent*dJoint*qParent);
// dTarget = cast_to_vector(!qParent*dTarget*qParent);
Vector3d axis = Vector3d::Cross(dJoint, dTarget);
double dot = Vector3d::Dot(dJoint, dTarget);
double angle = acos(Vector3d::Dot(dJoint, dTarget));
Quaterniond qtemp = !qParent*Quaterniond(0, axis.X(), axis.Y(), axis.Z())*qParent;
axis = Vector3d(qtemp.X(), qtemp.Y(), qtemp.Z());
Quaterniond rot;
rot.FromAxisAngle(angle, axis.X(), axis.Y(), axis.Z());
qtemp = rot*Quaterniond(0, dJoint.X(), dJoint.Y(), dJoint.Z())*!rot;
Vector3d pos(qtemp.X(), qtemp.Y(), qtemp.Z());
Vector3d err = pos - dTarget;
if( angle == 0.f ||
(axis.X() == 0 && axis.Y() == 0 && axis.Z() ==0) ||
dJoint == dTarget)
return globals[hJoint].getOffset();
joints[hParent].Rotate(rot);
//rotateJoint(hParent, rot);
updateGlobalPostures(hParent);
//updateGlobalPostures(0);
return globals[hJoint].getOffset();
}
inline Pose Pose::inverse()
{
// Convention: x = R^T * (x' - T)
// = R^T * x' + (-R^T *T)
Quaterniond inv_orientation = m_orientation.inverse();
Vector3d inv_position = - (inv_orientation * m_position);
return Pose(inv_orientation, inv_position);
}