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

added more patches to collada, fixed several problems in collada export/import that led to invalid files, added a program to annotate the pr2 file with extra robot information

This commit is contained in:
rdiankov 2010-12-20 10:03:36 +00:00
parent 0e6ee0cf9d
commit 697db704d3
4 changed files with 416 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

368
collada_urdf/annotate_pr2dae.py Executable file
View File

@ -0,0 +1,368 @@
#!/usr/bin/python
# -*- coding: utf-8 -*-
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
import sys
if __name__=="__main__":
if len(sys.argv) < 2:
print "Usage:\n\tannotate_pr2dae.py [collada file]\n\nAnnoates the PR2 with the OpenRAVE profile tags.\nThis file will be in existence until URDF can support the extra information providedhere"
sys.exit(0)
dae = open(sys.argv[1],'r').read()
if dae.find('<extra type="collision">') < 0:
index = dae.find('</kinematics_model>')
dae = dae[:index] + """
<extra type="collision">
<technique profile="OpenRAVE">
<ignore_link_pair link0="kmodel0/base_link" link1="kmodel0/br_caster_l_wheel_link"/>
<ignore_link_pair link0="kmodel0/base_link" link1="kmodel0/br_caster_r_wheel_link"/>
<ignore_link_pair link0="kmodel0/base_link" link1="kmodel0/fr_caster_l_wheel_link"/>
<ignore_link_pair link0="kmodel0/base_link" link1="kmodel0/fr_caster_r_wheel_link"/>
<ignore_link_pair link0="kmodel0/base_link" link1="kmodel0/fl_caster_l_wheel_link"/>
<ignore_link_pair link0="kmodel0/base_link" link1="kmodel0/fl_caster_r_wheel_link"/>
<ignore_link_pair link0="kmodel0/base_link" link1="kmodel0/bl_caster_l_wheel_link"/>
<ignore_link_pair link0="kmodel0/base_link" link1="kmodel0/bl_caster_r_wheel_link"/>
<ignore_link_pair link0="kmodel0/base_link" link1="kmodel0/l_shoulder_pan_link"/>
<ignore_link_pair link0="kmodel0/base_link" link1="kmodel0/r_shoulder_pan_link"/>
<ignore_link_pair link0="kmodel0/torso_lift_link" link1="kmodel0/l_shoulder_lift_link"/>
<ignore_link_pair link0="kmodel0/torso_lift_link" link1="kmodel0/r_shoulder_lift_link"/>
<ignore_link_pair link0="kmodel0/l_shoulder_pan_link" link1="kmodel0/l_upper_arm_roll_link"/>
<ignore_link_pair link0="kmodel0/r_shoulder_pan_link" link1="kmodel0/r_upper_arm_roll_link"/>
<ignore_link_pair link0="kmodel0/l_elbow_flex_link" link1="kmodel0/l_forearm_link"/>
<ignore_link_pair link0="kmodel0/r_elbow_flex_link" link1="kmodel0/r_forearm_link"/>
<ignore_link_pair link0="kmodel0/l_upper_arm_link" link1="kmodel0/l_forearm_roll_link"/>
<ignore_link_pair link0="kmodel0/r_upper_arm_link" link1="kmodel0/r_forearm_roll_link"/>
<ignore_link_pair link0="kmodel0/l_forearm_link" link1="kmodel0/l_gripper_palm_link"/>
<ignore_link_pair link0="kmodel0/r_forearm_link" link1="kmodel0/r_gripper_palm_link"/>
<ignore_link_pair link0="kmodel0/l_forearm_link" link1="kmodel0/l_wrist_roll_link"/>
<ignore_link_pair link0="kmodel0/r_forearm_link" link1="kmodel0/r_wrist_roll_link"/>
<ignore_link_pair link0="kmodel0/l_gripper_l_finger_link" link1="kmodel0/l_gripper_r_finger_link"/>
<ignore_link_pair link0="kmodel0/r_gripper_l_finger_link" link1="kmodel0/r_gripper_r_finger_link"/>
<ignore_link_pair link0="kmodel0/l_gripper_l_finger_tip_link" link1="kmodel0/l_gripper_r_finger_link"/>
<ignore_link_pair link0="kmodel0/r_gripper_l_finger_tip_link" link1="kmodel0/r_gripper_r_finger_link"/>
<ignore_link_pair link0="kmodel0/l_gripper_l_finger_link" link1="kmodel0/l_gripper_r_finger_tip_link"/>
<ignore_link_pair link0="kmodel0/r_gripper_l_finger_link" link1="kmodel0/r_gripper_r_finger_tip_link"/>
<ignore_link_pair link0="kmodel0/torso_lift_link" link1="kmodel0/head_tilt_link"/>
</technique>
</extra>
""" + dae[index:]
if dae.find('<extra type="library_sensors"') < 0:
index = dae.find('<scene>')
dae = dae[:index] + """
<extra type="library_sensors" id="libsensors">
<technique profile="OpenRAVE">
<sensor type="base_laser2d" id="base_laser">
<angle_range>-130 129.75</angle_range>
<distance_range>0.023 60.0</distance_range>
<angle_increment>0.25</angle_increment>
<time_increment>1.73611115315e-05</time_increment>
<measurement_time>0.05</measurement_time>
<interface_type>
<technique profile="OpenRAVE">
<interface>BaseLaser2D</interface>
</technique>
</interface_type>
</sensor>
<sensor type="base_laser2d" id="tilt_laser">
<angle_range>-90 89.75</angle_range>
<distance_range>0.023 10.0</distance_range>
<angle_increment>0.25</angle_increment>
<time_increment>1.73611115315e-05</time_increment>
<measurement_time>0.025</measurement_time>
<interface_type>
<technique profile="OpenRAVE">
<interface>BaseLaser2D</interface>
</technique>
</interface_type>
</sensor>
<sensor type="base_pinhole_camera" id="high_def_sensor">
<image_dimensions>2448 2050 3</image_dimensions>
<format>uint8</format>
<measurement_time>0.05</measurement_time>
<intrinsic>2955 0 1224.5 0 2955 1025.5</intrinsic>
<distortion>0 0 0 0 0</distortion>
<interface_type>
<technique profile="OpenRAVE">
<interface>BaseCamera</interface>
</technique>
</interface_type>
</sensor>
<sensor type="base_pinhole_camera" id="wide_l_stereo_camera_sensor">
<image_dimensions>640 480 1</image_dimensions>
<format>uint8</format>
<measurement_time>0.04</measurement_time>
<intrinsic>395.71449999999999 0.0 335.86279000000002 0.0 395.71449999999999 245.62557000000001</intrinsic>
<distortion>0 0 0 0 0</distortion>
<interface_type>
<technique profile="OpenRAVE">
<interface>BaseCamera</interface>
</technique>
</interface_type>
</sensor>
<sensor type="base_pinhole_camera" id="wide_r_stereo_camera_sensor">
<image_dimensions>640 480 1</image_dimensions>
<format>uint8</format>
<measurement_time>0.04</measurement_time>
<intrinsic>395.71449999999999 0.0 335.86279000000002 0.0 395.71449999999999 245.62557000000001</intrinsic>
<distortion>0 0 0 0 0</distortion>
<interface_type>
<technique profile="OpenRAVE">
<interface>BaseCamera</interface>
</technique>
</interface_type>
</sensor>
<sensor type="base_stereo_camera" id="wide_stereo_camera_sensor">
<instance_sensor url="#wide_l_stereo_camera_sensor">
<rectification>0.99956000000000012 0.0027200000000000002 -0.029390000000000003 -0.0027700000000000003 0.99999000000000005 -0.0016800000000000001 0.029390000000000003 0.0017600000000000001 0.99957000000000007</rectification>
</instance_sensor>
<instance_sensor url="#wide_r_stereo_camera_sensor">
<rectification>0.99941000000000013 0.0035200000000000001 -0.034260000000000006 -0.0034600000000000004 0.99999000000000005 0.0017800000000000001 0.034270000000000002 -0.0016600000000000002 0.99941000000000013</rectification>
</instance_sensor>
</sensor>
<sensor type="base_pinhole_camera" id="l_forearm_cam_sensor">
<image_dimensions>640 480 1</image_dimensions>
<format>uint8</format>
<measurement_time>0.04</measurement_time>
<intrinsic>426.35142000000002 0.0 313.91464000000002 0.0 426.51092999999997 238.27394000000001</intrinsic>
<distortion>0 0 0 0 0</distortion>
<interface_type>
<technique profile="OpenRAVE">
<interface>BaseCamera</interface>
</technique>
</interface_type>
</sensor>
<sensor type="base_pinhole_camera" id="r_forearm_cam_sensor">
<image_dimensions>640 480 1</image_dimensions>
<format>uint8</format>
<measurement_time>0.04</measurement_time>
<intrinsic>430.5514 0.0 320.85068000000001 0.0 429.22170999999997 240.4314</intrinsic>
<distortion>0 0 0 0 0</distortion>
<interface_type>
<technique profile="OpenRAVE">
<interface>BaseCamera</interface>
</technique>
</interface_type>
</sensor>
</technique>
</extra>
""" + dae[index:]
manipulators = [('leftarm',"""
<extra type="manipulator" name="leftarm">
<technique profile="OpenRAVE">
<frame_origin link="kmodel0/torso_lift_link"/>
<frame_tip link="kmodel0/l_gripper_palm_link">
<translate>0.18 0 0</translate>
<rotate>0 1 0 90</rotate>
</frame_tip>
<gripper_axis axis="kmodel0/l_gripper_l_finger_joint/axis0">
<closingdirection>
<float>-1</float>
</closingdirection>
</gripper_axis>
<iksolver type="Transform6D">
<free_axis axis="kmodel0/l_upper_arm_roll_joint/axis0"/>
<interface_type>
<technique profile="OpenRAVE">
<interface>ikfast_pr2_leftarm</interface>
</technique>
</interface_type>
</iksolver>
</technique>
</extra>
"""),
('leftarm_torso',"""
<extra type="manipulator" name="leftarm_torso">
<technique profile="OpenRAVE">
<frame_origin link="kmodel0/base_link"/>
<frame_tip link="kmodel0/l_gripper_palm_link">
<translate>0.18 0 0</translate>
<rotate>0 1 0 90</rotate>
</frame_tip>
<gripper_axis axis="kmodel0/l_gripper_l_finger_joint/axis0">
<closingdirection>
<float>-1</float>
</closingdirection>
</gripper_axis>
<iksolver type="Transform6D">
<free_axis axis="kmodel0/torso_lift_joint/axis0"/>
<free_axis axis="kmodel0/l_upper_arm_roll_joint/axis0"/>
<interface_type>
<technique profile="OpenRAVE">
<interface>ikfast_pr2_leftarm_torso</interface>
</technique>
</interface_type>
</iksolver>
</technique>
</extra>
"""),
('rightarm',"""
<extra type="manipulator" name="rightarm">
<technique profile="OpenRAVE">
<frame_origin link="kmodel0/torso_lift_link"/>
<frame_tip link="kmodel0/r_gripper_palm_link">
<translate>0.18 0 0</translate>
<rotate>0 1 0 90</rotate>
</frame_tip>
<gripper_axis axis="kmodel0/r_gripper_l_finger_joint/axis0">
<closingdirection>
<float>-1</float>
</closingdirection>
</gripper_axis>
<iksolver type="Transform6D">
<free_axis axis="kmodel0/r_upper_arm_roll_joint/axis0"/>
<interface_type>
<technique profile="OpenRAVE">
<interface>ikfast_pr2_rightarm</interface>
</technique>
</interface_type>
</iksolver>
</technique>
</extra>
"""),
('rightarm_torso',"""
<extra type="manipulator" name="rightarm_torso">
<technique profile="OpenRAVE">
<frame_origin link="kmodel0/base_link"/>
<frame_tip link="kmodel0/r_gripper_palm_link">
<translate>0.18 0 0</translate>
<rotate>0 1 0 90</rotate>
</frame_tip>
<gripper_axis axis="kmodel0/r_gripper_l_finger_joint/axis0">
<closingdirection>
<float>-1</float>
</closingdirection>
</gripper_axis>
<iksolver type="Transform6D">
<free_axis axis="kmodel0/torso_lift_joint/axis0"/>
<free_axis axis="kmodel0/r_upper_arm_roll_joint/axis0"/>
<interface_type>
<technique profile="OpenRAVE">
<interface>ikfast_pr2_rightarm_torso</interface>
</technique>
</interface_type>
</iksolver>
</technique>
</extra>
"""),
('head',"""
<extra type="manipulator" name="head">
<technique profile="OpenRAVE">
<frame_origin link="kmodel0/torso_lift_link"/>
<frame_tip link="kmodel0/wide_stereo_optical_frame"/>
<iksolver type="Lookat3D">
<interface_type>
<technique profile="OpenRAVE">
<interface>ikfast_pr2_head</interface>
</technique>
</interface_type>
</iksolver>
</technique>
</extra>
"""),
('head_torso',"""
<extra type="manipulator" name="head_torso">
<technique profile="OpenRAVE">
<frame_origin link="kmodel0/base_link"/>
<frame_tip link="kmodel0/wide_stereo_optical_frame"/>
<iksolver type="Lookat3D">
<free_axis axis="kmodel0/torso_lift_joint/axis0"/>
<interface_type>
<technique profile="OpenRAVE">
<interface>ikfast_pr2_head_torso</interface>
</technique>
</interface_type>
</iksolver>
</technique>
</extra>
"""),
('rightarm_camera',"""
<extra type="manipulator" name="rightarm_camera">
<technique profile="OpenRAVE">
<frame_origin link="kmodel0/torso_lift_link"/>
<frame_tip link="kmodel0/r_forearm_cam_optical_frame">
</frame_tip>
<iksolver type="Ray4D">
</iksolver>
</technique>
</extra>
"""),
('leftarm_camera',"""
<extra type="manipulator" name="leftarm_camera">
<technique profile="OpenRAVE">
<frame_origin link="kmodel0/torso_lift_link"/>
<frame_tip link="kmodel0/l_forearm_cam_optical_frame">
</frame_tip>
<iksolver type="Ray4D">
</iksolver>
</technique>
</extra>
""")]
sensors = [('base_laser',"""
<extra type="attach_sensor" name="base_laser">
<technique profile="OpenRAVE">
<instance_sensor url="#base_laser"/>
<frame_origin link="kmodel0/base_laser_link"/>
</technique>
</extra>
"""),
('tilt_laser',"""
<extra type="attach_sensor" name="tilt_laser">
<technique profile="OpenRAVE">
<instance_sensor url="#tilt_laser"/>
<frame_origin link="kmodel0/laser_tilt_link"/>
</technique>
</extra>
"""),
('l_forearm_cam_optical_sensor',"""
<extra type="attach_sensor" name="l_forearm_cam_optical_sensor">
<technique profile="OpenRAVE">
<instance_sensor url="#l_forearm_cam_sensor"/>
<frame_origin link="kmodel0/l_forearm_cam_optical_frame"/>
</technique>
</extra>
"""),
('r_forearm_cam_optical_sensor',"""
<extra type="attach_sensor" name="r_forearm_cam_optical_sensor">
<technique profile="OpenRAVE">
<instance_sensor url="#l_forearm_cam_sensor"/>
<frame_origin link="kmodel0/r_forearm_cam_optical_frame"/>
</technique>
</extra>
"""),
('narrow_stereo_optical_sensor',"""
<extra type="attach_sensor" name="narrow_stereo_optical_sensor">
<technique profile="OpenRAVE">
<instance_sensor url="#narrow_stereo_camera_sensor"/>
<frame_origin link="kmodel0/narrow_stereo_optical_frame"/>
</technique>
</extra>
"""),
('wide_stereo_optical_sensor',"""
<extra type="attach_sensor" name="wide_stereo_optical_sensor">
<technique profile="OpenRAVE">
<instance_sensor url="#wide_stereo_camera_sensor"/>
<frame_origin link="kmodel0/wide_stereo_optical_frame"/>
</technique>
</extra>
""")]
for name,xml in manipulators:
if dae.find('<extra type="manipulator" name="%s">'%name) < 0:
index = dae.find('</motion>')+9
dae = dae[:index] + xml + dae[index:]
for name,xml in sensors:
if dae.find('<extra type="attach_sensor" name="%s">'%name) < 0:
index = dae.find('</motion>')+9
dae = dae[:index] + xml + dae[index:]
open(sys.argv[1],'w').write(dae)

