void RobotModelBase::updateModelRec(Bodypart *part, Pose3D t)
{
//transform to body part position
t.translate(part->translation);
//if there is a joint on this body part (only not on root)
if (part->jointIndex >= 0)
{
t.translate(part->anchor); //go to joint position
jointPositions[part->jointIndex] = t; //set joint position
t.rotate(RotationMatrix(part->axis, jointAngles[part->jointIndex]));
t.translate(part->anchor * -1.0); //rotate part and go back to center
}
if (partOriginsAtJoint)
(partPositions[part->jointIndex + 1] = t).translate(part->anchor);
else
partPositions[part->jointIndex + 1] = t;
com += t.translation * part->mass; //calculate center of mass
mass += part->mass;
//continue with body parts mounted on this part
for (unsigned int i = 0; i < part->children.size(); i++)
updateModelRec(part->children[i], t);
}
Pose3D ViewBikeMath::calculateHandPos(const JointData& jointData, const JointData::Joint& joint, const RobotDimensions& robotDimensions)
{
Pose3D handPos;
float sign = joint == JointData::LShoulderPitch ? 1.f : -1.f;
handPos.translate(robotDimensions.armOffset.x, sign * robotDimensions.armOffset.y, robotDimensions.armOffset.z);
handPos.rotateY(-jointData.angles[joint]);
handPos.rotateZ(sign * jointData.angles[joint + 1]);
handPos.translate(robotDimensions.upperArmLength, 0, 0);
handPos.rotateX(jointData.angles[joint + 2] * sign);
handPos.rotateZ(sign * jointData.angles[joint + 3]);
return handPos;
}
Pose3D ViewBikeMath::calculateFootPos(const SensorData& sensorData, const JointData& jointData, const JointData::Joint& joint, const RobotDimensions& robotDimensions)
{
Pose3D footPos;
float sign = joint == JointData::LHipYawPitch ? -1.f : 1.f;
footPos.translate(0, -sign * (robotDimensions.lengthBetweenLegs / 2), 0)
.rotateX(-pi_4 * sign)
.rotateZ(jointData.angles[joint]*sign)
.rotateX(((jointData.angles[joint + 1] + pi_4)*sign))
.rotateY(jointData.angles[joint + 2])
.translate(0, 0, -robotDimensions.upperLegLength)
.rotateY(jointData.angles[joint + 3])
.translate(0, 0, -robotDimensions.lowerLegLength);
footPos.translation += Vector3<>(0, 0, -robotDimensions.heightLeg5Joint); //because inverse kinematics subtracts this
return footPos;
}
Pose3D KickEngineData::calcDesBodyAngle(JointRequest& jointData, const RobotDimensions& robotDimensions, JointData::Joint joint)
{
Pose3D footPos;
float sign = joint == JointData::LHipYawPitch ? -1.f : 1.f;
footPos.translate(0, 0, robotDimensions.heightLeg5Joint)
.rotateX(-jointData.angles[joint + 5]*sign)
.rotateY(-jointData.angles[joint + 4])
.translate(0, 0, robotDimensions.lowerLegLength)
.rotateY(-jointData.angles[joint + 3])
.translate(0, 0, robotDimensions.upperLegLength)
.rotateY(-jointData.angles[joint + 2])
.rotateX(((-jointData.angles[joint + 1] - pi_4)*sign))
.rotateZ(-jointData.angles[joint]*sign)
.rotateX(-pi_4 * sign);
return footPos;
}
void ODEOutBehaviorProvider::setBehaviorData()
{
int i = 0;
int numSet = 0;
int fallState = FallDownState::upright;
bool bumperRight = false, bumperLeft = false, chestButton = false;
// if(theKeyStates != NULL){
// bumperRight = theKeyStates->pressed[KeyStates::RBumperRight] || theKeyStates->pressed[KeyStates::RBumperLeft];
// bumperLeft = theKeyStates->pressed[KeyStates::LBumperRight] || theKeyStates->pressed[KeyStates::LBumperLeft];
// chestButton = theKeyStates->pressed[KeyStates::ChestButton];
// }
if(theFallDownState != NULL){
fallState = theFallDownState->state;
}
i = 0;
outBehaviorData.intData.clear();
outBehaviorData.intData.resize(OutBehaviorData::NUM_INT_DATA);
outBehaviorData.intData[i++] = fallState;//fallen state
outBehaviorData.intData[i++] = bumperRight;//bumper right
outBehaviorData.intData[i++] = bumperLeft;//bumper left
outBehaviorData.intData[i++] = chestButton;//chest button
// numSet = i;
// if(numSet == OutBehaviorData::NUM_INT_DATA)
// std::cout<<"Num of Data Set missmatch with NUM_INT_DATA: NumSet: "<<numSet<<", NUM_INT_DATA: "<<OutBehaviorData::NUM_INT_DATA<<std::endl;
// printf("%d Fallen and Button Data Set to :\n", outBehaviorData.intData.size());
// for(i = 0; i < numSet; i++)
// {
// printf("the %d number is ", i);
// std::cout<<outBehaviorData.intData[i]<<std::endl;
// }
//========for joint data
i = 0;
outBehaviorData.jointData.clear();
outBehaviorData.jointData.resize(OutBehaviorData::NUM_JOINT_DATA);
outBehaviorData.jointData[i++] =(float)theSensoryJointData->angles[JointData::HeadYaw];
outBehaviorData.jointData[i++] =(float)theSensoryJointData->angles[JointData::HeadPitch];
outBehaviorData.jointData[i++] =(float)theSensoryJointData->angles[JointData::RShoulderPitch];
outBehaviorData.jointData[i++] =(float)theSensoryJointData->angles[JointData::RShoulderRoll];
outBehaviorData.jointData[i++] =(float)theSensoryJointData->angles[JointData::RElbowYaw];
outBehaviorData.jointData[i++] =(float)theSensoryJointData->angles[JointData::RElbowRoll];
outBehaviorData.jointData[i++] =(float)theSensoryJointData->angles[JointData::LShoulderPitch];
outBehaviorData.jointData[i++] =(float)theSensoryJointData->angles[JointData::LShoulderRoll];
outBehaviorData.jointData[i++] =(float)theSensoryJointData->angles[JointData::LElbowYaw];
outBehaviorData.jointData[i++] =(float)theSensoryJointData->angles[JointData::LElbowRoll];
outBehaviorData.jointData[i++] =(float)theSensoryJointData->angles[JointData::RHipYawPitch];
outBehaviorData.jointData[i++] =(float)theSensoryJointData->angles[JointData::RHipRoll];
outBehaviorData.jointData[i++] =(float)theSensoryJointData->angles[JointData::RHipPitch];
outBehaviorData.jointData[i++] =(float)theSensoryJointData->angles[JointData::RKneePitch];
outBehaviorData.jointData[i++] =(float)theSensoryJointData->angles[JointData::RAnklePitch];
outBehaviorData.jointData[i++] =(float)theSensoryJointData->angles[JointData::RAnkleRoll];
outBehaviorData.jointData[i++] =(float)theSensoryJointData->angles[JointData::LHipYawPitch];
outBehaviorData.jointData[i++] =(float)theSensoryJointData->angles[JointData::LHipRoll];
outBehaviorData.jointData[i++] =(float)theSensoryJointData->angles[JointData::LHipPitch];
outBehaviorData.jointData[i++] =(float)theSensoryJointData->angles[JointData::LKneePitch];
outBehaviorData.jointData[i++] =(float)theSensoryJointData->angles[JointData::LAnklePitch];
outBehaviorData.jointData[i++] =(float)theSensoryJointData->angles[JointData::LAnkleRoll];
// numSet = i;
// if(numSet == OutBehaviorData::NUM_JOINT_DATA)
// std::cout<<"Num of Data Set missmatch with NUM_JOINT_DATA: NumSet: "<<numSet<<", NUM_JOINT_DATA: "<<OutBehaviorData::NUM_JOINT_DATA<<std::endl;
// printf("%d joint data Set to :\n", outBehaviorData.jointData.size());
// for(i = 0; i < numSet; i++)
// {
// printf("the %d number is ", i);
// std::cout<<outBehaviorData.jointData[i]<<std::endl;
// }
RotationMatrix Rsupport;
Rsupport = theNaoStructure->uLink[NaoStructure::bodyLink].R.invert();
Pose3D Tsupport;
Tsupport.rotation= Rsupport;
Tsupport.translate(theNaoStructure->uLink[NaoStructure::bodyLink].p*(-1));
if (theNaoStructure->supportingMode ==NaoConfig::SUPPORT_MODE_LEFT || theNaoStructure->supportingMode == NaoConfig::SUPPORT_MODE_DOUBLE_LEFT)
{
outBehaviorData.leftx = Tsupport.translation.x*1000;
outBehaviorData.lefty = Tsupport.translation.y*1000;
Pose3D Tswing;
Tswing.rotation = Rsupport;
Tswing.translate(theNaoStructure->uLink[NaoStructure::rFootLink].p-theNaoStructure->uLink[NaoStructure::bodyLink].p);
outBehaviorData.rightx = Tswing.translation.x*1000;
outBehaviorData.righty = Tswing.translation.y*1000;
}
else
{
outBehaviorData.rightx = Tsupport.translation.x*1000;
outBehaviorData.righty = Tsupport.translation.y*1000;
Pose3D Tswing;
Tswing.rotation = Rsupport;
Tswing.translate(theNaoStructure->uLink[NaoStructure::lFootLink].p-theNaoStructure->uLink[NaoStructure::bodyLink].p);
outBehaviorData.leftx = Tswing.translation.x*1000;
outBehaviorData.lefty = Tswing.translation.y*1000;
}
}
