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