44
collada_urdf/src/collada_urdf.cpp
View File

@ -469,34 +469,34 @@ protected:
} }
// write the bindings // write the bindings
string asmsym = str(boost::format("%s.%s")%asmid%_ikmout->ikm->getSid()); string asmsym = str(boost::format("%s_%s")%asmid%_ikmout->ikm->getSid());
string assym = str(boost::format("%s.%s")%_scene.kscene->getID()%_ikmout->ikm->getSid()); string assym = str(boost::format("%s_%s")%_scene.kscene->getID()%_ikmout->ikm->getSid());
FOREACH(it, _ikmout->vkinematicsbindings) { FOREACH(it, _ikmout->vkinematicsbindings) {
domKinematics_newparamRef abm = daeSafeCast<domKinematics_newparam>(ias_motion->add(COLLADA_ELEMENT_NEWPARAM)); domKinematics_newparamRef abm = daeSafeCast<domKinematics_newparam>(ias_motion->add(COLLADA_ELEMENT_NEWPARAM));
abm->setSid(asmsym.c_str()); abm->setSid(asmsym.c_str());
daeSafeCast<domKinematics_newparam::domSIDREF>(abm->add(COLLADA_ELEMENT_SIDREF))->setValue(it->first.c_str()); daeSafeCast<domKinematics_newparam::domSIDREF>(abm->add(COLLADA_ELEMENT_SIDREF))->setValue(str(boost::format("%s/%s")%askid%it->first).c_str());
domKinematics_bindRef ab = daeSafeCast<domKinematics_bind>(ias->add(COLLADA_ELEMENT_BIND)); domKinematics_bindRef ab = daeSafeCast<domKinematics_bind>(ias->add(COLLADA_ELEMENT_BIND));
ab->setSymbol(assym.c_str()); ab->setSymbol(assym.c_str());
daeSafeCast<domKinematics_param>(ab->add(COLLADA_ELEMENT_PARAM))->setRef(asmsym.c_str()); daeSafeCast<domKinematics_param>(ab->add(COLLADA_ELEMENT_PARAM))->setRef(str(boost::format("%s/%s")%asmid%asmsym).c_str());
_iasout->vkinematicsbindings.push_back(make_pair(string(ab->getSymbol()), it->second)); _iasout->vkinematicsbindings.push_back(make_pair(string(ab->getSymbol()), it->second));
} }
for(size_t idof = 0; idof < _ikmout->vaxissids.size(); ++idof) { for(size_t idof = 0; idof < _ikmout->vaxissids.size(); ++idof) {
const axis_sids& kas = _ikmout->vaxissids.at(idof); const axis_sids& kas = _ikmout->vaxissids.at(idof);
domKinematics_newparamRef abm = daeSafeCast<domKinematics_newparam>(ias_motion->add(COLLADA_ELEMENT_NEWPARAM)); domKinematics_newparamRef abm = daeSafeCast<domKinematics_newparam>(ias_motion->add(COLLADA_ELEMENT_NEWPARAM));
abm->setSid(str(boost::format("%s.%s")%asmid%kas.axissid).c_str()); abm->setSid(str(boost::format("%s_%s")%asmid%kas.axissid).c_str());
daeSafeCast<domKinematics_newparam::domSIDREF>(abm->add(COLLADA_ELEMENT_SIDREF))->setValue(kas.axissid.c_str()); daeSafeCast<domKinematics_newparam::domSIDREF>(abm->add(COLLADA_ELEMENT_SIDREF))->setValue(str(boost::format("%s/%s")%askid%kas.axissid).c_str());
domKinematics_bindRef ab = daeSafeCast<domKinematics_bind>(ias->add(COLLADA_ELEMENT_BIND)); domKinematics_bindRef ab = daeSafeCast<domKinematics_bind>(ias->add(COLLADA_ELEMENT_BIND));
ab->setSymbol(str(boost::format("%s.%s")%assym%kas.axissid).c_str()); ab->setSymbol(str(boost::format("%s_%s")%assym%kas.axissid).c_str());
daeSafeCast<domKinematics_param>(ab->add(COLLADA_ELEMENT_PARAM))->setRef(str(boost::format("%s.%s")%asmid%kas.axissid).c_str()); daeSafeCast<domKinematics_param>(ab->add(COLLADA_ELEMENT_PARAM))->setRef(str(boost::format("%s/%s_%s")%asmid%asmid%kas.axissid).c_str());
string valuesid; string valuesid;
if( kas.valuesid.size() > 0 ) { if( kas.valuesid.size() > 0 ) {
domKinematics_newparamRef abmvalue = daeSafeCast<domKinematics_newparam>(ias_motion->add(COLLADA_ELEMENT_NEWPARAM)); domKinematics_newparamRef abmvalue = daeSafeCast<domKinematics_newparam>(ias_motion->add(COLLADA_ELEMENT_NEWPARAM));
abmvalue->setSid(str(boost::format("%s.%s")%asmid%kas.valuesid).c_str()); abmvalue->setSid(str(boost::format("%s_%s")%asmid%kas.valuesid).c_str());
daeSafeCast<domKinematics_newparam::domSIDREF>(abmvalue->add(COLLADA_ELEMENT_SIDREF))->setValue(kas.valuesid.c_str()); daeSafeCast<domKinematics_newparam::domSIDREF>(abmvalue->add(COLLADA_ELEMENT_SIDREF))->setValue(str(boost::format("%s/%s")%askid%kas.valuesid).c_str());
domKinematics_bindRef abvalue = daeSafeCast<domKinematics_bind>(ias->add(COLLADA_ELEMENT_BIND)); domKinematics_bindRef abvalue = daeSafeCast<domKinematics_bind>(ias->add(COLLADA_ELEMENT_BIND));
valuesid = str(boost::format("%s.%s")%assym%kas.valuesid); valuesid = str(boost::format("%s_%s")%assym%kas.valuesid);
abvalue->setSymbol(valuesid.c_str()); abvalue->setSymbol(valuesid.c_str());
daeSafeCast<domKinematics_param>(abvalue->add(COLLADA_ELEMENT_PARAM))->setRef(str(boost::format("%s.%s")%asmid%kas.valuesid).c_str()); daeSafeCast<domKinematics_param>(abvalue->add(COLLADA_ELEMENT_PARAM))->setRef(str(boost::format("%s/%s_%s")%asmid%asmid%kas.valuesid).c_str());
} }
_iasout->vaxissids.push_back(axis_sids(ab->getSymbol(),valuesid,kas.jointnodesid)); _iasout->vaxissids.push_back(axis_sids(ab->getSymbol(),valuesid,kas.jointnodesid));
} }
@ -514,7 +514,7 @@ protected:
string symscope, refscope; string symscope, refscope;
if( sidscope.size() > 0 ) { if( sidscope.size() > 0 ) {
symscope = sidscope+string("."); symscope = sidscope+string("_");
refscope = sidscope+string("/"); refscope = sidscope+string("/");
} }
string ikmsid = str(boost::format("%s_inst")%kmout->kmodel->getID()); string ikmsid = str(boost::format("%s_inst")%kmout->kmodel->getID());
@ -536,7 +536,7 @@ protected:
ref[index] = '.'; ref[index] = '.';
index = ref.find("/",index+1); index = ref.find("/",index+1);
} }
string sid = symscope+ikmsid+"."+ref; string sid = symscope+ikmsid+"_"+ref;
kbind->setSid(sid.c_str()); kbind->setSid(sid.c_str());
daeSafeCast<domKinematics_newparam::domSIDREF>(kbind->add(COLLADA_ELEMENT_SIDREF))->setValue((refscope+ikmsid+"/"+it->sid).c_str()); daeSafeCast<domKinematics_newparam::domSIDREF>(kbind->add(COLLADA_ELEMENT_SIDREF))->setValue((refscope+ikmsid+"/"+it->sid).c_str());
double value=0; double value=0;
@ -721,7 +721,7 @@ protected:
pdomlink->setSid(linksid.c_str()); pdomlink->setSid(linksid.c_str());
domNodeRef pnode = daeSafeCast<domNode>(pnodeparent->add(COLLADA_ELEMENT_NODE)); domNodeRef pnode = daeSafeCast<domNode>(pnodeparent->add(COLLADA_ELEMENT_NODE));
string nodeid = str(boost::format("v%s.node%d")%strModelUri%linkindex); string nodeid = str(boost::format("v%s_node%d")%strModelUri%linkindex);
pnode->setId( nodeid.c_str() ); pnode->setId( nodeid.c_str() );
string nodesid = str(boost::format("node%d")%linkindex); string nodesid = str(boost::format("node%d")%linkindex);
pnode->setSid(nodesid.c_str()); pnode->setSid(nodesid.c_str());
@ -742,7 +742,7 @@ protected:
if( !!geometry ) { if( !!geometry ) {
int igeom = 0; int igeom = 0;
string geomid = str(boost::format("g%s.%s.geom%d")%strModelUri%linksid%igeom); string geomid = str(boost::format("g%s_%s_geom%d")%strModelUri%linksid%igeom);
domGeometryRef pdomgeom = _WriteGeometry(geometry, geomid); domGeometryRef pdomgeom = _WriteGeometry(geometry, geomid);
domInstance_geometryRef pinstgeom = daeSafeCast<domInstance_geometry>(pnode->add(COLLADA_ELEMENT_INSTANCE_GEOMETRY)); domInstance_geometryRef pinstgeom = daeSafeCast<domInstance_geometry>(pnode->add(COLLADA_ELEMENT_INSTANCE_GEOMETRY));
pinstgeom->setUrl((string("#")+geomid).c_str()); pinstgeom->setUrl((string("#")+geomid).c_str());
@ -752,7 +752,7 @@ protected:
domBind_materialRef pmat = daeSafeCast<domBind_material>(pinstgeom->add(COLLADA_ELEMENT_BIND_MATERIAL)); domBind_materialRef pmat = daeSafeCast<domBind_material>(pinstgeom->add(COLLADA_ELEMENT_BIND_MATERIAL));
domBind_material::domTechnique_commonRef pmattec = daeSafeCast<domBind_material::domTechnique_common>(pmat->add(COLLADA_ELEMENT_TECHNIQUE_COMMON)); domBind_material::domTechnique_commonRef pmattec = daeSafeCast<domBind_material::domTechnique_common>(pmat->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
domInstance_materialRef pinstmat = daeSafeCast<domInstance_material>(pmattec->add(COLLADA_ELEMENT_INSTANCE_MATERIAL)); domInstance_materialRef pinstmat = daeSafeCast<domInstance_material>(pmattec->add(COLLADA_ELEMENT_INSTANCE_MATERIAL));
pinstmat->setTarget(xsAnyURI(*pdomgeom, string("#")+geomid+string(".mat"))); pinstmat->setTarget(xsAnyURI(*pdomgeom, string("#")+geomid+string("_mat")));
pinstmat->setSymbol("mat0"); pinstmat->setSymbol("mat0");
} }
@ -850,8 +850,8 @@ protected:
void _WriteMaterial(const string& geometry_id, boost::shared_ptr<urdf::Material> material) void _WriteMaterial(const string& geometry_id, boost::shared_ptr<urdf::Material> material)
{ {
string effid = geometry_id+string(".eff"); string effid = geometry_id+string("_eff");
string matid = geometry_id+string(".mat"); string matid = geometry_id+string("_mat");
domMaterialRef pdommat = daeSafeCast<domMaterial>(_materialsLib->add(COLLADA_ELEMENT_MATERIAL)); domMaterialRef pdommat = daeSafeCast<domMaterial>(_materialsLib->add(COLLADA_ELEMENT_MATERIAL));
pdommat->setId(matid.c_str()); pdommat->setId(matid.c_str());
domInstance_effectRef pdominsteff = daeSafeCast<domInstance_effect>(pdommat->add(COLLADA_ELEMENT_INSTANCE_EFFECT)); domInstance_effectRef pdominsteff = daeSafeCast<domInstance_effect>(pdommat->add(COLLADA_ELEMENT_INSTANCE_EFFECT));
@ -872,7 +872,7 @@ protected:
// <material id="g1.link0.geom0.eff"> // <material id="g1.link0.geom0.eff">
domMaterialRef dommaterial = daeSafeCast<domMaterial>(_materialsLib->add(COLLADA_ELEMENT_MATERIAL)); domMaterialRef dommaterial = daeSafeCast<domMaterial>(_materialsLib->add(COLLADA_ELEMENT_MATERIAL));
string material_id = geometry_id + string(".mat"); string material_id = geometry_id + string("_mat");
dommaterial->setId(material_id.c_str()); dommaterial->setId(material_id.c_str());
{ {
// <instance_effect url="#g1.link0.geom0.eff"/> // <instance_effect url="#g1.link0.geom0.eff"/>
@ -905,10 +905,10 @@ protected:
domAccessorRef pacc; domAccessorRef pacc;
domFloat_arrayRef parray; domFloat_arrayRef parray;
{ {
pvertsource->setId(str(boost::format("%s.positions")%pdomgeom->getID()).c_str()); pvertsource->setId(str(boost::format("%s_positions")%pdomgeom->getID()).c_str());
parray = daeSafeCast<domFloat_array>(pvertsource->add(COLLADA_ELEMENT_FLOAT_ARRAY)); parray = daeSafeCast<domFloat_array>(pvertsource->add(COLLADA_ELEMENT_FLOAT_ARRAY));
parray->setId(str(boost::format("%s.positions-array")%pdomgeom->getID()).c_str()); parray->setId(str(boost::format("%s_positions-array")%pdomgeom->getID()).c_str());
parray->setDigits(6); // 6 decimal places parray->setDigits(6); // 6 decimal places
domSource::domTechnique_commonRef psourcetec = daeSafeCast<domSource::domTechnique_common>(pvertsource->add(COLLADA_ELEMENT_TECHNIQUE_COMMON)); domSource::domTechnique_commonRef psourcetec = daeSafeCast<domSource::domTechnique_common>(pvertsource->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));

24
colladadom/collada-dom.patch
View File

@ -74,3 +74,27 @@ Index: dom/make/domTest.mk
// First see if the whole thing resolves correctly // First see if the whole thing resolves correctly
if (daeElement* result = finder(container, s, profile)) if (daeElement* result = finder(container, s, profile))
return result; return result;
@@ -355,6 +367,23 @@
if ((!member.empty() || haveArrayIndex1) && result.scalar == NULL)
return daeSidRef::resolveData();
+ if( !!result.elt && !result.array && !result.scalar ) {
+ // if newparam, follow its SIDREF (Rosen Diankov)
+ if( strcmp(result.elt->getElementName(),"newparam") == 0) {
+ daeElement* psidref = result.elt->getChild("SIDREF");
+ if( !!psidref ) {
+ daeSidRef::resolveData newresult;
+ newresult = resolveImpl(daeSidRef(string("./") + psidref->getCharData(),result.elt->getParent(),sidRef.profile));
+ if( !newresult.elt ) {
+ newresult = resolveImpl(daeSidRef(psidref->getCharData(),result.elt->getParent(),sidRef.profile));
+ }
+ if( !!newresult.elt ) {
+ return newresult;
+ }
+ }
+ }
+ }
+
// SID resolution was successful.
return result;
}

4
urdf/src/collada_model_reader.cpp
View File

@ -1858,7 +1858,7 @@ protected:
if( !!pbind->getSymbol() && strcmp(pbind->getSymbol(), ref) == 0 ) { if( !!pbind->getSymbol() && strcmp(pbind->getSymbol(), ref) == 0 ) {
// found a match // found a match
if( !!pbind->getParam() ) { if( !!pbind->getParam() ) {
return searchBinding(pbind->getParam()->getRef(), pbindelt); return daeSidRef(pbind->getParam()->getRef(), pbindelt).resolve().elt;
} }
else if( !!pbind->getSIDREF() ) { else if( !!pbind->getSIDREF() ) {
return daeSidRef(pbind->getSIDREF()->getValue(), pbindelt).resolve().elt; return daeSidRef(pbind->getSIDREF()->getValue(), pbindelt).resolve().elt;
@ -2197,7 +2197,7 @@ protected:
ROS_WARN_STREAM("bind_kinematics_model does not reference element\n"); ROS_WARN_STREAM("bind_kinematics_model does not reference element\n");
} }
else { else {
ROS_WARN_STREAM(str(boost::format("bind_kinematics_model cannot find reference to %s%s:\n")%pelt->getElementName())); ROS_WARN_STREAM(str(boost::format("bind_kinematics_model cannot find reference to %s:\n")%pelt->getElementName()));
} }
continue; continue;
} }