17 #include "libopenrave.h"
18 #include <boost/lexical_cast.hpp>
32 std::vector<dReal> data;
37 O <<
"</data>" << endl;
39 O <<
"<description><![CDATA[" <<
GetDescription() <<
"]]></description>" << endl;
45 it->second->Serialize(newwriter,options);
49 O <<
"</trajectory>" << endl;
55 stringstream::streampos pos = I.tellg();
59 string pbuf = buf.str();
60 const char* p = strcasestr(pbuf.c_str(),
"</trajectory>");
64 ppsize=(p-pbuf.c_str())+13;
65 I.seekg((
size_t)pos+ppsize);
72 return shared_from_this();
79 Init(r->GetConfigurationSpecification());
81 r->GetWaypoints(0,r->GetNumWaypoints(),data);
88 vector<dReal> vinternaldata;
89 Sample(vinternaldata,time);
90 data.resize(spec.
GetDOF());
97 vector<dReal> vinternaldata;
99 data.resize(spec.
GetDOF()*(endindex-startindex),0);
100 if( startindex < endindex ) {
109 std::vector<dReal> data;
115 if( itgroup->name.size() >= 12 && itgroup->name.substr(0,12) == string(
"joint_values") ) {
116 tp.
q.resize(itgroup->dof);
117 std::copy(data.begin()+itgroup->offset,data.begin()+itgroup->offset+itgroup->dof,tp.
q.begin());
126 std::vector<dReal> vdata;
131 if( itgroup->name.size() >= 12 && itgroup->name.substr(0,12) == string(
"joint_values") ) {
132 joint_values = *itgroup;
134 else if( itgroup->name.size() >= 16 && itgroup->name.substr(0,16) == string(
"affine_transform") ) {
135 affine_transform = *itgroup;
136 stringstream ss(affine_transform.
name);
137 string semantic, robotname, dofs;
138 ss >> semantic >> robotname >> dofs;
140 affinedofs = boost::lexical_cast<
int>(dofs);
145 for(
size_t i = 0; i < __vdeprecatedpoints.size(); ++i) {
147 tp.
q.resize(joint_values.
dof);
149 std::copy(itdata+joint_values.
offset,itdata+joint_values.
offset+joint_values.
dof,tp.
q.begin());
154 return __vdeprecatedpoints;
161 std::vector<dReal> v; v.reserve(p.
q.size()+p.
qdot.size()+8);
163 if( p.
q.size() > 0 ) {
165 spec.
_vgroups.back().name =
"joint_values";
168 v.resize(dof+spec.
_vgroups.back().dof);
169 for(
size_t i = 0; i < p.
q.size(); ++i) {
174 if( p.
qdot.size() > 0 ) {
175 BOOST_ASSERT(p.
qdot.size() == p.
q.size());
177 spec.
_vgroups.back().name =
"joint_velocities";
180 v.resize(dof+spec.
_vgroups.back().dof);
181 for(
size_t i = 0; i < p.
q.size(); ++i) {
182 v[dof+i] = p.
qdot[i];
184 dof += p.
qdot.size();
191 v.resize(dof+spec.
_vgroups.back().dof);
196 spec.
_vgroups.back().name =
"deltatime";
199 v.resize(dof+spec.
_vgroups.back().dof);
213 else if( !!probot ) {
215 vector<int> indices(probot->GetDOF());
216 for(
int i = 0; i < probot->GetDOF(); ++i) {
219 probot->SetActiveDOFs(indices);
223 RAVELOG_WARN(
"CalcTrajTiming failed, need to specify robot\n");