void BallJointConstraint::updateDynamics(Eigen::MatrixXd & _J1, Eigen::VectorXd & _C, Eigen::VectorXd & _CDot, int _rowIndex) {
getJacobian();
_J1.block(_rowIndex, 0, 3, mBodyNode1->getSkeleton()->getNumGenCoords()) = mJ1;
Eigen::Vector3d worldP1 = mBodyNode1->getWorldTransform() * mOffset1;
Eigen::VectorXd qDot1 = mBodyNode1->getSkeleton()->get_dq();
_C.segment(_rowIndex, 3) = worldP1 - mOffset2;
_CDot.segment(_rowIndex, 3) = mJ1 * qDot1;
}
void PointConstraint::updateDynamics(std::vector<Eigen::MatrixXd> & _J, Eigen::VectorXd & _C, Eigen::VectorXd & _CDot, int _rowIndex) {
getJacobian();
dynamics::Skeleton *skel = mBody->getSkeleton();
_J[mSkelIndex].block(_rowIndex, 0, 3, skel->getDOF()) = mJ;
Eigen::Vector3d worldP = mBody->getWorldTransform() * mOffset;
Eigen::VectorXd qDot = skel->get_dq();
_C.segment(_rowIndex, 3) = worldP - mTarget;
_CDot.segment(_rowIndex, 3) = mJ * qDot;
}
void PointConstraint::updateDynamics(std::vector<Eigen::MatrixXd> & _J, Eigen::VectorXd & _C, Eigen::VectorXd & _CDot, int _rowIndex) {
getJacobian();
SkeletonDynamics *skel = (SkeletonDynamics*)mBody->getSkel();
_J[mSkelIndex].block(_rowIndex, 0, 3, skel->getNumDofs()) = mJ;
Vector3d worldP = xformHom(mBody->getWorldTransform(), mOffset);
VectorXd qDot = skel->getQDotVector();
_C.segment(_rowIndex, 3) = worldP - mTarget;
_CDot.segment(_rowIndex, 3) = mJ * qDot;
}
double Jacobian::getManipulabilityMeasure(const Eigen::VectorXd& joint_values)
{
ROS_ASSERT(initialized_);
Eigen::MatrixXd jac;
getJacobian(joint_values, jac);
Eigen::MatrixXd JJT = jac * jac.transpose();
return sqrt(JJT.determinant());
}
void ContactDynamics::updateNBMatrices() {
mN = MatrixXd::Zero(getNumTotalDofs(), getNumContacts());
mB = MatrixXd::Zero(getNumTotalDofs(), getNumContacts() * getNumContactDirections());
for (int i = 0; i < getNumContacts(); i++) {
Contact& c = mCollisionChecker->getContact(i);
Vector3d p = c.point;
int skelID1 = mBodyIndexToSkelIndex[c.collisionNode1->getIndex()];
int skelID2 = mBodyIndexToSkelIndex[c.collisionNode2->getIndex()];
Vector3d N21 = c.normal;
Vector3d N12 = -c.normal;
MatrixXd B21 = getTangentBasisMatrix(p, N21);
MatrixXd B12 = -B21;
if (!mSkels[skelID1]->getImmobileState()) {
int index1 = mIndices[skelID1];
int NDOF1 = c.collisionNode1->getBodyNode()->getSkel()->getNumDofs();
// Vector3d N21 = c.normal;
MatrixXd J21t = getJacobian(c.collisionNode1->getBodyNode(), p);
mN.block(index1, i, NDOF1, 1).noalias() = J21t * N21;
//B21 = getTangentBasisMatrix(p, N21);
mB.block(index1, i * getNumContactDirections(), NDOF1, getNumContactDirections()).noalias() = J21t * B21;
}
if (!mSkels[skelID2]->getImmobileState()) {
int index2 = mIndices[skelID2];
int NDOF2 = c.collisionNode2->getBodyNode()->getSkel()->getNumDofs();
//Vector3d N12 = -c.normal;
//if (B21.rows() == 0)
// B12 = getTangentBasisMatrix(p, N12);
//else
// B12 = -B21;
MatrixXd J12t = getJacobian(c.collisionNode2->getBodyNode(), p);
mN.block(index2, i, NDOF2, 1).noalias() = J12t * N12;
mB.block(index2, i * getNumContactDirections(), NDOF2, getNumContactDirections()).noalias() = J12t * B12;
}
}
}
void ClosedLoopConstraint::updateDynamics(std::vector<Eigen::MatrixXd> & _J, Eigen::VectorXd & _C, Eigen::VectorXd & _CDot, int _rowIndex) {
getJacobian();
_J[mSkelIndex2].block(_rowIndex, 0, 3, mBody2->getSkel()->getNumDofs()).setZero();
_J[mSkelIndex1].block(_rowIndex, 0, 3, mBody1->getSkel()->getNumDofs()) = mJ1;
_J[mSkelIndex2].block(_rowIndex, 0, 3, mBody2->getSkel()->getNumDofs()) += mJ2;
Vector3d worldP1 = xformHom(mBody1->getWorldTransform(), mOffset1);
Vector3d worldP2 = xformHom(mBody2->getWorldTransform(), mOffset2);
VectorXd qDot1 = ((SkeletonDynamics*)mBody1->getSkel())->getPoseVelocity();
VectorXd qDot2 = ((SkeletonDynamics*)mBody2->getSkel())->getPoseVelocity();
_C.segment(_rowIndex, 3) = worldP1 - worldP2;
_CDot.segment(_rowIndex, 3) = mJ1 * qDot1 + mJ2 * qDot2;
}
void BallJointConstraint::updateDynamics(Eigen::MatrixXd & _J1, Eigen::MatrixXd & _J2, Eigen::VectorXd & _C, Eigen::VectorXd & _CDot, int _rowIndex) {
getJacobian();
_J2.block(_rowIndex, 0, 3, mBodyNode2->getSkeleton()->getNumGenCoords()).setZero();
_J1.block(_rowIndex, 0, 3, mBodyNode1->getSkeleton()->getNumGenCoords()) = mJ1;
_J2.block(_rowIndex, 0, 3, mBodyNode2->getSkeleton()->getNumGenCoords()) += mJ2;
Eigen::Vector3d worldP1 = mBodyNode1->getWorldTransform() * mOffset1;
Eigen::VectorXd qDot1 = ((dynamics::Skeleton*)mBodyNode1->getSkeleton())->getGenVels();
Eigen::VectorXd worldP2 = mBodyNode2->getWorldTransform() * mOffset2;
Eigen::VectorXd qDot2 = ((dynamics::Skeleton*)mBodyNode2->getSkeleton())->getGenVels();
_C.segment(_rowIndex, 3) = worldP1 - worldP2;
_CDot.segment(_rowIndex, 3) = mJ1 * qDot1 + mJ2 * qDot2;
}
math::Jacobian
TemplatedJacobianNode<NodeType>::getJacobian(
const Eigen::Vector3d& _offset,
const Frame* _inCoordinatesOf) const
{
if(this == _inCoordinatesOf)
return getJacobian(_offset);
else if(_inCoordinatesOf->isWorld())
return getWorldJacobian(_offset);
Eigen::Isometry3d T = getTransform(_inCoordinatesOf);
T.translation() = - T.linear() * _offset;
return math::AdTJac(T, static_cast<const NodeType*>(this)->getJacobian());
}
void ClosedLoopConstraint::updateDynamics(std::vector<Eigen::MatrixXd>* _J,
Eigen::VectorXd* _C,
Eigen::VectorXd* _CDot,
int _rowIndex)
{
getJacobian();
_J->at(mSkelIndex2).block(_rowIndex, 0, 3,
mBody2->getSkeleton()->getNumGenCoords()).setZero();
_J->at(mSkelIndex1).block(_rowIndex, 0, 3,
mBody1->getSkeleton()->getNumGenCoords()) = mJ1;
_J->at(mSkelIndex2).block(_rowIndex, 0, 3,
mBody2->getSkeleton()->getNumGenCoords()) += mJ2;
Eigen::Vector3d worldP1 = mBody1->getWorldTransform() * mOffset1;
Eigen::Vector3d worldP2 = mBody2->getWorldTransform() * mOffset2;
Eigen::VectorXd qDot1 = mBody1->getSkeleton()->get_dq();
Eigen::VectorXd qDot2 = mBody2->getSkeleton()->get_dq();
_C->segment(_rowIndex, 3) = worldP1 - worldP2;
_CDot->segment(_rowIndex, 3) = mJ1 * qDot1 + mJ2 * qDot2;
}
bool OrientVectorToVectorTask::getCommand(ControlModel & model, TaskCommand & command)
{
// Get the latest joint state information
Vector Q(model.getNumDOFs());
Vector Qd(model.getNumDOFs());
model.getLatestFullState(Q, Qd);
// Get orientation of the local body vector in the world frame
Rbody = RigidBodyDynamics::CalcBodyWorldOrientation(model.rbdlModel(), Q, bodyId_, false); // TODO: Use Latest State
if (tare)
{
CONTROLIT_INFO_RT << "Taring the goal orientation!";
goalVector_ = Rbody.transpose() * bodyFrameVector_; // assumes goalVector_ is in world coordinate frame
tare = 0;
}
// Just in case, normalize the current and goal vectors
bodyFrameVector_.normalize();
goalVector_.normalize();
// Check if latched status has been updated
updateLatch(&model);
getJacobian(Jtloc);
// Compute the goal vector in the frameName_ coordinate frame
// goalHeading = goalVector_;
paramGoalHeading->set(goalVector_); // sets member variable "goalHeading"
if(frameId_ != -1) // The goal vector is in robot frame, either latched or not
{
if(isLatched)
Rframe = latchedRotation;
else
Rframe = RigidBodyDynamics::CalcBodyWorldOrientation(model.rbdlModel(), Q, frameId_, false); // TODO: Use Latest State
goalHeading = Rframe.transpose() * goalVector_;
}
// Get the heading in the frameName_ coordinate frame
actualHeading = Rbody.transpose() * bodyFrameVector_;
paramActualHeading->set(actualHeading);
// Compute the error (goal heading - current heading)
/*if (goalHeading.cols() != actualHeading.cols() || goalHeading.rows() != actualHeading.rows())
{
CONTROLIT_ERROR << "Matrix size error:\n"
<< " - goalHeading =\n" << goalHeading << "\n"
<< " - goalVector =\n" << goalVector_ << "\n"
<< " - frameId_ = " << frameId_ << "\n"
<< " - RFrame =\n" << Rframe << "\n"
<< " - actualHeading=\n" << actualHeading;
}*/
e0 = goalHeading - actualHeading;
// Compute the angle between the goal heading and current heading.
// The equation is cos^-1(goalHeading dot actualHeading / (|goalHeading| * |actualHeading|)).
double error = std::acos(goalHeading.dot(actualHeading) / (goalHeading.norm() * actualHeading.norm()))
* 180 / 3.141592653589793238463;
paramErrorAngle->set(error);
// Set the command type
command.type = commandType_;
// Compute the command
// The goal velocity is zero, thus the velocity error is -Jtloc * Qd.
controller->computeCommand(e0, -Jtloc * Qd, command.command, this);
Vector base_vector;
base_vector.setZero(3);
Vector tran_BodyToBase = RigidBodyDynamics::CalcBodyToBaseCoordinates(model.rbdlModel(), Q, bodyId_, base_vector, false);
// If we are able to grab the lock on visualizationPublisher,
// publish the visualization markers.
if (visualizationPublisher.trylock())
{
// Create a marker showing the goal vector
visualizationPublisher.msg_.markers[0].header.stamp = ros::Time::now();
visualizationPublisher.msg_.markers[0].points[0].x = tran_BodyToBase[0];
visualizationPublisher.msg_.markers[0].points[0].y = tran_BodyToBase[1];
visualizationPublisher.msg_.markers[0].points[0].z =tran_BodyToBase[2];
visualizationPublisher.msg_.markers[0].points[1].x = tran_BodyToBase[0] + goalHeading(0);
visualizationPublisher.msg_.markers[0].points[1].y = tran_BodyToBase[1] + goalHeading(1);
visualizationPublisher.msg_.markers[0].points[1].z = tran_BodyToBase[2] + goalHeading(2);
// Create a marker showing the current heading
visualizationPublisher.msg_.markers[1].header.stamp = ros::Time::now();
visualizationPublisher.msg_.markers[1].points[1].x = bodyFrameVector_(0);
visualizationPublisher.msg_.markers[1].points[1].y = bodyFrameVector_(1);
visualizationPublisher.msg_.markers[1].points[1].z = bodyFrameVector_(2);
// Publish the MarkerArray
visualizationPublisher.unlockAndPublish();
}
return true;
}
//==============================================================================
void EndEffector::updateWorldJacobian() const
{
mWorldJacobian = math::AdRJac(getWorldTransform(), getJacobian());
mIsWorldJacobianDirty = false;
}
void JtATIController::updateHook()
{
if(use_ft_sensor && port_ftdata.read(wrench_msg) != RTT::NewData)
return;
if(!updateState()) return;
jnt_trq_cmd.setZero();
if(0)
{
getMassMatrix(mass);
id_dyn_solver->JntToMass(jnt_pos_kdl,mass_kdl);
RTT::log(RTT::Warning) << "mass : \n"<<mass<<RTT::endlog();
RTT::log(RTT::Warning) << "mass_kdl : \n"<<mass_kdl.data<<RTT::endlog();
}
if(use_ft_sensor){
if(use_kdl){
// remove B(q,qdot)
jnt_vel_kdl.data.setZero();
// Get the Wrench in the last segment's frame
tf::wrenchMsgToKDL(wrench_msg.wrench,wrench_kdl);
f_ext_kdl.back() = wrench_kdl;
// Get The full dynamics with external forces
id_rne_solver->CartToJnt(jnt_pos_kdl,jnt_vel_kdl,jnt_acc_kdl,f_ext_kdl,jnt_trq_kdl);
// Get Joint Gravity torque
id_dyn_solver->JntToGravity(jnt_pos_kdl,gravity_kdl);
// remove G(q)
jnt_trq_cmd = jnt_trq_kdl.data - gravity_kdl.data;
RTT::log(RTT::Debug) << "Adding FT data : \n"<<jnt_trq_cmd.transpose()<<RTT::endlog();
}else{
// Get Jacobian with KRC
getJacobian(J_tip_base);
getCartesianPosition(cart_pos);
tf::poseMsgToKDL(cart_pos,tool_in_base_frame);
tf::poseMsgToEigen(cart_pos,tool_in_base_frame_eigen);
J_tip_base.changeBase(tool_in_base_frame.M);
RTT::log(RTT::Debug) << "J : \n"<<J_tip_base.data<<RTT::endlog();
// Get Jacobian with KDL
jnt_to_jac_solver->JntToJac(jnt_pos_kdl,J_kdl,kdl_chain.getNrOfSegments());
RTT::log(RTT::Debug) << "J_kdl : \n"<<J_kdl.data<<RTT::endlog();
tf::wrenchMsgToEigen(wrench_msg.wrench,wrench);
//tf::wrenchMsgToKDL(wrench_msg.wrench,wrench_kdl);
wrench_kdl = tool_in_base_frame.M * wrench_kdl;
///tf::wrenchKDLToEigen(wrench_kdl,wrench);
jnt_trq_cmd = J_tip_base.data.transpose() * wrench;
}
}
if(use_kdl_gravity)
{
getGravityTorque(jnt_grav);
id_dyn_solver->JntToGravity(jnt_pos_kdl,gravity_kdl);
jnt_trq_cmd += kg.asDiagonal() * gravity_kdl.data - jnt_grav;
RTT::log(RTT::Debug) << "Adding gravity : \n"<<(kg.asDiagonal() * gravity_kdl.data - jnt_grav).transpose()<<RTT::endlog();
}
if(compensate_coriolis)
{
id_dyn_solver->JntToCoriolis(jnt_pos_kdl,jnt_vel_kdl,coriolis_kdl);
jnt_trq_cmd -= coriolis_kdl.data.asDiagonal()*jnt_vel;
RTT::log(RTT::Debug) << "Removing Coriolis : \n"<<(coriolis_kdl.data.asDiagonal()*jnt_vel).transpose()<<RTT::endlog();
}
if(add_damping)
{
jnt_trq_cmd -= kd.asDiagonal() * jnt_vel;
RTT::log(RTT::Debug) << "Adding damping : \n"<<-(kd.asDiagonal() * jnt_vel).transpose()<<RTT::endlog();
}
RTT::log(RTT::Debug) << "jnt_trq_cmd : \n"<<jnt_trq_cmd.transpose()<<RTT::endlog();
sendJointTorque(jnt_trq_cmd);
}
void ChompOptimizer::calculateCollisionIncrements()
{
double potential;
double vel_mag_sq;
double vel_mag;
Eigen::Vector3d potential_gradient;
Eigen::Vector3d normalized_velocity;
Eigen::Matrix3d orthogonal_projector;
Eigen::Vector3d curvature_vector;
Eigen::Vector3d cartesian_gradient;
collision_increments_.setZero(num_vars_free_, num_joints_);
int startPoint = 0;
int endPoint = free_vars_end_;
// In stochastic descent, simply use a random point in the trajectory, rather than all the trajectory points.
// This is faster and guaranteed to converge, but it may take more iterations in the worst case.
if(parameters_->getUseStochasticDescent())
{
startPoint = (int)(((double)random() / (double)RAND_MAX)*(free_vars_end_ - free_vars_start_) + free_vars_start_);
if(startPoint < free_vars_start_) startPoint = free_vars_start_;
if(startPoint > free_vars_end_) startPoint = free_vars_end_;
endPoint = startPoint;
}
else
{
startPoint = free_vars_start_;
}
for(int i = startPoint; i <= endPoint; i++)
{
for(int j = 0; j < num_collision_points_; j++)
{
potential = collision_point_potential_[i][j];
if(potential < 0.0001)
continue;
potential_gradient = -collision_point_potential_gradient_[i][j];
vel_mag = collision_point_vel_mag_[i][j];
vel_mag_sq = vel_mag * vel_mag;
// all math from the CHOMP paper:
normalized_velocity = collision_point_vel_eigen_[i][j] / vel_mag;
orthogonal_projector = Eigen::Matrix3d::Identity() - (normalized_velocity * normalized_velocity.transpose());
curvature_vector = (orthogonal_projector * collision_point_acc_eigen_[i][j]) / vel_mag_sq;
cartesian_gradient = vel_mag * (orthogonal_projector * potential_gradient - potential * curvature_vector);
// pass it through the jacobian transpose to get the increments
getJacobian(i, collision_point_pos_eigen_[i][j], collision_point_joint_names_[i][j] ,jacobian_);
if(parameters_->getUsePseudoInverse())
{
calculatePseudoInverse();
collision_increments_.row(i - free_vars_start_).transpose() -= jacobian_pseudo_inverse_ * cartesian_gradient;
}
else
{
collision_increments_.row(i - free_vars_start_).transpose() -= jacobian_.transpose() * cartesian_gradient;
}
/*
if(point_is_in_collision_[i][j])
{
break;
}
*/
}
}
//cout << collision_increments_ << endl;
}
CIKChain::CIKChain(CIKDummy* tip,float interpolFact,int jointNbToExclude)
{
tooltip=tip;
chainIsValid=true;
// interpolFact is the interpolation factor we use to compute the target pose:
// interpolPose=tooltipPose*(1-interpolFact)+targetPose*interpolFact
// We get the jacobian and the rowJointIDs:
C4X4Matrix oldMatr;
int theConstraints=tip->constraints;
tip->baseReorient.setIdentity();
CMatrix* Ja=NULL;
if (tip->loopClosureDummy)
{
// The purpose of the following code is to have tip and target dummies reoriented
// and the appropriate constraints calculated automatically (only for loop closure dummies!)
tip->constraints=0;
int posDimensions=0;
int orDimensions=0;
C3Vector firstJointZAxis(0.0f,0.0f,1.0f);
C4X4Matrix firstJointCumul;
// here we reorient tip and targetCumulativeMatrix
// 1. We find the first revolute joint:
CIKObject* iterat=tip->parent;
while (iterat!=NULL)
{
if ( (iterat->objectType==IK_JOINT_TYPE)&&((CIKJoint*)iterat)->revolute&&((CIKJoint*)iterat)->active )
break;
iterat=iterat->parent;
}
if (iterat!=NULL)
{ // We have the first revolute joint from tip
orDimensions=1;
CIKObject* firstJoint=iterat;
firstJointCumul=C4X4Matrix(firstJoint->getCumulativeTransformationPart1(true).getMatrix());
firstJointZAxis=firstJointCumul.M.axis[2];
C3Vector normalVect;
// We search for a second revolute joint which is not aligned with the first one:
iterat=iterat->parent;
while (iterat!=NULL)
{
if ( (iterat->objectType==IK_JOINT_TYPE)&&((CIKJoint*)iterat)->revolute&&((CIKJoint*)iterat)->active )
{
C4X4Matrix secondJointCumul(iterat->getCumulativeTransformationPart1(true).getMatrix());
C3Vector secondJointZAxis(secondJointCumul.M.axis[2]);
if (fabs(firstJointZAxis*secondJointZAxis)<0.999999f) // Approx. 0.08 degrees
{
normalVect=(firstJointZAxis^secondJointZAxis).getNormalized();
if (firstJointZAxis*secondJointZAxis<0.0f)
secondJointZAxis=secondJointZAxis*-1.0f;
firstJointZAxis=((firstJointZAxis+secondJointZAxis)/2.0f).getNormalized();
break;
}
}
iterat=iterat->parent;
}
if (iterat!=NULL)
{
orDimensions=2;
// We search for a third revolute joint which is not orthogonal with normalVect:
iterat=iterat->parent;
while (iterat!=NULL)
{
if ( (iterat->objectType==IK_JOINT_TYPE)&&((CIKJoint*)iterat)->revolute&&((CIKJoint*)iterat)->active )
{
C4X4Matrix thirdJointCumul(iterat->getCumulativeTransformationPart1(true).getMatrix());
C3Vector thirdJointZAxis(thirdJointCumul.M.axis[2]);
if (fabs(normalVect*thirdJointZAxis)>0.0001f) // Approx. 0.005 degrees
{
orDimensions=3;
break;
}
}
iterat=iterat->parent;
}
}
}
if ( (orDimensions==1)||(orDimensions==2) )
{
// We align the tip dummy's z axis with the joint axis
// (and do the same transformation to targetCumulativeMatrix)
C4Vector rot(C4X4Matrix(firstJointZAxis).M.getQuaternion());
C4Vector tipParentInverse(tip->getParentCumulativeTransformation(true).Q.getInverse());
C4Vector tipNewLocal(tipParentInverse*rot);
C4Vector postTr(tip->getLocalTransformation(true).Q.getInverse()*tipNewLocal);
tip->transformation.Q=tipNewLocal;
C7Vector postTr2;
postTr2.setIdentity();
postTr2.Q=postTr;
tip->targetCumulativeTransformation=tip->targetCumulativeTransformation*postTr2;
}
Ja=getJacobian(tip,oldMatr,rowJoints,jointNbToExclude);
C3Vector posV;
for (int i=0;i<Ja->cols;i++)
{
// 1. Position space:
C3Vector vc((*Ja)(0,i),(*Ja)(1,i),(*Ja)(2,i));
float l=vc.getLength();
if (l>0.0001f) // 0.1 mm
{
vc=vc/l;
if (posDimensions==0)
{
posV=vc;
posDimensions++;
}
else if (posDimensions==1)
{
if (fabs(posV*vc)<0.999999f) // Approx. 0.08 degrees
{
posV=(posV^vc).getNormalized();
posDimensions++;
}
}
else if (posDimensions==2)
{
if (fabs(posV*vc)>0.0001f) // Approx. 0.005 degrees
posDimensions++;
}
}
}
if (posDimensions!=3)
{
if (posDimensions!=0)
{
C4X4Matrix aligned(posV);
tip->baseReorient=aligned.getInverse().getTransformation();
for (int i=0;i<Ja->cols;i++)
{
posV(0)=(*Ja)(0,i);
posV(1)=(*Ja)(1,i);
posV(2)=(*Ja)(2,i);
posV=tip->baseReorient.Q*posV; // baseReorient.X is 0 anyway...
(*Ja)(0,i)=posV(0);
(*Ja)(1,i)=posV(1);
(*Ja)(2,i)=posV(2);
}
oldMatr=tip->baseReorient.getMatrix()*oldMatr;
if (posDimensions==1)
tip->constraints+=IK_Z_CONSTRAINT;
else
tip->constraints+=(IK_X_CONSTRAINT+IK_Y_CONSTRAINT);
}
}
else
tip->constraints+=(IK_X_CONSTRAINT+IK_Y_CONSTRAINT+IK_Z_CONSTRAINT);
int doF=Ja->cols;
if (orDimensions==1)
{
if (doF-posDimensions>=1)
tip->constraints+=IK_GAMMA_CONSTRAINT;
}
else if (orDimensions==2)
{
if (doF-posDimensions>=1) // Is this correct?
tip->constraints+=IK_GAMMA_CONSTRAINT;
}
else if (orDimensions==3)
{
if (doF-posDimensions>=3)
tip->constraints+=(IK_ALPHA_BETA_CONSTRAINT|IK_GAMMA_CONSTRAINT);
}
theConstraints=tip->constraints;
}
else
Ja=getJacobian(tip,oldMatr,rowJoints,jointNbToExclude);
if (Ja==NULL)
{ // Error or not enough joints (effJoint=joints-jointNbToExclude) to get a Jacobian
delete Ja;
// We create dummy objects (so that we don't get an error when destroyed)
matrix=new CMatrix(0,0);
errorVector=new CMatrix(0,1);
chainIsValid=false;
return;
}
// oldMatr now contains the cumulative transf. matr. of tooltip relative to base
C4X4Matrix oldMatrInv(oldMatr.getInverse());
int doF=Ja->cols;
int equationNumber=0;
C4X4Matrix m;
C4X4Matrix targetCumulativeMatrix((tip->baseReorient*tip->targetCumulativeTransformation).getMatrix());
m.buildInterpolation(oldMatr,targetCumulativeMatrix,interpolFact);
// We prepare matrix and errorVector and their respective sizes:
if (theConstraints&IK_X_CONSTRAINT)
equationNumber++;
if (theConstraints&IK_Y_CONSTRAINT)
equationNumber++;
if (theConstraints&IK_Z_CONSTRAINT)
equationNumber++;
if (theConstraints&IK_ALPHA_BETA_CONSTRAINT)
equationNumber=equationNumber+2;
if (theConstraints&IK_GAMMA_CONSTRAINT)
equationNumber++;
matrix=new CMatrix(equationNumber,doF);
errorVector=new CMatrix(equationNumber,1);
// We set up the position/orientation errorVector and the matrix:
float positionWeight=1.0f;
float orientationWeight=1.0f;
int pos=0;
if (theConstraints&IK_X_CONSTRAINT)
{
for (int i=0;i<doF;i++)
(*matrix)(pos,i)=(*Ja)(0,i);
(*errorVector)(pos,0)=(m.X(0)-oldMatr.X(0))*positionWeight;
pos++;
}
if (theConstraints&IK_Y_CONSTRAINT)
{
for (int i=0;i<doF;i++)
(*matrix)(pos,i)=(*Ja)(1,i);
(*errorVector)(pos,0)=(m.X(1)-oldMatr.X(1))*positionWeight;
pos++;
}
if (theConstraints&IK_Z_CONSTRAINT)
{
for (int i=0;i<doF;i++)
(*matrix)(pos,i)=(*Ja)(2,i);
(*errorVector)(pos,0)=(m.X(2)-oldMatr.X(2))*positionWeight;
pos++;
}
if (theConstraints&IK_ALPHA_BETA_CONSTRAINT)
{
for (int i=0;i<doF;i++)
{
(*matrix)(pos,i)=(*Ja)(3,i);
(*matrix)(pos+1,i)=(*Ja)(4,i);
}
C4X4Matrix diff(oldMatrInv*m);
C3Vector euler(diff.M.getEulerAngles());
(*errorVector)(pos,0)=euler(0)*orientationWeight/IK_DIVISION_FACTOR;
(*errorVector)(pos+1,0)=euler(1)*orientationWeight/IK_DIVISION_FACTOR;
pos=pos+2;
}
if (theConstraints&IK_GAMMA_CONSTRAINT)
{
for (int i=0;i<doF;i++)
(*matrix)(pos,i)=(*Ja)(5,i);
C4X4Matrix diff(oldMatrInv*m);
C3Vector euler(diff.M.getEulerAngles());
(*errorVector)(pos,0)=euler(2)*orientationWeight/IK_DIVISION_FACTOR;
pos++;
}
delete Ja; // We delete the jacobian!
}
