/******************************************************************************
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);
}
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);
};
};
};