BodyState.cpp
Go to the documentation of this file.
00001 #include <hrpModel/Link.h>
00002 #include <hrpModel/Sensor.h>
00003 #include "BodyState.h"
00004 
00005 using namespace hrp;
00006 
00007 void BodyState::set(BodyPtr i_body)
00008 {
00009     Link *root = i_body->rootLink();
00010     p = root->p;
00011     R = root->attitude();
00012     q.resize(i_body->numLinks());
00013     for (unsigned int i=0; i<i_body->numLinks(); i++){
00014         q[i] = i_body->link(i)->q;
00015     }
00016     int n;
00017     n = i_body->numSensors(Sensor::FORCE);
00018     force.resize(n);
00019     for(int id = 0; id < n; ++id){
00020         ForceSensor* sensor = i_body->sensor<ForceSensor>(id);
00021         setVector3(sensor->f,   force[id], 0);
00022         setVector3(sensor->tau, force[id], 3);
00023     }
00024 
00025     n = i_body->numSensors(Sensor::RATE_GYRO);
00026     rate.resize(n);
00027     for(int id=0; id < n; ++id){
00028         RateGyroSensor* sensor = i_body->sensor<RateGyroSensor>(id);
00029         setVector3(sensor->w, rate[id]);
00030     }
00031     
00032     n = i_body->numSensors(Sensor::ACCELERATION);
00033     acc.resize(n);
00034     for(int id=0; id < n; ++id){
00035         AccelSensor* sensor = i_body->sensor<AccelSensor>(id);
00036         setVector3(sensor->dv, acc[id]);
00037     }
00038 
00039     n = i_body->numSensors(Sensor::RANGE);
00040     range.resize(n);
00041     for(int id=0; id < n; ++id){
00042         RangeSensor* sensor = i_body->sensor<RangeSensor>(id);
00043         range[id] = sensor->distances;
00044     }
00045 }


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:54