void NiutHumanReader::updateJoint(int i, int j, Joint& curJoint, int toasterId, std::vector<int>& trackedJoints, const niut_msgs::niut_HUMAN_LIST::ConstPtr& msg) {
std::map<int, std::string> niutToString;
niutToString[0] = "HEAD";
niutToString[3] = "TORSO";
niutToString[9] = "L_HAND";
niutToString[15] = "R_HIP";
niutToString[22] = "R_HAND";
double x, y, z;
int niutJoint = trackedJoints[j];
std::string jointString = niutToString[niutJoint];
//Allocate Joint if needed
if (lastConfig_[toasterId]->skeleton_[jointString] == NULL) {
Joint* tmpJoint = new Joint(10001 + j, toasterId);
tmpJoint->setName(jointString);
tmpJoint->setAgentId(toasterId);
lastConfig_[toasterId]->skeleton_[jointString] = tmpJoint;
}
x = msg->filtered_users[i].skeleton.joint[niutJoint].position.x;
y = msg->filtered_users[i].skeleton.joint[niutJoint].position.y;
z = msg->filtered_users[i].skeleton.joint[niutJoint].position.z;
curJoint.position_.set<0>(x);
curJoint.position_.set<1>(y);
curJoint.position_.set<2>(z);
projectJoint(curJoint, kinectPos_);
}
Skeleton* createFreeFloatingTwoLinkRobot(Vector3d dim1,
Vector3d dim2, TypeOfDOF type2,
bool finished = true)
{
Skeleton* robot = new Skeleton();
// Create the first link, the joint with the ground and its shape
double mass = 1.0;
BodyNode* node = new BodyNode("link1");
Joint* joint = new FreeJoint();
joint->setName("joint1");
Shape* shape = new BoxShape(dim1);
node->setLocalCOM(Vector3d(0.0, 0.0, dim1(2)/2.0));
node->addVisualizationShape(shape);
node->addCollisionShape(shape);
node->setMass(mass);
node->setParentJoint(joint);
robot->addBodyNode(node);
// Create the second link, the joint with link1 and its shape
BodyNode* parent_node = robot->getBodyNode("link1");
node = new BodyNode("link2");
joint = create1DOFJoint(0.0, -DART_PI, DART_PI, type2);
joint->setName("joint2");
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
T.translate(Eigen::Vector3d(0.0, 0.0, dim1(2)));
joint->setTransformFromParentBodyNode(T);
shape = new BoxShape(dim2);
node->setLocalCOM(Vector3d(0.0, 0.0, dim2(2)/2.0));
node->addVisualizationShape(shape);
node->addCollisionShape(shape);
node->setMass(mass);
node->setParentJoint(joint);
parent_node->addChildBodyNode(node);
robot->addBodyNode(node);
// If finished, initialize the skeleton
if(finished)
{
addEndEffector(robot, node, dim2);
robot->init();
}
return robot;
}
void SpencerRobotReader::updateRobot(tf::TransformListener &listener) {
Robot* curRobot = lastConfig_["spencer"];
Joint* curJoint = new Joint("spencer_base_link", "spencer");
curJoint->setName(spencerJointsName_[0]);
// We start with base:
setRobotJointLocation(listener, curJoint);
curRobot->setOrientation(curJoint->getOrientation());
curRobot->setPosition(curJoint->getPosition());
curRobot->setTime(curJoint->getTime());
//printf("spencer robot: %f, %f, %f\n", curRobot->getPosition().get<0>(), curRobot->getPosition().get<1>(), curRobot->getPosition().get<2>());
delete curJoint;
}
/**
* Sets the parameters of the selected joint.
* \param prefParam parameter to set
* \param value parameter value cast to (void *)
**/
void Skeleton::setSelectedJointParameters(enum ANIMATA_PREFERENCES prefParam,
void *value)
{
for (unsigned i = 0; i < joints->size(); i++) {
Joint *j = (*joints)[i];
if (j->selected) {
switch (prefParam) {
case PREFS_JOINT_NAME:
j->setName(*((const char **)value));
break;
case PREFS_JOINT_X:
j->position.x = *((float *)value);
break;
case PREFS_JOINT_Y:
j->position.y = *((float *)value);
break;
case PREFS_JOINT_FIXED:
j->fixed = *((int *)value);
break;
case PREFS_JOINT_OSC: {
int osc = *((int *)value);
j->osc = osc;
// add or remove the joint from the vector of joints
// needed to be sent via OSC
if (osc) {
ui->editorBox->addToOSCJoints(j);
}
else {
ui->editorBox->deleteFromOSCJoints(j);
}
break;
}
default:
break;
}
}
}
}
void ToasterSimuRobotReader::robotJointStateCallBack(const toaster_msgs::RobotListStamped::ConstPtr& msg) {
//std::cout << "[area_manager][DEBUG] new data for robot received" << std::endl;
Robot* curRobot;
double roll, pitch, yaw;
for (unsigned int i = 0; i < msg->robotList.size(); i++) {
// If this robot is not assigned we have to allocate data.
if (lastConfig_.find(msg->robotList[i].meAgent.meEntity.id) == lastConfig_.end()) {
curRobot = new Robot(msg->robotList[i].meAgent.meEntity.id);
} else
curRobot = lastConfig_[msg->robotList[i].meAgent.meEntity.id];
std::vector<double> robOrientation;
double roll, pitch, ywa;
bg::model::point<double, 3, bg::cs::cartesian> robPosition;
Mobility curRobMobility = FULL;
curRobot->setId(msg->robotList[i].meAgent.meEntity.id);
curRobot->setName(msg->robotList[i].meAgent.meEntity.name);
curRobot->setMobility(curRobMobility);
curRobot->setTime(msg->robotList[i].meAgent.meEntity.time);
curRobot->busyHands_ = msg->robotList[i].meAgent.busyHands;
robPosition.set<0>(msg->robotList[i].meAgent.meEntity.pose.position.x);
robPosition.set<1>(msg->robotList[i].meAgent.meEntity.pose.position.y);
robPosition.set<2>(msg->robotList[i].meAgent.meEntity.pose.position.z);
curRobot->setPosition(robPosition);
tf::Quaternion q;
tf::quaternionMsgToTF(msg->robotList[i].meAgent.meEntity.pose.orientation, q);
tf::Matrix3x3(q).getRPY(roll, pitch, yaw);
robOrientation.push_back(roll);
robOrientation.push_back(pitch);
robOrientation.push_back(yaw);
curRobot->setOrientation(robOrientation);
lastConfig_[curRobot->getId()] = curRobot;
//TODO: fullRobot case
if (msg->robotList[i].meAgent.skeletonJoint.size() > 0) {
Joint * curJnt;
for (unsigned int i_jnt = 0; i_jnt < msg->robotList[i].meAgent.skeletonJoint.size(); i_jnt++) {
// If this joint is not assigned we have to allocate data.
if (lastConfig_[curRobot->getId()]->skeleton_[msg->robotList[i].meAgent.skeletonJoint[i_jnt].meEntity.name ] == NULL) {
curJnt = new Joint(msg->robotList[i].meAgent.skeletonJoint[i_jnt].meEntity.id, msg->robotList[i].meAgent.meEntity.id);
} else
curJnt = lastConfig_[curRobot->getId()]->skeleton_[msg->robotList[i].meAgent.skeletonJoint[i_jnt].meEntity.name ];
std::vector<double> jointOrientation;
bg::model::point<double, 3, bg::cs::cartesian> jointPosition;
curJnt->setName(msg->robotList[i].meAgent.skeletonJoint[i_jnt].meEntity.name);
curJnt->setAgentId(curRobot->getId());
curJnt->setTime(msg->robotList[i].meAgent.skeletonJoint[i_jnt].meEntity.time);
jointPosition.set<0>(msg->robotList[i].meAgent.skeletonJoint[i_jnt].meEntity.pose.position.x);
jointPosition.set<1>(msg->robotList[i].meAgent.skeletonJoint[i_jnt].meEntity.pose.position.y);
jointPosition.set<2>(msg->robotList[i].meAgent.skeletonJoint[i_jnt].meEntity.pose.position.z);
curJnt->setPosition(jointPosition);
tf::Quaternion q;
tf::quaternionMsgToTF(msg->robotList[i].meAgent.skeletonJoint[i_jnt].meEntity.pose.orientation, q);
tf::Matrix3x3(q).getRPY(roll, pitch, yaw);
jointOrientation.push_back(roll);
jointOrientation.push_back(pitch);
jointOrientation.push_back(yaw);
curJnt->setOrientation(jointOrientation);
curJnt->position = msg->robotList[i].meAgent.skeletonJoint[i_jnt].position;
lastConfig_[curRobot->getId()]->skeleton_[curJnt->getName()] = curJnt;
}
}
}
}
/// Loads skeleton.
void IO::loadSkeleton(TiXmlNode *parent, Skeleton *skeleton, Mesh *m)
{
// skeleton object is missing
if (parent == NULL)
return;
// load joints
TiXmlNode *jointsNode = parent->FirstChild("joints");
if (jointsNode == NULL)
return;
TiXmlNode *jointNode = NULL;
int jointCount = 0;
int loadedJointCount = 0;
while ((jointNode = jointsNode->IterateChildren(jointNode)))
{
TiXmlElement *j = jointNode->ToElement();
const char *name;
float x, y;
int selected;
int osc;
int fixed;
jointCount++;
/* if there's a critical error the joint is skipped */
QUERY_CRITICAL_ATTR(j, "x", x);
QUERY_CRITICAL_ATTR(j, "y", y);
/* loadedJointCount holds the number of actually loaded joints */
loadedJointCount++;
QUERY_ATTR(j, "selected", selected, 0);
QUERY_ATTR(j, "osc", osc, 0);
QUERY_ATTR(j, "fixed", fixed, 0);
name = j->Attribute("name"); // can be NULL
Joint *joint = skeleton->addJoint(x, y);
joint->selected = selected;
joint->osc = osc;
joint->fixed = fixed;
if (name)
joint->setName(name);
// add or remove the joint from the vector of joints
// needed to be sent via OSC
if (osc)
{
ui->editorBox->addToOSCJoints(joint);
}
}
// skip the loading of bones if there was a problematic joint
if (jointCount != loadedJointCount)
return;
// load bones
TiXmlNode *bonesNode = parent->FirstChild("bones");
if (bonesNode == NULL)
return;
TiXmlNode *boneNode = NULL;
vector<Joint*> *joints = skeleton->getJoints();
while ((boneNode = bonesNode->IterateChildren(boneNode)))
{
TiXmlElement *b = boneNode->ToElement();
const char *name;
int j0, j1;
float size, stiffness, lengthMult;
float lengthMultMin, lengthMultMax, time, tempo;
int selected;
float radius;
name = b->Attribute("name"); // can be NULL
QUERY_CRITICAL_ATTR(b, "j0", j0);
QUERY_CRITICAL_ATTR(b, "j1", j1);
QUERY_CRITICAL_ATTR(b, "size", size);
QUERY_ATTR(b, "stiffness", stiffness, BONE_DEFAULT_DAMP);
QUERY_ATTR(b, "lm", lengthMult, BONE_DEFAULT_LENGTH_MULT);
QUERY_ATTR(b, "lmmin", lengthMultMin, BONE_DEFAULT_LENGTH_MULT_MIN);
QUERY_ATTR(b, "lmmax", lengthMultMax, BONE_DEFAULT_LENGTH_MULT_MAX);
QUERY_ATTR(b, "tempo", tempo, 0);
QUERY_ATTR(b, "time", time, 0);
QUERY_ATTR(b, "selected", selected, 0);
QUERY_ATTR(b, "radius", radius, 1);
if ((j0 >= jointCount) || (j1 >= jointCount))
continue;
Joint *j0p = (*joints)[j0];
Joint *j1p = (*joints)[j1];
Bone *bone = skeleton->addBone(j0p, j1p);
if (name)
bone->setName(name);
bone->setOrigSize(size);
bone->damp = stiffness;
bone->setLengthMult(lengthMult);
bone->setLengthMultMin(lengthMultMin);
bone->setLengthMultMax(lengthMultMax);
bone->setTempo(tempo);
bone->setTime(time);
bone->selected = selected;
bone->setRadiusMult(radius);
// load attached vertices
TiXmlNode *attachedNode = boneNode->FirstChild("attached");
if (attachedNode == NULL)
continue;
TiXmlNode *vertexNode = NULL;
// vector to hold vertices to be attached
vector<Vertex *> *vertsToAttach = new vector<Vertex *>;
// all vertices in mesh
vector<Vertex *> *vertices = m->getVertices();
// iterate over children to find all vertices
while ((vertexNode = attachedNode->IterateChildren(vertexNode)))
{
TiXmlElement *vertexXML = vertexNode->ToElement();
int id;
QUERY_CRITICAL_ATTR(vertexXML, "id", id);
if ((id >= (int)(vertices->size())) || (id < 0))
continue;
Vertex *v = (*vertices)[id];
vertsToAttach->push_back(v);
}
// setup parameter arrays
vertexNode = NULL;
float *dsts, *weights, *ca, *sa;
int count = vertsToAttach->size();
dsts = new float[count];
weights = new float[count];
ca = new float[count];
sa = new float[count];
// load parameters of attached vertices
int i = 0;
while ((vertexNode = attachedNode->IterateChildren(vertexNode)))
{
TiXmlElement *vertexXML = vertexNode->ToElement();
int id;
float d, w;
float s, c;
QUERY_CRITICAL_ATTR(vertexXML, "id", id);
QUERY_CRITICAL_ATTR(vertexXML, "d", d);
QUERY_CRITICAL_ATTR(vertexXML, "w", w);
QUERY_CRITICAL_ATTR(vertexXML, "sa", s);
QUERY_CRITICAL_ATTR(vertexXML, "ca", c);
if ((id >= (int)(vertices->size())) || (id < 0))
continue;
dsts[i] = d;
weights[i] = w;
ca[i] = c;
sa[i] = s;
i++;
}
bone->attachVertices(vertsToAttach, dsts, weights, ca, sa);
vertsToAttach->clear();
delete vertsToAttach;
}
}
void ToasterHumanReader::humanJointStateCallBack(const toaster_msgs::HumanList::ConstPtr& msg) {
//std::cout << "[area_manager][DEBUG] new data for human received with time " << msg->humanList[0].meAgent.meEntity.time << std::endl;
Human * curHuman;
for (unsigned int i = 0; i < msg->humanList.size(); i++) {
// If this human is not assigned we have to allocate data.
if (lastConfig_.find(msg->humanList[i].meAgent.meEntity.id) == lastConfig_.end()) {
curHuman = new Human(msg->humanList[i].meAgent.meEntity.id);
} else
curHuman = lastConfig_[msg->humanList[i].meAgent.meEntity.id];
std::vector<double> humanOrientation;
bg::model::point<double, 3, bg::cs::cartesian> humanPosition;
Mobility curHumanMobility = FULL;
curHuman->setId(msg->humanList[i].meAgent.meEntity.id);
curHuman->setName(msg->humanList[i].meAgent.meEntity.name);
curHuman->setMobility(curHumanMobility);
curHuman->setTime(msg->humanList[i].meAgent.meEntity.time);
curHuman->busyHands_ = msg->humanList[i].meAgent.busyHands;
humanPosition.set<0>(msg->humanList[i].meAgent.meEntity.positionX);
humanPosition.set<1>(msg->humanList[i].meAgent.meEntity.positionY);
humanPosition.set<2>(msg->humanList[i].meAgent.meEntity.positionZ);
curHuman->setPosition(humanPosition);
humanOrientation.push_back(msg->humanList[i].meAgent.meEntity.orientationRoll);
humanOrientation.push_back(msg->humanList[i].meAgent.meEntity.orientationPitch);
humanOrientation.push_back(msg->humanList[i].meAgent.meEntity.orientationYaw);
curHuman->setOrientation(humanOrientation);
lastConfig_[curHuman->getId()] = curHuman;
//TODO: fullHuman
if (fullHuman_) {
Joint * curJnt;
for (unsigned int i_jnt = 0; i_jnt < msg->humanList[i].meAgent.skeletonNames.size(); i_jnt++) {
// If this joint is not assigned we have to allocate data.
if (lastConfig_[curHuman->getId()]->skeleton_[msg->humanList[i].meAgent.skeletonNames[i_jnt] ] == NULL) {
curJnt = new Joint(msg->humanList[i].meAgent.skeletonJoint[i_jnt].meEntity.id, msg->humanList[i].meAgent.meEntity.id);
} else
curJnt = lastConfig_[curHuman->getId()]->skeleton_[msg->humanList[i].meAgent.skeletonNames[i_jnt] ];
std::vector<double> jointOrientation;
bg::model::point<double, 3, bg::cs::cartesian> jointPosition;
curJnt->setName(msg->humanList[i].meAgent.skeletonNames[i_jnt]);
curJnt->setAgentId(curHuman->getId());
curJnt->setTime(msg->humanList[i].meAgent.skeletonJoint[i_jnt].meEntity.time);
jointPosition.set<0>(msg->humanList[i].meAgent.skeletonJoint[i_jnt].meEntity.positionX);
jointPosition.set<1>(msg->humanList[i].meAgent.skeletonJoint[i_jnt].meEntity.positionY);
jointPosition.set<2>(msg->humanList[i].meAgent.skeletonJoint[i_jnt].meEntity.positionZ);
curJnt->setPosition(jointPosition);
jointOrientation.push_back(msg->humanList[i].meAgent.skeletonJoint[i_jnt].meEntity.orientationRoll);
jointOrientation.push_back(msg->humanList[i].meAgent.skeletonJoint[i_jnt].meEntity.orientationPitch);
jointOrientation.push_back(msg->humanList[i].meAgent.skeletonJoint[i_jnt].meEntity.orientationYaw);
curJnt->setOrientation(jointOrientation);
lastConfig_[curHuman->getId()]->skeleton_[curJnt->getName()] = curJnt;
}
}
}
}
/* ********************************************************************************************* */
int main(int argc, char* argv[]) {
// Create Left Leg skeleton
Skeleton LeftLegSkel;
// Pointers to be used during the Skeleton building
Matrix3d inertiaMatrix;
inertiaMatrix << 0, 0, 0, 0, 0, 0, 0, 0, 0;
double mass = 1.0;
// ***** BodyNode 1: Left Hip Yaw (LHY) ***** *
BodyNode* node = new BodyNode("LHY");
Joint* joint = create1DOFJoint(NULL, node, 0.0, 0.0, DART_PI, DOF_YAW);
joint->setName("LHY");
Shape* shape = new BoxShape(Vector3d(0.3, 0.3, 1.0));
node->addVisualizationShape(shape);
node->addCollisionShape(shape);
node->setMass(mass);
LeftLegSkel.addBodyNode(node);
// ***** BodyNode 2: Left Hip Roll (LHR) whose parent is: LHY *****\
BodyNode* parent_node = LeftLegSkel.getBodyNode("LHY");
node = new BodyNode("LHR");
joint = create1DOFJoint(parent_node, node, 0.0, 0.0, DART_PI, DOF_ROLL);
joint->setName("LHR");
Eigen::Isometry3d T(Eigen::Translation3d(0.0, 0.0, 0.5));
joint->setTransformFromParentBodyNode(T);
shape = new BoxShape(Vector3d(0.3, 0.3, 1.0));
shape->setOffset(Vector3d(0.0, 0.0, 0.5));
node->setLocalCOM(shape->getOffset());
node->setMass(mass);
node->addVisualizationShape(shape);
node->addCollisionShape(shape);
LeftLegSkel.addBodyNode(node);
// ***** BodyNode 3: Left Hip Pitch (LHP) whose parent is: LHR *****
parent_node = LeftLegSkel.getBodyNode("LHR");
node = new BodyNode("LHP");
joint = create1DOFJoint(parent_node, node, 0.0, 0.0, DART_PI, DOF_ROLL);
joint->setName("LHP");
T = Eigen::Translation3d(0.0, 0.0, 1.0);
joint->setTransformFromParentBodyNode(T);
shape = new BoxShape(Vector3d(0.3, 0.3, 1.0));
shape->setOffset(Vector3d(0.0, 0.0, 0.5));
node->setLocalCOM(shape->getOffset());
node->setMass(mass);
Shape* shape1 = new EllipsoidShape(Vector3d(0.3, 0.3, 1.0));
shape1->setOffset(Vector3d(0.0, 0.0, 0.5));
node->addVisualizationShape(shape1);
node->addCollisionShape(shape);
LeftLegSkel.addBodyNode(node);
// Initialize the skeleton
LeftLegSkel.initDynamics();
// Window stuff
MyWindow window(&LeftLegSkel);
glutInit(&argc, argv);
window.initWindow(640, 480, "Skeleton example");
glutMainLoop();
return 0;
}
