Ejemplo n.º 1
0
//#################### 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();
}
Ejemplo n.º 2
0
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();
}
Ejemplo n.º 3
0
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;
}
Ejemplo n.º 5
0
//#################### 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();
}
Ejemplo n.º 6
0
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);
}