int JointInvertPolarity(const KDL::Joint & old_joint, const KDL::Frame & old_f_tip, KDL::Joint & new_joint, KDL::Frame & new_f_tip)
{
switch(old_joint.getType())
{
case Joint::RotAxis:
case Joint::RotX:
case Joint::RotY:
case Joint::RotZ:
//\todo document this
new_f_tip = KDL::Frame(old_f_tip.M.Inverse(),-(old_f_tip.M.Inverse()*old_joint.JointOrigin()));
new_joint = Joint(old_joint.getName(),-(old_f_tip.M.Inverse()*old_f_tip.p), -(old_f_tip.M.Inverse()*old_joint.JointAxis()), Joint::RotAxis);
break;
case Joint::TransAxis:
case Joint::TransX:
case Joint::TransY:
case Joint::TransZ:
new_f_tip = KDL::Frame(old_f_tip.M.Inverse(),-(old_f_tip.M.Inverse()*old_f_tip.p));
new_joint = Joint(old_joint.getName(),-(old_f_tip.M.Inverse()*old_joint.JointOrigin()), -(old_f_tip.M.Inverse()*old_joint.JointAxis()), Joint::TransAxis);
break;
case Joint::None:
default:
new_f_tip = KDL::Frame(old_f_tip.M.Inverse(),-(old_f_tip.M.Inverse()*old_f_tip.p));
new_joint = Joint(old_joint.getName(), Joint::None);
break;
}
#ifndef NDEBUG
std::cerr << "Exiting joint polarity inversion, checking all is working" << std::endl;
double q = 0.83;
std::cerr << old_joint.pose(q)*old_f_tip*new_joint.pose(q)*new_f_tip << std::endl;
#endif
return 0;
}
/*!
* \brief Constructor of the Jaco robot
* \details Sets all parameter from the Jaco serial robot and loads the mesh models.
* \author Sascha Kaden
* \date 2016-10-22
*/
SerialRobot2D::SerialRobot2D()
: SerialRobot("SerialRobot2D", 5, std::make_pair(util::Vecd(-util::pi(), -util::pi(), -util::pi(), -util::pi(), -util::pi()), util::Vecd(util::pi(), util::pi(), util::pi(),
util::pi(), util::pi())),
std::vector<DofType>(
{DofType::planarRot, DofType::planarRot, DofType::planarRot, DofType::planarRot, DofType::planarRot})) {
m_alpha = util::Vecd(0, 0, 0, 0, 0);
m_alpha = util::degToRad<5>(m_alpha);
m_a = util::Vecd(100, 100, 100, 100, 100);
m_d = util::Vecd(0, 0, 0, 0, 0);
setBaseOffset(util::Vecd(100, 0, 0, 0, 0, 0));
ModelFactoryPqp modelFactory;
m_baseModel = modelFactory.createModel("assets/robotModels/2Dline.obj");
Joint joint;
joint = Joint(-util::pi(), util::pi(), modelFactory.createModel("assets/robotModels/2Dline.obj"));
m_joints.push_back(joint);
joint = Joint(-util::pi(), util::pi(), modelFactory.createModel("assets/robotModels/2Dline.obj"));
m_joints.push_back(joint);
joint = Joint(-util::pi(), util::pi(), modelFactory.createModel("assets/robotModels/2Dline.obj"));
m_joints.push_back(joint);
joint = Joint(-util::pi(), util::pi(), modelFactory.createModel("assets/robotModels/2Dline.obj"));
m_joints.push_back(joint);
joint = Joint(-util::pi(), util::pi(), modelFactory.createModel("assets/robotModels/2Dline.obj"));
m_joints.push_back(joint);
}
/**
Merging a plane (xxi) and an axix (oof) constraint may result in:
<ul>
<li> prismatic : cylindar's axis lying in the plane
<li> revolute : plane perpendicular to the cylindar's axis
<li> fixed : all other cases
</ul>
*/
Joint meld_xxi_oof(const Joint& plane, const Joint& axis, const bool flip = false) {
//log4cpp::CategoryStream& log = plane.log_cf.infoStream();
//
isis_LOG(lg, isis_FILE, isis_INFO) << (flip ? "meld_oof_xxi" : "meld_xxi_oof");
e3ga::vector x1 = axis.location;
e3ga::vector b3 = e3ga::unit(axis.orientation);
e3ga::vector b2 = e3ga::unit(plane.orientation);
if ( e3ga::zero((b2 ^ b3), DEFAULT_TOLERANCE) ) {
// axis perpendicular to plane = revolute
/**
Find the intersection between the axis and the plane.
http://en.wikipedia.org/wiki/Line%E2%80%93plane_intersection
*/
e3ga::vector x2 = plane.location;
double d = ((x2 - x1) % b2) / (b3 % b2);
e3ga::vector r3 = (d * b3) + x1;
Joint result(REVOLUTE, r3, b3, 0.0, plane, axis);
return result;
}
/**
http://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions#Rotation_matrix_.E2.86.94_Euler_axis.2Fangle
*/
e3ga::vector b1 = e3ga::unit( b2 << *b3 );
double theta = acos(0.5*
(b1.get_e1() + b2.get_e2() + b3.get_e3() - 1.0));
if ( e3ga::zero( (b2 % b3) , DEFAULT_TOLERANCE) ) {
// axis parallel to plane = prismatic
// is the axis or the plane the reference location?
// if (flip)
// b? is the projection of the axis onto the plane.
// return Joint(PRISMATIC, x1, b?, -theta, plane, axis);
return Joint(PRISMATIC, x1, b3, theta, plane, axis);
}
double half_csc_theta = 0.5 / sin( theta );
// compute the euler axis
e3ga::vector ea(e3ga::vector::coord_e1_e2_e3,
half_csc_theta * (b3.get_e2() - b2.get_e3()),
half_csc_theta * (b1.get_e3() - b3.get_e1()),
half_csc_theta * (b2.get_e1() - b1.get_e2()) );
// compute the location for the fixed joint
// 1 - intersection between the axis and the plane.
// 2 - the location of the axis.
// 3 - the location of the plane.
//
// 1 is probably the best choice but 2 is easier.
e3ga::vector x2 = axis.location;
return Joint(FIXED, x2, ea, theta, plane, axis);
}
Joint_handle CSkeleton::createJoint(std::string _name/* = "ROOT"*/, JointType _type/* = JOINT_FREE*/, const Vector3d& _vOffset/* = Vector3d(0, 0, 0)*/, const Quaterniond& _qRotation/* = Quaterniond(1, 0, 0, 0)*/, const Vector3d& _vScale/* = Vector3d(1, 1, 1)*/)
{
size_t n = joints.size();
joints.resize(n+1, Joint(_name, _type, _vOffset, _qRotation, _vScale));
globals.resize(n+1, Joint(_name, _type));
parents.resize(n+1, 0);
children.resize(n+1);
return n;
}
CSkeleton::CSkeleton(void)
{
joints.resize(1, Joint("ROOT"));
globals.resize(1, Joint("ROOT"));
children.resize(1);
parents.resize(1);
parents[0] = 0;
}
void CSkeleton::destroy(void)
{
joints.clear();
globals.clear();
children.clear();
parents.clear();
joints.resize(1, Joint("ROOT"));
globals.resize(1, Joint("ROOT"));
children.resize(1);
parents.resize(1);
parents[0] = 0;
}
//====================================
//引数のファイルからパックファイルを作成します。
//====================================
HRESULT MyCompressData::JointFromFile(char *inFileName)
{
FILE *fin = fopen(inFileName,"r");
if(fin == NULL)
{
assert("そんなファイルないですよ");
return E_FAIL;
}
char outFileName[FILE_NAME_LENGTH];
if(NULL == fgets(outFileName,FILE_NAME_LENGTH,fin))
{
assert("出力するファイルを書きましょう。");
return E_FAIL;
}
RejectLastNL(outFileName);
char fileList[FILE_LIST_LENGTH] = "";
char buffer[FILE_NAME_LENGTH];
while(NULL != fgets(buffer,FILE_NAME_LENGTH,fin))
{
RejectLastNL(buffer);
MakeFileList(buffer,fileList);
}
fclose(fin);
return Joint(fileList,outFileName);
}
Tree TestDoubleJoint(){
Tree three_link("fake_root_link");
three_link.addSegment(Segment("first_link",Joint("fake_fixed_joint",Joint::None),
Frame::DH(4.0,M_PI_2/2,-3.0,-3.0),
RigidBodyInertia(10,Vector(3,-4,-5),RotationalInertia(0,0.35,0,4,2,0))),"fake_root_link");
three_link.addSegment(Segment("second_link",Joint("first_joint",Joint::RotZ),
Frame::DH(4.0,M_PI_2/2,-3.0,-3.0),
RigidBodyInertia(10,Vector(3,-4,-5),RotationalInertia(0,0.35,0,4,2,0))),"first_link");
three_link.addSegment(Segment("third_link",Joint("second_joint",Joint::RotZ),
Frame::DH(4.0,M_PI_2/2,-3.0,-3.0),
RigidBodyInertia(10,Vector(3,-4,-5),RotationalInertia(0,0.35,0,4,2,0))),"second_link");
return three_link;
}
Skeleton::Skeleton(const char* name)
: m_vJoints(0)
, m_vGlobalPose(0)
, m_vWorldGlobalPose(0)
{
m_ID = ComponentID;
std::string sFileName(name);
sFileName = "../Assets/" + sFileName;
FILE* pFile = fopen(C_STR(sFileName, "_skeleton.bufa"), "r");
char c[256];
int iNumJoints, parentIndex;
float transform[16];
fscanf(pFile, "%s", &c);
fscanf(pFile, "%i", &iNumJoints);
m_vJoints.Resize(iNumJoints);
m_vGlobalPose.Resize(iNumJoints);
m_vWorldGlobalPose.Resize(iNumJoints);
for (int i = 0; i < iNumJoints; ++i)
{
fscanf(pFile, "%s", &c);
for (int j = 0; j < 16; ++j)
{
fscanf(pFile, "%f", &transform[j]);
}
fscanf(pFile, "%i", &parentIndex);
m_vJoints.Add(Joint(transform, parentIndex));
m_vGlobalPose.Add(new Matrix4);
m_vWorldGlobalPose.Add(new Matrix4);
}
fclose(pFile);
}
Chain KukaLWR_DHnew(){
Chain kukaLWR_DHnew;
//joint 0
kukaLWR_DHnew.addSegment(Segment(Joint(Joint::None),
Frame::DH_Craig1989(0.0, 0.0, 0.31, 0.0)
));
//joint 1
kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
Frame::DH_Craig1989(0.0, 1.5707963, 0.0, 0.0),
Frame::DH_Craig1989(0.0, 1.5707963, 0.0, 0.0).Inverse()*RigidBodyInertia(2,
Vector::Zero(),
RotationalInertia(0.0,0.0,0.0115343,0.0,0.0,0.0))));
//joint 2
kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
Frame::DH_Craig1989(0.0, -1.5707963, 0.4, 0.0),
Frame::DH_Craig1989(0.0, -1.5707963, 0.4, 0.0).Inverse()*RigidBodyInertia(2,
Vector(0.0,-0.3120511,-0.0038871),
RotationalInertia(-0.5471572,-0.0000302,-0.5423253,0.0,0.0,0.0018828))));
//joint 3
kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
Frame::DH_Craig1989(0.0, -1.5707963, 0.0, 0.0),
Frame::DH_Craig1989(0.0, -1.5707963, 0.0, 0.0).Inverse()*RigidBodyInertia(2,
Vector(0.0,-0.0015515,0.0),
RotationalInertia(0.0063507,0.0,0.0107804,0.0,0.0,-0.0005147))));
//joint 4
kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
Frame::DH_Craig1989(0.0, 1.5707963, 0.39, 0.0),
Frame::DH_Craig1989(0.0, 1.5707963, 0.39, 0.0).Inverse()*RigidBodyInertia(2,
Vector(0.0,0.5216809,0.0),
RotationalInertia(-1.0436952,0.0,-1.0392780,0.0,0.0,0.0005324))));
//joint 5
kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
Frame::DH_Craig1989(0.0, 1.5707963, 0.0, 0.0),
Frame::DH_Craig1989(0.0, 1.5707963, 0.0, 0.0).Inverse()*RigidBodyInertia(2,
Vector(0.0,0.0119891,0.0),
RotationalInertia(0.0036654,0.0,0.0060429,0.0,0.0,0.0004226))));
//joint 6
kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
Frame::DH_Craig1989(0.0, -1.5707963, 0.0, 0.0),
Frame::DH_Craig1989(0.0, -1.5707963, 0.0, 0.0).Inverse()*RigidBodyInertia(2,
Vector(0.0,0.0080787,0.0),
RotationalInertia(0.0010431,0.0,0.0036376,0.0,0.0,0.0000101))));
//joint 7
kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
Frame::Identity(),
RigidBodyInertia(2,
Vector::Zero(),
RotationalInertia(0.000001,0.0,0.0001203,0.0,0.0,0.0))));
return kukaLWR_DHnew;
}
Skeleton app::skeletonOSCMessageToSkeleton(ofxOscMessage * msg)
{
//If this message is not formatted correctly
if(msg->getNumArgs() !=numSkeletonOSCArgs)
{
cout << "Skeleton OSC message does not have " << numSkeletonOSCArgs << " args as required... - has " << msg->getNumArgs() << endl;
}
//This is tied directly to the order of information within the packet
Skeleton s;
// 0 - bundle index, not used here
// 1 - skeleton index
s.idx = msg->getArgAsInt32(1);
// 2-HandLeftx
// 3-HandLefty
Joint handleft = Joint(ofPoint(msg->getArgAsFloat(2),msg->getArgAsFloat(3)), Joint::Type_HandLeft);
// 4-WristLeftx
// 5-WristLefty
Joint wristleft = Joint(ofPoint(msg->getArgAsFloat(4),msg->getArgAsFloat(5)), Joint::Type_WristLeft);
// 6-ElbowLeftx
// 7-ElbowLefty
Joint elbowleft = Joint(ofPoint(msg->getArgAsFloat(6),msg->getArgAsFloat(7)), Joint::Type_ElbowLeft);
// 8-ElbowRightx
// 9-ElbowRighty
Joint elbowright = Joint(ofPoint(msg->getArgAsFloat(8),msg->getArgAsFloat(9)), Joint::Type_ElbowRight);
// 10-WristRightx
// 11-WristRight7
Joint wristright = Joint(ofPoint(msg->getArgAsFloat(10),msg->getArgAsFloat(11)), Joint::Type_WristRight);
// 12-HandRightx
// 13-HandRighty
Joint handright = Joint(ofPoint(msg->getArgAsFloat(12),msg->getArgAsFloat(13)), Joint::Type_HandRight);
// 14-ShoulderLeftx
// 15-ShoulderLefty
Joint shoulderleft = Joint(ofPoint(msg->getArgAsFloat(14),msg->getArgAsFloat(15)), Joint::Type_ShoulderLeft);
// 16-ShoulderRightx
// 17-ShoulderRight7
Joint shoulderright = Joint(ofPoint(msg->getArgAsFloat(16),msg->getArgAsFloat(17)), Joint::Type_ShoulderRight);
// 18 - kinect id
s.src_kinect = msg->getArgAsInt32(18);
//Now construct the list of bones
//Hand->wrist
s.bones.push_back(Bone(handleft,wristleft));
s.bones.push_back(Bone(handright,wristright));
//Wrist->Elbow
s.bones.push_back(Bone(wristleft,elbowleft));
s.bones.push_back(Bone(wristright,elbowright));
//Elbow->Shoulder
s.bones.push_back(Bone(elbowleft,shoulderleft));
s.bones.push_back(Bone(elbowright,shoulderright));
//Shoulders
s.bones.push_back(Bone(shoulderleft,shoulderright));
return s;
}
// construct joint
Joint toKdl(urdf::JointPtr jnt)
{
Frame F_parent_jnt = toKdl(jnt->parent_to_joint_origin_transform);
switch (jnt->type){
case urdf::Joint::FIXED:{
return Joint(jnt->name, Joint::None);
}
case urdf::Joint::REVOLUTE:{
Vector axis = toKdl(jnt->axis);
return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::RotAxis);
}
case urdf::Joint::CONTINUOUS:{
Vector axis = toKdl(jnt->axis);
return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::RotAxis);
}
case urdf::Joint::PRISMATIC:{
Vector axis = toKdl(jnt->axis);
return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::TransAxis);
}
default:{
std::cerr << "Converting unknown joint type of joint " << jnt->name << " into a fixed joint" << std::endl;
return Joint(jnt->name, Joint::None);
}
}
return Joint();
}
// construct joint
Joint toKdl(boost::shared_ptr<urdf::Joint> jnt)
{
Frame F_parent_jnt = toKdl(jnt->parent_to_joint_origin_transform);
switch (jnt->type){
case urdf::Joint::FIXED:{
return Joint(jnt->name, Joint::None);
}
case urdf::Joint::REVOLUTE:{
Vector axis = toKdl(jnt->axis);
return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::RotAxis);
}
case urdf::Joint::CONTINUOUS:{
Vector axis = toKdl(jnt->axis);
return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::RotAxis);
}
case urdf::Joint::PRISMATIC:{
Vector axis = toKdl(jnt->axis);
return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::TransAxis);
}
default:{
std::cout<<"Converting unknown joint type of joint "<<jnt->name.c_str()<<" into a fixed joint."<<std::endl;
// ROS_WARN("Converting unknown joint type of joint '%s' into a fixed joint", jnt->name.c_str());
return Joint(jnt->name, Joint::None);
}
}
return Joint();
}
Chain Puma560(){
Chain puma560;
puma560.addSegment(Segment());
puma560.addSegment(Segment(Joint(Joint::RotZ),
Frame::DH(0.0,M_PI_2,0.0,0.0),
RigidBodyInertia(0,Vector::Zero(),RotationalInertia(0,0.35,0,0,0,0))));
puma560.addSegment(Segment(Joint(Joint::RotZ),
Frame::DH(0.4318,0.0,0.0,0.0),
RigidBodyInertia(17.4,Vector(-.3638,.006,.2275),RotationalInertia(0.13,0.524,0.539,0,0,0))));
puma560.addSegment(Segment());
puma560.addSegment(Segment(Joint(Joint::RotZ),
Frame::DH(0.0203,-M_PI_2,0.15005,0.0),
RigidBodyInertia(4.8,Vector(-.0203,-.0141,.070),RotationalInertia(0.066,0.086,0.0125,0,0,0))));
puma560.addSegment(Segment(Joint(Joint::RotZ),
Frame::DH(0.0,M_PI_2,0.4318,0.0),
RigidBodyInertia(0.82,Vector(0,.019,0),RotationalInertia(1.8e-3,1.3e-3,1.8e-3,0,0,0))));
puma560.addSegment(Segment());
puma560.addSegment(Segment());
puma560.addSegment(Segment(Joint(Joint::RotZ),
Frame::DH(0.0,-M_PI_2,0.0,0.0),
RigidBodyInertia(0.34,Vector::Zero(),RotationalInertia(.3e-3,.4e-3,.3e-3,0,0,0))));
puma560.addSegment(Segment(Joint(Joint::RotZ),
Frame::DH(0.0,0.0,0.0,0.0),
RigidBodyInertia(0.09,Vector(0,0,.032),RotationalInertia(.15e-3,0.15e-3,.04e-3,0,0,0))));
puma560.addSegment(Segment());
return puma560;
}
/*!
* \brief Constructor of the Jaco robot
* \details Sets all parameter from the Jaco serial robot and loads the mesh models.
* \author Sascha Kaden
* \date 2016-10-22
*/
KukaKR5::KukaKR5()
: SerialRobot("KukaKR5", 6,
std::make_pair(util::Vecd(-2.70526, -1.13446, -1.18682, -6.10865, 0.872665, -util::twoPi()),
util::Vecd(2.70526, util::pi(), 1.8326f, 6.10865f, 5.41052f, 2.96706f)),
std::vector<DofType>({DofType::volumetricPos, DofType::volumetricPos, DofType::volumetricPos,
DofType::volumetricRot, DofType::volumetricRot, DofType::volumetricRot})) {
m_alpha = util::Vecd(90, 0, 90, 90, 90, 0);
m_alpha = util::degToRad<6>(m_alpha);
m_a = util::Vecd(180, 600, 120, 0, 0, 0);
m_d = util::Vecd(400, 0, 0, 620, 0, 115);
ModelFactoryPqp modelFactoryPqp;
m_baseModel = modelFactoryPqp.createModel("meshes/KukaKR5/link0.stl");
Joint joint;
// -155, 155
joint = Joint(-2.70526f, 2.70526f, modelFactoryPqp.createModel("meshes/KukaKR5/link1.stl"));
m_joints.push_back(joint);
// -65, 180
joint = Joint(-1.13446f, util::pi(), modelFactoryPqp.createModel("meshes/KukaKR5/link2.stl"));
m_joints.push_back(joint);
// -68, 105
joint = Joint(-1.18682f, 1.8326f, modelFactoryPqp.createModel("meshes/KukaKR5/link3.stl"));
m_joints.push_back(joint);
// -350, 350
joint = Joint(-6.10865f, 6.10865f, modelFactoryPqp.createModel("meshes/KukaKR5/link4.stl"));
m_joints.push_back(joint);
// 50, 310
joint = Joint(0.872665f, 5.41052f, modelFactoryPqp.createModel("meshes/KukaKR5/link5.stl"));
m_joints.push_back(joint);
// -360, 170
joint = Joint(-util::twoPi(), 2.96706f, modelFactoryPqp.createModel("meshes/KukaKR5/link6.stl"));
m_joints.push_back(joint);
}
bool Skeleton::Create(UInt32 jointCount)
{
#if NAZARA_UTILITY_SAFE
if (jointCount == 0)
{
NazaraError("Joint count must be over zero");
return false;
}
#endif
m_impl = new SkeletonImpl;
m_impl->joints.resize(jointCount, Joint(this));
return true;
}
Chain d6(){
Chain d6;
d6.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(L0,0,0))));
d6.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(L1,0,0))));
d6.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(L2,0,0))));
d6.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(L3,0,0))));
d6.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(L4,0,0))));
d6.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(L5,0,0))));
return d6;
}
Robot wbot::generateRobot(){
Robot robot{};
std::ifstream infile("wbotJoints.txt");
std::string str;
while (std::getline(infile, str)){
std::istringstream buf(str);
std::istream_iterator<std::string> beg(buf), end;
std::vector<std::string> tokens(beg, end); // done!
robot.addJoint(tokens[0], Joint(std::stoi(tokens[1]), 512, 100, 1024, 0));
}
return robot;
}
bool treeFromUrdfModel(const urdf::ModelInterface& robot_model, Tree& tree, const bool consider_root_link_inertia)
{
if (consider_root_link_inertia) {
//For giving a name to the root of KDL using the robot name,
//as it is not used elsewhere in the KDL tree
std::string fake_root_name = "__kdl_import__" + robot_model.getName()+"__fake_root__";
std::string fake_root_fixed_joint_name = "__kdl_import__" + robot_model.getName()+"__fake_root_fixed_joint__";
tree = Tree(fake_root_name);
const urdf::ConstLinkPtr root = robot_model.getRoot();
// constructs the optional inertia
RigidBodyInertia inert(0);
if (root->inertial)
inert = toKdl(root->inertial);
// constructs the kdl joint
Joint jnt = Joint(fake_root_fixed_joint_name, Joint::None);
// construct the kdl segment
Segment sgm(root->name, jnt, Frame::Identity(), inert);
// add segment to tree
tree.addSegment(sgm, fake_root_name);
} else {
tree = Tree(robot_model.getRoot()->name);
// warn if root link has inertia. KDL does not support this
if (robot_model.getRoot()->inertial)
std::cerr << "The root link " << robot_model.getRoot()->name <<
" has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF." << std::endl;
}
// add all children
for (size_t i=0; i<robot_model.getRoot()->child_links.size(); i++)
if (!addChildrenToTree(robot_model.getRoot()->child_links[i], tree))
return false;
return true;
}
void determineJoints( Stroke* other, Array<Joint>& joints )
{
if ( (m_attributes&ATTRIB_CLASSBITS)
!= (other->m_attributes&ATTRIB_CLASSBITS)
|| hasAttribute(ATTRIB_GROUND)
|| hasAttribute(ATTRIB_UNJOINABLE)
|| other->hasAttribute(ATTRIB_UNJOINABLE)) {
// cannot joint goals or tokens to other things
// and no point jointing ground endpts
return;
}
transform();
for ( unsigned char end=0; end<2; end++ ) {
if ( !m_jointed[end] ) {
const Vec2& p = m_xformedPath.endpt(end);
if ( other->distanceTo( p ) <= JOINT_TOLERANCE ) {
joints.append( Joint(this,other,end) );
}
}
}
}
//-----------------------------------------------------------------------------------
Joint Skeleton::GetJoint(int index)
{
return Joint(m_names[index], m_parentIndices[index], m_modelToBoneSpace[index], m_boneToModelSpace[index]);
}
Chain d2(){
Chain d2;
d2.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(L0,0,0))));
d2.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(L1,0,0))));
return d2;
}
void RTSPServer::Stop()
{
exitThread_ = true;
started_ = false;
Joint();
}
