//#################### PRIVATE METHODS ####################
void AnimationController::reset_controller()
{
m_state = AS_REST;
m_animationName = "<rest>";
m_animationTime = 0;
set_pose(m_skeleton->make_rest_pose());
m_transitionStart.reset();
}
void vrpn_Tracker_PhaseSpace::report_rigid(vrpn_int32 sensor, const OWL::Rigid& r, bool is_stylus)
{
d_sensor = sensor;
if(debug)
{
printf("[debug] sensor=%d type=rigid tracker=%d x=%f y=%f z=%f w=%f a=%f b=%f c=%f cond=%f\n", d_sensor, r.id, r.pose[0], r.pose[1], r.pose[2], r.pose[3], r.pose[4], r.pose[5], r.pose[6], r.cond);
}
if(r.cond <= 0) return;
// set the position/orientation
set_pose(r);
send_report();
}
int vrpn_Poser_Remote::request_pose_relative( const struct timeval t,
const vrpn_float64 position_delta[3],
const vrpn_float64 quaternion[4])
{
// Set the requested pose
set_pose(t, position_delta, quaternion);
// Send position request
if (client_send_pose_relative() != 0) {
fprintf(stderr, "vrpn_Poser_Remote: request_pose_relative failed\n");
return 0;
}
return 1;
}
void ArmDemoTask::init_demo()
{
set_demo_mode(DEFAULT_MODE);
demoTrajectoryCommand.InitStruct();
demoTrajectoryCommand.Position.Type = Kinova::CARTESIAN_VELOCITY;
set_fingers(demoTrajectoryCommand.Position.Fingers, 0, 0, 0);
numDemoCartesianVelocities = 4;
// must be 12 or less
set_pose(demoCartesianVelocities[0], 0, 0, 5, 0, 0, 0);
set_pose(demoCartesianVelocities[1], 0, -5, -5, 0, 0, 0);
set_pose(demoCartesianVelocities[2], 0, 5, 0, 0, 0, 0);
set_pose(demoCartesianVelocities[3], 0, 0, 0, 0, 0, 0);
demoPositionCommand.InitStruct();
demoPositionCommand.Position.Type = Kinova::CARTESIAN_POSITION;
// must be 12 or less positions
int i = 0;
set_pose(demoCartesianPositions[i++], -0.23, -0.69, 0.08, 1.136, -1.165, 2.445);
set_pose(demoCartesianPositions[i++], -0.23, -0.69, 0.08, 2.954, -0.195, 2.445); // point down
set_pose(demoCartesianPositions[i++], -0.23, -0.69, -0.15, 2.954, -0.195, 2.445); // move down
set_pose(demoCartesianPositions[i++], -0.23, -0.69, 0.08, 2.954, -0.195, 2.445); // back up
//set_pose(demoCartesianPositions[i++], -0.23, -0.69, -0.15, 2.950, -0.195, 2.445);
set_pose(demoCartesianPositions[i++], -0.09, -0.69, 0.07, 2.950, -0.195, 2.445);
set_pose(demoCartesianPositions[i++], -0.03, -0.32, 0.22, 2.950, -0.195, 2.445);
set_pose(demoCartesianPositions[i++], -0.013,-0.53, 0.08, 0.054, -1.355, 0.960);
// set_pose(demoCartesianPositions[i++], -0.013, -0.53, 0.08, 1.855, 1.383, -2.711);
set_pose(demoCartesianPositions[i++], -0.23, -0.69, -0.15, 2.954, -0.195, 2.445); // move down
numDemoCartesianPositions = i;
}
//#################### CONSTRUCTORS ####################
Skeleton::Skeleton(const BoneHierarchy_Ptr& boneHierarchy, const std::map<std::string,Animation_CPtr>& animations)
: m_boneHierarchy(boneHierarchy), m_animations(animations)
{
set_pose(boneHierarchy->configure_pose(make_rest_pose()));
build_to_bone_matrices();
}
void AnimationController::update_pose(int milliseconds)
{
switch(m_state)
{
case AS_REST:
{
set_pose(m_skeleton->make_rest_pose());
break;
}
case AS_PLAY:
{
Animation_CPtr animation = m_skeleton->animation(m_animationName);
int animationLength = static_cast<int>(animation->length() * 1000);
m_animationTime += milliseconds;
// Loop if we've gone past the end of the animation.
if(animationLength > 0) m_animationTime %= animationLength;
else m_animationTime = 0;
// Calculate the keyframe index and interpolation parameter.
int lastKeyframe = animation->keyframe_count() - 1;
double animationFraction = animationLength > 0 ? (double)m_animationTime / animationLength : 0;
double keyframePos = animationFraction * lastKeyframe;
int keyframeIndex = static_cast<int>(ceil(keyframePos));
double dummy;
double t = modf(keyframePos, &dummy); // t is the floating-point part of the keyframe position
// Clamp the keyframe index to be safe.
keyframeIndex = std::min(keyframeIndex, lastKeyframe);
Pose_CPtr newPose = animation->keyframe(keyframeIndex);
if(m_interpolateKeyframes)
{
int oldKeyframeIndex = (keyframeIndex + lastKeyframe) % animation->keyframe_count();
Pose_CPtr oldPose = animation->keyframe(oldKeyframeIndex);
set_pose(Pose::interpolate(oldPose, newPose, t));
}
else
{
set_pose(newPose);
}
break;
}
case AS_TRANSITION:
{
Pose_CPtr newPose;
if(m_animationName != "<rest>")
{
// If the new animation isn't the rest animation, transition to its start keyframe.
newPose = m_skeleton->animation(m_animationName)->keyframe(0);
}
else
{
newPose = m_skeleton->make_rest_pose();
}
m_animationTime += milliseconds;
if(m_animationTime >= TRANSITION_TIME)
{
// The transition is over.
m_state = m_animationName != "<rest>" ? AS_PLAY : AS_REST;
m_animationTime = 0;
m_transitionStart.reset();
set_pose(newPose);
}
else
{
// Still transitioning: interpolate between the transition start pose and the new pose.
double t = (double)m_animationTime / TRANSITION_TIME;
set_pose(Pose::interpolate(m_transitionStart, newPose, t));
}
break;
}
}
}
void VelodynePlaybackServer::applanixPoseHandler(const applanix::ApplanixPose& pose) {
set_pose(pose.smooth_x, pose.smooth_y, pose.smooth_z, pose.roll, pose.pitch, pose.yaw, pose.timestamp);
}