void RosSchunk::moveJoints(arma::vec positions) {
std::vector<double> stdPos = armadilloToStdVec(positions);
std_msgs::Float64MultiArray newJoints;
for(int i = 0; i < stdPos.size(); ++i) {
if(stdPos.at(i) != SDH_IGNORE_JOINT)
newJoints.data.push_back(stdPos.at(i));
else
newJoints.data.push_back(currentCommandedPos.at(i));
}
currentCommandedPos = newJoints.data;
// get current position
ros::spinOnce();
targetReached = false;
movementStarted = false;
initCurrentPos = currentPos;
trajPub.publish(newJoints);
if(waitForReached) {
while(!targetReached)
ros::spinOnce();
}
}
bool CustomKinestheticTeacher::isDifferentialCommandSafe(arma::vec diffCommand,arma::vec currentJointStates){
bool okay=true;
int checkTimesAhead=3;
for (auto i = 1;i<=checkTimesAhead;i++)
if (isColliding(armadilloToStdVec(i*diffCommand + currentJointStates)))
okay=false;
return okay;
}
void KukieControlQueue::computeCurrentCartPose() {
if(!kinematicsInitialized) {
kin = KUKADU_SHARED_PTR<Kinematics>(new KomoPlanner(shared_from_this(),
resolvePath("$KUKADU_HOME/external/komo/share/data/kuka/data/iis_robot.kvg"),
resolvePath("$KUKADU_HOME/external/komo/share/data/kuka/config/MT.cfg"),
getRobotSidePrefix(), acceptCollisions));
kinematicsInitialized = true;
}
while(getQueueRunning()) {
forwadKinMutex.lock();
currCarts = kin->computeFk(armadilloToStdVec(getCurrentJoints().joints));
forwadKinMutex.unlock();
}
}
std::vector<arma::vec> Kinematics::computeIk(arma::vec currentJointState, const geometry_msgs::Pose &goal) {
return computeIk(armadilloToStdVec(currentJointState), goal);
}
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));
}
