/* static */ bool ocraWbiConversions::eigenDispdToWbiFrame(const Eigen::Displacementd &disp, wbi::Frame &frame) { double p_wbi[3]; Eigen::Vector3d p; p = disp.getTranslation(); p_wbi[0] = p[0]; p_wbi[1] = p[1]; p_wbi[2] = p[2]; double _x, _y, _z, _w; _x = disp.getRotation().x(); _y = disp.getRotation().y(); _z = disp.getRotation().z(); _w = disp.getRotation().w(); wbi::Rotation R; R.quaternion(_x, _y, _z, _w); wbi::Frame _frame(R, p_wbi); frame = _frame; // Check if y-translation value is the same between the two data containers if (frame.p[1]!=p(1,0)){ std::cerr << "Displacementd could not be converted to Wbi Frame \n"; return false; } return true; }
/* static */ bool ocraWbiConversions::wbiFrameToEigenDispd(const wbi::Frame &frame, Eigen::Displacementd &disp) { double _x, _y, _z, _w; frame.R.getQuaternion(_x, _y, _z, _w); double x, y, z; x = frame.p[0]; y = frame.p[1]; z = frame.p[2]; Eigen::Vector3d trans; trans << x, y, z; Eigen::Displacementd _disp(x,y,z,_w,_x,_y,_z); disp = _disp; // Check if y-translation value is the same between the two data containers if (disp.getTranslation()(1,0)!=y){ std::cerr << "Wbi Frame could not be converted to Displacementd\n"; return false; } return true; }
// namespace sequence{ void PoseTest::doInit(wocra::wOcraController& ctrl, wocra::wOcraModel& model) { ocraWbiModel& wbiModel = dynamic_cast<ocraWbiModel&>(model); // Task Coeffs double Kp = 10.0; double Kd = 2.0 * sqrt(Kp); double wFullPosture = 0.001; double wPartialPosture = 0.01; double wLeftHandTask = 1.0; // Full posture task Eigen::VectorXd nominal_q = Eigen::VectorXd::Zero(model.nbInternalDofs()); getNominalPosture(model, nominal_q); taskManagers["tmFull"] = new wocra::wOcraFullPostureTaskManager(ctrl, model, "fullPostureTask", ocra::FullState::INTERNAL, Kp, Kd, wFullPosture, nominal_q); // Partial (torso) posture task Eigen::VectorXi torso_indices(3); Eigen::VectorXd torsoTaskPosDes(3); torso_indices << wbiModel.getDofIndex("torso_pitch"), wbiModel.getDofIndex("torso_roll"), wbiModel.getDofIndex("torso_yaw"); torsoTaskPosDes << M_PI / 18, 0, 0; taskManagers["tmPartialTorso"] = new wocra::wOcraPartialPostureTaskManager(ctrl, model, "partialPostureTorsoTask", ocra::FullState::INTERNAL, torso_indices, Kp, Kd, wPartialPosture, torsoTaskPosDes); lHandIndex = model.getSegmentIndex("l_hand"); Eigen::Vector3d YZ_disp; YZ_disp << 0.0, 0.2, 0.2; Eigen::Displacementd startingDispd = model.getSegmentPosition(lHandIndex); std::cout << startingDispd << std::endl; // start and end positions Eigen::Vector3d desiredPos = startingDispd.getTranslation() + YZ_disp; // Eigen::Rotation3d desiredOrientation = Eigen::Rotation3d(1.0, 0.0, 0.0, 0.0); Eigen::Rotation3d desiredOrientation = startingDispd.getRotation(); endingDispd = Eigen::Displacementd(desiredPos, desiredOrientation);//, startingDispd.getRotation());//.inverse()); // endingDispd = startingDispd; std::cout << endingDispd << std::endl; // Left hand cartesian task taskManagers["tmLeftHandPose"] = new wocra::wOcraSegPoseTaskManager(ctrl, model, "leftHandPoseTask", "l_hand", ocra::XYZ, Kp, Kd, wLeftHandTask, endingDispd); }
// namespace sequence{ void TrajectoryTrackingTest::doInit(wocra::wOcraController& ctrl, wocra::wOcraModel& model) { ocraWbiModel& wbiModel = dynamic_cast<ocraWbiModel&>(model); // Task Coeffs double Kp = 20.0; double Kd = 4.0 * sqrt(Kp); double Kp_hand = 40.0; double Kd_hand = 2.0 *sqrt(Kp_hand); double wFullPosture = 0.0001; double wPartialPosture = 0.1; double wLeftHandTask = 1.0; // Full posture task Eigen::VectorXd nominal_q = Eigen::VectorXd::Zero(model.nbInternalDofs()); getNominalPosture(model, nominal_q); taskManagers["tmFull"] = new wocra::wOcraFullPostureTaskManager(ctrl, model, "fullPostureTask", ocra::FullState::INTERNAL, Kp, Kd, wFullPosture, nominal_q); // Partial (torso) posture task Eigen::VectorXi torso_indices(3); Eigen::VectorXd torsoTaskPosDes(3); torso_indices << wbiModel.getDofIndex("torso_pitch"), wbiModel.getDofIndex("torso_roll"), wbiModel.getDofIndex("torso_yaw"); torsoTaskPosDes << 0, -10.0*(M_PI / 180.0), 40.0*(M_PI / 180.0); // torsoTaskPosDes << 0.0, 0.0, 0.0; taskManagers["tmPartialTorso"] = new wocra::wOcraPartialPostureTaskManager(ctrl, model, "partialPostureTorsoTask", ocra::FullState::INTERNAL, torso_indices, 6., 2.0 * sqrt(6.), wPartialPosture, torsoTaskPosDes); // Right hand cartesian task Eigen::Vector3d posRHandDesDelta(0.1, 0.08, 0.15); Eigen::Vector3d posRHandDes = model.getSegmentPosition(model.getSegmentIndex("r_hand")).getTranslation(); posRHandDes = posRHandDes + posRHandDesDelta; taskManagers["tmSegCartHandRight"] = new wocra::wOcraSegCartesianTaskManager(ctrl, model, "rightHandCartesianTask", "r_hand", ocra::XYZ, Kp_hand, Kd_hand, 1.0, posRHandDes); /** * Left hand task. Pick one of these booleans to test the different constructors. */ //*************** Type of Trajectory ******************// bool isLinInterp = false; bool isMinJerk = true; //*****************************************************// //***************** Type of Reference ******************// isDisplacementd = false; isRotation3d = false; isCartesion = false; isCartesionWaypoints = true; //*****************************************************// int boolSum = isCartesion + isCartesionWaypoints + isDisplacementd + isRotation3d; if (boolSum>1){std::cout << "\nYou picked too many reference types." << std::endl;} else if (boolSum<1){std::cout << "\nYou picked too few reference types." << std::endl;} lHandIndex = model.getSegmentIndex("l_hand"); Eigen::Vector3d YZ_disp; YZ_disp << -0.15, 0.1, 0.2; // start and end displacements & rotations Eigen::Displacementd startingDispd = model.getSegmentPosition(lHandIndex); Eigen::Rotation3d startingRotd = startingDispd.getRotation(); endingRotd = Eigen::Rotation3d(0.105135, 0.0828095, 0.253438, -0.958049); endingDispd = Eigen::Displacementd(startingDispd.getTranslation() + YZ_disp, endingRotd); // start and end positions Eigen::Vector3d startingPos = startingDispd.getTranslation(); desiredPos = startingPos + YZ_disp; // multiple position waypoints Eigen::MatrixXd waypoints(3,5); Eigen::MatrixXd squareDisplacement(3,5); waypoints << startingPos, startingPos, startingPos, startingPos, startingPos; squareDisplacement << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.15, 0.15, 0.0, 0.0, 0.0, 0.0, 0.25, 0.25, 0.0; waypoints += squareDisplacement; if (isLinInterp) { /** * Linear interpolation trajectory constructor tests: */ leftHandTrajectory = new wocra::wOcraLinearInterpolationTrajectory(); if (isDisplacementd) {leftHandTrajectory->setWaypoints(startingDispd, endingDispd);} else if (isRotation3d) {leftHandTrajectory->setWaypoints(startingRotd, endingRotd);} else if (isCartesion) {leftHandTrajectory->setWaypoints(startingPos, desiredPos);} else if (isCartesionWaypoints) {leftHandTrajectory->setWaypoints(waypoints);} else {std::cout << "\nGotta pick a reference type m**********r!" << std::endl;} } else if (isMinJerk) { /** * Minimum jerk trajectory constructor tests: */ leftHandTrajectory = new wocra::wOcraMinimumJerkTrajectory(); if (isDisplacementd) {leftHandTrajectory->setWaypoints(startingDispd, endingDispd);} else if (isRotation3d) {leftHandTrajectory->setWaypoints(startingRotd, endingRotd);} else if (isCartesion) {leftHandTrajectory->setWaypoints(startingPos, desiredPos);} else if (isCartesionWaypoints) {leftHandTrajectory->setWaypoints(waypoints);} else {std::cout << "\nGotta pick a reference type m**********r!" << std::endl;} } else{std::cout << "\nGotta pick a trajectory type m**********r!" << std::endl;} if (isDisplacementd) {taskManagers["tmLeftHandPose"] = new wocra::wOcraSegPoseTaskManager(ctrl, model, "leftHandPoseTask", "l_hand", ocra::XYZ, Kp_hand, Kd_hand, wLeftHandTask, startingDispd);} else if (isRotation3d) {taskManagers["tmLeftHandOrient"] = new wocra::wOcraSegOrientationTaskManager(ctrl, model, "leftHandOrientationTask", "l_hand", Kp_hand, Kd_hand, wLeftHandTask, startingRotd);} else if (isCartesion) {taskManagers["tmLeftHandCart"] = new wocra::wOcraSegCartesianTaskManager(ctrl, model, "leftHandCartesianTask", "l_hand", ocra::XYZ, Kp_hand, Kd_hand, wLeftHandTask, startingPos);} else if (isCartesionWaypoints) {taskManagers["tmLeftHandCart"] = new wocra::wOcraSegCartesianTaskManager(ctrl, model, "leftHandCartesianTask", "l_hand", ocra::XYZ, Kp_hand, Kd_hand, wLeftHandTask, startingPos);} // tmSegCartHandRight = new wocra::wOcraSegCartesianTaskManager(ctrl, model, "rightHandCartesianTask", "r_hand", ocra::XYZ, Kp_hand, Kd_hand, wLeftHandTask, posRHandDes); }
Eigen::VectorXd TrajectoryThread::getCurrentTaskStateAsVector() { Eigen::VectorXd startVector = Eigen::VectorXd::Zero(weightDimension); ocra::TaskState state = task->getTaskState(); switch (taskType) { case ocra::Task::META_TASK_TYPE::UNKNOWN: { // do nothing }break; case ocra::Task::META_TASK_TYPE::POSITION: { startVector = state.getPosition().getTranslation(); }break; case ocra::Task::META_TASK_TYPE::ORIENTATION: { Eigen::Rotation3d quat = state.getPosition().getRotation(); startVector = Eigen::VectorXd(4); startVector << quat.w(), quat.x(), quat.y(), quat.z(); }break; case ocra::Task::META_TASK_TYPE::POSE: { Eigen::Displacementd disp = state.getPosition(); startVector = Eigen::VectorXd(7); startVector << disp.x(), disp.y(), disp.z(), disp.qw(), disp.qx(), disp.qy(), disp.qz(); }break; case ocra::Task::META_TASK_TYPE::FORCE: { startVector = state.getWrench(); }break; case ocra::Task::META_TASK_TYPE::COM: { startVector = state.getPosition().getTranslation(); }break; case ocra::Task::META_TASK_TYPE::COM_MOMENTUM: { startVector = state.getPosition().getTranslation(); }break; case ocra::Task::META_TASK_TYPE::PARTIAL_POSTURE: { startVector = state.getQ(); }break; case ocra::Task::META_TASK_TYPE::FULL_POSTURE: { startVector = state.getQ(); }break; case ocra::Task::META_TASK_TYPE::PARTIAL_TORQUE: { startVector = state.getTorque(); }break; case ocra::Task::META_TASK_TYPE::FULL_TORQUE: { startVector = state.getTorque(); }break; default: { }break; } return startVector; }