void Node::localRefinement()
{
DenseVector sol = stdToEigenVec(solLowerBound);
constraints->localRefinement(sol);
}
arma::vec CustomKinestheticTeacher::getNextDifferentialCommand(Eigen::MatrixXd jacobian,arma::vec currentJointState, ControllerType myType){
std::vector<double> sensorReading= armadilloToStdVec(robotinoQueue->getCurrentProcessedCartesianFrcTrq().joints);
auto numberOfCartesianFTs=jacobian.rows();
if (numberOfCartesianFTs != sensorReading.size()){
cout << "Problem in sensor readings vector size" << endl;
return stdToArmadilloVec({0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0});
}
std::vector<double> forceVector = scaleForcesTorques(sensorReading);
std::vector<double> additiveDifferential;
switch(myType){
case JACOBIAN:
{
additiveDifferential = eigenToStdVec(jacobian.transpose() * stdToEigenVec(forceVector));
additiveDifferential= scaleJointCommands(additiveDifferential);
break;
}
case INVERSE://Pseudo inverse..horrible!
{
Eigen::JacobiSVD<Eigen::MatrixXd> svd(jacobian, Eigen::ComputeThinU | Eigen::ComputeThinV);
additiveDifferential= scaleJointCommands(eigenToStdVec(0.15*svd.solve(stdToEigenVec(forceVector))));
break;
}
case IK:
{
const double scaleIK=0.2;
forceVector = armadilloToStdVec( scaleIK*stdToArmadilloVec(forceVector));
geometry_msgs::Pose currentPose = mvKin->computeFk(armadilloToStdVec(currentJointState));
geometry_msgs::Pose newPose;
newPose.position.x= currentPose.position.x + forceVector.at(0);
newPose.position.y=currentPose.position.y + forceVector.at(1);
newPose.position.z=currentPose.position.z + forceVector.at(2);
tf::Quaternion quat(currentPose.orientation.x,currentPose.orientation.y,currentPose.orientation.z,currentPose.orientation.w);
tf::Matrix3x3 m(quat);
double roll,pitch,yaw;
m.getRPY(roll,pitch,yaw);
roll+= forceVector.at(3);
pitch+= forceVector.at(4);
yaw+= forceVector.at(5);
newPose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
auto planIK = mvKin->computeIk(armadilloToStdVec(currentJointState),newPose);
auto target =currentJointState;
if (planIK.size()>0)
target=planIK.back();
else
cout << "IK solution not found" << endl;
additiveDifferential = armadilloToStdVec( target - currentJointState);
if (isBigJump(additiveDifferential)){
additiveDifferential={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
cout << " Big Jump from IK solution" << endl;
}
break;
}
case HYBRID:
{
const double scaleIK=0.1;
additiveDifferential={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
forceVector = armadilloToStdVec( scaleIK*stdToArmadilloVec(forceVector));
geometry_msgs::Pose currentPose = mvKin->computeFk(armadilloToStdVec(currentJointState));
geometry_msgs::Pose newPose;
newPose.position.x= currentPose.position.x + forceVector.at(0);
newPose.position.y=currentPose.position.y + forceVector.at(1);
newPose.position.z=currentPose.position.z + forceVector.at(2);
tf::Quaternion quat(currentPose.orientation.x,currentPose.orientation.y,currentPose.orientation.z,currentPose.orientation.w);
tf::Matrix3x3 m(quat);
double roll,pitch,yaw;
m.getRPY(roll,pitch,yaw);
roll+= forceVector.at(3);
pitch+= forceVector.at(4);
yaw+= forceVector.at(5);
newPose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
auto planIK = mvKin->computeIk(armadilloToStdVec(currentJointState),newPose);
auto target =currentJointState;
bool jacobianUse = false;
if (planIK.size()>0){
target=planIK.back();
additiveDifferential = armadilloToStdVec( target - currentJointState);
}
else
jacobianUse =true;
if (isBigJump(additiveDifferential))
jacobianUse =true;
if (jacobianUse){
additiveDifferential = eigenToStdVec(jacobian.transpose() * stdToEigenVec(forceVector));
additiveDifferential= scaleJointCommands(additiveDifferential);
cout << " jacobian" << endl;
}
break;
}
}
return stdToArmadilloVec(capVec(additiveDifferential,MAXIMUM_JOINT_STEP));
}
