/* 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;
}
