本文整理汇总了C++中kdl::Frame类的典型用法代码示例。如果您正苦于以下问题：C++ Frame类的具体用法？C++ Frame怎么用？C++ Frame使用的例子？那么, 这里精选的类代码示例或许可以为您提供帮助。
在下文中一共展示了Frame类的15个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: getXInv
void ChainKinematics::getXInv(double (&t)[3], double (&rot)[9], const double* q, const int segmentNr) const {
int i,j;
KDL::JntArray qJA(Njnt_);
for(i=0; i<Njnt_; i++)
qJA(i) = q[i];
KDL::Frame frame;
//compute forward kinematics
checkSegmentNr(segmentNr);
if(fksolverPos_->JntToCart(qJA, frame, segmentNr) < 0) {
std::cerr << "ERROR: [ChainKinematics][getXInv] something went wrong during JntToCart! Exiting!" << std::endl;
exit(-1);
}
//get inverse transformation
frame = frame.Inverse();
for(i=0; i<3; i++) {
t[i] = frame.p(i);
for(j=0; j<3; j++)
rot[i*3+j] = frame.M(i,j);
}
}示例2: set_marker
void MultiTaskPriorityInverseKinematics::set_marker(KDL::Frame x, int index, int id)
{
sstr_.str("");
sstr_.clear();
if (links_index_[index] == -1)
sstr_<<"end_effector";
else
sstr_<<"link_"<<(links_index_[index]-1);
msg_marker_.markers[index].header.frame_id = "world";
msg_marker_.markers[index].header.stamp = ros::Time();
msg_marker_.markers[index].ns = sstr_.str();
msg_marker_.markers[index].id = id;
msg_marker_.markers[index].type = visualization_msgs::Marker::SPHERE;
msg_marker_.markers[index].action = visualization_msgs::Marker::ADD;
msg_marker_.markers[index].pose.position.x = x.p(0);
msg_marker_.markers[index].pose.position.y = x.p(1);
msg_marker_.markers[index].pose.position.z = x.p(2);
msg_marker_.markers[index].pose.orientation.x = 0.0;
msg_marker_.markers[index].pose.orientation.y = 0.0;
msg_marker_.markers[index].pose.orientation.z = 0.0;
msg_marker_.markers[index].pose.orientation.w = 1.0;
msg_marker_.markers[index].scale.x = 0.01;
msg_marker_.markers[index].scale.y = 0.01;
msg_marker_.markers[index].scale.z = 0.01;
msg_marker_.markers[index].color.a = 1.0;
msg_marker_.markers[index].color.r = 0.0;
msg_marker_.markers[index].color.g = 1.0;
msg_marker_.markers[index].color.b = 0.0;
}示例3: getX
void ChainKinematics::getX(double (&t)[3], double (&rot)[9], const double* q, const int segmentNr) const {
int i,j;
////DEBUG
//std::cout << "++++++++++++++++++++ DEBUG 0" << std::endl;
//std::cout << "q = ";
//for(i=0; i<Njnt_; i++)
//std::cout << q[i] << " " << std::endl;
//std::cout << std::endl;
KDL::JntArray qJA(Njnt_);
for(i=0; i<Njnt_; i++)
qJA(i) = q[i];
KDL::Frame frame;
//compute forward kinematics
checkSegmentNr(segmentNr);
if(fksolverPos_->JntToCart(qJA, frame, segmentNr) < 0) {
std::cerr << "ERROR: [ChainKinematics][getX] something went wrong during JntToCart! Exiting!" << std::endl;
exit(-1);
}
for(i=0; i<3; i++) {
t[i] = frame.p(i);
for(j=0; j<3; j++)
rot[i*3+j] = frame.M(i,j);
}
}示例4: checkIdentity
bool checkIdentity(KDL::Frame frame, double tol = 1e-4)
{
for(int i=0; i < 3; i++ )
{
for(int j=0; j < 3; j++ )
{
double err;
if( i == j )
{
err = fabs(frame.M(i,j)-1);
}
else
{
err = fabs(frame.M(i,j));
}
if( err < tol ) return false;
}
}
for(int i =0; i < 3; i++ )
{
double err = fabs(frame.p[i]);
if( err < tol ) return false;
}
return true;
}示例5: KDLToEigenMatrix
Eigen::Matrix4f KDLToEigenMatrix(const KDL::Frame &p)
{
Eigen::Matrix4f b = Eigen::Matrix4f::Identity();
for(int i=0; i < 3; i++)
{
for(int j=0; j<3; j++)
{
b(i,j) = p.M(i,j);
}
b(i,3) = p.p(i);
}
return b;
}示例6: transformEigenToKDL
void leatherman::transformEigenToKDL(const Eigen::Affine3d &e, KDL::Frame &k)
{
k.p[0] = e(0,3);
k.p[1] = e(1,3);
k.p[2] = e(2,3);
k.M(0,0) = e(0,0);
k.M(0,1) = e(0,1);
k.M(0,2) = e(0,2);
k.M(1,0) = e(1,0);
k.M(1,1) = e(1,1);
k.M(1,2) = e(1,2);
k.M(2,0) = e(2,0);
k.M(2,1) = e(2,1);
k.M(2,2) = e(2,2);
}示例7:
//------------------------------------------------------------------------------
// Constructor hard-coded for LWR4 DH
//------------------------------------------------------------------------------
LWR4Kinematics::LWR4Kinematics(KDL::Frame _tool_to_ee){
tr_tool_to_ee = _tool_to_ee;
// just to prevent inverting at the run time
tr_ee_to_tool = _tool_to_ee.Inverse();
jlim1 = 2.97; // rads = 170 degrees for joints: 1 3 5 7
jlim2 = 2.09; // rads = 120 degrees for joints: 2 4 6
config_fk = 0;
n_joints = 7;
psi_fk = 0.0;
dbs=0.31;
dse=0.4;
dew=0.39;
dwt=0.078;
// q_ik = std::vector<double>(7, 0.0);
vbs = tf::Vector3(0.0,0.0,dbs);
vwt = tf::Vector3(0.0,0.0,dwt);
};示例8: transformKDLToEigen
void leatherman::transformKDLToEigen(const KDL::Frame &k, Eigen::Affine3d &e)
{
e(0,3) = k.p[0];
e(1,3) = k.p[1];
e(2,3) = k.p[2];
e(0,0) = k.M(0,0);
e(0,1) = k.M(0,1);
e(0,2) = k.M(0,2);
e(1,0) = k.M(1,0);
e(1,1) = k.M(1,1);
e(1,2) = k.M(1,2);
e(2,0) = k.M(2,0);
e(2,1) = k.M(2,1);
e(2,2) = k.M(2,2);
e(3,0) = 0.0;
e(3,1) = 0.0;
e(3,2) = 0.0;
e(3,3) = 1.0;
}示例9: graspPointCB
void UrdfModelMarker::graspPointCB( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ){
//linkMarkerMap[feedback->marker_name].gp.pose = feedback->pose;
//publishMarkerPose(feedback);
//publishMarkerMenu(feedback, jsk_interactive_marker::MarkerMenu::MOVE);
KDL::Vector graspVec(feedback->mouse_point.x, feedback->mouse_point.y, feedback->mouse_point.z);
KDL::Frame parentFrame;
tf::poseMsgToKDL (linkMarkerMap[feedback->marker_name].pose, parentFrame);
graspVec = parentFrame.Inverse(graspVec);
geometry_msgs::Pose p;
p.position.x = graspVec.x();
p.position.y = graspVec.y();
p.position.z = graspVec.z();
p.orientation = linkMarkerMap[feedback->marker_name].gp.pose.orientation;
linkMarkerMap[feedback->marker_name].gp.pose = p;
linkMarkerMap[feedback->marker_name].gp.displayGraspPoint = true;
addChildLinkNames(model->getRoot(), true, false);
//addChildLinkNames(model->getRoot(), true, false, true, 0);
}示例10: PoseTFToKDL
TEST(TFKDLConversions, tf_kdl_pose)
{
tf::Pose t;
tf::Quaternion tq;
tq[0] = gen_rand(-1.0,1.0);
tq[1] = gen_rand(-1.0,1.0);
tq[2] = gen_rand(-1.0,1.0);
tq[3] = gen_rand(-1.0,1.0);
tq.normalize();
t.setOrigin(tf::Vector3(gen_rand(-10,10),gen_rand(-10,10),gen_rand(-10,10)));
t.setRotation(tq);
//test tf->kdl
KDL::Frame k;
PoseTFToKDL(t,k);
for(int i=0; i < 3; i++)
{
ASSERT_NEAR(t.getOrigin()[i],k.p[i],1e-6);
for(int j=0; j < 3; j++)
{
ASSERT_NEAR(t.getBasis()[i][j],k.M(i,j),1e-6);
}
}
//test kdl->tf
tf::Pose r;
PoseKDLToTF(k,r);
for(int i=0; i< 3; i++)
{
ASSERT_NEAR(t.getOrigin()[i],r.getOrigin()[i],1e-6);
for(int j=0; j < 3; j++)
{
ASSERT_NEAR(t.getBasis()[i][j],r.getBasis()[i][j],1e-6);
}
}
}示例11: set_marker
void DynamicSlidingModeControllerTaskSpace::set_marker(KDL::Frame x, int id)
{
msg_marker_.header.frame_id = "world";
msg_marker_.header.stamp = ros::Time();
msg_marker_.ns = "end_effector";
msg_marker_.id = id;
msg_marker_.type = visualization_msgs::Marker::SPHERE;
msg_marker_.action = visualization_msgs::Marker::ADD;
msg_marker_.pose.position.x = x.p(0);
msg_marker_.pose.position.y = x.p(1);
msg_marker_.pose.position.z = x.p(2);
msg_marker_.pose.orientation.x = 0.0;
msg_marker_.pose.orientation.y = 0.0;
msg_marker_.pose.orientation.z = 0.0;
msg_marker_.pose.orientation.w = 1.0;
msg_marker_.scale.x = 0.005;
msg_marker_.scale.y = 0.005;
msg_marker_.scale.z = 0.005;
msg_marker_.color.a = 1.0;
msg_marker_.color.r = 0.0;
msg_marker_.color.g = 1.0;
msg_marker_.color.b = 0.0;
}示例12: getQ
void ChainKinematics::getQ(double* q, const double* qInit, const double (&t)[3], const double (&rot)[9]) const {
int i,j;
KDL::JntArray qInitJA(Njnt_);
for(i=0; i<Njnt_; i++)
qInitJA(i) = qInit[i];
KDL::Frame frame;
for(i=0; i<3; i++) {
frame.p(i) = t[i];
for(j=0; j<3; j++)
frame.M(i,j) = rot[i*3+j];
}
KDL::JntArray qJA;
if(iksolverPos_->CartToJnt(qInitJA, frame, qJA) < 0) {
std::cerr << "ERROR: [ChainKinematics][getQ] something went wrong during CartToJnt! Exiting!" << std::endl;
exit(-1);
}
for(i=0; i<Njnt_; i++)
q[i] = qJA(i);
}示例13: addLinkContacts
void CollisionModel::addLinkContacts(double dist_range, const std::string &link_name, const pcl::PointCloud<pcl::PointNormal>::Ptr &res,
const KDL::Frame &T_W_L, const std::vector<ObjectModel::Feature > &ob_features,
const KDL::Frame &T_W_O) {
const double lambda = - std::log(0.01) / (dist_range * dist_range);
std::list<std::pair<int, double> > link_pt;
for (int poidx = 0; poidx < ob_features.size(); poidx++) {
double min_dist = dist_range + 1.0;
for (int pidx = 0; pidx < res->points.size(); pidx++) {
KDL::Vector p1(res->points[pidx].x, res->points[pidx].y, res->points[pidx].z);
const KDL::Vector &p2( ob_features[poidx].T_O_F_.p );
double dist = (T_W_L * p1 - T_W_O * p2).Norm();
if (dist < min_dist) {
min_dist = dist;
}
}
if (min_dist < dist_range) {
link_pt.push_back( std::make_pair(poidx, min_dist) );
}
}
if ( link_pt.size() > 0 ) {
link_models_map_[link_name].features_.resize( link_pt.size() );
std::vector<KDL::Frame > T_L_F_vec( link_pt.size() );
col_link_names_.push_back(link_name);
int fidx = 0;
KDL::Vector col_pt;
double sum_weight = 0.0;
for (std::list<std::pair<int, double> >::const_iterator it = link_pt.begin(); it != link_pt.end(); it++, fidx++) {
int poidx = it->first;
link_models_map_[link_name].features_[fidx].pc1 = ob_features[poidx].pc1_;
link_models_map_[link_name].features_[fidx].pc2 = ob_features[poidx].pc2_;
KDL::Frame T_W_F = T_W_O * ob_features[poidx].T_O_F_;
T_L_F_vec[fidx] = T_W_L.Inverse() * T_W_F;
double dist = it->second;
link_models_map_[link_name].features_[fidx].dist = dist;
double weight = std::exp(-lambda * dist * dist);
link_models_map_[link_name].features_[fidx].weight = weight;
col_pt = col_pt + weight * T_L_F_vec[fidx].p;
sum_weight += weight;
}
link_models_map_[link_name].T_L_C_ = KDL::Frame(col_pt / sum_weight);
for (int fidx = 0; fidx < link_models_map_[link_name].features_.size(); fidx++) {
link_models_map_[link_name].features_[fidx].T_C_F = link_models_map_[link_name].T_L_C_.Inverse() * T_L_F_vec[fidx];
link_models_map_[link_name].features_[fidx].weight /= sum_weight;
}
}
}示例14: toUrdf
/**
*
* @param jnt the KDL::Joint to convert (axis and origin expressed in the
* predecessor frame of reference, as by KDL convention)
* @param frameToTip the predecessor/successor frame transformation
* @param H_new_old_predecessor in the case the predecessor frame is being
* modified to comply to URDF constraints (frame origin on the joint axis)
* this matrix the transformation from the old frame to the new frame (H_new_old)
* @param H_new_old_successor in the case the successor frame is being
* modified to comply to URDF constraints (frame origin on the joint axis)
* this matrix the transformation from the old frame to the new frame (H_new_old)
*/
urdf::Joint toUrdf(const KDL::Joint & jnt,
const KDL::Frame & frameToTip,
const KDL::Frame & H_new_old_predecessor,
KDL::Frame & H_new_old_successor)
{
//URDF constaints the successor link frame origin to lay on the axis
//of the joint ( see : http://www.ros.org/wiki/urdf/XML/joint )
//Then if the JointOrigin of the KDL joint is not zero, it is necessary
//to move the link frame (then it is necessary to change also the spatial inertia)
//and the definition of the childrens of the successor frame
urdf::Joint ret;
ret.name = jnt.getName();
H_new_old_successor = getH_new_old(jnt,frameToTip);
ret.parent_to_joint_origin_transform = toUrdf(H_new_old_predecessor*frameToTip*(H_new_old_successor.Inverse()));
switch(jnt.getType())
{
case KDL::Joint::RotAxis:
case KDL::Joint::RotX:
case KDL::Joint::RotY:
case KDL::Joint::RotZ:
//using continuos if no joint limits are specified
ret.type = urdf::Joint::CONTINUOUS;
//in urdf, the joint axis is expressed in the joint/successor frame
//in kdl, the joint axis is expressed in the predecessor rame
ret.axis = toUrdf(((frameToTip).M.Inverse((jnt.JointAxis()))));
break;
case KDL::Joint::TransAxis:
case KDL::Joint::TransX:
case KDL::Joint::TransY:
case KDL::Joint::TransZ:
ret.type = urdf::Joint::PRISMATIC;
//in urdf, the joint axis is expressed in the joint/successor frame
//in kdl, the joint axis is expressed in the predecessor rame
ret.axis = toUrdf((frameToTip.M.Inverse(jnt.JointAxis())));
break;
default:
std::cerr << "[WARN] Converting unknown joint type of joint " << jnt.getTypeName() << " into a fixed joint" << std::endl;
case KDL::Joint::None:
ret.type = urdf::Joint::FIXED;
}
return ret;
}示例15: setWorld_StanceFoot
void kinematic_filter::setWorld_StanceFoot(const KDL::Frame& World_StanceFoot)
{
this->StanceFoot_World=World_StanceFoot.Inverse();
this->World_StanceFoot=World_StanceFoot;
}