BodyState.cpp
Go to the documentation of this file.
1 #include <hrpModel/Link.h>
2 #include <hrpModel/Sensor.h>
3 #include "BodyState.h"
4 
5 using namespace hrp;
6 
7 void BodyState::set(BodyPtr i_body)
8 {
9  Link *root = i_body->rootLink();
10  p = root->p;
11  R = root->attitude();
12  q.resize(i_body->numLinks());
13  for (unsigned int i=0; i<i_body->numLinks(); i++){
14  q[i] = i_body->link(i)->q;
15  }
16  int n;
17  n = i_body->numSensors(Sensor::FORCE);
18  force.resize(n);
19  for(int id = 0; id < n; ++id){
20  ForceSensor* sensor = i_body->sensor<ForceSensor>(id);
21  setVector3(sensor->f, force[id], 0);
22  setVector3(sensor->tau, force[id], 3);
23  }
24 
25  n = i_body->numSensors(Sensor::RATE_GYRO);
26  rate.resize(n);
27  for(int id=0; id < n; ++id){
28  RateGyroSensor* sensor = i_body->sensor<RateGyroSensor>(id);
29  setVector3(sensor->w, rate[id]);
30  }
31 
32  n = i_body->numSensors(Sensor::ACCELERATION);
33  acc.resize(n);
34  for(int id=0; id < n; ++id){
35  AccelSensor* sensor = i_body->sensor<AccelSensor>(id);
36  setVector3(sensor->dv, acc[id]);
37  }
38 
39  n = i_body->numSensors(Sensor::RANGE);
40  range.resize(n);
41  for(int id=0; id < n; ++id){
42  RangeSensor* sensor = i_body->sensor<RangeSensor>(id);
43  range[id] = sensor->distances;
44  }
45 }
std::vector< double > distances
void setVector3(const Vector3 &v3, V &v, size_t top=0)
png_uint_32 i
n
void set(hrp::BodyPtr i_body)
Definition: BodyState.cpp:7
static int id


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