void RosSchunk::closeHand(double percentage, double velocity) {
if(percentage >= 0.0 && percentage <= 1.1) {
vector<double> hand_pose;
switch(currentGraspId) {
case eGID_CENTRICAL:
hand_pose = generateCentricalPose(percentage);
break;
case eGID_CYLINDRICAL:
hand_pose = generateCylindricalPose(percentage);
break;
case eGID_PARALLEL:
hand_pose = generateParallelPose(percentage);
break;
case eGID_SPHERICAL:
hand_pose = generateSphericalPose(percentage);
break;
default:
string msg = "(RosSchunk) grasp type not supported";
cerr << msg << endl;
throw KukaduException(msg.c_str());
}
moveJoints(stdToArmadilloVec(hand_pose));
} else {
string msg = "(RosSchunk) grasp percentage out of range";
cerr << msg << endl;
throw KukaduException(msg.c_str());
}
}
mes_result PlottingControlQueue::getCurrentCartesianPos() {
mes_result res;
res.time = 0.0;
res.joints = stdToArmadilloVec(createJointsVector(7, fakeCurrentPose.position.x, fakeCurrentPose.position.y, fakeCurrentPose.position.z,
fakeCurrentPose.orientation.x, fakeCurrentPose.orientation.y, fakeCurrentPose.orientation.z, fakeCurrentPose.orientation.w));
return res;
}
void KukieControlQueue::jntFrcTrqCallback(const std_msgs::Float64MultiArray& msg) {
if(isRealRobot)
currentJntFrqTrq = stdToArmadilloVec(msg.data);
else {
currentJntFrqTrq = vec(getMovementDegreesOfFreedom());
currentJntFrqTrq.fill(0.0);
}
}
void RosSchunk::publishSingleJoint(int idx, double pos) {
vector<double> command;
for(int i = 0; i < currentCommandedPos.size(); ++i)
if(i == idx)
command.push_back(pos);
else
command.push_back(SDH_IGNORE_JOINT);
moveJoints(stdToArmadilloVec(command));
}
pcl::PointCloud<pcl::PointXYZ>::Ptr PlanarCutTransformator::transformPc(pcl::PointCloud<pcl::PointXYZ>::Ptr pc) {
pcl::PointCloud<pcl::PointXYZ> retPc;
pcl::PointCloud<pcl::PointXYZ>::iterator pointIt = pc->begin();
pcl::PointCloud<pcl::PointXYZ>::iterator lastIt = pc->end();
while(pointIt != lastIt) {
pcl::PointXYZ currentPoint = *pointIt;
vec r = stdToArmadilloVec(createJointsVector(3, currentPoint.x, currentPoint.y, currentPoint.z));
vec comp = normalVec.t() * (r - plainOriginVec);
double coordinate = comp(0);
if(coordinate >= 0) {
retPc.push_back(*pointIt);
} else {
// point gets kicked out
}
++pointIt;
}
return retPc.makeShared();
}
RosSchunk::RosSchunk(ros::NodeHandle node, std::string type, std::string hand) {
this->node = node;
waitForReached = true;
trajPub = node.advertise<std_msgs::Float64MultiArray>(string("/") + type + string("/") + hand + "_sdh/joint_control/move", 1);
this->hand = hand;
stateSub = node.subscribe(string("/") + type + string("/") + hand + "_sdh/joint_control/get_state", 1, &RosSchunk::stateCallback, this);
tactileSub = node.subscribe(string("/") + type + string("/") + hand + "_sdh/sensoring/tactile", 1, &RosSchunk::tactileCallback, this);
previousCurrentPosQueueSize = 10;
isFirstCallback = true;
ros::Rate r(5);
while(isFirstCallback) {
ros::spinOnce();
r.sleep();
}
currentCommandedPos = currentPos;
currentGraspId = eGID_PARALLEL;
moveJoints(stdToArmadilloVec(currentPos));
}
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));
}
bool CustomKinestheticTeacher::isColliding(std::vector<double> jointStates){
auto pose = mvKin->computeFk(jointStates);
return mvKin->isColliding(stdToArmadilloVec(jointStates),pose);
}
