/* ************************************************************************** */
Pose2MobileArm::Pose2MobileArm(const Arm& arm, const gtsam::Pose3& base_T_arm) :
Base(arm.dof() + 3, arm.dof() + 1), base_T_arm_(base_T_arm), arm_(arm) {
// check arm base pose, warn if non-zero
if (!arm_.base_pose().equals(Pose3::identity(), 1e-6))
cout << "[Pose2MobileArm] WARNING: Arm has non-zero base pose, this base pose will be override. "
"Set the base_T_arm in Pose2MobileArm." << endl;
}
/* ************************************************************************** */
Pose2MobileVetLinArm::Pose2MobileVetLinArm(const Arm& arm, const gtsam::Pose3& base_T_torso,
const gtsam::Pose3& torso_T_arm, bool reverse_linact) :
Base(arm.dof() + 4, arm.dof() + 2), base_T_torso_(base_T_torso), torso_T_arm_(torso_T_arm),
reverse_linact_(reverse_linact), arm_(arm) {
// check arm base pose, warn if non-zero
if (!arm_.base_pose().equals(Pose3::identity(), 1e-6))
cout << "[Pose2MobileArm] WARNING: Arm has non-zero base pose, this base pose will be override. "
"Set the base_T_torso and torso_T_arm in Pose2MobileArm." << endl;
}