static void reportJoint(int index, JointState joint) { // Handy for debugging
std::cout << "\n";
std::cout << index << " " << joint.getName().toUtf8().data() << "\n";
std::cout << " pos:" << joint.getPosition() << "/" << joint.getPositionInParentFrame() << " from " << joint.getParentIndex() << "\n";
std::cout << " rot:" << safeEulerAngles(joint.getRotation()) << "/" << safeEulerAngles(joint.getRotationInParentFrame()) << "/" << safeEulerAngles(joint.getRotationInBindFrame()) << "\n";
std::cout << "\n";
}
void FaceModel::maybeUpdateNeckRotation(const JointState& parentState, const FBXJoint& joint, JointState& state) {
// get the rotation axes in joint space and use them to adjust the rotation
glm::mat3 axes = glm::mat3_cast(glm::quat());
glm::mat3 inverse = glm::mat3(glm::inverse(parentState.getTransform() * glm::translate(state.getDefaultTranslationInParentFrame()) *
joint.preTransform * glm::mat4_cast(joint.preRotation)));
state.setRotationInParentFrame(glm::angleAxis(- RADIANS_PER_DEGREE * _owningHead->getFinalRoll(), glm::normalize(inverse * axes[2]))
* glm::angleAxis(RADIANS_PER_DEGREE * _owningHead->getFinalYaw(), glm::normalize(inverse * axes[1]))
* glm::angleAxis(- RADIANS_PER_DEGREE * _owningHead->getFinalPitch(), glm::normalize(inverse * axes[0]))
* joint.rotation);
}
void FaceModel::maybeUpdateEyeRotation(const JointState& parentState, const FBXJoint& joint, JointState& state) {
// likewise with the eye joints
// NOTE: at the moment we do the math in the world-frame, hence the inverse transform is more complex than usual.
glm::mat4 inverse = glm::inverse(glm::mat4_cast(_rotation) * parentState.getTransform() *
glm::translate(state.getDefaultTranslationInParentFrame()) *
joint.preTransform * glm::mat4_cast(joint.preRotation * joint.rotation));
glm::vec3 front = glm::vec3(inverse * glm::vec4(_owningHead->getFinalOrientationInWorldFrame() * IDENTITY_FRONT, 0.0f));
glm::vec3 lookAt = glm::vec3(inverse * glm::vec4(_owningHead->getLookAtPosition() +
_owningHead->getSaccade() - _translation, 1.0f));
glm::quat between = rotationBetween(front, lookAt);
const float MAX_ANGLE = 30.0f * RADIANS_PER_DEGREE;
state.setRotationInParentFrame(glm::angleAxis(glm::clamp(glm::angle(between), -MAX_ANGLE, MAX_ANGLE), glm::axis(between)) *
joint.rotation);
}
void SkeletonModel::maybeUpdateLeanRotation(const JointState& parentState, JointState& state) {
if (!_owningAvatar->isMyAvatar()) {
return;
}
// get the rotation axes in joint space and use them to adjust the rotation
glm::vec3 xAxis(1.0f, 0.0f, 0.0f);
glm::vec3 yAxis(0.0f, 1.0f, 0.0f);
glm::vec3 zAxis(0.0f, 0.0f, 1.0f);
glm::quat inverse = glm::inverse(parentState.getRotation() * state.getDefaultRotationInParentFrame());
state.setRotationInConstrainedFrame(
glm::angleAxis(- RADIANS_PER_DEGREE * _owningAvatar->getHead()->getFinalLeanSideways(), inverse * zAxis)
* glm::angleAxis(- RADIANS_PER_DEGREE * _owningAvatar->getHead()->getFinalLeanForward(), inverse * xAxis)
* glm::angleAxis(RADIANS_PER_DEGREE * _owningAvatar->getHead()->getTorsoTwist(), inverse * yAxis)
* state.getFBXJoint().rotation, LEAN_PRIORITY);
}
void FaceModel::maybeUpdateEyeRotation(Model* model, const JointState& parentState, const JointState& state, int index) {
// likewise with the eye joints
// NOTE: at the moment we do the math in the world-frame, hence the inverse transform is more complex than usual.
glm::mat4 inverse = glm::inverse(glm::mat4_cast(model->getRotation()) * parentState.getTransform() *
glm::translate(_rig->getJointDefaultTranslationInConstrainedFrame(index)) *
state.getPreTransform() * glm::mat4_cast(state.getPreRotation() * state.getDefaultRotation()));
glm::vec3 front = glm::vec3(inverse * glm::vec4(_owningHead->getFinalOrientationInWorldFrame() * IDENTITY_FRONT, 0.0f));
glm::vec3 lookAtDelta = _owningHead->getCorrectedLookAtPosition() - model->getTranslation();
glm::vec3 lookAt = glm::vec3(inverse * glm::vec4(lookAtDelta + glm::length(lookAtDelta) * _owningHead->getSaccade(), 1.0f));
glm::quat between = rotationBetween(front, lookAt);
const float MAX_ANGLE = 30.0f * RADIANS_PER_DEGREE;
_rig->setJointRotationInConstrainedFrame(index, glm::angleAxis(glm::clamp(glm::angle(between),
-MAX_ANGLE, MAX_ANGLE), glm::axis(between)) *
state.getDefaultRotation(), DEFAULT_PRIORITY);
}
bool IKSolver::solveIK(const string &tipLinkName, const Eigen::Vector3d &pos, const Eigen::Quaterniond &orient, const JointState &cs, JointState &state)
{
using namespace Eigen ;
assert(model_) ;
Affine3d solver_tip_to_pose_tip = model_->getWorldTransform(solver_->getTipName()).inverse() * model_->getWorldTransform(tipLinkName) ;
Affine3d frame = model_->getWorldTransform(solver_->getBaseName()) ;
Affine3d pose_ = frame.inverse() * Translation3d(pos) * orient * solver_tip_to_pose_tip.inverse() ;
geometry_msgs::Pose pose = eigenPoseToROS(pose_.translation(), Quaterniond(pose_.rotation())) ;
vector<double> solution ;
int error_code ;
vector<double> seed_state_vals = cs.getValues(joint_names_) ;
if ( solver_->searchPositionIK(pose, seed_state_vals, 1.0, solution,
boost::bind(&IKSolver::initialPoseCheck, this, _1, _2, _3),
boost::bind(&IKSolver::collisionCheck, this, _1, _2, _3),
error_code ) )
{
state = JointState(joint_names_, solution) ;
return true ;
}
else return false ;
}
void FaceModel::maybeUpdateNeckRotation(const JointState& parentState, const FBXJoint& joint, JointState& state) {
Avatar* owningAvatar = static_cast<Avatar*>(_owningHead->_owningAvatar);
// get the rotation axes in joint space and use them to adjust the rotation
glm::mat3 axes = glm::mat3_cast(glm::quat());
glm::mat3 inverse = glm::mat3(glm::inverse(parentState.getTransform() *
glm::translate(state.getDefaultTranslationInConstrainedFrame()) *
joint.preTransform * glm::mat4_cast(joint.preRotation)));
glm::vec3 pitchYawRoll = safeEulerAngles(_owningHead->getFinalOrientationInLocalFrame());
glm::vec3 lean = glm::radians(glm::vec3(_owningHead->getFinalLeanForward(),
_owningHead->getTorsoTwist(),
_owningHead->getFinalLeanSideways()));
pitchYawRoll -= lean;
state.setRotationInConstrainedFrame(glm::angleAxis(-pitchYawRoll.z, glm::normalize(inverse * axes[2]))
* glm::angleAxis(pitchYawRoll.y, glm::normalize(inverse * axes[1]))
* glm::angleAxis(-pitchYawRoll.x, glm::normalize(inverse * axes[0]))
* joint.rotation, DEFAULT_PRIORITY);
}
void FaceModel::maybeUpdateNeckRotation(const JointState& parentState, const JointState& state, int index) {
// get the rotation axes in joint space and use them to adjust the rotation
glm::mat3 axes = glm::mat3_cast(glm::quat());
glm::mat3 inverse = glm::mat3(glm::inverse(parentState.getTransform() *
glm::translate(_rig->getJointDefaultTranslationInConstrainedFrame(index)) *
state.getPreTransform() * glm::mat4_cast(state.getPreRotation())));
glm::vec3 pitchYawRoll = safeEulerAngles(_owningHead->getFinalOrientationInLocalFrame());
glm::vec3 lean = glm::radians(glm::vec3(_owningHead->getFinalLeanForward(),
_owningHead->getTorsoTwist(),
_owningHead->getFinalLeanSideways()));
pitchYawRoll -= lean;
_rig->setJointRotationInConstrainedFrame(index,
glm::angleAxis(-pitchYawRoll.z, glm::normalize(inverse * axes[2]))
* glm::angleAxis(pitchYawRoll.y, glm::normalize(inverse * axes[1]))
* glm::angleAxis(-pitchYawRoll.x, glm::normalize(inverse * axes[0]))
* state.getDefaultRotation(), DEFAULT_PRIORITY);
}
std::pair< bool, JointLimitRange::OutOfBounds > JointLimitRange::verifyValidity(const JointState& state) const
{
using std::make_pair;
if( state.hasPosition() )
{
if( min.hasPosition() && min.position > state.position )
return make_pair(false, OutOfBounds( "position", min.position, max.position, state.position ));
if( max.hasPosition() && max.position < state.position )
return make_pair(false, OutOfBounds( "position", min.position, max.position, state.position ));
}
if( state.hasSpeed() )
{
if( min.hasSpeed() && min.speed > state.speed )
return make_pair(false, OutOfBounds( "speed", min.speed, max.speed, state.speed ));
if( max.hasSpeed() && max.speed < state.speed )
return make_pair(false, OutOfBounds( "speed", min.speed, max.speed, state.speed ));
}
if( state.hasEffort() )
{
if( min.hasEffort() && min.effort > state.effort )
return make_pair(false, OutOfBounds( "effort", min.effort, max.effort, state.effort ));
if( max.hasEffort() && max.effort < state.effort )
return make_pair(false, OutOfBounds( "effort", min.effort, max.effort, state.effort ));
}
if( state.hasRaw() )
{
if( min.hasRaw() && min.raw > state.raw )
return make_pair(false, OutOfBounds( "raw", min.raw, max.raw, state.raw ));
if( max.hasRaw() && max.raw < state.raw )
return make_pair(false, OutOfBounds( "raw", min.raw, max.raw, state.raw ));
}
if( state.hasAcceleration() )
{
if( min.hasAcceleration() && min.acceleration > state.acceleration )
return make_pair(false, OutOfBounds( "acceleration", min.acceleration, max.acceleration, state.acceleration ));
if( max.hasAcceleration() && max.acceleration < state.acceleration )
return make_pair(false, OutOfBounds( "acceleration", min.acceleration, max.acceleration, state.acceleration ));
}
return make_pair(true, OutOfBounds());
}
