示例#1
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;
}
示例#2
0
bool Transformation::robotWithCameraRotationToImage(const Vector2<>& point,
    const CameraMatrix& cameraMatrix, const CameraInfo& cameraInfo,
    Vector2<>& pointInImage)
{
  Pose3D cameraRotatedMatrix;
  cameraRotatedMatrix.rotateZ(cameraMatrix.rotation.getZAngle());
  const Vector3<> point3D(point.x, point.y, 0.f);
  return robotToImage(cameraRotatedMatrix * point3D, cameraMatrix, cameraInfo, pointInImage);
}
示例#3
0
bool Transformation::imageToRobotWithCameraRotation(const Vector2<int>& pointInImage,
    const CameraMatrix& cameraMatrix, const CameraInfo& cameraInfo,
    Vector2<>& relativePosition)
{
  const bool ret = imageToRobot(pointInImage, cameraMatrix, cameraInfo, relativePosition);
  if(ret)
  {
    Pose3D cameraRotatedMatrix;
    cameraRotatedMatrix.rotateZ(cameraMatrix.rotation.getZAngle());
    const Vector3<> point3D = cameraRotatedMatrix.invert() * Vector3<>(relativePosition.x, relativePosition.y, 0.f);
    relativePosition.x = point3D.x;
    relativePosition.y = point3D.y;
  }
  return ret;
}