openrave.org

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
trajectory.cpp
Go to the documentation of this file.
1 // -*- coding: utf-8 -*-
2 // Copyright (C) 2006-2011 Rosen Diankov <rosen.diankov@gmail.com>
3 //
4 // This file is part of OpenRAVE.
5 // OpenRAVE is free software: you can redistribute it and/or modify
6 // it under the terms of the GNU Lesser General Public License as published by
7 // the Free Software Foundation, either version 3 of the License, or
8 // at your option) any later version.
9 //
10 // This program is distributed in the hope that it will be useful,
11 // but WITHOUT ANY WARRANTY; without even the implied warranty of
12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 // GNU Lesser General Public License for more details.
14 //
15 // You should have received a copy of the GNU Lesser General Public License
16 // along with this program. If not, see <http://www.gnu.org/licenses/>.
17 #include "libopenrave.h"
18 #include <boost/lexical_cast.hpp>
19 #include <openrave/planningutils.h>
20 #include <openrave/xmlreaders.h>
21 
22 namespace OpenRAVE {
23 
25 {
26 }
27 
28 void TrajectoryBase::serialize(std::ostream& O, int options) const
29 {
30  O << "<trajectory type=\"" << GetXMLId() << "\">" << endl << GetConfigurationSpecification();
31  O << "<data count=\"" << GetNumWaypoints() << "\">" << endl;
32  std::vector<dReal> data;
33  GetWaypoints(0,GetNumWaypoints(),data);
34  FOREACHC(it,data){
35  O << *it << " ";
36  }
37  O << "</data>" << endl;
38  if( GetDescription().size() > 0 ) {
39  O << "<description><![CDATA[" << GetDescription() << "]]></description>" << endl;
40  }
41  if( GetReadableInterfaces().size() > 0 ) {
43  FOREACHC(it, GetReadableInterfaces()) {
44  BaseXMLWriterPtr newwriter = writer->AddChild(it->first);
45  it->second->Serialize(newwriter,options);
46  }
47  writer->Serialize(O);
48  }
49  O << "</trajectory>" << endl;
50 }
51 
53 {
54  stringbuf buf;
55  stringstream::streampos pos = I.tellg();
56  I.get(buf, 0); // get all the data, yes this is inefficient, not sure if there anyway to search in streams
57  BOOST_ASSERT(!!I);
58 
59  string pbuf = buf.str();
60  const char* p = strcasestr(pbuf.c_str(), "</trajectory>");
61  int ppsize=-1;
62  if( p != NULL ) {
63  I.clear();
64  ppsize=(p-pbuf.c_str())+13;
65  I.seekg((size_t)pos+ppsize);
66  }
67  else {
68  throw OPENRAVE_EXCEPTION_FORMAT("error, failed to find </trajectory> in %s",buf.str(),ORE_InvalidArguments);
69  }
71  LocalXML::ParseXMLData(BaseXMLReaderPtr(&reader,utils::null_deleter()), pbuf.c_str(), ppsize);
72  return shared_from_this();
73 }
74 
75 void TrajectoryBase::Clone(InterfaceBaseConstPtr preference, int cloningoptions)
76 {
77  InterfaceBase::Clone(preference,cloningoptions);
78  TrajectoryBaseConstPtr r = RaveInterfaceConstCast<TrajectoryBase>(preference);
79  Init(r->GetConfigurationSpecification());
80  vector<dReal> data;
81  r->GetWaypoints(0,r->GetNumWaypoints(),data);
82  Insert(0,data);
83 }
84 
85 void TrajectoryBase::Sample(std::vector<dReal>& data, dReal time, const ConfigurationSpecification& spec) const
86 {
87  RAVELOG_VERBOSE(str(boost::format("TrajectoryBase::Sample: calling slow implementation %s")%GetXMLId()));
88  vector<dReal> vinternaldata;
89  Sample(vinternaldata,time);
90  data.resize(spec.GetDOF());
91  ConfigurationSpecification::ConvertData(data.begin(),spec,vinternaldata.begin(),GetConfigurationSpecification(),1,GetEnv());
92 }
93 
94 void TrajectoryBase::GetWaypoints(size_t startindex, size_t endindex, std::vector<dReal>& data, const ConfigurationSpecification& spec) const
95 {
96  RAVELOG_VERBOSE(str(boost::format("TrajectoryBase::GetWaypoints: calling slow implementation %s")%GetXMLId()));
97  vector<dReal> vinternaldata;
98  GetWaypoints(startindex,endindex,vinternaldata);
99  data.resize(spec.GetDOF()*(endindex-startindex),0);
100  if( startindex < endindex ) {
101  ConfigurationSpecification::ConvertData(data.begin(),spec,vinternaldata.begin(),GetConfigurationSpecification(),endindex-startindex,GetEnv());
102  }
103 }
104 
105 // Old API
106 
108 {
109  std::vector<dReal> data;
110  Sample(data,time);
111  tp.q.resize(0);
112  tp.trans = Transform();
114  FOREACHC(itgroup,GetConfigurationSpecification()._vgroups) {
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());
118  }
119  }
120  return true;
121 }
122 
123 const std::vector<TrajectoryBase::Point>& TrajectoryBase::GetPoints() const
124 {
125  __vdeprecatedpoints.resize(GetNumWaypoints());
126  std::vector<dReal> vdata;
127  GetWaypoints(0,GetNumWaypoints(),vdata);
128  ConfigurationSpecification::Group joint_values, affine_transform;
129  int affinedofs=0;
130  FOREACHC(itgroup,GetConfigurationSpecification()._vgroups) {
131  if( itgroup->name.size() >= 12 && itgroup->name.substr(0,12) == string("joint_values") ) {
132  joint_values = *itgroup;
133  }
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;
139  if( !!ss ) {
140  affinedofs = boost::lexical_cast<int>(dofs);
141  }
142  }
143  }
144 
145  for(size_t i = 0; i < __vdeprecatedpoints.size(); ++i) {
146  TrajectoryBase::Point& tp = __vdeprecatedpoints[i];
147  tp.q.resize(joint_values.dof);
148  std::vector<dReal>::iterator itdata = vdata.begin()+GetConfigurationSpecification().GetDOF()*i;
149  std::copy(itdata+joint_values.offset,itdata+joint_values.offset+joint_values.dof,tp.q.begin());
150  tp.trans = Transform();
151  RaveGetTransformFromAffineDOFValues(tp.trans,itdata+affine_transform.offset,affinedofs);
152  }
153 
154  return __vdeprecatedpoints;
155 }
156 
158 {
160  spec._vgroups.reserve(4);
161  std::vector<dReal> v; v.reserve(p.q.size()+p.qdot.size()+8);
162  int dof = 0;
163  if( p.q.size() > 0 ) {
165  spec._vgroups.back().name = "joint_values";
166  spec._vgroups.back().offset = dof;
167  spec._vgroups.back().dof = p.q.size();
168  v.resize(dof+spec._vgroups.back().dof);
169  for(size_t i = 0; i < p.q.size(); ++i) {
170  v[dof+i] = p.q[i];
171  }
172  dof += p.q.size();
173  }
174  if( p.qdot.size() > 0 ) {
175  BOOST_ASSERT(p.qdot.size() == p.q.size());
177  spec._vgroups.back().name = "joint_velocities";
178  spec._vgroups.back().offset = dof;
179  spec._vgroups.back().dof = p.qdot.size();
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];
183  }
184  dof += p.qdot.size();
185  }
186 
188  spec._vgroups.back().name = str(boost::format("affine_transform __dummy__ %d")%DOF_Transform);
189  spec._vgroups.back().offset = dof;
190  spec._vgroups.back().dof = RaveGetAffineDOF(DOF_Transform);
191  v.resize(dof+spec._vgroups.back().dof);
193  dof += spec._vgroups.back().dof;
194 
196  spec._vgroups.back().name = "deltatime";
197  spec._vgroups.back().offset = dof;
198  spec._vgroups.back().dof = dof;
199  v.resize(dof+spec._vgroups.back().dof);
200  v.at(dof) = p.time;
201  dof += 1;
202  if( GetConfigurationSpecification().GetDOF() == 0 ) {
203  Init(spec);
204  }
205  Insert(GetNumWaypoints(),v,spec);
206 }
207 
208 bool TrajectoryBase::CalcTrajTiming(RobotBasePtr probot, int interp, bool autocalc, bool activedof, dReal fmaxvelmult)
209 {
210  if( activedof ) {
211  planningutils::RetimeActiveDOFTrajectory(shared_trajectory(),probot, !autocalc,fmaxvelmult);
212  }
213  else if( !!probot ) {
214  RobotBase::RobotStateSaver saver(probot);
215  vector<int> indices(probot->GetDOF());
216  for(int i = 0; i < probot->GetDOF(); ++i) {
217  indices[i] = i;
218  }
219  probot->SetActiveDOFs(indices);
220  planningutils::RetimeActiveDOFTrajectory(shared_trajectory(),probot,!autocalc,fmaxvelmult);
221  }
222  else {
223  RAVELOG_WARN("CalcTrajTiming failed, need to specify robot\n");
224  }
225  return true;
226 }
227 
228 } // end namespace OpenRAVE