本文整理汇总了C++中kdl::Chain::getNrOfJoints方法的典型用法代码示例。如果您正苦于以下问题:C++ Chain::getNrOfJoints方法的具体用法?C++ Chain::getNrOfJoints怎么用?C++ Chain::getNrOfJoints使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类kdl::Chain
的用法示例。
在下文中一共展示了Chain::getNrOfJoints方法的15个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: Test
void RobotAlgos::Test(void)
{
//Definition of a kinematic chain & add segments to the chain
KDL::Chain chain;
chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.0,1.020))));
chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.480))));
chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.645))));
chain.addSegment(Segment(Joint(Joint::RotZ)));
chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.120))));
chain.addSegment(Segment(Joint(Joint::RotZ)));
// Create solver based on kinematic chain
ChainFkSolverPos_recursive fksolver = ChainFkSolverPos_recursive(chain);
// Create joint array
unsigned int nj = chain.getNrOfJoints();
KDL::JntArray jointpositions = JntArray(nj);
// Assign some values to the joint positions
for(unsigned int i=0;i<nj;i++){
float myinput;
printf ("Enter the position of joint %i: ",i);
scanf ("%e",&myinput);
jointpositions(i)=(double)myinput;
}
// Create the frame that will contain the results
KDL::Frame cartpos;
// Calculate forward position kinematics
int kinematics_status;
kinematics_status = fksolver.JntToCart(jointpositions,cartpos);
if(kinematics_status>=0){
std::cout << cartpos <<std::endl;
printf("%s \n","Succes, thanks KDL!");
}else{
printf("%s \n","Error: could not calculate forward kinematics :(");
}
//Creation of the solvers:
ChainFkSolverPos_recursive fksolver1(chain);//Forward position solver
ChainIkSolverVel_pinv iksolver1v(chain);//Inverse velocity solver
ChainIkSolverPos_NR iksolver1(chain,fksolver1,iksolver1v,100,1e-6);//Maximum 100 iterations, stop at accuracy 1e-6
//Creation of jntarrays:
JntArray result(chain.getNrOfJoints());
JntArray q_init(chain.getNrOfJoints());
//Set destination frame
Frame F_dest=cartpos;
iksolver1.CartToJnt(q_init,F_dest,result);
for(unsigned int i=0;i<nj;i++){
printf ("Axle %i: %f \n",i,result(i));
}
}
示例2: printKDLChain
void leatherman::printKDLChain(const KDL::Chain &c, std::string text)
{
ROS_INFO("[%s] # segments: %d # joints: %d", text.c_str(), c.getNrOfSegments(), c.getNrOfJoints());
double r,p,y,jnt_pos = 0;
for(size_t j = 0; j < c.getNrOfSegments(); ++j)
{
c.getSegment(j).pose(jnt_pos).M.GetRPY(r,p,y);
ROS_INFO("[%s] [%2d] segment: %21s xyz: %0.3f %0.3f %0.3f rpy: %0.3f %0.3f %0.3f",
text.c_str(), int(j),
c.getSegment(j).getName().c_str(),
c.getSegment(j).pose(jnt_pos).p.x(),
c.getSegment(j).pose(jnt_pos).p.y(),
c.getSegment(j).pose(jnt_pos).p.z(),
r,p,y);
c.getSegment(j).getJoint().pose(jnt_pos).M.GetRPY(r,p,y);
ROS_INFO("[%s] joint: %21s xyz: %0.3f %0.3f %0.3f rpy: %0.3f %0.3f %0.3f type: %s",
text.c_str(),
c.getSegment(j).getJoint().getName().c_str(),
c.getSegment(j).getJoint().pose(jnt_pos).p.x(),
c.getSegment(j).getJoint().pose(jnt_pos).p.y(),
c.getSegment(j).getJoint().pose(jnt_pos).p.z(),
r,p,y,
c.getSegment(j).getJoint().getTypeName().c_str());
}
}
示例3: getKDLChainInfo
void getKDLChainInfo(kinematics_msgs::KinematicSolverInfo &chain_info)
{
unsigned int nj = chain.getNrOfJoints();
unsigned int nl = chain.getNrOfSegments();
ROS_DEBUG("nj: %d", nj);
ROS_DEBUG("nl: %d", nl);
//---setting up response
//joint_names
for(unsigned int i=0; i<nj; i++)
{
ROS_DEBUG("joint_name[%d]: %s", i, chain.getSegment(i).getJoint().getName().c_str());
chain_info.joint_names.push_back(chain.getSegment(i).getJoint().getName());
}
//limits
//joint limits are only saved in KDL::ChainIkSolverPos_NR_JL iksolverpos -> ik_solve().....but this is not visible here!
/*for(unsigned int i=0; i<nj; i++)
{
chain_info.limits[i].joint_name=chain.getSegment(i).getJoint().getName();
chain_info.limits[i].has_position_limits=
chain_info.limits[i].min_position=
chain_info.limits[i].max_position=
chain_info.limits[i].has_velocity_limits=
chain_info.limits[i].max_velocity=
chain_info.limits[i].has_acceleration_limits=
chain_info.limits[i].max_acceleration=
}*/
//link_names
for(unsigned int i=0; i<nl; i++)
{
chain_info.link_names.push_back(chain.getSegment(i).getName());
}
}
示例4: ChainJntToJacSolver
TEST_F(PR2IKTest, SingleRowRotation)
{
YAML::Node node = YAML::LoadFile("pr2_left_arm_scope.yaml");
ASSERT_NO_THROW(node.as< giskard_core::ScopeSpec >());
giskard_core::ScopeSpec scope_spec = node.as<giskard_core::ScopeSpec>();
ASSERT_NO_THROW(giskard_core::generate(scope_spec));
giskard_core::Scope scope = giskard_core::generate(scope_spec);
ASSERT_TRUE(scope.has_double_expression("pr2_rot_x"));
KDL::Expression<double>::Ptr exp = scope.find_double_expression("pr2_rot_x");
ASSERT_TRUE(exp.get());
std::string base = "base_link";
std::string tip = "l_wrist_roll_link";
KDL::Chain chain;
ASSERT_TRUE(tree.getChain(base, tip, chain));
ASSERT_EQ(chain.getNrOfJoints(), exp->number_of_derivatives());
boost::shared_ptr<KDL::ChainJntToJacSolver> jac_solver;
jac_solver = boost::shared_ptr<KDL::ChainJntToJacSolver>(
new KDL::ChainJntToJacSolver(chain));
for(int i=-11; i<12; ++i)
{
std::vector<double> exp_in;
KDL::JntArray solver_in(exp->number_of_derivatives());
for(size_t j=0; j<exp->number_of_derivatives(); ++j)
{
double value = 0.1*i;
exp_in.push_back(value);
solver_in(j) = value;
}
exp->setInputValues(exp_in);
exp->value();
KDL::Jacobian solver_jac(chain.getNrOfJoints());
ASSERT_GE(jac_solver->JntToJac(solver_in, solver_jac), 0.0);
for (size_t j=0; j<chain.getNrOfJoints(); ++j)
EXPECT_NEAR(solver_jac(3,j), exp->derivative(j), KDL::epsilon);
}
}
示例5:
ChainKinematics::ChainKinematics(const KDL::Chain chain, const double lambda) :
kinChain_(chain),
Njnt_(chain.getNrOfJoints()),
Nsegment_(chain.getNrOfSegments()) {
initialize(lambda);
}
示例6: idynChainGet_usingKDL_aux
KDL::Wrenches idynChainGet_usingKDL_aux(iCub::iDyn::iDynChain & idynChain, iCub::iDyn::iDynInvSensor & idynSensor,yarp::sig::Vector & ddp0)
{
yarp::sig::Vector q,dq,ddq;
q = idynChain.getAng();
dq = idynChain.getDAng();
ddq = idynChain.getD2Ang();
KDL::Chain kdlChain;
std::vector<std::string> la_joints;
la_joints.push_back("left_shoulder_pitch");
la_joints.push_back("left_shoulder_roll");
la_joints.push_back("left_arm_ft_sensor");
la_joints.push_back("left_shoulder_yaw");
la_joints.push_back("left_elbow");
la_joints.push_back("left_wrist_prosup");
la_joints.push_back("left_wrist_pitch");
la_joints.push_back("left_wrist_yaw");
idynSensorChain2kdlChain(idynChain,idynSensor,kdlChain,la_joints,la_joints);
std::cout << kdlChain << std::endl;
int nj = idynChain.getN();
//cout << "idynChainGet_usingKDL_aux with sensor" << " nrJoints " << kdlChain.getNrOfJoints() << " nrsegments " << kdlChain.getNrOfSegments() << endl;
assert(nj==kdlChain.getNrOfJoints());
assert(nj+1==kdlChain.getNrOfSegments());
KDL::JntArray jointpositions = KDL::JntArray(nj);
KDL::JntArray jointvel = KDL::JntArray(nj);
KDL::JntArray jointacc = KDL::JntArray(nj);
KDL::JntArray torques = KDL::JntArray(nj);
for(unsigned int i=0;i<nj;i++){
jointpositions(i)=q[i];
jointvel(i) = dq[i];
jointacc(i) = ddq[i];
}
KDL::Wrenches f_ext(nj+1);
KDL::Wrenches f_int(nj+1);
KDL::Vector grav_kdl;
idynVector2kdlVector(idynChain.getH0().submatrix(0,2,0,2).transposed()*ddp0,grav_kdl);
KDL::ChainIdSolver_RNE_IW neSolver = KDL::ChainIdSolver_RNE_IW(kdlChain,-grav_kdl);
int ret = neSolver.CartToJnt_and_internal_wrenches(jointpositions,jointvel,jointacc,f_ext,torques,f_int);
assert(ret >= 0);
return f_int;
}
示例7: initInternals
void ArmKinematics::initInternals(const KDL::Chain& chain)
{
dof_ = chain.getNrOfJoints();
q_tmp_.resize(dof_);
deleteInternals();
fk_solver_ = new KDL::ChainFkSolverPos_recursive(chain);
ik_solver_ = new KDL::ChainIkSolverVel_wdls(chain);
}
示例8:
FkLookup::ChainFK::ChainFK(const KDL::Tree& tree, const std::string& root, const std::string& tip):root_name(root),tip_name(tip){
KDL::Chain chain;
tree.getChain(root_name, tip_name, chain);
solver = new KDL::ChainFkSolverPos_recursive(chain);
unsigned int num = chain.getNrOfSegments();
for(unsigned int i = 0; i < num; ++i){
const KDL::Joint &joint = chain.getSegment(i).getJoint();
if (joint.getType() != KDL::Joint::None) joints.insert(std::make_pair(joint.getName(),joints.size()));
}
ROS_ASSERT(joints.size() == chain.getNrOfJoints());
}
示例9: computeRNEDynamicsForChain
void computeRNEDynamicsForChain(KDL::Tree& a_tree, const std::string& rootLink, const std::string& tipLink, KDL::Vector& grav,
std::vector<kdle::JointState>& jointState, std::vector<kdle::SegmentState>& linkState)
{
KDL::Chain achain;
a_tree.getChain(rootLink, tipLink, achain);
KDL::JntArray q(achain.getNrOfJoints());
KDL::JntArray q_dot(achain.getNrOfJoints());
KDL::JntArray q_dotdot(achain.getNrOfJoints());
JntArray torques(achain.getNrOfJoints());
KDL::Wrenches f_ext;
f_ext.resize(achain.getNrOfSegments());
std::cout << endl << endl;
printf("RNE dynamics values \n");
KDL::ChainIdSolver_RNE *rneDynamics = new ChainIdSolver_RNE(achain, -grav);
for (unsigned int i = 0; i < achain.getNrOfJoints(); ++i)
{
q(i) = jointState[i].q;
q_dot(i) = jointState[i].qdot;
q_dotdot(i) = jointState[i].qdotdot;
printf("q, qdot %f, %f\n", q(i), q_dot(i));
}
rneDynamics->CartToJnt(q, q_dot, q_dotdot, f_ext, torques);
for (unsigned int i = 0; i < achain.getNrOfJoints(); ++i)
{
printf("index, q, qdot, torques %d, %f, %f, %f\n", i, q(i), q_dot(i), torques(i));
}
return;
}
示例10: fk_solve_TCP
bool fk_solve_TCP(kinematics_msgs::GetPositionFK::Request &req,
kinematics_msgs::GetPositionFK::Response &res )
{
ROS_INFO("[TESTING]: get_fk_TCP_service has been called!");
unsigned int nj = chain.getNrOfJoints();
ChainFkSolverPos_recursive fksolver1(chain);//Forward position solver
JntArray conf(nj);
geometry_msgs::PoseStamped pose;
tf::Stamped<tf::Pose> tf_pose;
for(int i = 0; i < nj; i++)
conf(i) = req.robot_state.joint_state.position[i];
Frame F_ist;
res.pose_stamped.resize(1);
res.fk_link_names.resize(1);
if(fksolver1.JntToCart(conf, F_ist) >= 0)
{
std::cout << "Calculated Position out of Configuration:\n";
std::cout << F_ist <<"\n";
//TODO: fill out response!!!
tf_pose.frame_id_ = "base_link";//root_name_;
tf_pose.stamp_ = ros::Time();
tf::PoseKDLToTF(F_ist,tf_pose);
try
{
//tf_.transformPose(req.header.frame_id,tf_pose,tf_pose);
}
catch(...)
{
ROS_ERROR("Could not transform FK pose to frame: %s",req.header.frame_id.c_str());
res.error_code.val = res.error_code.FRAME_TRANSFORM_FAILURE;
return false;
}
tf::poseStampedTFToMsg(tf_pose,pose);
res.pose_stamped[0] = pose;
res.fk_link_names[0] = "arm_7_link";
res.error_code.val = res.error_code.SUCCESS;
}
else
{
ROS_ERROR("Could not compute FK for arm_7_link");
res.error_code.val = res.error_code.NO_FK_SOLUTION;
}
return true;
}
示例11: n_tilde
VirtualForcePublisher()
{
ros::NodeHandle n_tilde("~");
ros::NodeHandle n;
// subscribe to joint state
joint_state_sub_ = n.subscribe("joint_states", 1, &VirtualForcePublisher::callbackJointState, this);
wrench_stamped_pub_ = n.advertise<geometry_msgs::WrenchStamped>("wrench", 1);
// set publish frequency
double publish_freq;
n_tilde.param("publish_frequency", publish_freq, 50.0);
publish_interval_ = ros::Duration(1.0/std::max(publish_freq,1.0));
//set time constant of low pass filter
n_tilde.param("time_constant", t_const_, 0.3);
// set root and tip
n_tilde.param("root", root, std::string("torso_lift_link"));
n_tilde.param("tip", tip, std::string("l_gripper_tool_frame"));
// load robot model
urdf::Model robot_model;
robot_model.initParam("robot_description");
KDL::Tree kdl_tree;
kdl_parser::treeFromUrdfModel(robot_model, kdl_tree);
kdl_tree.getChain(root, tip, chain_);
jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(chain_));
jnt_pos_.resize(chain_.getNrOfJoints());
jacobian_.resize(chain_.getNrOfJoints());
for (size_t i=0; i<chain_.getNrOfSegments(); i++) {
if (chain_.getSegment(i).getJoint().getType() != KDL::Joint::None) {
std::cerr << "kdl_chain(" << i << ") " << chain_.getSegment(i).getJoint().getName().c_str() << std::endl;
}
}
}
示例12: initialize_kinematics_from_urdf
bool kdl_urdf_tools::initialize_kinematics_from_urdf(
const std::string &robot_description,
const std::string &root_link,
const std::string &tip_link,
unsigned int &n_dof,
KDL::Chain &kdl_chain,
KDL::Tree &kdl_tree,
urdf::Model &urdf_model)
{
if(robot_description.length() == 0) {
ROS_ERROR("URDF string is empty.");
return false;
}
// Construct an URDF model from the xml string
urdf_model.initString(robot_description);
// Get a KDL tree from the robot URDF
if (!kdl_parser::treeFromUrdfModel(urdf_model, kdl_tree)){
ROS_ERROR("Failed to construct kdl tree");
return false;
}
// Populate the KDL chain
if(!kdl_tree.getChain(root_link, tip_link, kdl_chain))
{
ROS_ERROR_STREAM("Failed to get KDL chain from tree: ");
ROS_ERROR_STREAM(" "<<root_link<<" --> "<<tip_link);
ROS_ERROR_STREAM(" Tree has "<<kdl_tree.getNrOfJoints()<<" joints");
ROS_ERROR_STREAM(" Tree has "<<kdl_tree.getNrOfSegments()<<" segments");
ROS_ERROR_STREAM(" The segments are:");
KDL::SegmentMap segment_map = kdl_tree.getSegments();
KDL::SegmentMap::iterator it;
for( it=segment_map.begin();
it != segment_map.end();
it++ )
{
ROS_ERROR_STREAM( " "<<(*it).first);
}
return false;
}
// Store the number of degrees of freedom of the chain
n_dof = kdl_chain.getNrOfJoints();
return true;
}
示例13: controllerStateCallback
void controllerStateCallback(const pr2_controllers_msgs::JointTrajectoryControllerState::ConstPtr& msg)
{
unsigned int nj = chain.getNrOfJoints();
JntArray q(7);
std::cout << "Numbers of joints " << nj << "\n";
for(int i = 0; i < 7; i++)
{
q(i) = msg->actual.positions[i];
}
std::cout << q(0) << " " << q(1) << " " << q(2) << " " << q(3) << " " << q(4) << " " << q(5) << " " << q(6) << "\n";
Frame F_ist;
fksolver1->JntToCart(q, F_ist);
std::cout << F_ist <<"\n";
}
示例14: initialise
void FingerKinematics::initialise(KDL::Chain &chain,const std::string name){
this->name = name;
for(std::size_t i = 0; i < chain.segments.size();i++){
joint_names.push_back(chain.segments[i].getJoint().getName());
}
ik_solver_vel_wdls = new KDL::ChainIkSolverVel_wdls(chain, 0.001, 5);
ik_solver_vel_wdls->setLambda(0.01);
Eigen::MatrixXd TS; TS.resize(6,6); TS.setIdentity(); TS(3,3) = 0; TS(4,4) = 0; TS(5,5) = 0;
ik_solver_vel_wdls->setWeightTS(TS);
fk_solver_pos = new KDL::ChainFkSolverPos_recursive(chain);
ik_solver_pos_NR = new KDL::ChainIkSolverPos_NR(chain,*fk_solver_pos,*ik_solver_vel_wdls);
num_joints = chain.getNrOfJoints();
q.resize(num_joints);
q.data.setZero();
q_out = q;
}
示例15: controllerStateCallback
void controllerStateCallback(const sensor_msgs::JointState::ConstPtr& msg)
{
unsigned int nj = chain.getNrOfJoints();
std::vector<std::string> names = msg->name;
std::vector<double> positions = msg->position;
JntArray q = parseJointStates(names,positions);
std::cout << "Joints: " << q(0) << " " << q(1) << " " << q(2) << " " << q(3) << " " << q(4) << " " << q(5) << " " << q(6) << "\n";
JntArray q_out(7);
Frame F_ist;
fksolver1->JntToCart(q, F_ist);
std::cout << F_ist <<"\n";
int ret = iksolver1v->CartToJnt(q, getTwist(F_ist), q_out);
if(ret >= 0)
{
sendVel(q, q_out);
std::cout << q_out(0) << " " << q_out(1) << " " << q_out(2) << " " << q_out(3) << " " << q_out(4) << " " << q_out(5) << " " << q_out(6) << "\n";
}
else
std::cout << "Something went wrong" << "\n";
}