// 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 TrajectoryTrackingTest::doUpdate(double time, wocra::wOcraModel& state, void** args)
{
if (isDisplacementd)
{
wocra::wOcraSegPoseTaskManager* tmp_tmLeftHandPose = dynamic_cast<wocra::wOcraSegPoseTaskManager*>(taskManagers["tmLeftHandPose"]);
Eigen::Displacementd desiredPose;
Eigen::Twistd desiredVelocity;
Eigen::Twistd desiredAcceleration;
leftHandTrajectory->getDesiredValues(time, desiredPose, desiredVelocity, desiredAcceleration);
tmp_tmLeftHandPose->setState(desiredPose, desiredVelocity, desiredAcceleration);
// tmp_tmLeftHandPose->setState(desiredPose, desiredVelocity, Eigen::Twistd::Zero());
std::cout << "\nFinal desired pose: " << endingDispd << std::endl;
std::cout << "Desired pose: " << desiredPose << std::endl;
std::cout << "Desired vel: " << desiredVelocity.transpose() << std::endl;
std::cout << "Desired acc: " << desiredAcceleration.transpose() << std::endl;
std::cout << "Current pose: " << state.getSegmentPosition(lHandIndex) << std::endl;
std::cout << "Error: " << tmp_tmLeftHandPose->getTaskError().transpose() << " norm: " << tmp_tmLeftHandPose->getTaskErrorNorm() << std::endl;
}
else if (isRotation3d)
{
wocra::wOcraSegOrientationTaskManager* tmp_tmLeftHandOrient = dynamic_cast<wocra::wOcraSegOrientationTaskManager*>(taskManagers["tmLeftHandOrient"]);
Eigen::Rotation3d desiredOrientation;
leftHandTrajectory->getDesiredValues(time, desiredOrientation);
tmp_tmLeftHandOrient->setOrientation(desiredOrientation);
std::cout << "\nFinal desired orientation: " << endingRotd << std::endl;
std::cout << "Desired orientation: " << desiredOrientation << std::endl;
std::cout << "Current orientation: " << state.getSegmentPosition(lHandIndex).getRotation() << std::endl;
std::cout << "Error: " << tmp_tmLeftHandOrient->getTaskError().transpose() << " norm: " << tmp_tmLeftHandOrient->getTaskErrorNorm() << std::endl;
}
else if (isCartesion || isCartesionWaypoints)
{
wocra::wOcraSegCartesianTaskManager* tmp_tmLeftHandCart = dynamic_cast<wocra::wOcraSegCartesianTaskManager*>(taskManagers["tmLeftHandCart"]);
Eigen::MatrixXd desiredPosVelAcc = leftHandTrajectory->getDesiredValues(time);
// Eigen::MatrixXd H_adj = state.getSegmentPosition(lHandIndex).getRotation().inverse().adjoint();
// H_adj*
tmp_tmLeftHandCart->setState(desiredPosVelAcc.col(0));//, desiredPosVelAcc.col(1), desiredPosVelAcc.col(2));
// if(isCartesion){std::cout << "\nFinal desired position: " << desiredPos.transpose() << std::endl;}
// std::cout << "\nDesired position: " << desiredPosVelAcc.col(0).transpose() << std::endl;
// std::cout << "Current position: " << state.getSegmentPosition(lHandIndex).getTranslation().transpose()<< std::endl;
// std::cout << "Error: " << tmp_tmLeftHandCart->getTaskError().transpose() << " norm: " << tmp_tmLeftHandCart->getTaskErrorNorm() << std::endl;
}
}
void CartesianTest::doUpdate(double time, wocra::wOcraModel& state, void** args)
{
wocra::wOcraSegCartesianTaskManager* tmp_tmLeftHandCart = dynamic_cast<wocra::wOcraSegCartesianTaskManager*>(taskManagers["tmLeftHandCart"]);
// tmLeftHandCart->setPosition(desiredPos);
std::cout << "\n---\nDesired position: " << desiredPos.transpose() << std::endl;
std::cout << "Current pose: " << state.getSegmentPosition(lHandIndex).getTranslation().transpose()<< std::endl;
std::cout << "Error: " << tmp_tmLeftHandCart->getTaskError().transpose() << "\n" << std::endl;
}
void PoseTest::doUpdate(double time, wocra::wOcraModel& state, void** args)
{
wocra::wOcraSegPoseTaskManager* tmp_tmLeftHandPose = dynamic_cast<wocra::wOcraSegPoseTaskManager*>(taskManagers["tmLeftHandPose"]);
// tmLeftHandCart->setPosition(desiredPos);
std::cout << "\n---\nDesired pose: " << endingDispd << std::endl;
std::cout << "Current pose: " << state.getSegmentPosition(lHandIndex)<< std::endl;
std::cout << "Error: " << tmp_tmLeftHandPose->getTaskError().transpose() << "\n" << std::endl;
// tmLeftHandPose->printTaskErrors();
}
// 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);
}
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 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);
}