void SkeletonModel::applyPalmData(int jointIndex, PalmData& palm) {
if (jointIndex == -1 || jointIndex >= _jointStates.size()) {
return;
}
const FBXGeometry& geometry = _geometry->getFBXGeometry();
float sign = (jointIndex == geometry.rightHandJointIndex) ? 1.0f : -1.0f;
int parentJointIndex = geometry.joints.at(jointIndex).parentIndex;
if (parentJointIndex == -1) {
return;
}
// rotate palm to align with its normal (normal points out of hand's palm)
glm::quat inverseRotation = glm::inverse(_rotation);
glm::vec3 palmPosition = inverseRotation * (palm.getPosition() - _translation);
glm::vec3 palmNormal = inverseRotation * palm.getNormal();
glm::vec3 fingerDirection = inverseRotation * palm.getFingerDirection();
glm::quat palmRotation = rotationBetween(geometry.palmDirection, palmNormal);
palmRotation = rotationBetween(palmRotation * glm::vec3(-sign, 0.0f, 0.0f), fingerDirection) * palmRotation;
if (Menu::getInstance()->isOptionChecked(MenuOption::AlternateIK)) {
setHandPosition(jointIndex, palmPosition, palmRotation);
} else if (Menu::getInstance()->isOptionChecked(MenuOption::AlignForearmsWithWrists)) {
float forearmLength = geometry.joints.at(jointIndex).distanceToParent * extractUniformScale(_scale);
glm::vec3 forearm = palmRotation * glm::vec3(sign * forearmLength, 0.0f, 0.0f);
setJointPosition(parentJointIndex, palmPosition + forearm,
glm::quat(), false, -1, false, glm::vec3(0.0f, -1.0f, 0.0f), PALM_PRIORITY);
JointState& parentState = _jointStates[parentJointIndex];
parentState.setRotationInBindFrame(palmRotation, PALM_PRIORITY);
// lock hand to forearm by slamming its rotation (in parent-frame) to identity
_jointStates[jointIndex].setRotationInConstrainedFrame(glm::quat(), PALM_PRIORITY);
} else {
inverseKinematics(jointIndex, palmPosition, palmRotation, PALM_PRIORITY);
}
}
bool RobotSim::inverseKinematicsAbs(Transformation3D t, vector<double> &_q, unsigned char conf)
{
Transformation3D t_base=links[0]->getAbsoluteT3D();
Transformation3D aux=t_base.inverted();
Transformation3D t_p=(aux)*(t);
return inverseKinematics(t_p,_q,conf);
}
bool InverseKinematics::sInverseKinematics(const MultiBody& mb, MultiBodyConfig& mbc,
const sva::PTransformd& ef_target)
{
checkMatchQ(mb, mbc);
checkMatchBodyPos(mb, mbc);
checkMatchJointConf(mb, mbc);
checkMatchParentToSon(mb, mbc);
return inverseKinematics(mb, mbc, ef_target);
}
void Wheels::getDesiredWheelPositions(long &desiredLeft, long &desiredRight, long &desiredBack) {
int diffX = localCurrentX - localFinalX;
int diffY = localCurrentY - localFinalY;
int diffTheta = globalCurrentTheta - globalFinalTheta;
Serial.print("diff x: ");
Serial.print(diffX);
Serial.print(", diff y: ");
Serial.print(diffY);
Serial.print(", theta diff: ");
Serial.println(diffTheta);
inverseKinematics(diffX, diffY, diffTheta, desiredLeft, desiredRight, desiredBack);
}
bool RobotSim::moveTo(Transformation3D t3d, unsigned char conf)
{
/***
the T3D is a relative transformation to the base (link[0])
***/
if(conf==0x0)conf=getCurrentConfiguration();
vector<double> q;
if(inverseKinematics(t3d,q,conf))
{
for(int i=0;i<(int)joints.size();i++)
joints[i]->setValue(q[i]);
return true;
}
else return false;
}
/*!*****************************************************************************
*******************************************************************************
\note run_goto_task
\date Dec. 1997
\remarks
run the task from the task servo: REAL TIME requirements!
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
static int
run_goto_cart_task(void)
{
int j, i;
double sum=0;
double aux;
/* has the movement time expired? I intentially run 0.5 sec longer */
if (tau <= -0.5 || (tau <= 0.0 && special_flag)) {
freeze();
return TRUE;
}
/* progress by min jerk in cartesian space */
calculate_min_jerk_next_step(cnext,ctarget,tau,time_step,cnext);
tau -= time_step;
/* shuffle the target for the inverse kinematics */
for (i=1; i<=n_endeffs; ++i) {
for (j=1; j<=N_CART; ++j) {
aux = cnext[i].x[j] - cart_des_state[i].x[j];
cart[(i-1)*6+j] = cnext[i].xd[j] + 20.*aux;
}
}
/* inverse kinematics */
for (i=1; i<=n_dofs; ++i) {
target[i].th = joint_des_state[i].th;
}
if (!inverseKinematics(target,endeff,joint_opt_state,
cart,cstatus,time_step)) {
freeze();
return FALSE;
}
/* prepare inverse dynamics */
for (i=1; i<=n_dofs; ++i) {
aux = (target[i].thd-joint_des_state[i].thd)*(double)task_servo_rate;
target[i].thdd = filt(aux,&(fthdd[i]));
joint_des_state[i].thdd = target[i].thdd;
joint_des_state[i].thd = target[i].thd;
joint_des_state[i].th = target[i].th;
if (joint_des_state[i].th > joint_range[i][MAX_THETA]) {
joint_des_state[i].th = joint_range[i][MAX_THETA];
joint_des_state[i].thd = 0.0;
joint_des_state[i].thdd = 0.0;
}
if (joint_des_state[i].th < joint_range[i][MIN_THETA]) {
joint_des_state[i].th = joint_range[i][MIN_THETA];
joint_des_state[i].thd = 0.0;
joint_des_state[i].thdd = 0.0;
}
}
/* compute inverse dynamics */
SL_InvDyn(joint_state,joint_des_state,endeff,&base_state,&base_orient);
return TRUE;
}
