UAV_kinematics::UAV_kinematics(const std::string& aName,
const shared_ptr< frame_3D<double> >& aBaseFrame) :
inverse_kinematics_model(aName),
m_base_frame(aBaseFrame) {
m_motion_frame = shared_ptr< frame_3D<double> >(new frame_3D<double>(), scoped_deleter());
shared_ptr< frame_3D<double> > EE_frame(new frame_3D<double>(), scoped_deleter());
shared_ptr< jacobian_3D_3D<double> > joint_jacobian(new jacobian_3D_3D<double>(), scoped_deleter());
shared_ptr< free_joint_3D > joint_1(new free_joint_3D(
"UAV_free_joint",
m_motion_frame,
m_base_frame,
EE_frame,
joint_jacobian),
scoped_deleter());
m_output_frame = shared_ptr< joint_dependent_frame_3D >(new joint_dependent_frame_3D(EE_frame), scoped_deleter());
m_output_frame->add_joint(m_motion_frame, joint_jacobian);
m_chain = shared_ptr< kte_map_chain >(new kte_map_chain("UAV_quadrotor_kin_model"), scoped_deleter());
*m_chain << joint_1;
};
void free_joint_2D::doMotion(kte_pass_flag aFlag, const shared_ptr<frame_storage>& aStorage) {
if((!mEnd) || (!mBase))
return;
if(!mCoord) {
*mEnd = *mBase;
} else {
*mEnd = (*mBase) * (*mCoord);
mEnd->Parent = mBase->Parent;
if(mJacobian) {
mJacobian->Parent = mEnd;
mJacobian->vel_vel = vect< vect<double,2>,2>(vect<double,2>(1.0,0.0),vect<double,2>(0.0,1.0));
mJacobian->vel_avel = vect<double,2>();
mJacobian->avel_vel = vect<double,2>();
mJacobian->avel_avel = 1.0;
mJacobian->vel_acc = vect< vect<double,2>,2>();
mJacobian->vel_aacc = vect<double,2>();
mJacobian->avel_acc = vect<double,2>();
mJacobian->avel_aacc = 0.0;
};
};
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->frame_2D_mapping[mCoord]))
aStorage->frame_2D_mapping[mCoord] = shared_ptr< frame_2D<double> >(new frame_2D<double>((*mCoord)),scoped_deleter());
else
(*(aStorage->frame_2D_mapping[mCoord])) = (*mCoord);
};
};
};
void rigid_link_2D::doMotion(kte_pass_flag aFlag, const shared_ptr<frame_storage>& aStorage) {
if((!mBase) || (!mEnd))
return;
mEnd->Parent = mBase->Parent;
mEnd->Position = mBase->Position + mBase->Rotation * mPoseOffset.Position;
mEnd->Velocity = mBase->Velocity + mBase->Rotation * (mBase->AngVelocity % mPoseOffset.Position);
mEnd->Acceleration = mBase->Acceleration + mBase->Rotation * ( (-mBase->AngVelocity * mBase->AngVelocity) * mPoseOffset.Position + mBase->AngAcceleration % mPoseOffset.Position);
mEnd->Rotation = mBase->Rotation * mPoseOffset.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);
};
};
void rigid_link_gen::doMotion(kte_pass_flag aFlag, const shared_ptr<frame_storage>& aStorage) {
if((!mBase) || (!mEnd))
return;
mEnd->q = mBase->q + mOffset;
mEnd->q_dot = mBase->q_dot;
mEnd->q_ddot = mBase->q_ddot;
if((aFlag == store_kinematics) && (aStorage)) {
if(!(aStorage->gen_coord_mapping[mBase]))
aStorage->gen_coord_mapping[mBase] = shared_ptr< gen_coord<double> >(new gen_coord<double>((*mBase)),scoped_deleter());
else
(*(aStorage->gen_coord_mapping[mBase])) = (*mBase);
if(!(aStorage->gen_coord_mapping[mEnd]))
aStorage->gen_coord_mapping[mEnd] = shared_ptr< gen_coord<double> >(new gen_coord<double>((*mEnd)),scoped_deleter());
else
(*(aStorage->gen_coord_mapping[mEnd])) = (*mEnd);
};
};
void rigid_link_3D::doMotion(kte_pass_flag aFlag, const shared_ptr<frame_storage>& aStorage) {
if((!mBase) || (!mEnd))
return;
(*mEnd) = (*mBase) * mPoseOffset;
if((aFlag == store_kinematics) && (aStorage)) {
if(!(aStorage->frame_3D_mapping[mBase]))
aStorage->frame_3D_mapping[mBase] = shared_ptr< frame_3D<double> >(new frame_3D<double>((*mBase)),scoped_deleter());
else
(*(aStorage->frame_3D_mapping[mBase])) = (*mBase);
if(!(aStorage->frame_3D_mapping[mEnd]))
aStorage->frame_3D_mapping[mEnd] = shared_ptr< frame_3D<double> >(new frame_3D<double>((*mEnd)),scoped_deleter());
else
(*(aStorage->frame_3D_mapping[mEnd])) = (*mEnd);
};
};
void line_point_mindist_3D::doMotion(kte_pass_flag aFlag, const shared_ptr<frame_storage>& aStorage) {
if((!mBase) || (!mEnd))
return;
mEnd->Parent = mBase->Parent;
mEnd->Position = (mBase->Position * mTangent) * mTangent - mOriginMinDist;
mEnd->Velocity = (mBase->Velocity * mTangent) * mTangent;
mEnd->Acceleration = (mBase->Acceleration * mTangent) * mTangent;
mEnd->Quat = mBase->Quat;
mEnd->AngVelocity = mBase->AngVelocity;
mEnd->AngAcceleration = mBase->AngAcceleration;
if((aFlag == store_kinematics) && (aStorage)) {
if(!(aStorage->frame_3D_mapping[mBase]))
aStorage->frame_3D_mapping[mBase] = shared_ptr< frame_3D<double> >(new frame_3D<double>((*mBase)),scoped_deleter());
else
(*(aStorage->frame_3D_mapping[mBase])) = (*mBase);
if(!(aStorage->frame_3D_mapping[mEnd]))
aStorage->frame_3D_mapping[mEnd] = shared_ptr< frame_3D<double> >(new frame_3D<double>((*mEnd)),scoped_deleter());
else
(*(aStorage->frame_3D_mapping[mEnd])) = (*mEnd);
};
};
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);
};
};
};
void prismatic_joint_3D::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 {
rot_mat_3D<double> R(mBase->Quat.getRotMat());
vect<double,3> tmp_pos = mCoord->q * mAxis;
vect<double,3> tmp_vel = mCoord->q_dot * mAxis;
mEnd->Position = mBase->Position + R * tmp_pos;
mEnd->Velocity = mBase->Velocity + R * ( (mBase->AngVelocity % tmp_pos) + tmp_vel );
mEnd->Acceleration = mBase->Acceleration + R * ( (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 = vect<double,3>();
mJacobian->qd_acc = vect<double,3>();
mJacobian->qd_aacc = vect<double,3>();
};
};
mEnd->Quat = mBase->Quat;
mEnd->AngVelocity = mBase->AngVelocity;
mEnd->AngAcceleration = mBase->AngAcceleration;
mEnd->UpdateQuatDot();
if((aFlag == store_kinematics) && (aStorage)) {
if(!(aStorage->frame_3D_mapping[mBase]))
aStorage->frame_3D_mapping[mBase] = shared_ptr< frame_3D<double> >(new frame_3D<double>((*mBase)),scoped_deleter());
else
(*(aStorage->frame_3D_mapping[mBase])) = (*mBase);
if(!(aStorage->frame_3D_mapping[mEnd]))
aStorage->frame_3D_mapping[mEnd] = shared_ptr< frame_3D<double> >(new frame_3D<double>((*mEnd)),scoped_deleter());
else
(*(aStorage->frame_3D_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);
};
};
};
void virtual_kte_interface_2D::doMotion(kte_pass_flag aFlag, const shared_ptr<frame_storage>& aStorage) {
if((!mEnd) || (!mBase))
return;
(*mEnd) = (*mBase);
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);
};
};
std::pair< shared_ptr< colored_model_3D >, shared_ptr< proxy_query_model_3D > > kte_chain_geometry_3D::attachToKTEChain(const kte::kte_map_chain& aKTEChain) const {
detail::kte_geom_3D_visitor vis = detail::kte_geom_3D_visitor(this);
kte::visit_kte_chain< detail::kte_geom_visit_3D_types >(vis, aKTEChain);
// then, construct the colored_model_3D from the resulting (attached) geometries:
shared_ptr< colored_model_3D > result_geom = shared_ptr< colored_model_3D >(new colored_model_3D(aKTEChain.getName() + "_geom"), scoped_deleter());
for(std::map< std::string, std::vector< colored_geometry_3D > >::const_iterator it = mGeomList.begin(); it != mGeomList.end(); ++it) {
for(std::size_t i = 0; i < it->second.size(); ++i) {
if(it->second[i].mGeom) {
result_geom->addAnchor(it->second[i].mGeom->getAnchor());
result_geom->addElement(it->second[i].mColor, it->second[i].mGeom);
};
};
};
// then, construct the proxy_query_model_3D from the resulting (attached) geometries:
shared_ptr< proxy_query_model_3D > result_prox = shared_ptr< proxy_query_model_3D >(new proxy_query_model_3D(aKTEChain.getName() + "_proxy"), scoped_deleter());
for(std::map< std::string, std::vector< shared_ptr< shape_3D > > >::const_iterator it = mProxyShapeList.begin(); it != mProxyShapeList.end(); ++it) {
for(std::size_t i = 0; i < it->second.size(); ++i) {
if(it->second[i]) {
result_prox->addShape(it->second[i]);
};
};
};
return std::pair< shared_ptr< colored_model_3D >, shared_ptr< proxy_query_model_3D > >(result_geom, result_prox);
};
void torsion_damper_3D::doMotion(kte_pass_flag aFlag, const shared_ptr<frame_storage>& aStorage) {
if((!mAnchor1) || (!mAnchor2))
return;
if((aFlag == store_kinematics) && (aStorage)) {
if(!(aStorage->frame_3D_mapping[mAnchor1]))
aStorage->frame_3D_mapping[mAnchor1] = shared_ptr< frame_3D<double> >(new frame_3D<double>((*mAnchor1)),scoped_deleter());
else
(*(aStorage->frame_3D_mapping[mAnchor1])) = (*mAnchor1);
if(!(aStorage->frame_3D_mapping[mAnchor2]))
aStorage->frame_3D_mapping[mAnchor2] = shared_ptr< frame_3D<double> >(new frame_3D<double>((*mAnchor2)),scoped_deleter());
else
(*(aStorage->frame_3D_mapping[mAnchor2])) = (*mAnchor2);
};
};
void inertia_3D::doMotion(kte_pass_flag aFlag, const shared_ptr<frame_storage>& aStorage) {
if((!mCenterOfMass) || (!mCenterOfMass->mFrame))
return;
if((aFlag == store_kinematics) && (aStorage)) {
if(!(aStorage->frame_3D_mapping[mCenterOfMass->mFrame]))
aStorage->frame_3D_mapping[mCenterOfMass->mFrame] = shared_ptr< frame_3D<double> >(new frame_3D<double>((*mCenterOfMass->mFrame)),scoped_deleter());
else
(*(aStorage->frame_3D_mapping[mCenterOfMass->mFrame])) = (*mCenterOfMass->mFrame);
};
};
void revolute_joint_2D::doMotion(kte_pass_flag aFlag, const shared_ptr<frame_storage>& aStorage) {
if((!mEnd) || (!mBase))
return;
mEnd->Parent = mBase->Parent;
mEnd->Position = mBase->Position;
mEnd->Velocity = mBase->Velocity;
mEnd->Acceleration = mBase->Acceleration;
if(!mAngle) {
mEnd->Rotation = mBase->Rotation;
mEnd->AngVelocity = mBase->AngVelocity;
mEnd->AngAcceleration = mBase->AngAcceleration;
} else {
mEnd->Rotation = mBase->Rotation * rot_mat_2D<double>(mAngle->q);
mEnd->AngVelocity = mBase->AngVelocity + mAngle->q_dot;
mEnd->AngAcceleration = mBase->AngAcceleration + mAngle->q_ddot;
if(mJacobian) {
mJacobian->Parent = mEnd;
mJacobian->qd_vel = vect<double,2>();
mJacobian->qd_avel = 1.0;
mJacobian->qd_acc = vect<double,2>();
mJacobian->qd_aacc = 0.0;
};
};
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(mAngle) {
if(!(aStorage->gen_coord_mapping[mAngle]))
aStorage->gen_coord_mapping[mAngle] = shared_ptr< gen_coord<double> >(new gen_coord<double>((*mAngle)),scoped_deleter());
else
(*(aStorage->gen_coord_mapping[mAngle])) = (*mAngle);
};
};
};
void revolute_joint_3D::doMotion(kte_pass_flag aFlag, const shared_ptr<frame_storage>& aStorage) {
if((!mEnd) || (!mBase))
return;
mEnd->Parent = mBase->Parent;
mEnd->Position = mBase->Position;
mEnd->Velocity = mBase->Velocity;
mEnd->Acceleration = mBase->Acceleration;
if(!mAngle) {
mEnd->Quat = mBase->Quat;
mEnd->AngVelocity = mBase->AngVelocity;
mEnd->AngAcceleration = mBase->AngAcceleration;
} else {
quaternion<double> tmp_quat(axis_angle<double>(mAngle->q,mAxis).getQuaternion());
rot_mat_3D<double> R2(tmp_quat.getRotMat());
mEnd->Quat = mBase->Quat * tmp_quat;
mEnd->AngVelocity = (mBase->AngVelocity * R2) + mAngle->q_dot * mAxis;
mEnd->AngAcceleration = (mBase->AngAcceleration * R2) + ((mBase->AngVelocity * R2) % (mAngle->q_dot * mAxis)) + mAngle->q_ddot * mAxis;
if(mJacobian) {
mJacobian->Parent = mEnd;
mJacobian->qd_vel = vect<double,3>();
mJacobian->qd_avel = mAxis;
mJacobian->qd_acc = vect<double,3>();
mJacobian->qd_aacc = vect<double,3>();
};
};
mEnd->UpdateQuatDot();
if((aFlag == store_kinematics) && (aStorage)) {
if(!(aStorage->frame_3D_mapping[mBase]))
aStorage->frame_3D_mapping[mBase] = shared_ptr< frame_3D<double> >(new frame_3D<double>((*mBase)),scoped_deleter());
else
(*(aStorage->frame_3D_mapping[mBase])) = (*mBase);
if(!(aStorage->frame_3D_mapping[mEnd]))
aStorage->frame_3D_mapping[mEnd] = shared_ptr< frame_3D<double> >(new frame_3D<double>((*mEnd)),scoped_deleter());
else
(*(aStorage->frame_3D_mapping[mEnd])) = (*mEnd);
if(mAngle) {
if(!(aStorage->gen_coord_mapping[mAngle]))
aStorage->gen_coord_mapping[mAngle] = shared_ptr< gen_coord<double> >(new gen_coord<double>((*mAngle)),scoped_deleter());
else
(*(aStorage->gen_coord_mapping[mAngle])) = (*mAngle);
};
};
};
register_type_impl() : ptr(new so_type_descriptor<T,Version>(),scoped_deleter()) {
detail::add_base_type<BaseList>::to(ptr);
so_type_repo::getInstance().addType(ptr);
};
