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: adding instance_kinematics_scene bindings

This commit is contained in:
tfield 2010-03-23 03:04:31 +00:00
parent 6369746caf
commit 953a2b17bb
1 changed files with 38 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
collada_urdf/src/urdf_to_collada.cpp
View File

@ -77,13 +77,6 @@ public:
domInstance_with_extraRef piscene; domInstance_with_extraRef piscene;
}; };
struct LINKOUTPUT
{
list<int> listusedlinks;
daeElementRef plink;
domNodeRef pnode;
};
urdf::Model* robot_; urdf::Model* robot_;
boost::shared_ptr<DAE> collada_; boost::shared_ptr<DAE> collada_;
@ -655,10 +648,17 @@ public:
} }
// </kinematics_model> // </kinematics_model>
// <instance_kinematics_model url="#k1" sid="inst_k1">
domInstance_kinematics_modelRef ikm = daeSafeCast<domInstance_kinematics_model>(scene.kscene->createAndPlace(COLLADA_ELEMENT_INSTANCE_KINEMATICS_MODEL));
string model_id = kmodel->getID(); string model_id = kmodel->getID();
string inst_model_sid = string("inst_") + model_id; string inst_model_sid = string("inst_") + model_id;
// <bind_kinematics_scene>
// <bind_kinematics_model node="node0">
domBind_kinematics_modelRef kmodel_bind = daeSafeCast<domBind_kinematics_model>(scene.kiscene->createAndPlace(COLLADA_ELEMENT_BIND_KINEMATICS_MODEL));
kmodel_bind->setNode(robot_->getName().c_str());
daeSafeCast<domCommon_param>(kmodel_bind->createAndPlace(COLLADA_ELEMENT_PARAM))->setValue(string(string(scene.kscene->getID()) + string(".") + inst_model_sid).c_str());
// <instance_kinematics_model url="#k1" sid="inst_k1">
domInstance_kinematics_modelRef ikm = daeSafeCast<domInstance_kinematics_model>(scene.kscene->createAndPlace(COLLADA_ELEMENT_INSTANCE_KINEMATICS_MODEL));
ikm->setUrl((string("#") + model_id).c_str()); ikm->setUrl((string("#") + model_id).c_str());
ikm->setSid(inst_model_sid.c_str()); ikm->setSid(inst_model_sid.c_str());
{ {
@ -676,28 +676,50 @@ public:
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;
int idof = 0; // @todo assuming 1 dof joints
string joint_sid = joint_sids_[urdf_joint->name]; string joint_sid = joint_sids_[urdf_joint->name];
string axis_name = string("axis") + boost::lexical_cast<string>(idof);
// <newparam sid="kscene.inst_k1.joint0.axis0"> // <newparam sid="kscene.inst_k1.joint0.axis0">
domKinematics_newparamRef newparam = daeSafeCast<domKinematics_newparam>(ikm->createAndPlace(COLLADA_ELEMENT_NEWPARAM)); domKinematics_newparamRef newparam = daeSafeCast<domKinematics_newparam>(ikm->createAndPlace(COLLADA_ELEMENT_NEWPARAM));
string newparam_sid = string("kscene.inst_") + model_id + string(".") + joint_sid + string(".axis0"); string newparam_sid = string("kscene.inst_") + model_id + string(".") + joint_sid + string(".") + axis_name;
newparam->setSid(newparam_sid.c_str()); newparam->setSid(newparam_sid.c_str());
{ {
// <SIDREF>kscene/inst_k1/joint0/axis0</SIDREF> // <SIDREF>kscene/inst_k1/joint0/axis0</SIDREF>
string sidref = string("kscene/inst_") + model_id + string("/") + joint_sid + string("/axis0"); string sidref = string("kscene/inst_") + model_id + string("/") + joint_sid + string("/") + axis_name;
daeSafeCast<domKinematics_newparam::domSIDREF>(newparam->createAndPlace(COLLADA_ELEMENT_SIDREF))->setValue(sidref.c_str()); daeSafeCast<domKinematics_newparam::domSIDREF>(newparam->createAndPlace(COLLADA_ELEMENT_SIDREF))->setValue(sidref.c_str());
} }
// </newparam> // </newparam>
// <newparam sid="kscene.inst_k1.joint0.axis0_value"> // <newparam sid="kscene.inst_k1.joint0.axis0_value">
domKinematics_newparamRef newparam_value = daeSafeCast<domKinematics_newparam>(ikm->createAndPlace(COLLADA_ELEMENT_NEWPARAM)); domKinematics_newparamRef newparam_value = daeSafeCast<domKinematics_newparam>(ikm->createAndPlace(COLLADA_ELEMENT_NEWPARAM));
string newparam_value_sid = string("kscene.inst_") + model_id + string(".") + joint_sid + string(".axis0_value"); string newparam_value_sid = string("kscene.inst_") + model_id + string(".") + joint_sid + string(".") + axis_name + string("_value");
newparam_value->setSid(newparam_value_sid.c_str()); newparam_value->setSid(newparam_value_sid.c_str());
{ {
// <float>0</float> // <float>0</float>
daeSafeCast<domKinematics_newparam::domFloat>(newparam_value->createAndPlace(COLLADA_ELEMENT_FLOAT))->setValue(0.0f); daeSafeCast<domKinematics_newparam::domFloat>(newparam_value->createAndPlace(COLLADA_ELEMENT_FLOAT))->setValue(0.0f);
} }
// </newparam> // </newparam>
// <bind_joint_axis target="node0/joint_1_axis0">
domBind_joint_axisRef joint_bind = daeSafeCast<domBind_joint_axis>(scene.kiscene->createAndPlace(COLLADA_ELEMENT_BIND_JOINT_AXIS));
joint_bind->setTarget((string("node_") + joint_sid + string("_") + axis_name).c_str());
{
// <axis>
domCommon_sidref_or_paramRef axis_bind = daeSafeCast<domCommon_sidref_or_param>(joint_bind->createAndPlace(COLLADA_ELEMENT_AXIS));
{
daeSafeCast<domCommon_param>(axis_bind->createAndPlace(COLLADA_TYPE_PARAM))->setValue(newparam_sid.c_str());
}
// </axis>
// <value>
domCommon_float_or_paramRef value_bind = daeSafeCast<domCommon_float_or_param>(joint_bind->createAndPlace(COLLADA_ELEMENT_VALUE));
{
daeSafeCast<domCommon_param>(value_bind->createAndPlace(COLLADA_TYPE_PARAM))->setValue(newparam_value_sid.c_str());
}
}
// </bind_joint_axis>
} }
} }
// </instance_kinematics_model> // </instance_kinematics_model>
@ -718,8 +740,8 @@ public:
string attachment_joint = string("k1/") + joint_sids_[urdf_joint->name]; string attachment_joint = string("k1/") + joint_sids_[urdf_joint->name];
attachment_full->setJoint(attachment_joint.c_str()); attachment_full->setJoint(attachment_joint.c_str());
{ {
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);
addTranslate(attachment_full, urdf_joint->parent_to_joint_origin_transform.position);
addKinematicLink(robot_->getLink(urdf_joint->child_link_name), attachment_full, link_num); addKinematicLink(robot_->getLink(urdf_joint->child_link_name), attachment_full, link_num);
} }
// </attachment_full> // </attachment_full>
@ -740,7 +762,7 @@ public:
void addMaterials() { void addMaterials() {
urdf::Color ambient, diffuse; urdf::Color ambient, diffuse;
ambient.init("0 0 0 0"); ambient.init("1 1 1 0");
diffuse.init("1 1 1 0"); diffuse.init("1 1 1 0");
for (map<string, boost::shared_ptr<urdf::Link> >::const_iterator i = robot_->links_.begin(); i != robot_->links_.end(); i++) { for (map<string, boost::shared_ptr<urdf::Link> >::const_iterator i = robot_->links_.begin(); i != robot_->links_.end(); i++) {
@ -833,11 +855,10 @@ public:
link_num++; link_num++;
{ {
if (urdf_link->parent_joint != NULL) { if (urdf_link->parent_joint != NULL) {
// <translate>x y z</translate>
addTranslate(node, urdf_link->parent_joint->parent_to_joint_origin_transform.position);
// <rotate>x y z w</rotate> // <rotate>x y z w</rotate>
addRotate(node, urdf_link->parent_joint->parent_to_joint_origin_transform.rotation); addRotate(node, urdf_link->parent_joint->parent_to_joint_origin_transform.rotation);
// <translate>x y z</translate>
addTranslate(node, urdf_link->parent_joint->parent_to_joint_origin_transform.position);
// <rotate sid="node_joint0_axis0">x y z angle</rotate> // <rotate sid="node_joint0_axis0">x y z angle</rotate>
domRotateRef joint_rotate = addRotate(node, urdf_link->parent_joint->parent_to_joint_origin_transform.rotation); domRotateRef joint_rotate = addRotate(node, urdf_link->parent_joint->parent_to_joint_origin_transform.rotation);