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()); } }
void TogersonMetricLearner::generateD() { int xsCount = xsSet.size(); D = mat(xsCount, xsCount); for(int i = 0; i < xsSet.size(); ++i) { vec x1 = xsSet.at(i); vector<int> idxs = getDRowIdxs(x1); if(xsCount != idxs.size()) { string msg = "(TogersonMetricLearner) provided distances not complete"; cerr << msg << endl; throw KukaduException(msg.c_str()); } int x1Idx = xsSet.find(x1).second - 1; for(int j = 0; j < idxs.size(); ++j) { int x2Idx = xsSet.find(expandedX1s.at(idxs.at(j))).second - 1; D(x1Idx, j) = distances.at(idxs.at(j)); } } }
// TODO: implement == operator int DictionaryTrajectory::operator==(DictionaryTrajectory const& comp) const { string errStr = "(DictionaryTrajectory) == operator not implemted yet"; cerr << errStr << endl; throw KukaduException(errStr.c_str()); }
void Mahalanobis::setM(arma::mat M) { if(M.n_cols != M.n_rows) { string error = "(Mahalanobis) not a squared matrix"; cerr << error << endl; throw KukaduException(error.c_str()); } this->M = M; }
// TODO: adapt DMPGeneralizer to new architecture KUKADU_SHARED_PTR<JointDmp> DMPGeneralizer::generalizeDmp(GenericKernel* trajectoryKernel, GenericKernel* parameterKernel, vec query, double beta) { string errStr = "(DMPGeneralizer) not adapted to new implementation yet"; cerr << errStr << endl; throw KukaduException(errStr.c_str()); vector<vec> dmpCoeffs; int queryCount = dictTraj->getQueryPoints().size(); int degreesOfFreedom = dictTraj->getQueryPoints().at(0).getDmp()->getDegreesOfFreedom(); // int samplePointCount = dictTraj->getQueryPoints().at(0).getDmp()->getDesignMatrix(0).n_rows; vec g(degreesOfFreedom); vector<vec> sampleXs; for(int i = 0; i < queryCount; ++i) sampleXs.push_back(dictTraj->getQueryPoints().at(i).getQueryPoint()); for(int j = 0; j < degreesOfFreedom; ++j) { vector<mat> designMatrices; vector<vec> sampleTs; for(int i = 0; i < queryCount; ++i) { sampleTs.push_back(dictTraj->getQueryPoints().at(i).getDmp()->getFitYs().at(j)); designMatrices.push_back(dictTraj->getQueryPoints().at(i).getDmp()->getDesignMatrix(j)); } LWRRegressor* reg = new LWRRegressor(sampleXs, sampleTs, trajectoryKernel, designMatrices); vec res = reg->fitAtPosition(query); dmpCoeffs.push_back(res); } cout << endl; for(int j = 0; j < degreesOfFreedom; ++j) { vec gs(queryCount); for(int i = 0; i < queryCount; ++i) gs(i) = dictTraj->getQueryPoints().at(i).getDmp()->getG()(j); GaussianProcessRegressor* reg = new GaussianProcessRegressor(sampleXs, gs, parameterKernel, beta); double goal = reg->fitAtPosition(query)(0); g(j) = goal; } return KUKADU_SHARED_PTR<JointDmp>(new JointDmp(vec(), vector<vec>(), vector<vec>(), dmpCoeffs, dictTraj->getQueryPoints().at(0).getDmp()->getDmpBase(), vector<mat>(), dictTraj->getQueryPoints().at(0).getDmp()->getTau(), dictTraj->getQueryPoints().at(0).getDmp()->getAz(), dictTraj->getQueryPoints().at(0).getDmp()->getBz(), dictTraj->getQueryPoints().at(0).getDmp()->getAx())); }
PoWER::PoWER(KUKADU_SHARED_PTR<TrajectoryExecutor> trajEx, std::vector<KUKADU_SHARED_PTR<Trajectory> > initDmp, vector<double> explorationSigmas, int updatesPerRollout, int importanceSamplingCount, KUKADU_SHARED_PTR<CostComputer> cost, KUKADU_SHARED_PTR<ControlQueue> simulationQueue, KUKADU_SHARED_PTR<ControlQueue> executionQueue, double ac, double dmpStepSize, double tolAbsErr, double tolRelErr, unsigned seed) : GeneralReinforcer(trajEx, cost, simulationQueue, executionQueue) { // init sampler generator = kukadu_mersenne_twister(seed); if(explorationSigmas.size() < initDmp.at(0)->getCoefficients().at(0).n_elem) { cerr << "you defined too less exploration sigmas" << endl; throw KukaduException("you defined too less exploration sigmas"); } for(int i = 0; i < initDmp.at(0)->getCoefficients().at(0).n_elem; ++i) { kukadu_normal_distribution normal(0, explorationSigmas.at(i)); normals.push_back(normal); } construct(initDmp, explorationSigmas, updatesPerRollout, importanceSamplingCount, cost, simulationQueue, executionQueue, ac, dmpStepSize, tolAbsErr, tolRelErr); }
std::vector<mes_result> ControlQueue::jointPtp(arma::vec joints) { jointPtpRunning = true; if(!continueCollecting) { continueCollecting = true; jointsColletorThr = KUKADU_SHARED_PTR<kukadu_thread>(new kukadu_thread(&ControlQueue::jointsCollector, this)); jointPtpInternal(joints); continueCollecting = false; jointsColletorThr->join(); } else { throw KukaduException("(ControlQueue) only one ptp at a time can be executed"); } jointPtpRunning = false; return collectedJoints; }
std::vector<mes_result> ControlQueue::cartesianPtp(geometry_msgs::Pose pos, double maxForce) { cartesianPtpRunning = true; if(!continueCollecting) { continueCollecting = true; jointsColletorThr = KUKADU_SHARED_PTR<kukadu_thread>(new kukadu_thread(&ControlQueue::jointsCollector, this)); cartPtpInternal(pos, maxForce); continueCollecting = false; jointsColletorThr->join(); } else { throw KukaduException("(ControlQueue) only one ptp at a time can be executed"); } cartesianPtpRunning = false; return collectedJoints; }
void KukieControlQueue::constructQueue(double sleepTime, std::string commandTopic, std::string retPosTopic, std::string switchModeTopic, std::string retCartPosTopic, std::string cartStiffnessTopic, std::string jntStiffnessTopic, std::string ptpTopic, std::string commandStateTopic, std::string ptpReachedTopic, std::string addLoadTopic, std::string jntFrcTrqTopic, std::string cartFrcTrqTopic, std::string cartPtpTopic, std::string cartPtpReachedTopic, std::string cartMoveRfQueueTopic, std::string cartMoveWfQueueTopic, std::string cartPoseRfTopic, std::string jntSetPtpThreshTopic, bool acceptCollisions, ros::NodeHandle node ) { set_ctrlc_exit_handler(); this->acceptCollisions = acceptCollisions; this->ptpTopic = ptpTopic; this->addLoadTopic = addLoadTopic; this->commandTopic = commandTopic; this->cartPtpTopic = cartPtpTopic; this->retJointPosTopic = retPosTopic; this->jntFrcTrqTopic = jntFrcTrqTopic; this->switchModeTopic = switchModeTopic; this->retCartPosTopic = retCartPosTopic; this->ptpReachedTopic = ptpReachedTopic; this->cartFrcTrqTopic = cartFrcTrqTopic; this->stiffnessTopic = cartStiffnessTopic; this->jntStiffnessTopic = jntStiffnessTopic; this->commandStateTopic = commandStateTopic; this->cartPtpReachedTopic = cartPtpReachedTopic; this->cartMoveRfQueueTopic = cartMoveRfQueueTopic; this->cartMoveWfQueueTopic = cartMoveWfQueueTopic; this->jntSetPtpThreshTopic = jntSetPtpThreshTopic; loop_rate = new ros::Rate(1.0 / sleepTime); cartesianPtpReached = 0; setInitValues(); this->node = node; if(sleepTime == 0.0) { ROS_ERROR("(KukieControlQueue) the sleep time you provided is 0. note that it is required in seconds"); throw KukaduException("(KukieControlQueue) the sleep time you provided is 0. note that it is required in seconds"); } subJntPos = node.subscribe(retPosTopic, 2, &KukieControlQueue::robotJointPosCallback, this); subComState = node.subscribe(commandStateTopic, 2, &KukieControlQueue::commandStateCallback, this); subPtpReached = node.subscribe(ptpReachedTopic, 2, &KukieControlQueue::ptpReachedCallback, this); subjntFrcTrq = node.subscribe(jntFrcTrqTopic, 2, &KukieControlQueue::jntFrcTrqCallback, this); subCartFrqTrq = node.subscribe(cartFrcTrqTopic, 2, &KukieControlQueue::cartFrcTrqCallback, this); subCartPtpReached = node.subscribe(cartPtpReachedTopic, 2, &KukieControlQueue::cartPtpReachedCallback, this); subCartPoseRf = node.subscribe(cartPoseRfTopic, 2, &KukieControlQueue::cartPosRfCallback, this); pub_set_cart_stiffness = node.advertise<iis_robot_dep::CartesianImpedance>(stiffnessTopic, 1); pub_set_joint_stiffness = node.advertise<iis_robot_dep::FriJointImpedance>(jntStiffnessTopic, 1); pubCartPtp = node.advertise<geometry_msgs::Pose>(cartPtpTopic, 1); pubCartMoveRfQueue = node.advertise<geometry_msgs::Pose>(cartMoveRfQueueTopic, 1); pubCartMoveWfQueue = node.advertise<geometry_msgs::Pose>(cartMoveWfQueueTopic, 1); pub_set_ptp_thresh = node.advertise<std_msgs::Float64>(jntSetPtpThreshTopic, 1); pubCommand = node.advertise<std_msgs::Float64MultiArray>(commandTopic, 10); pubSwitchMode = node.advertise<std_msgs::Int32>(switchModeTopic, 1); pubPtp = node.advertise<std_msgs::Float64MultiArray>(ptpTopic, 10); isRealRobot = (getRobotDeviceType().compare("real")) ? false : true; // this is required because shared_from_this can't be called in constructor (initializiation happens by lazy loading) plannerInitialized = false; kinematicsInitialized = false; ros::Rate r(10); while(!firstJointsReceived || !firstModeReceived) r.sleep(); currentControlType = impMode; currentCartFrqTrq = vec(6); currentCartFrqTrq.fill(0.0); currentJntFrqTrq = vec(getMovementDegreesOfFreedom()); currentJntFrqTrq.fill(0.0); usleep(1e6); }
void PlottingControlQueue::startJointRollBackMode(double possibleTime) { throw KukaduException("(PlottingControlQueue) roll back mode not supported"); }
void PlottingControlQueue::stopJointRollBackMode() { throw KukaduException("(PlottingControlQueue) roll back mode not supported"); }
void PlottingControlQueue::rollBack(double time) { throw KukaduException("(PlottingControlQueue) roll back mode not supported"); }