Esempio n. 1
0
/******************************************************************************
Move the camera backward
******************************************************************************/
void Camera3::MoveBackward(const double dt)
{
	Vector3 tmp_target(target.x, 0, target.z);
	Vector3 tmp_pos(position.x, 0, position.z);
	Vector3 view = (tmp_target - tmp_pos).Normalized();
	position += view * movingVelocity * (float)dt;
	target += view * movingVelocity * (float)dt;

	//reset flag
	//myKeys[key] = false;
}
void OmplRosRPYIKStateTransformer::omplStateToPose(const ompl::base::State &ompl_state,
                                                   geometry_msgs::Pose &pose)
{
  
  tf::Vector3 tmp_pos(ompl_state.as<ompl::base::CompoundState>()->as<ompl::base::RealVectorStateSpace::StateType>(real_vector_index_)->values[x_index_],
                    ompl_state.as<ompl::base::CompoundState>()->as<ompl::base::RealVectorStateSpace::StateType>(real_vector_index_)->values[y_index_],
                    ompl_state.as<ompl::base::CompoundState>()->as<ompl::base::RealVectorStateSpace::StateType>(real_vector_index_)->values[z_index_]);
  tf::Quaternion tmp_rot;
  tmp_rot.setRPY(ompl_state.as<ompl::base::CompoundState>()->as<ompl::base::RealVectorStateSpace::StateType>(real_vector_index_)->values[roll_index_],
                 ompl_state.as<ompl::base::CompoundState>()->as<ompl::base::RealVectorStateSpace::StateType>(real_vector_index_)->values[pitch_index_],
                 ompl_state.as<ompl::base::CompoundState>()->as<ompl::base::RealVectorStateSpace::StateType>(real_vector_index_)->values[yaw_index_]);
  tf::Transform tmp_transform(tmp_rot,tmp_pos);
  tf::poseTFToMsg(tmp_transform,pose);  
}
Esempio n. 3
0
void prismatic_joint_2D::doMotion(kte_pass_flag aFlag, const shared_ptr<frame_storage>& aStorage) {
  if((!mEnd) || (!mBase))
    return;
  
  mEnd->Parent = mBase->Parent;
  
  if(!mCoord) {
    mEnd->Position = mBase->Position;
    mEnd->Velocity = mBase->Velocity;
    mEnd->Acceleration = mBase->Acceleration;
  } else {
    vect<double,2> tmp_pos(mCoord->q * mAxis);
    vect<double,2> tmp_vel(mCoord->q_dot * mAxis);
    mEnd->Position = mBase->Position + mBase->Rotation * tmp_pos;
    mEnd->Velocity = mBase->Velocity + mBase->Rotation * ( (mBase->AngVelocity % tmp_pos) + tmp_vel );
    mEnd->Acceleration = mBase->Acceleration + mBase->Rotation * ( (-mBase->AngVelocity * mBase->AngVelocity) * tmp_pos + (2.0 * mBase->AngVelocity) % tmp_vel + mBase->AngAcceleration % tmp_pos + (mCoord->q_ddot * mAxis) );
 
    if(mJacobian) {
      mJacobian->Parent = mEnd;
      
      mJacobian->qd_vel = mAxis;
      mJacobian->qd_avel = 0.0;
      mJacobian->qd_acc = vect<double,2>();
      mJacobian->qd_aacc = 0.0;
    };
  };
  
  mEnd->Rotation = mBase->Rotation;
  mEnd->AngVelocity = mBase->AngVelocity;
  mEnd->AngAcceleration = mBase->AngAcceleration;
  
  if((aFlag == store_kinematics) && (aStorage)) {
    if(!(aStorage->frame_2D_mapping[mBase]))
      aStorage->frame_2D_mapping[mBase] = shared_ptr< frame_2D<double> >(new frame_2D<double>((*mBase)),scoped_deleter());
    else
      (*(aStorage->frame_2D_mapping[mBase])) = (*mBase);
    if(!(aStorage->frame_2D_mapping[mEnd]))
      aStorage->frame_2D_mapping[mEnd] = shared_ptr< frame_2D<double> >(new frame_2D<double>((*mEnd)),scoped_deleter());
    else
      (*(aStorage->frame_2D_mapping[mEnd])) = (*mEnd);
    if(mCoord) {
      if(!(aStorage->gen_coord_mapping[mCoord]))
        aStorage->gen_coord_mapping[mCoord] = shared_ptr< gen_coord<double> >(new gen_coord<double>((*mCoord)),scoped_deleter());
      else
        (*(aStorage->gen_coord_mapping[mCoord])) = (*mCoord);
    };
  };
};