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