bkinnightskytw
/
kdl_parser
mirror of https://github.com/existedinnettw/kdl_parser.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

collada_urdf: reference joints explicitly rather than by instance

This commit is contained in:
tfield 2010-03-09 07:15:45 +00:00
parent 1642af2b54
commit 4e96ca0e44
1 changed files with 6 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
collada_urdf/src/urdf_to_collada.cpp
View File

@ -218,7 +218,6 @@ public:
setupPhysics(scene); setupPhysics(scene);
addGeometries(); addGeometries();
addJoints();
addKinematics(scene); addKinematics(scene);
addVisuals(scene); addVisuals(scene);
addMaterials(); addMaterials();
@ -502,13 +501,13 @@ public:
} }
} }
void addJoints() { void addJoints(daeElementRef parent) {
int joint_num = 0; int joint_num = 0;
for (map<string, boost::shared_ptr<urdf::Joint> >::const_iterator i = robot_->joints_.begin(); i != robot_->joints_.end(); i++) { for (map<string, boost::shared_ptr<urdf::Joint> >::const_iterator i = robot_->joints_.begin(); i != robot_->joints_.end(); i++) {
boost::shared_ptr<urdf::Joint> urdf_joint = i->second; boost::shared_ptr<urdf::Joint> urdf_joint = i->second;
// <joint name="base_laser_joint" sid="joint0"> // <joint name="base_laser_joint" sid="joint0">
domJointRef joint = daeSafeCast<domJoint>(jointsLib_->createAndPlace(COLLADA_ELEMENT_JOINT)); domJointRef joint = daeSafeCast<domJoint>(parent->createAndPlace(COLLADA_ELEMENT_JOINT));
string joint_sid = string("joint") + boost::lexical_cast<string>(joint_num); string joint_sid = string("joint") + boost::lexical_cast<string>(joint_num);
joint_num++; joint_num++;
joint->setName(urdf_joint->name.c_str()); joint->setName(urdf_joint->name.c_str());
@ -644,16 +643,7 @@ public:
{ {
// <technique_common> // <technique_common>
domKinematics_model_techniqueRef technique = daeSafeCast<domKinematics_model_technique>(kmodel->createAndPlace(COLLADA_ELEMENT_TECHNIQUE_COMMON)); domKinematics_model_techniqueRef technique = daeSafeCast<domKinematics_model_technique>(kmodel->createAndPlace(COLLADA_ELEMENT_TECHNIQUE_COMMON));
for (map<string, boost::shared_ptr<urdf::Joint> >::const_iterator i = robot_->joints_.begin(); i != robot_->joints_.end(); i++) { addJoints(technique);
boost::shared_ptr<urdf::Joint> urdf_joint = i->second;
// <instance_joint url="#joint1">
domInstance_jointRef instance_joint = daeSafeCast<domInstance_joint>(technique->createAndPlace(COLLADA_ELEMENT_INSTANCE_JOINT));
string joint_id = joint_sids_[urdf_joint->name];
string instance_joint_url = string("#") + joint_id;
instance_joint->setUrl(instance_joint_url.c_str());
// </instance_joint>
}
// </technique_common> // </technique_common>
// <link ...> // <link ...>
@ -723,7 +713,8 @@ public:
// <attachment_full joint="k1/joint0"> // <attachment_full joint="k1/joint0">
domLink::domAttachment_fullRef attachment_full = daeSafeCast<domLink::domAttachment_full>(link->createAndPlace(COLLADA_TYPE_ATTACHMENT_FULL)); domLink::domAttachment_fullRef attachment_full = daeSafeCast<domLink::domAttachment_full>(link->createAndPlace(COLLADA_TYPE_ATTACHMENT_FULL));
attachment_full->setJoint(urdf_joint->name.c_str()); string attachment_joint = string("k1/") + joint_sids_[urdf_joint->name];
attachment_full->setJoint(attachment_joint.c_str());
{ {
addTranslate(attachment_full, urdf_joint->parent_to_joint_origin_transform.position); addTranslate(attachment_full, urdf_joint->parent_to_joint_origin_transform.position);
addRotate(attachment_full, urdf_joint->parent_to_joint_origin_transform.rotation); addRotate(attachment_full, urdf_joint->parent_to_joint_origin_transform.rotation);
@ -912,7 +903,7 @@ public:
az = r.z * inv_len; az = r.z * inv_len;
} }
else { else {
// Angle is 0 (mod 2*pi), so any axis will do. // Angle is 0 (mod 2*pi), so any axis will do
aa = 0.0; aa = 0.0;
ax = 1.0; ax = 1.0;
ay = 0.0; ay = 0.0;