本文整理汇总了C++中kdl::Frame::DH方法的典型用法代码示例。如果您正苦于以下问题：C++ Frame::DH方法的具体用法？C++ Frame::DH怎么用？C++ Frame::DH使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类kdl::Frame的用法示例。
在下文中一共展示了Frame::DH方法的2个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: idynChain2kdlChain
bool idynChain2kdlChain(iCub::iDyn::iDynChain & idynChain,KDL::Chain & kdlChain,std::vector<std::string> link_names,std::vector<std::string> joint_names, std::string final_frame_name, std::string initial_frame_name, int max_links)
{
int n_links, i;
bool use_names;
n_links = idynChain.getN();
//In iDyn, the joints are only 1 DOF (not 0, not more)
int n_joints = idynChain.getN();
if(n_links <= 0 ) return false;
if( (int)link_names.size() < n_links || (int)joint_names.size() < n_joints ) {
use_names = false;
} else {
use_names = true;
}
KDL::Frame kdlFrame = KDL::Frame();
KDL::Frame kdl_H = KDL::Frame();
KDL::Joint kdlJoint = KDL::Joint();
kdlChain = KDL::Chain();
KDL::Segment kdlSegment = KDL::Segment();
KDL::RigidBodyInertia kdlRigidBodyInertia = KDL::RigidBodyInertia();
if( initial_frame_name.length() != 0 ) {
idynMatrix2kdlFrame(idynChain.getH0(),kdlFrame);
kdlSegment = KDL::Segment(initial_frame_name,KDL::Joint(initial_frame_name+"_joint",KDL::Joint::None),kdlFrame);
kdlChain.addSegment(kdlSegment);
}
for(i=0; i<n_links && i < max_links; i++)
{
//forgive him, as he does not know what is doing
iCub::iKin::iKinLink & link_current = idynChain[i];
//For the first link and the last link, take in account also H0 and HN
if ( i == n_links - 1) {
if( final_frame_name.length() == 0 ) {
idynMatrix2kdlFrame(idynChain.getHN(),kdl_H);
kdlFrame = kdlFrame.DH(link_current.getA(),link_current.getAlpha(),link_current.getD(),link_current.getOffset())*kdl_H;
} else {
kdlFrame = kdlFrame.DH(link_current.getA(),link_current.getAlpha(),link_current.getD(),link_current.getOffset());
}
} else {
kdlFrame = kdlFrame.DH(link_current.getA(),link_current.getAlpha(),link_current.getD(),link_current.getOffset());
}
bool ret = idynDynamicalParameters2kdlRigidBodyInertia(idynChain.getMass(i),idynChain.getCOM(i).subcol(0,3,3),idynChain.getInertia(i),kdlRigidBodyInertia);
if( !ret ) return false;
KDL::Joint jnt_idyn;
//\todo: joint can also be blocked at values different from 0.0
if( idynChain.isLinkBlocked(i) ) {
if( use_names ) {
kdlSegment = KDL::Segment(link_names[i],KDL::Joint(joint_names[i],KDL::Joint::None),kdlFrame,kdlRigidBodyInertia);
} else {
kdlSegment = KDL::Segment(KDL::Joint(KDL::Joint::None),kdlFrame,kdlRigidBodyInertia);
}
} else {
if( use_names ) {
kdlSegment = KDL::Segment(link_names[i],KDL::Joint(joint_names[i],KDL::Joint::RotZ),kdlFrame,kdlRigidBodyInertia);
} else {
kdlSegment = KDL::Segment(KDL::Joint(KDL::Joint::RotZ),kdlFrame,kdlRigidBodyInertia);
}
}
kdlChain.addSegment(kdlSegment);
}
//if specified, add a final fake link
if( final_frame_name.length() != 0 ) {
idynMatrix2kdlFrame(idynChain.getHN(),kdlFrame);
kdlSegment = KDL::Segment(final_frame_name,KDL::Joint(final_frame_name+"_joint",KDL::Joint::None),kdlFrame);
kdlChain.addSegment(kdlSegment);
}
//Considering the H0 transformation
if( initial_frame_name.length() == 0 ) {
KDL::Chain new_chain;
KDL::Frame kdl_H0;
idynMatrix2kdlFrame(idynChain.getH0(),kdl_H0);
//std::cout << "KDL_h0 " << kdl_H0 << std::endl;
addBaseTransformation(kdlChain,new_chain,kdl_H0);
kdlChain = new_chain;
}
return true;
}示例2: idynSensorChain2kdlChain
bool idynSensorChain2kdlChain(iCub::iDyn::iDynChain & idynChain,
iCub::iDyn::iDynInvSensor & idynSensor,
KDL::Chain & kdlChain,
KDL::Frame & H_sensor_dh_child,
std::vector<std::string> link_names,std::vector<std::string> joint_names, std::string final_frame_name, std::string initial_frame_name, int max_links)
{
bool use_names;
int n_links, i, sensor_link;
n_links = idynChain.getN();
sensor_link = idynSensor.getSensorLink();
int kdl_links = n_links + 1; //The sensor links transform a link in two different links
int kdl_joints = kdl_links;
if(n_links <= 0 ) return false;
if( (int)link_names.size() < kdl_links || (int)joint_names.size() < kdl_joints ) {
use_names = false;
} else {
use_names = true;
}
KDL::Frame kdlFrame = KDL::Frame();
KDL::Frame kdl_H;
kdlChain = KDL::Chain();
KDL::Segment kdlSegment = KDL::Segment();
KDL::RigidBodyInertia kdlRigidBodyInertia = KDL::RigidBodyInertia();
int kdl_i = 0;
if( initial_frame_name.length() != 0 ) {
idynMatrix2kdlFrame(idynChain.getH0(),kdlFrame);
kdlSegment = KDL::Segment(initial_frame_name,KDL::Joint(initial_frame_name+"_joint",KDL::Joint::None),kdlFrame);
kdlChain.addSegment(kdlSegment);
}
KDL::Frame remainder_frame = KDL::Frame::Identity();
for(i=0; i<n_links; i++)
{
if( i != sensor_link ) {
//forgive him, as he does not know what is doing
iCub::iKin::iKinLink & link_current = idynChain[i];
//For the last link, take in account also HN
if ( i == n_links - 1) {
idynMatrix2kdlFrame(idynChain.getHN(),kdl_H);
kdlFrame = kdlFrame.DH(link_current.getA(),link_current.getAlpha(),link_current.getD(),link_current.getOffset())*kdl_H;
} else {
kdlFrame = kdlFrame.DH(link_current.getA(),link_current.getAlpha(),link_current.getD(),link_current.getOffset());
}
bool ret = idynDynamicalParameters2kdlRigidBodyInertia(idynChain.getMass(i),idynChain.getCOM(i).subcol(0,3,3),idynChain.getInertia(i),kdlRigidBodyInertia);
assert(ret);
if(!ret) return false;
//For the joint after the ft sensor, consider the offset between the ft sensor
// and the joint
if( idynChain.isLinkBlocked(i) ) {
// if it is blocked, it is easy to add the remainder frame
if( use_names ) {
kdlSegment = KDL::Segment(link_names[kdl_i],KDL::Joint(joint_names[kdl_i],KDL::Joint::None),remainder_frame*kdlFrame,kdlRigidBodyInertia);
kdl_i++;
} else {
kdlSegment = KDL::Segment(KDL::Joint(KDL::Joint::None),remainder_frame*kdlFrame,kdlRigidBodyInertia);
}
} else {
KDL::Vector new_joint_axis, new_joint_origin;
new_joint_axis = remainder_frame.M*KDL::Vector(0.0,0.0,0.1);
new_joint_origin = remainder_frame.p;
if( use_names ) {
kdlSegment = KDL::Segment(link_names[kdl_i],KDL::Joint(joint_names[kdl_i],new_joint_origin,new_joint_axis,KDL::Joint::RotAxis),remainder_frame*kdlFrame,kdlRigidBodyInertia);
kdl_i++;
} else {
kdlSegment = KDL::Segment(KDL::Joint(new_joint_origin,new_joint_axis,KDL::Joint::RotAxis),remainder_frame*kdlFrame,kdlRigidBodyInertia);
}
}
kdlChain.addSegment(kdlSegment);
remainder_frame = KDL::Frame::Identity();
} else {
//( i == segment_link )
double m,m0,m1;
yarp::sig::Vector r0(3),r1(3),r(3),rgg0(3),rgg1(3),r_i_s_wrt_i(3),r_i_C0_wrt_i;
yarp::sig::Matrix I,I0,I1;
yarp::sig::Matrix R_s_wrt_i;
iCub::iKin::iKinLink & link_current = idynChain[sensor_link];
KDL::Frame kdlFrame_0 = KDL::Frame();
KDL::Frame kdlFrame_1 = KDL::Frame();
//Imagine that we have i, s , i+1
//yarp::sig::Matrix H_0;
//The angle of the sensor link joint is put to 0 and then restored
double angSensorLink = link_current.getAng();
yarp::sig::Matrix H_sensor_link = (link_current.getH(0.0)); //H_{i-1}_i
link_current.setAng(angSensorLink);
//idynSensor.getH() <--> H_i_s
yarp::sig::Matrix H_0 = H_sensor_link * (idynSensor.getH()); // H_{i-1}_s = H_{i-1}_i*H_i_s ?
//.........这里部分代码省略.........