// namespace sequence{
void JointTest::doInit(wocra::wOcraController& ctrl, wocra::wOcraModel& model)
{
ocraWbiModel& wbiModel = dynamic_cast<ocraWbiModel&>(model);
// Full posture task
nDoF = model.nbInternalDofs();
// Eigen::VectorXd nominal_q = Eigen::VectorXd::Zero(nDoF);
// getNominalPosture(model, nominal_q);
q_init = model.getJointPositions();
q_des = q_init;
jointMin = model.getJointLowerLimits();
jointMax = model.getJointUpperLimits();
taskManagers["tmFull"] = new wocra::wOcraFullPostureTaskManager(ctrl, model, "fullPostureTask", ocra::FullState::INTERNAL, 20.0, 2.0*sqrt(20), 1.0, q_init);
for (int i=0; i<nDoF; i++){
jointNames[i] = wbiModel.getJointName(i);
}
taskErr = 0.0;
jIndex = 0;
goToMin = true;
goToMax = false;
counter = 401;
}
void Sequence_iCub_01_Standing::doInit(wocra::wOcraController& ctrl, wocra::wOcraModel& model)
{
// Initialise full posture task
Eigen::VectorXd q_full = Eigen::VectorXd::Zero(model.nbInternalDofs());
q_full[model.getDofIndex("l_elbow_pitch")] = PI/8.0;
q_full[model.getDofIndex("r_elbow_pitch")] = PI/8.0;
q_full[model.getDofIndex("l_knee")] = -0.05;
q_full[model.getDofIndex("r_knee")] = -0.05;
q_full[model.getDofIndex("l_ankle_pitch")] = -0.05;
q_full[model.getDofIndex("r_ankle_pitch")] = -0.05;
q_full[model.getDofIndex("l_shoulder_roll")] = PI/8.0;
q_full[model.getDofIndex("r_shoulder_roll")] = PI/8.0;
taskManagers["tmFull"] = new wocra::wOcraFullPostureTaskManager(ctrl, model, "fullPostureTask", ocra::FullState::INTERNAL, 9.0, 2*sqrt(9.0), 0.0001, q_full, false);
// Initialise waist pose
taskManagers["tmSegPoseWaist"] = new wocra::wOcraSegPoseTaskManager(ctrl, model, "waistPoseTask", "waist", ocra::XYZ, 36.0, 2*sqrt(36.0), 1.0, Eigen::Displacementd(0.0,0.0,0.58,-M_SQRT1_2,0.0,0.0,M_SQRT1_2), false);
// Initialise partial posture task
Eigen::VectorXi sdofs(3);
sdofs << model.getDofIndex("torso_pitch"), model.getDofIndex("torso_roll"), model.getDofIndex("torso_yaw");
Eigen::VectorXd zero = Eigen::VectorXd::Zero(3);
taskManagers["tmPartialBack"] = new wocra::wOcraPartialPostureTaskManager(ctrl, model, "partialPostureBackTask", ocra::FullState::INTERNAL, sdofs, 16.0, 2*sqrt(16.0), 0.001, zero, false);
double mu_sys = 0.5;
double margin = 0.0;
double sqrt2on2 = sqrt(2.0)/2.0;
Eigen::Rotation3d rotLZdown = Eigen::Rotation3d(-sqrt2on2, 0.0, -sqrt2on2, 0.0) * Eigen::Rotation3d(0.0, 1.0, 0.0, 0.0);
Eigen::Rotation3d rotRZdown = Eigen::Rotation3d(0.0, sqrt2on2, 0.0, sqrt2on2) * Eigen::Rotation3d(0.0, 1.0, 0.0, 0.0);
// Initialise left foot contacts
std::vector<Eigen::Displacementd> LFContacts;
LFContacts.push_back(Eigen::Displacementd(Eigen::Vector3d(-.039,-.027,-.031), rotLZdown));
LFContacts.push_back(Eigen::Displacementd(Eigen::Vector3d(-.039, .027,-.031), rotLZdown));
LFContacts.push_back(Eigen::Displacementd(Eigen::Vector3d(-.039, .027, .099), rotLZdown));
LFContacts.push_back(Eigen::Displacementd(Eigen::Vector3d(-.039,-.027, .099), rotLZdown));
taskManagers["tmFootContactLeft"] = new wocra::wOcraContactSetTaskManager(ctrl, model, "leftFootContactTask", "l_foot", LFContacts, mu_sys, margin, false);
// Initailise right foot contacts
std::vector<Eigen::Displacementd> RFContacts;
RFContacts.push_back(Eigen::Displacementd(Eigen::Vector3d(-.039,-.027, .031), rotRZdown));
RFContacts.push_back(Eigen::Displacementd(Eigen::Vector3d(-.039, .027, .031), rotRZdown));
RFContacts.push_back(Eigen::Displacementd(Eigen::Vector3d(-.039, .027,-.099), rotRZdown));
RFContacts.push_back(Eigen::Displacementd(Eigen::Vector3d(-.039,-.027,-.099), rotRZdown));
taskManagers["tmFootContactRight"] = new wocra::wOcraContactSetTaskManager(ctrl, model, "RightFootContactTask", "r_foot", RFContacts, mu_sys, margin, false);
}
// 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);
}