Status BaseMultiPos::
update(Model const & model)
{
Status st;
Vector cur_eepos(Vector::Zero(3));
Vector delta;
Vector v_delta;
size_t ndof(model.getUnconstrainedNDOF());
for (size_t ii(0); ii < task_table_.size(); ++ii) {
st = task_table_[ii]->update(model);
if ( ! st) { return st; }
}
for(int ii=0; ii<3; ii++) {
cur_eepos[ii] = ee_pos_[3*cur_row_+ii];
}
delta = cur_eepos - ee_task_->getActual();
jspace::Constraint* constraint = model.getConstraint();
if (constraint) {
jspace::State fullState(model.getNDOF(),model.getNDOF(),6);
constraint->getFullState(model.getState(),fullState);
v_delta = ee_task_->getJacobian()*fullState.velocity_;
}
else {
v_delta = ee_task_->getJacobian()*model.getState().velocity_;
}
if (delta.norm() < threshold_ && v_delta.norm() < vel_threshold_) {
if(forward_) {
if (cur_row_ < (ee_pos_.rows()/3)-1) {
++cur_row_;
for(size_t jj(0); jj<3; ++jj) {
cur_eepos[jj] = ee_pos_[3*cur_row_+jj];
}
st = ee_goal_->set(cur_eepos);
if (! st) { return st; }
}
else { forward_ = false; }
}
else {
if (cur_row_ > 0) {
--cur_row_;
for(size_t jj(0); jj<3; ++jj) {
cur_eepos[jj] = ee_pos_[3*cur_row_+jj];
}
st = ee_goal_->set(cur_eepos);
if (! st) { return st; }
}
else { forward_ = true; }
}
}
return st;
}
jspace::Status OpspacePlanarController::
setGains(jspace::Vector const & kp, jspace::Vector const & kd)
{
if ((kp.size() != ndof_) || (kd.size() != ndof_)) {
return jspace::Status(false, "NDOF mismatch in provided gains");
}
op_kp_ = kp[0];
op_kd_ = kd[0];
op_kp_by_kd_ = fabs(op_kd_) < 1e-2 ? -1 : op_kp_ / op_kd_;
op_vmax_ = kp[0] * 1e-3;
jspace::Vector jkp(ndof_);
jspace::Vector jkd(ndof_);
ssize_t ii(2);
ssize_t jj(0);
for (/**/; jj < ndof_; ++jj) {
if ((jj == q1_index_) || (jj == q2_index_)) {
jkp[jj] = 0;
jkd[jj] = 0;
}
else {
jkp[jj] = kp[ii];
jkd[jj] = kd[ii];
++ii;
}
}
return jgoal_controller_.setGains(jkp, jkd);
}
jspace::Status OpspacePlanarController::
setGoal(jspace::Vector const & goal)
{
if (goal.size() != ndof_) {
return jspace::Status(false, "NDOF mismatch in provided goal");
}
double const gdist_sqr(pow(goal[0], 2) + pow(goal[1], 2));
if (gdist_sqr <= gdist_sqr_min_) {
return jspace::Status(false, "the goal is too close to the origin");
}
if (gdist_sqr >= gdist_sqr_max_) {
return jspace::Status(false, "the goal is too far from the origin");
}
op_goal_.coeffRef(0) = goal[0];
op_goal_.coeffRef(1) = goal[1];
jspace::Vector jgoal(ndof_);
ssize_t ii(2);
ssize_t jj(0);
for (/**/; jj < ndof_; ++jj) {
if ((jj == q1_index_) || (jj == q2_index_)) {
jgoal[jj] = 0;
}
else {
jgoal[jj] = goal[ii];
++ii;
}
}
return jgoal_controller_.setGoal(jgoal);
}
int i () {
void j (int f = 4);
{
void j (int f);
j(); // expected-error{{too few arguments to function call, expected 1, have 0; did you mean '::j'?}}
}
void jj (int f = 4);
{
void jj (int f); // expected-note{{'jj' declared here}}
jj(); // expected-error{{too few arguments to function call, single argument 'f' was not specified}}
}
}
Stmt lowerTranspose(Var target, const IndexExpr* iexpr,
Environment* env, Storage* storage) {
simit_iassert(isa<IndexedTensor>(iexpr->value));
simit_iassert(isa<VarExpr>(to<IndexedTensor>(iexpr->value)->tensor));
Var source = to<VarExpr>(to<IndexedTensor>(iexpr->value)->tensor)->var;
auto sourceIndex = storage->getStorage(source).getTensorIndex();
auto targetIndex = storage->getStorage(target).getTensorIndex();
auto sourceType = source.getType().toTensor();
auto iRange = sourceType->getOuterDimensions()[0];
Var i("i", Int);
Var ij("ij", Int);
Var j("j", Int);
Var locVar(INTERNAL_PREFIX("locVar"), Int);
Stmt locStmt = CallStmt::make({locVar}, intrinsics::loc(),
{Load::make(sourceIndex.getColidxArray(),ij), i,
targetIndex.getRowptrArray(),
targetIndex.getColidxArray()});
Stmt body;
auto blockType = *sourceType->getBlockType().toTensor();
if (blockType.order() == 0) { // Not blocked
body = Store::make(target, locVar, Load::make(source, ij));
}
else { // Blocked
simit_iassert(blockType.order() == 2);
Var ii("ii", Int);
Var jj("jj", Int);
auto d1 = blockType.getOuterDimensions()[0];
auto d2 = blockType.getOuterDimensions()[1];
Expr l1 = Length::make(d1);
Expr l2 = Length::make(d2);
body = Store::make(target, locVar*l1*l2 + ii*l2 + jj,
Load::make(source, ij*l1*l2 + ii*l2 + jj));
body = For::make(jj, ForDomain(d2), body);
body = For::make(ii, ForDomain(d1), body);
}
simit_iassert(body.defined());
Expr start = Load::make(sourceIndex.getRowptrArray(), i);
Expr stop = Load::make(sourceIndex.getRowptrArray(), i+1);
Stmt innerLoop = ForRange::make(ij, start, stop, Block::make(locStmt, body));
return For::make(i, iRange, innerLoop);
}
int main() {
std::vector<int> vi;
equal_skips = 0;
push_back(vi, 123);
BOOST_TEST_EQ(equal_skips, 0u);
unsigned const cnt =
#ifndef BOOST_CONTRACT_NO_POSTCONDITIONS
1
#else
0
#endif
;
j jj(456);
std::vector<j> vj;
equal_skips = 0;
push_back(vj, jj);
BOOST_TEST_EQ(equal_skips, cnt);
return boost::report_errors();
}
int main(int argc, char** argv) {
ros::init(argc, argv, "wbc_fakecamera");
ros::NodeHandle node("~");
char local_port [10];
char cam_port [10];
sprintf(local_port, "%d", CS8C_PORT+2);
sprintf(cam_port, "%d", CLIENT_PORT+2);
UdpOSI camUDP( local_port, "127.0.0.1", cam_port, 0);
int datapts = 56;
Position3d p3dData[datapts];
/*for (size_t ii(0); ii< datapts; ++ii) {
p3dData[ii].x = 47.5;
p3dData[ii].y = 5;
p3dData[ii].z = 43.5;
}*/
for (size_t ii(0); ii<8; ++ii) {
for (size_t jj(0); jj<7; ++jj) {
p3dData[ii*7+jj].x = (0.35+0.05*ii)*1e2;
p3dData[ii*7+jj].y = (-0.15+0.05*jj)*1e2;
p3dData[ii*7+jj].z = (0.24+pow(0.35+0.05*ii-0.40,2))*1e2;
}
}
int bytes = 0;
int count = 1;
while(ros::ok()) {
bytes = camUDP.sendPacket((char*)p3dData, sizeof(p3dData));
if (bytes != 0 ) {
fprintf(stderr,"Failed to send cam data");
return 0;
}
}
}
int main(int argc,char **argv)
{
myregex jj(".*@company.com",REG_ICASE);
int match = jj.search("*****@*****.**");
printf("match for [email protected]=%d\n",match);
assert(match!=0);
match = jj.search("*****@*****.**");
printf("match for [email protected]=%d\n",match);
assert(match==0);
linelist_file f;
if(f.readfile("test_linelist.txt")){
fprintf(stderr,"Cannot read test_linelist.txt\n");
exit(1);
}
for(int i=0;names[i];i++){
if(f.contains(names[i])) printf("contains %s\n",names[i]);
}
printf("\n");
for(int i=0;names[i];i++){
if(!f.contains(names[i])) printf("does not contain %s\n",names[i]);
}
}
bool tao_tree_info_s::
sort()
{
// swap any out-of-order entries (yes yes, this is
// suboptimal... let's just assume that in most cases they will
// already be correctly ordered anyway)
for (ssize_t ii(0); ii < info.size(); ++ii) {
if (info[ii].id != ii) {
for (ssize_t jj(ii + 1); jj < info.size(); ++jj) {
if (info[jj].id == ii) {
std::swap(info[ii], info[jj]);
break;
}
}
}
}
// check if it worked
for (ssize_t ii(0); ii < info.size(); ++ii) {
if (info[ii].id != ii) {
return false;
}
}
return true;
}
void Mapper2d::
InitRemovemask()
{
m_removemask_x0 = 0;
m_removemask_y0 = 0;
m_removemask_x1 = 0;
m_removemask_y1 = 0;
for (addmask_t::const_iterator ii(m_addmask.begin()); ii != m_addmask.end(); ++ii)
for (addmask_t::const_iterator jj(m_addmask.begin()); jj != m_addmask.end(); ++jj) {
ssize_t const ix(ii->first.v0 - jj->first.v0); // "minus" because it's a convolution
ssize_t const iy(ii->first.v1 - jj->first.v1);
m_removemask.insert(make_pair(index_t(ix, iy), false));
if (ix < m_removemask_x0)
m_removemask_x0 = ix;
if (ix > m_removemask_x1)
m_removemask_x1 = ix;
if (iy < m_removemask_y0)
m_removemask_y0 = iy;
if (iy > m_removemask_y1)
m_removemask_y1 = iy;
}
for (addmask_t::const_iterator ii(m_addmask.begin()); ii != m_addmask.end(); ++ii)
m_removemask[ii->first] = true;
}
bool DelayHistogram::
DumpTable(FILE * of) const
{
if (nsets < 1)
return false;
fprintf(of, "+----------------+-------------------");
for (size_t ii(1); ii < nsets; ++ii)
fprintf(of, "-+-------------------");
fprintf(of, "-+\n| HISTOGRAM | %18.18s", m_name[0].c_str());
for (size_t ii(1); ii < nsets; ++ii)
fprintf(of, " | %18.18s", m_name[ii].c_str());
fprintf(of, " |\n+----------------+-------------------");
for (size_t ii(1); ii < nsets; ++ii)
fprintf(of, "-+-------------------");
fprintf(of, "-+\n| min %8.3f | %8.3f",
(m_ms_min_all == numeric_limits<double>::max() ? -1 : m_ms_min_all),
(m_ms_min[0] == numeric_limits<double>::max() ? -1 : m_ms_min[0]));
for (size_t ii(1); ii < nsets; ++ii)
fprintf(of, " | %8.3f",
(m_ms_min[ii] == numeric_limits<double>::max() ? -1 : m_ms_min[ii]));
fprintf(of, " |\n| max %8.3f | %8.3f",
(m_ms_max_all == numeric_limits<double>::min() ? -1 : m_ms_max_all),
(m_ms_max[0] == numeric_limits<double>::min() ? -1 : m_ms_max[0]));
for (size_t ii(1); ii < nsets; ++ii)
fprintf(of, " | %8.3f",
(m_ms_max[ii] == numeric_limits<double>::min() ? -1 : m_ms_max[ii]));
fprintf(of, " |\n| mean ");
for (size_t ii(0); ii < nsets; ++ii)
fprintf(of, " | %8.3f",
(m_ms_count[ii] == 0 ? -1.0 : m_ms_sum[ii] / m_ms_count[ii]));
fprintf(of, " |\n+----------------+-------------------");
for (size_t ii(1); ii < nsets; ++ii)
fprintf(of, "-+-------------------");
fprintf(of, "-+\n| below %8.3f | %18zu", m_bin_floor[0], m_below[0]);
for (size_t ii(1); ii < nsets; ++ii)
fprintf(of, " | %18zu", m_below[ii]);
fprintf(of, " |\n+----------------+-------------------");
for (size_t ii(1); ii < nsets; ++ii)
fprintf(of, "-+-------------------");
if (m_lower < nbins) {
char const * prf("-+");
for (size_t jj(m_lower); jj <= m_upper; ++jj) {
fprintf(of, "%s\n| [%2zu] %8.3f | %18zu", prf, jj, m_bin_floor[jj], m_bin[jj]);
prf = " |";
for (size_t ii(1); ii < nsets; ++ii)
fprintf(of, " | %18zu", m_bin[ii * nbins + jj]);
}
}
else
fprintf(of, "-+\n| (all bins are empty)");
fprintf(of, " |\n+----------------+-------------------");
for (size_t ii(1); ii < nsets; ++ii)
fprintf(of, "-+-------------------");
fprintf(of, "-+\n| above %8.3f | %18zu", m_bin_floor[nbins-1] + ms_range / nbins, m_above[0]);
for (size_t ii(1); ii < nsets; ++ii)
fprintf(of, " | %18zu", m_above[ii]);
fprintf(of, " |\n+----------------+-------------------");
for (size_t ii(1); ii < nsets; ++ii)
fprintf(of, "-+-------------------");
fprintf(of, "-+\n");
return true;
}
void instruction_decode(int index){
if(test==1) printf("enter ID, with index=%d\n",index);
if(index==0 || index==34){ //NOP or HALT
return;
}
/**stall detect**/
if(index>=1&&index<=9){
if(DM_dest_reg==RS || DM_dest_reg==RT){
if(dest_reg!=RS && dest_reg!=RT){
stall=1;
}
}
}
else if(index>=10&&index<=12){
if(DM_dest_reg==RT){
if(dest_reg!=RT){
stall=1;
}
}
}
else if(index==13){
if(dest_reg==RS){
stall=1;
}
}
else if(index>=16&&index<=22 || index>=27&&index<=30){
if(DM_dest_reg==RS){
if(dest_reg!=RS){
stall=1;
}
}
}
else if(index>=23&&index<=25){
if(DM_dest_reg==RS || DM_dest_reg==RT){
if(dest_reg!=RS && dest_reg!=RT){
stall=1;
}
}
}
else if(index>=31&&index<=33){
if(dest_reg==RS || dest_reg==RT){
stall=1;
}
if(DM_dest_reg==RS || DM_dest_reg==RT){
if(EX_index>=18&&EX_index<=22){
stall=1;
}
}
}
/**flush detect**/
if(index==13){
jr(RS);
flush=1;
}
else if(index==14){
jj(C);
flush=1;
}
else if(index==15){
jal(C);
flush=1;
}
else if(index==31){
beq(RS,RT,signedC);
flush=1;
}
else if(index==32){
bne(RS,RT,signedC);
flush=1;
}
else if(index==33){
bgtz(RS,signedC);
flush=1;
}
ID_prev=index;
EX_index=index;
}
void wbcComponent::updateHook(){
port_fri_joint_impedance.write(fri_joint_impedance);
//Read and update state
if (port_joint_state.read(joint_state) == NewData) {
port_mass_matrix.read(mass_matrix);
for (size_t ii(0); ii<7; ++ii) {
for (size_t jj(0); jj<7; ++jj) {
A(ii,jj) = mass_matrix.mass[7*ii+jj];
}
}
//Hacks to try to make it function better
A(5,5) += 0.005;
controller->setAmatrix(A);
model->setKukaAMatrix(A);
//Data collection
port_fri_joint_state.read(fri_joint_state);
for (size_t ii(0); ii < 7; ++ii) {
gravity[ii] = fri_joint_state.gravity[ii];
msrJntTrq[ii] = fri_joint_state.msrJntTrq[ii];
estExtJntTrq[ii] = fri_joint_state.estExtJntTrq[ii];
}
controller->setgTrq(gravity);
controller->setMsrJntTrq(msrJntTrq);
controller->setEstExtJntTrq(estExtJntTrq);
double t = joint_state.header.stamp.toSec();
robot_state.header.stamp.fromSec ( t );
for (size_t ii(0); ii < model->getNDOF(); ++ii) {
state.position_[ii] = joint_state.position[ii];
robot_state.position[ii] = joint_state.position[ii];
if (t_prev == 0) {
state.velocity_[ii] = (state.position_[ii]-pos_prev[ii])/(t-t_prev);
robot_state.velocity[ii] = state.velocity_[ii];
}
else {
state.velocity_[ii] = 0;
robot_state.velocity[ii] = 0;
}
}
pos_prev = state.position_;
t_prev = t;
//Update model
model->update(state);
//Compute command
jspace::Status status(controller->computeCommand(*model, *skill, command));
if ( ! status) {
errx(EXIT_FAILURE,"controller->computeCommand() failed: %s", status.errstr.c_str());
}
//Command output
for (size_t ii(0); ii < model->getNDOF(); ++ii) {
joint_efforts.efforts[ii] = command[ii];
robot_state.effort[ii] = command[ii];
robot_state.position[ii] = model->getState().position_[ii];
robot_state.velocity[ii] = model->getState().velocity_[ii];
}
port_joint_efforts.write(joint_efforts);
//skill->dbg(temp_skill_state, "\n\n**************************************************", "");
//controller->dbg(temp_skill_state, "--------------------------------------------------", "");
//skill_state = temp_skill_state.str();
//temp_skill_state.str("");
}
skill->dbg(cout, "\n\n**************************************************", "");
//controller->dbg(cout, "--------------------------------------------------", "");
cout << "--------------------------------------------------\n";
jspace::pretty_print(model->getState().position_, cout, "jpos", " ");
jspace::pretty_print(controller->getCommand(), cout, "gamma", " ");
//Logger output
//Write to state
port_robot_state.write(robot_state);
port_skill_state.write(skill_state);
}
