LegMotion::LegMotion(std::shared_ptr<ThreadManager> threadManager_ptr, std::shared_ptr<MotorControl> mc_ptr, XmlParser& config, const bool isMotorActivated):
m_bIsMotorActivated(isMotorActivated),
m_threadManager(threadManager_ptr),
m_motion(mc_ptr)
{
float distanceThreshold = config.getIntValue(XmlPath::LegsMotors / XmlPath::DISTANCETHRESHOLD);
float angleThreshold = config.getIntValue(XmlPath::LegsMotors / XmlPath::ANGLETHRESHOLD);
float maxPosError = config.getIntValue(XmlPath::LegsMotors / XmlPath::MaxPosError);
int iterationMax = config.getIntValue(XmlPath::LegsMotors / XmlPath::ITERATIONMAX);
m_motionControl = std::make_shared<MotionControl>(distanceThreshold, angleThreshold, maxPosError, iterationMax);
m_stepHeight = config.getIntValue(XmlPath::LegsMotors / XmlPath::StepHeight);
m_stepLength = config.getIntValue(XmlPath::LegsMotors / XmlPath::StepLength);
m_stepTime = config.getIntValue(XmlPath::LegsMotors / XmlPath::StepTime);
m_pelvisTrajectoryType = config.getIntValue(XmlPath::LegsMotors / XmlPath::UseCOM) == 0 ? Trajectory::ZMP : Trajectory::COM;
//Compensation offsets
float RightPelvisPitchCompensationOffset = config.getIntValue(XmlPath::LegsMotors / XmlPath::RightPelvisPitchCompensationOffset);
float RightPelvisRollCompensationOffset = config.getIntValue(XmlPath::LegsMotors / XmlPath::RightPelvisRollCompensationOffset);
float RightPelvisYawCompensationOffset = config.getIntValue(XmlPath::LegsMotors / XmlPath::RightPelvisYawCompensationOffset);
float RightPelvisxCompensationOffset = config.getIntValue(XmlPath::LegsMotors / XmlPath::RightPelvisxCompensationOffset);
float RightPelvisyCompensationOffset = config.getIntValue(XmlPath::LegsMotors / XmlPath::RightPelvisyCompensationOffset);
float RightPelviszCompensationOffset = config.getIntValue(XmlPath::LegsMotors / XmlPath::RightPelviszCompensationOffset);
float LeftPelvisPitchCompensationOffset = config.getIntValue(XmlPath::LegsMotors / XmlPath::LeftPelvisPitchCompensationOffset);
float LeftPelvisRollCompensationOffset = config.getIntValue(XmlPath::LegsMotors / XmlPath::LeftPelvisRollCompensationOffset);
float LeftPelvisYawCompensationOffset = config.getIntValue(XmlPath::LegsMotors / XmlPath::LeftPelvisYawCompensationOffset);
float LeftPelvisxCompensationOffset = config.getIntValue(XmlPath::LegsMotors / XmlPath::LeftPelvisxCompensationOffset);
float LeftPelvisyCompensationOffset = config.getIntValue(XmlPath::LegsMotors / XmlPath::LeftPelvisyCompensationOffset);
float LeftPelviszCompensationOffset = config.getIntValue(XmlPath::LegsMotors / XmlPath::LeftPelviszCompensationOffset);
float RightFootPitchCompensationOffset = config.getIntValue(XmlPath::LegsMotors / XmlPath::RightFootPitchCompensationOffset);
float RightFootRollCompensationOffset = config.getIntValue(XmlPath::LegsMotors / XmlPath::RightFootRollCompensationOffset);
float RightFootYawCompensationOffset = config.getIntValue(XmlPath::LegsMotors / XmlPath::RightFootYawCompensationOffset);
float RightFootxCompensationOffset = config.getIntValue(XmlPath::LegsMotors / XmlPath::RightFootxCompensationOffset);
float RightFootyCompensationOffset = config.getIntValue(XmlPath::LegsMotors / XmlPath::RightFootyCompensationOffset);
float RightFootzCompensationOffset = config.getIntValue(XmlPath::LegsMotors / XmlPath::RightFootzCompensationOffset);
float LeftFootPitchCompensationOffset = config.getIntValue(XmlPath::LegsMotors / XmlPath::LeftFootPitchCompensationOffset);
float LeftFootRollCompensationOffset = config.getIntValue(XmlPath::LegsMotors / XmlPath::LeftFootRollCompensationOffset);
float LeftFootYawCompensationOffset = config.getIntValue(XmlPath::LegsMotors / XmlPath::LeftFootYawCompensationOffset);
float LeftFootxCompensationOffset = config.getIntValue(XmlPath::LegsMotors / XmlPath::LeftFootxCompensationOffset);
float LeftFootyCompensationOffset = config.getIntValue(XmlPath::LegsMotors / XmlPath::LeftFootyCompensationOffset);
float LeftFootzCompensationOffset = config.getIntValue(XmlPath::LegsMotors / XmlPath::LeftFootzCompensationOffset);
float PelvisKickOffsetRX = config.getIntValue(XmlPath::LegsMotors / XmlPath::PelvisKickOffsetRX);
float PelvisKickOffsetRY = config.getIntValue(XmlPath::LegsMotors / XmlPath::PelvisKickOffsetRY);
float KickBackOffsetRY = config.getIntValue(XmlPath::LegsMotors / XmlPath::KickBackOffsetRY);
float KickBackOffsetRZ = config.getIntValue(XmlPath::LegsMotors / XmlPath::KickBackOffsetRZ);
float KickForwardOffsetRY = config.getIntValue(XmlPath::LegsMotors / XmlPath::KickForwardOffsetRY);
float KickForwardOffsetRZ = config.getIntValue(XmlPath::LegsMotors / XmlPath::KickForwardOffsetRZ);
m_vRightFootPosOffset = Eigen::Vector3f(RightFootxCompensationOffset, RightFootyCompensationOffset, RightFootzCompensationOffset);
m_vRightFootAngleOffset = Eigen::Vector3f(RightFootPitchCompensationOffset, RightFootRollCompensationOffset, RightFootYawCompensationOffset);
m_vLeftFootPosOffset = Eigen::Vector3f(LeftFootxCompensationOffset, LeftFootyCompensationOffset, LeftFootzCompensationOffset);
m_vLeftFootAngleOffset = Eigen::Vector3f(LeftFootPitchCompensationOffset, LeftFootRollCompensationOffset, LeftFootYawCompensationOffset);
m_vRightPelvisPosOffset = Eigen::Vector3f(RightPelvisxCompensationOffset, RightPelvisyCompensationOffset, RightPelviszCompensationOffset);
m_vRightPelvisAngleOffset = Eigen::Vector3f(RightPelvisPitchCompensationOffset, RightPelvisRollCompensationOffset, RightPelvisYawCompensationOffset);
m_vLeftPelvisPosOffset = Eigen::Vector3f(LeftPelvisxCompensationOffset, LeftPelvisyCompensationOffset, LeftPelviszCompensationOffset);
m_vLeftPelvisAngleOffset = Eigen::Vector3f(LeftPelvisPitchCompensationOffset, LeftPelvisRollCompensationOffset, LeftPelvisYawCompensationOffset);
m_PelvisKickOffsetR = Eigen::Vector3f(PelvisKickOffsetRX, PelvisKickOffsetRY, 0);
m_KickBackOffsetR = Eigen::Vector3f(0, KickBackOffsetRY, KickBackOffsetRZ);
m_KickForwardOffsetR = Eigen::Vector3f(0, KickForwardOffsetRY, KickForwardOffsetRZ);
m_pelvisPermanentPitch = config.getIntValue(XmlPath::LegsMotors / XmlPath::PermanentPelvisPitch);
m_vInitialPosition = m_motionControl->GetInitialQPosition();
}
