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

collada exporter using soft limits if they exist (thanks to Kei Okada)

This commit is contained in:
rdiankov 2011-07-12 20:07:37 +09:00
parent 48bf8cff23
commit c463e9e6e0
1 changed files with 79 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
collada_urdf/src/collada_urdf.cpp
View File

@ -77,10 +77,12 @@ public:
ResourceIOStream(const resource_retriever::MemoryResource& res)
: res_(res)
, pos_(res.data.get())
{}
{
}
~ResourceIOStream()
{}
{
}
size_t Read(void* buffer, size_t size, size_t count)
{
@ -96,7 +98,9 @@ public:
return to_read;
}
size_t Write( const void* buffer, size_t size, size_t count) { ROS_BREAK(); return 0; }
size_t Write( const void* buffer, size_t size, size_t count) {
ROS_BREAK(); return 0;
}
aiReturn Seek( size_t offset, aiOrigin origin)
{
@ -135,7 +139,8 @@ public:
return res_.size;
}
void Flush() {}
void Flush() {
}
private:
resource_retriever::MemoryResource res_;
@ -195,7 +200,9 @@ public:
return new ResourceIOStream(res);
}
void Close(Assimp::IOStream* stream) { delete stream; }
void Close(Assimp::IOStream* stream) {
delete stream;
}
private:
mutable resource_retriever::Retriever retriever_;
@ -229,7 +236,8 @@ private:
struct axis_output
{
//axis_output(const string& sid, KinBody::JointConstPtr pjoint, int iaxis) : sid(sid), pjoint(pjoint), iaxis(iaxis) {}
axis_output() : iaxis(0) {}
axis_output() : iaxis(0) {
}
string sid, nodesid;
boost::shared_ptr<const urdf::Joint> pjoint;
int iaxis;
@ -242,7 +250,8 @@ private:
struct axis_sids
{
axis_sids(const string& axissid, const string& valuesid, const string& jointnodesid) : axissid(axissid), valuesid(valuesid), jointnodesid(jointnodesid) {}
axis_sids(const string& axissid, const string& valuesid, const string& jointnodesid) : axissid(axissid), valuesid(valuesid), jointnodesid(jointnodesid) {
}
string axissid, valuesid, jointnodesid;
};
@ -270,7 +279,8 @@ public:
_collada->setDatabase(NULL);
_importer.SetIOHandler(new ResourceIOSystem());
}
virtual ~ColladaWriter() {}
virtual ~ColladaWriter() {
}
boost::shared_ptr<DAE> convert()
{
@ -350,8 +360,12 @@ public:
}
protected:
virtual void handleError(daeString msg) { throw ColladaUrdfException(msg); }
virtual void handleWarning(daeString msg) { std::cerr << "COLLADA DOM warning: " << msg << std::endl; }
virtual void handleError(daeString msg) {
throw ColladaUrdfException(msg);
}
virtual void handleWarning(daeString msg) {
std::cerr << "COLLADA DOM warning: " << msg << std::endl;
}
void _CreateScene()
{
@ -440,6 +454,10 @@ protected:
flower = pjoint->limits->lower;
fupper = pjoint->limits->upper;
}
if( !!pjoint->safety ) {
flower = pjoint->safety->soft_lower_limit;
fupper = pjoint->safety->soft_upper_limit;
}
if( flower == fupper ) {
bactive = false;
}
@ -544,11 +562,14 @@ protected:
if( !!it->pjoint->limits ) {
flower = it->pjoint->limits->lower;
fupper = it->pjoint->limits->upper;
}
if( !!it->pjoint->safety ) {
flower = it->pjoint->safety->soft_lower_limit;
fupper = it->pjoint->safety->soft_upper_limit;
}
if( flower > 0 || fupper < 0 ) {
value = 0.5*(flower+fupper);
}
}
domKinematics_newparamRef pvalueparam = daeSafeCast<domKinematics_newparam>(_ikmout->ikm->add(COLLADA_ELEMENT_NEWPARAM));
pvalueparam->setSid((sid+string("_value")).c_str());
daeSafeCast<domKinematics_newparam::domFloat>(pvalueparam->add(COLLADA_ELEMENT_FLOAT))->setValue(value);
@ -605,7 +626,11 @@ protected:
pdomjoint->setSid(jointid.c_str() );
pdomjoint->setName(pjoint->name.c_str());
domAxis_constraintRef axis;
if( !!pjoint->limits ) {
if( !!pjoint->safety ) {
lmin=pjoint->safety->soft_lower_limit;
lmax=pjoint->safety->soft_upper_limit;
}
else if( !!pjoint->limits ) {
lmin=pjoint->limits->lower;
lmax=pjoint->limits->upper;
}