Пример #1
0
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);
}
Пример #2
0
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;
}
Пример #3
0
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;
}
Пример #4
0
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;

	}
	

}