hrpModel/OnlineViewerUtil.cpp
Go to the documentation of this file.
1 #include <hrpModel/Link.h>
2 #include "OnlineViewerUtil.h"
3 
4 using namespace hrp;
5 using namespace OpenHRP;
6 
7 void setupCharacterPosition(CharacterPosition &characterPosition, BodyPtr body)
8 {
9  characterPosition.characterName = CORBA::string_dup(body->name().c_str());
10  int numLinks = body->numLinks();
11  characterPosition.linkPositions.length(numLinks);
12 }
13 
14 void updateCharacterPosition(CharacterPosition &characterPosition, BodyPtr body)
15 {
16  for(unsigned int j=0; j < body->numLinks(); ++j) {
17  LinkPosition &linkPosition = characterPosition.linkPositions[j];
18  Link* link = body->link(j);
19 
20  setVector3(link->p, linkPosition.p);
21  setMatrix33ToRowMajorArray(link->attitude(), linkPosition.R);
22  }
23 }
24 
25 void getWorldState(WorldState& state, WorldBase& world)
26 {
27  state.time = world.currentTime();
28  for (unsigned int i=0; i<world.numBodies(); i++){
29  updateCharacterPosition(state.characterPositions[i], world.body(i));
30  }
31 }
32 
33 void initWorldState(WorldState& state, WorldBase& world)
34 {
35  state.characterPositions.length(world.numBodies());
36  for (unsigned int i=0; i<world.numBodies(); i++){
37  setupCharacterPosition(state.characterPositions[i], world.body(i));
38  }
39 }
40 
41 void initWorldState(WorldState& state, WorldBase& world,
42  std::vector<ColdetLinkPairPtr>& pairs)
43 {
44  initWorldState(state, world);
45 
46  OpenHRP::CollisionSequence& collisions = state.collisions;
47 
48  collisions.length(pairs.size());
49  for(size_t colIndex=0; colIndex < pairs.size(); ++colIndex){
50  hrp::ColdetLinkPairPtr linkPair = pairs[colIndex];
51  hrp::Link *link0 = linkPair->link(0);
52  hrp::Link *link1 = linkPair->link(1);
53  OpenHRP::LinkPair& pair = collisions[colIndex].pair;
54  pair.charName1 = CORBA::string_dup(link0->body->name().c_str());
55  pair.charName2 = CORBA::string_dup(link1->body->name().c_str());
56  pair.linkName1 = CORBA::string_dup(link0->name.c_str());
57  pair.linkName2 = CORBA::string_dup(link1->name.c_str());
58  }
59 }
void setVector3(const Vector3 &v3, V &v, size_t top=0)
Definition: Eigen3d.h:130
png_uint_32 i
Definition: png.h:2735
boost::shared_ptr< Body > BodyPtr
Definition: Body.h:315
void getWorldState(WorldState &state, WorldBase &world)
BodyPtr body(int index)
get body by index
void setMatrix33ToRowMajorArray(const Matrix33 &m33, Array &a, size_t top=0)
Definition: Eigen3d.h:105
void initWorldState(WorldState &state, WorldBase &world)
void setupCharacterPosition(CharacterPosition &characterPosition, BodyPtr body)
double currentTime(void) const
get current time
const std::string & name()
Definition: Body.h:56
void updateCharacterPosition(CharacterPosition &characterPosition, BodyPtr body)
boost::intrusive_ptr< ColdetLinkPair > ColdetLinkPairPtr
unsigned int numBodies()
get the number of bodies in this world


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:04