SequencePlayerService_impl.cpp
Go to the documentation of this file.
2 #include "SequencePlayer.h"
3 #include <hrpModel/Body.h>
4 #include <hrpModel/Link.h>
5 
7 {
8 }
9 
11 {
12 }
13 
15 {
17 }
18 
20 {
21  return m_player->waitInterpolationOfGroup(gname);
22 }
23 
24 CORBA::Boolean SequencePlayerService_impl::setJointAngles(const dSequence& jvs, CORBA::Double tm)
25 {
26  if (jvs.length() != (unsigned int)(m_player->robot()->numJoints())) {
27  std::cerr << __PRETTY_FUNCTION__ << " num of joint is differ, input:" << jvs.length() << ", robot:" << (unsigned int)(m_player->robot()->numJoints()) << std::endl;
28  return false;
29  }
30  return m_player->setJointAngles(jvs.get_buffer(), tm);
31 }
32 
33 CORBA::Boolean SequencePlayerService_impl::setJointAnglesWithMask(const dSequence& jvs, const bSequence& mask, CORBA::Double tm)
34 {
35  if (jvs.length() != (unsigned int)(m_player->robot()->numJoints())
36  || mask.length() != (unsigned int)(m_player->robot()->numJoints())) {
37  std::cerr << __PRETTY_FUNCTION__ << " num of joint is differ, input:" << jvs.length() << ", mask:" << mask.length() << ", robot" << (unsigned int)(m_player->robot()->numJoints()) << std::endl;
38  return false;
39  }
40  return m_player->setJointAngles(jvs.get_buffer(), mask.get_buffer(), tm);
41 }
42 
43 CORBA::Boolean SequencePlayerService_impl::setJointAnglesSequence(const dSequenceSequence& jvss, const dSequence& tms)
44 {
45  const OpenHRP::bSequence mask;
46  return setJointAnglesSequenceWithMask(jvss, mask, tms);
47 }
48 
50 {
51  return m_player->clearJointAngles();
52 }
53 
54 CORBA::Boolean SequencePlayerService_impl::setJointAnglesSequenceWithMask(const dSequenceSequence& jvss, const bSequence& mask, const dSequence& tms)
55 {
56  if (jvss.length() <= 0) {
57  std::cerr << __PRETTY_FUNCTION__ << " num of joint angles sequence is invalid:" << jvss.length() << " > 0" << std::endl;
58  return false;
59  }
60  if (jvss.length() != tms.length()) {
61  std::cerr << __PRETTY_FUNCTION__ << " length of joint angles sequence and time sequence differ, joint angle:" << jvss.length() << ", time:" << tms.length() << std::endl;
62  return false;
63  }
64  const dSequence& jvs = jvss[0];
65  if (jvs.length() != (unsigned int)(m_player->robot()->numJoints())) {
66  std::cerr << __PRETTY_FUNCTION__ << " num of joint is differ, input:" << jvs.length() << ", robot:" << (unsigned int)(m_player->robot()->numJoints()) << std::endl;
67  return false;
68  }
69 
70  if (mask.length() > 0 && mask.length() != (unsigned int)(m_player->robot()->numJoints())) {
71  std::cerr << __PRETTY_FUNCTION__ << " num of joint is differ, input:" << jvs.length() << ", mask:" << mask.length() << ", robot" << (unsigned int)(m_player->robot()->numJoints()) << std::endl;
72  return false;
73  }
74 
75  return m_player->setJointAnglesSequence(jvss, mask, tms);
76 }
77 
78 CORBA::Boolean SequencePlayerService_impl::setJointAnglesSequenceFull(const dSequenceSequence& jvss, const dSequenceSequence& vels, const dSequenceSequence& torques, const dSequenceSequence& poss, const dSequenceSequence& rpys, const dSequenceSequence& accs, const dSequenceSequence& zmps, const dSequenceSequence& wrenches, const dSequenceSequence& optionals, const dSequence &tms)
79 {
80  if (jvss.length() <= 0) {
81  std::cerr << __PRETTY_FUNCTION__ << " num of joint angles sequence is invalid:" << jvss.length() << " > 0" << std::endl;
82  return false;
83  }
84  if (jvss.length() != tms.length()) {
85  std::cerr << __PRETTY_FUNCTION__ << " length of joint angles sequence and time sequence differ, joint angle:" << jvss.length() << ", time:" << tms.length() << std::endl;
86  return false;
87  }
88  const dSequence& jvs = jvss[0];
89  if (jvs.length() != (unsigned int)(m_player->robot()->numJoints())) {
90  std::cerr << __PRETTY_FUNCTION__ << " num of joint is differ, input:" << jvs.length() << ", robot:" << (unsigned int)(m_player->robot()->numJoints()) << std::endl;
91  return false;
92  }
93  if (vels.length() > 0 ) {
94  const dSequence& vel = vels[0];
95  if (vels.length() != tms.length()) {
96  std::cerr << __PRETTY_FUNCTION__ << " length of joint velocitys sequence and time sequence differ, joint velocity:" << vels.length() << ", time:" << tms.length() << std::endl;
97  return false;
98  }
99  if ( vel.length() != (unsigned int)(m_player->robot()->numJoints())) {
100  std::cerr << __PRETTY_FUNCTION__ << " num of joint is differ, input:" << jvs.length() << ", vel:" << vel.length() << ", robot" << (unsigned int)(m_player->robot()->numJoints()) << std::endl;
101  return false;
102  }
103  }
104  if (torques.length() > 0 ) {
105  const dSequence& torque = torques[0];
106  if (torques.length() != tms.length()) {
107  std::cerr << __PRETTY_FUNCTION__ << " length of joint torque sequence and time sequence differ, joint torque:" << torques.length() << ", time:" << tms.length() << std::endl;
108  return false;
109  }
110  if ( torque.length() != (unsigned int)(m_player->robot()->numJoints())) {
111  std::cerr << __PRETTY_FUNCTION__ << " num of joint is differ, input:" << jvs.length() << ", torque:" << torque.length() << ", robot" << (unsigned int)(m_player->robot()->numJoints()) << std::endl;
112  return false;
113  }
114  }
115 
116  if (poss.length() > 0) {
117  const dSequence& pos = poss[0];
118  if (poss.length() != tms.length()) {
119  std::cerr << __PRETTY_FUNCTION__ << " length of base pos sequence and time sequence differ, pos:" << poss.length() << ", time:" << tms.length() << std::endl;
120  return false;
121  }
122  if ( pos.length() != 3) {
123  std::cerr << __PRETTY_FUNCTION__ << " num of base pos is differ, pos:" << pos.length() << std::endl;
124  return false;
125  }
126  }
127 
128  if (rpys.length() > 0) {
129  const dSequence& rpy = rpys[0];
130  if (rpys.length() != tms.length()) {
131  std::cerr << __PRETTY_FUNCTION__ << " length of base rpy sequence and time sequence differ, rpy:" << rpys.length() << ", time:" << tms.length() << std::endl;
132  return false;
133  }
134  if ( rpy.length() != 3) {
135  std::cerr << __PRETTY_FUNCTION__ << " num of base rpy is differ, rpy:" << rpy.length() << std::endl;
136  return false;
137  }
138  }
139 
140  if (accs.length() > 0 ) {
141  const dSequence& acc = accs[0];
142  if (accs.length() != tms.length()) {
143  std::cerr << __PRETTY_FUNCTION__ << " length of joint accocitys sequence and time sequence differ, joint accocity:" << accs.length() << ", time:" << tms.length() << std::endl;
144  return false;
145  }
146  if ( acc.length() != 3) {
147  std::cerr << __PRETTY_FUNCTION__ << " num of joint is differ, input:" << jvs.length() << ", acc:" << acc.length() << ", robot" << (unsigned int)(m_player->robot()->numJoints()) << std::endl;
148  return false;
149  }
150  }
151 
152  if (zmps.length() > 0) {
153  const dSequence& zmp = zmps[0];
154  if (zmps.length() != tms.length()) {
155  std::cerr << __PRETTY_FUNCTION__ << " length of zmp sequence and time sequence differ, zmp:" << zmps.length() << ", time:" << tms.length() << std::endl;
156  return false;
157  }
158  if ( zmp.length() != 3) {
159  std::cerr << __PRETTY_FUNCTION__ << " num of zmp is differ, zmp:" << zmp.length() << std::endl;
160  return false;
161  }
162  }
163 
164  if (wrenches.length() > 0) {
165  if (wrenches.length() != tms.length()) {
166  std::cerr << __PRETTY_FUNCTION__ << " length of wrench sequence and time sequence differ, wrench:" << wrenches.length() << ", time:" << tms.length() << std::endl;
167  return false;
168  }
169  // need to check size of wrench
170  //const dSequence& wrench = wrenches[0];
171  }
172 
173  if (optionals.length() > 0) {
174  if (optionals.length() != tms.length()) {
175  std::cerr << __PRETTY_FUNCTION__ << " length of optional sequence and time sequence differ, optional:" << optionals.length() << ", time:" << tms.length() << std::endl;
176  return false;
177  }
178  // need to check size of optional
179  //const dSequence& optional = optionasl[0];
180  }
181 
182  return m_player->setJointAnglesSequenceFull(jvss, vels, torques, poss, rpys, accs, zmps, wrenches, optionals, tms);
183 }
184 
185 CORBA::Boolean SequencePlayerService_impl::setJointAngle(const char *jname, CORBA::Double jv, CORBA::Double tm)
186 {
187  BodyPtr r = m_player->robot();
188  Link *l = r->link(jname);
189  if (!l){
190  std::cerr << "can't find(" << jname << ")" << std::endl;
191  return false;
192  }
193  int id = l->jointId;
194  return m_player->setJointAngle(id, jv, tm);
195 }
196 
197 CORBA::Boolean SequencePlayerService_impl::setBasePos(const dSequence& pos, CORBA::Double tm)
198 {
199  if (pos.length() != 3) return false;
200 
201  return m_player->setBasePos(pos.get_buffer(), tm);
202 }
203 
204 CORBA::Boolean SequencePlayerService_impl::setBaseRpy(const dSequence& rpy, CORBA::Double tm)
205 {
206  if (rpy.length() != 3) return false;
207 
208  return m_player->setBaseRpy(rpy.get_buffer(), tm);
209 }
210 
211 CORBA::Boolean SequencePlayerService_impl::setZmp(const dSequence& zmp, CORBA::Double tm)
212 {
213  if (zmp.length() != 3) return false;
214 
215  return m_player->setZmp(zmp.get_buffer(), tm);
216 }
217 
218 CORBA::Boolean SequencePlayerService_impl::setWrenches(const dSequence& wrenches, CORBA::Double tm)
219 {
220  //if (wrenches.length() != ) return false;
221 
222  return m_player->setWrenches(wrenches.get_buffer(), tm);
223 }
224 
225 CORBA::Boolean SequencePlayerService_impl::setTargetPose(const char* gname, const dSequence& xyz, const dSequence& rpy, CORBA::Double tm){
226  char* frame_name = (char *)strrchr(gname, ':');
227  if ( frame_name ) {
228  ((char *)gname)[frame_name - gname] = '\0'; // cut frame_name, gname[strpos(':')] = 0x00
229  frame_name++; // skip ":"
230  }
231  return m_player->setTargetPose(gname, xyz.get_buffer(), rpy.get_buffer(), tm, frame_name);
232 }
233 
235 {
236  return m_player->player()->isEmpty();
237 }
238 
239 void SequencePlayerService_impl::loadPattern(const char* basename, CORBA::Double tm)
240 {
241  if (!m_player->player()){
242  std::cerr << "player is not set"<< std::endl;
243  return;
244  }
245  m_player->loadPattern(basename, tm);
246 }
247 
249 {
250  m_player->player()->clear();
251 }
252 
253 CORBA::Boolean SequencePlayerService_impl::clearOfGroup(const char *gname, CORBA::Double i_limitation)
254 {
255  m_player->player()->clearOfGroup(gname, i_limitation);
256  return true;
257 }
258 
260 {
262 }
263 
264 CORBA::Boolean SequencePlayerService_impl::setInterpolationMode(OpenHRP::SequencePlayerService::interpolationMode i_mode_)
265 {
266  return m_player->setInterpolationMode(i_mode_);
267 }
268 
270 {
273 }
274 
276 {
277  m_player = i_player;
278 }
279 
280 void SequencePlayerService_impl::playPattern(const dSequenceSequence& pos, const dSequenceSequence& rpy, const dSequenceSequence& zmp, const dSequence& tm)
281 {
282  m_player->playPattern(pos, rpy, zmp, tm);
283 }
284 
285 CORBA::Boolean SequencePlayerService_impl::addJointGroup(const char* gname, const OpenHRP::SequencePlayerService::StrSequence& jnames)
286 {
287  return m_player->addJointGroup(gname, jnames);
288 }
289 
290 CORBA::Boolean SequencePlayerService_impl::removeJointGroup(const char* gname)
291 {
292  return m_player->removeJointGroup(gname);
293 }
294 
295 CORBA::Boolean SequencePlayerService_impl::setJointAnglesOfGroup(const char *gname, const dSequence& jvs, CORBA::Double tm)
296 {
297  return m_player->setJointAnglesOfGroup(gname, jvs, tm);
298 }
299 
300 CORBA::Boolean SequencePlayerService_impl::setJointAnglesSequenceOfGroup(const char *gname, const dSequenceSequence& jvss, const dSequence& tms)
301 {
302  if (jvss.length() != tms.length()) {
303  std::cerr << __PRETTY_FUNCTION__ << " length of joint angles sequence and time sequence differ, joint angle:" << jvss.length() << ", time:" << tms.length() << std::endl;
304  return false;
305  }
306  return m_player->setJointAnglesSequenceOfGroup(gname, jvss, tms);
307 }
308 
310 {
311  return m_player->clearJointAnglesOfGroup(gname);
312 }
313 
314 CORBA::Boolean SequencePlayerService_impl::playPatternOfGroup(const char *gname, const dSequenceSequence& pos, const dSequence& tm)
315 {
316  return m_player->playPatternOfGroup(gname, pos, tm);
317 }
318 
319 void SequencePlayerService_impl::setMaxIKError(CORBA::Double pos, CORBA::Double rot)
320 {
321  return m_player->setMaxIKError(pos, rot);
322 }
323 
325 {
326  return m_player->setMaxIKIteration(iter);
327 }
bool setTargetPose(const char *gname, const double *xyz, const double *rpy, double tm, const char *frame_name)
CORBA::Boolean removeJointGroup(const char *gname)
CORBA::Boolean setTargetPose(const char *gname, const dSequence &xyz, const dSequence &rpy, CORBA::Double tm)
CORBA::Boolean setJointAnglesSequence(const dSequenceSequence &jvs, const dSequence &tms)
bool setJointAnglesSequenceFull(const OpenHRP::dSequenceSequence i_jvss, const OpenHRP::dSequenceSequence i_vels, const OpenHRP::dSequenceSequence i_torques, const OpenHRP::dSequenceSequence i_poss, const OpenHRP::dSequenceSequence i_rpys, const OpenHRP::dSequenceSequence i_accs, const OpenHRP::dSequenceSequence i_zmps, const OpenHRP::dSequenceSequence i_wrenches, const OpenHRP::dSequenceSequence i_optionals, const dSequence i_tms)
CORBA::Boolean setJointAngles(const dSequence &jvs, CORBA::Double tm)
hrp::BodyPtr robot()
bool setWrenches(const double *wrenches, double tm)
CORBA::Boolean setJointAnglesSequenceFull(const dSequenceSequence &jvss, const dSequenceSequence &vels, const dSequenceSequence &torques, const dSequenceSequence &poss, const dSequenceSequence &rpys, const dSequenceSequence &accs, const dSequenceSequence &zmps, const dSequenceSequence &wrenches, const dSequenceSequence &optionals, const dSequence &tms)
bool setZmp(const double *zmp, double tm)
sequence player component
void playPattern(const dSequenceSequence &pos, const dSequenceSequence &rpy, const dSequenceSequence &zmp, const dSequence &tm)
bool setJointAngles(const double *angles, double tm)
CORBA::Boolean setZmp(const dSequence &zmp, CORBA::Double tm)
void player(SequencePlayer *i_player)
std::string basename(const std::string name)
bool setJointAnglesSequence(const OpenHRP::dSequenceSequence angless, const OpenHRP::bSequence &mask, const OpenHRP::dSequence &times)
bool removeJointGroup(const char *gname)
CORBA::Boolean setJointAnglesOfGroup(const char *gname, const dSequence &jvs, CORBA::Double tm)
void loadPattern(const char *basename, CORBA::Double tm)
void loadPattern(const char *basename, double time)
bool addJointGroup(const char *gname, const OpenHRP::SequencePlayerService::StrSequence &jnames)
bool setJointAnglesSequenceOfGroup(const char *gname, const OpenHRP::dSequenceSequence angless, const OpenHRP::dSequence &times)
bool setBaseRpy(const double *rpy, double tm)
void clearOfGroup(const char *gname, double i_timeLimit)
Definition: seqplay.cpp:521
bool setBasePos(const double *pos, double tm)
CORBA::Boolean waitInterpolationOfGroup(const char *gname)
bool waitInterpolationOfGroup(const char *gname)
bool clearJointAnglesOfGroup(const char *gname)
CORBA::Boolean setJointAnglesSequenceWithMask(const dSequenceSequence &jvs, const bSequence &mask, const dSequence &tms)
png_infop int png_uint_32 mask
CORBA::Boolean setInterpolationMode(OpenHRP::SequencePlayerService::interpolationMode i_mode_)
void playPattern(const OpenHRP::dSequenceSequence &pos, const OpenHRP::dSequenceSequence &rpy, const OpenHRP::dSequenceSequence &zmp, const OpenHRP::dSequence &tm)
void setMaxIKIteration(short iter)
CORBA::Boolean setJointAngle(const char *jname, CORBA::Double jv, CORBA::Double tm)
void clear(double i_timeLimit=0)
Definition: seqplay.cpp:263
CORBA::Boolean addJointGroup(const char *gname, const OpenHRP::SequencePlayerService::StrSequence &jnames)
seqplay * player()
void setMaxIKIteration(CORBA::Short iter)
#define __PRETTY_FUNCTION__
void setMaxIKError(double pos, double rot)
CORBA::Boolean setWrenches(const dSequence &wrenches, CORBA::Double tm)
bool setJointAngle(short id, double angle, double tm)
void setMaxIKError(CORBA::Double pos, CORBA::Double rot)
bool setJointAnglesOfGroup(const char *gname, const OpenHRP::dSequence &jvs, double tm)
bool isEmpty() const
Definition: seqplay.cpp:103
bool setInitialState(double tm=0.0)
typedef int
bool setInterpolationMode(OpenHRP::SequencePlayerService::interpolationMode i_mode_)
CORBA::Boolean clearOfGroup(const char *gname, CORBA::Double i_timelimit)
bool playPatternOfGroup(const char *gname, const OpenHRP::dSequenceSequence &pos, const OpenHRP::dSequence &tm)
CORBA::Boolean playPatternOfGroup(const char *gname, const dSequenceSequence &pos, const dSequence &tm)
CORBA::Boolean setJointAnglesWithMask(const dSequence &jvs, const bSequence &mask, CORBA::Double tm)
CORBA::Boolean clearJointAnglesOfGroup(const char *gname)
CORBA::Boolean setBasePos(const dSequence &pos, CORBA::Double tm)
CORBA::Boolean setJointAnglesSequenceOfGroup(const char *gname, const dSequenceSequence &jvs, const dSequence &tms)
CORBA::Boolean setBaseRpy(const dSequence &rpy, CORBA::Double tm)


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Sat Dec 17 2022 03:52:21