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)); }