Size MenuItem::GetMinSize() const { Size sz1 = GetTextSize(text, font); Size sz2(0, 0); if(accel) { sz2 = GetTextSize(GetKeyDesc(accel), font); sz2.cx += Zx(12); } Size lsz = min(maxiconsize, licon.GetSize()); Size rsz = ricon.GetSize(); return AddFrameSize(Size(max(lsz.cx, leftgap) + sz1.cx + max(sz2.cx, (rsz.cx ? Zx(16) : 0)) + max(rsz.cx, Zx(16)) + textgap + Zx(10), max(max(lsz.cy, rsz.cy) + Zy(4), sz1.cy + Zy(6)))); }
void HelpWindow::SetZoom() { zoom.d = 1000000 / Zy(800); current_link = Null; GoTo0(topic); Refresh(); }
virtual Size GetStdSize(const Value& q) const { ValueArray va = q; Size sz = GetTextSize(String(va[0]), fnt); sz.cx += Zx(20); sz.cy = max(sz.cy, Zy(16)); return sz; }
int KinematicSolver::forward(btTransform& pose) const { TRACER_ENTER_SCOPE("KinematicSolver::forward()"); if (false && forwardKinTimestamp_ == arm_->timestamp()) { pose = forwardKinCached_; return 0; } Arm::IdType armId_new = arm_->id(); int armId = armIdFromSerial(arm_->id()); pose = actual_world_to_ik_world(armId) * Tw2b * Zs(THS_TO_IK(armId,arm_->getJointById(Joint::IdType::SHOULDER_)->position())) * Xu * Ze(THE_TO_IK(armId,arm_->getJointById(Joint::IdType::ELBOW_)->position())) * Xf * Zr(THR_TO_IK(armId,arm_->getJointById(Joint::IdType::ROTATION_)->position())) * Zi(D_TO_IK(armId,arm_->getJointById(Joint::IdType::INSERTION_)->position())) * Xip * Zp(THP_TO_IK(armId,arm_->getJointById(Joint::IdType::WRIST_)->position())) * Xpy * Zy(THY_TO_IK_FROM_FINGERS(armId,arm_->getJointById(Joint::IdType::FINGER1_)->position(),arm_->getJointById(Joint::IdType::FINGER2_)->position())) * Tg; //int grasp = MECH_GRASP_FROM_MECH_FINGERS(armId,arm_->getJointById(Joint::Type::GRIPPER1_)->position(),arm_->getJointById(Joint::Type::GRIPPER2_)->position()); const_cast<KinematicSolver*>(this)->forwardKinCached_ = pose; const_cast<KinematicSolver*>(this)->forwardKinTimestamp_ = arm_->timestamp(); return 0; }
void SIC_StdFont(EscEscape& e) { if(e.GetCount() == 1) e = EscFont(StdFont()(Zy(e.Int(0)))); else e = EscFont(StdFont()); }
NewPackageFileWindow::NewPackageFileWindow() { CtrlLayoutOKCancel(*this, "New package file"); type.SetLineCy(max(Zy(16), Draw::GetStdFontCy())); type.SetDropLines(20); Type("cpp", "C++ source file"); Type("h", "C++ header file"); type.AddSeparator(); Type("lay", "Layout file (dialog templates)"); Type("iml", "Image file (icons)"); Type("icpp", "Initialization C++ source file"); Type("usc", "Escape script file (scripting TheIDE)"); Type("witz", "Skylark template file (web framework files)"); Type("qtf", "U++ rich text file"); type.AddSeparator(); Type("json", "JSON file"); Type("xml", "XML file"); Type("html", "HTML file"); Type("css", "CSS file"); type.AddSeparator(); Type("sch", "SQL schema file"); Type("ddl", "SQL DDL script file"); Type("sql", "SQL script file"); type.AddSeparator(); Type("java", "Java"); Type("js", "JavaScript"); Type("py", "Python"); type.AddSeparator(); Type("", "Other"); name << [=] { String ext = GetFileExt(~~name); if(ext.GetCount()) { ext = ext.Mid(1); type <<= type.HasKey(ext) ? ext : Null; } Sync(); }; name <<= ".cpp"; type <<= "cpp"; type << [=] { String ext = ~type; if(ext.GetCount()) { String h = ~name; name <<= ForceExt(h, "." + ext); int q = GetFileTitle(h).GetCount(); name.SetSelection(q, q); } Sync(); }; Sync(); }
SelectPackageDlg::SelectPackageDlg(const char *title, bool selectvars_, bool main) : selectvars(selectvars_) { CtrlLayoutOKCancel(*this, title); Sizeable().Zoomable(); Icon(IdeImg::MainPackage(), IdeImg::PackageLarge()); base.AutoHideSb(); base.NoGrid(); base.AddColumn("Assembly"); base.WhenCursor = THISBACK(OnBase); base.WhenBar = THISBACK(ToolBase); base.WhenLeftDouble = THISBACK(OnBaseEdit); ok.WhenAction = clist.WhenLeftDouble = alist.WhenLeftDouble = THISBACK(OnOK); cancel.WhenAction = WhenClose = THISBACK(OnCancel); clist.Columns(4); clist.WhenEnterItem = clist.WhenKillCursor = THISBACK(ListCursor); alist.AddColumn("Package").Add(3); alist.AddColumn("Nest"); alist.AddColumn("Description"); alist.AddIndex(); alist.ColumnWidths("108 79 317"); alist.WhenCursor = THISBACK(ListCursor); alist.EvenRowColor(); alist.SetLineCy(max(Zy(16), Draw::GetStdFontCy())); list.Add(clist.SizePos()); list.Add(alist.SizePos()); splitter.Horz(base, list); splitter.SetPos(2000); splitter.Zoom(selectvars ? -1 : 1); newu <<= THISBACK(OnNew); filter <<= THISBACK(OnFilter); filter.Add(MAIN|FIRST, "Main packages of first nest"); filter.Add(MAIN, "All main packages"); filter.Add(FIRST, "All packages of first nest"); filter.Add(0, "All packages"); filter <<= main ? MAIN|FIRST : 0; progress.Hide(); brief <<= THISBACK(SyncBrief); search.NullText("Search (Ctrl+K)", StdFont().Italic(), SColorDisabled()); search <<= THISBACK(SyncList); search.SetFilter(CharFilterDefaultToUpperAscii); SyncBrief(); description.NullText("Package description (Alt+Enter)", StdFont().Italic(), SColorDisabled()); description <<= THISBACK(ChangeDescription); ActiveFocus(brief ? (Ctrl&)clist : (Ctrl&)alist); clist.BackPaintHint(); alist.BackPaintHint(); base.BackPaintHint(); loadi = 0; loading = false; clist.WhenBar = alist.WhenBar = THISBACK(PackageMenu); }
ParaFormatting::ParaFormatting() { CtrlLayout(*this); tabtype.Add(ALIGN_LEFT, t_("Left")); tabtype.Add(ALIGN_RIGHT, t_("Right")); tabtype.Add(ALIGN_CENTER, t_("Centered")); tabfill.Add(0, t_("None")); tabfill.Add(1, t_("....")); tabfill.Add(2, t_("----")); tabfill.Add(3, t_("__")); tabs.AddColumn(t_("Tab position"), 2).Edit(tabpos).SetConvert(tabpos); tabs.AddColumn(t_("Type"), 2).Edit(tabtype).SetConvert(tabtype).InsertValue(ALIGN_LEFT); tabs.AddColumn(t_("Fill"), 1).Edit(tabfill).SetConvert(tabfill).InsertValue(0); tabs.ColumnWidths("103 89 78"); tabs.Appending().Removing().NoAskRemove(); tabs.WhenAcceptEdit = tabs.WhenArrayAction = THISBACK(SetMod); linespacing.Add(0, "1.0"); linespacing.Add(-1, "1.5"); linespacing.Add(-2, "2.0"); bullet.Add(RichPara::BULLET_NONE, RichEditImg::NoneBullet()); bullet.Add(RichPara::BULLET_ROUND, RichEditImg::RoundBullet()); bullet.Add(RichPara::BULLET_ROUNDWHITE, RichEditImg::RoundWhiteBullet()); bullet.Add(RichPara::BULLET_BOX, RichEditImg::BoxBullet()); bullet.Add(RichPara::BULLET_BOXWHITE, RichEditImg::BoxWhiteBullet()); bullet.Add(RichPara::BULLET_TEXT, RichEditImg::TextBullet()); bullet.SetDisplay(CenteredHighlightImageDisplay()); bullet.SetLineCy(RichEditImg::RoundBullet().GetHeight() + Zy(2)); for(int i = 0; i < 8; i++) { DropList& list = n[i]; list.Add(Null); list.Add(RichPara::NUMBER_NONE, " - "); list.Add(RichPara::NUMBER_1, "1, 2, 3"); list.Add(RichPara::NUMBER_0, "0, 1, 2"); list.Add(RichPara::NUMBER_a, "a, b, c"); list.Add(RichPara::NUMBER_A, "A, B, C"); list.Add(RichPara::NUMBER_i, "i, ii, iii"); list.Add(RichPara::NUMBER_I, "I, II, III"); list <<= THISBACK(SetupIndent); } before_number <<= after_number <<= reset_number <<= bullet <<= THISBACK(SetupIndent); EnableNumbering(); rulerink.NullText("---"); rulerstyle.SetDisplay(Single<RulerStyleDisplay>()); rulerstyle.Add(Null); rulerstyle.Add(RichPara::RULER_SOLID); rulerstyle.Add(RichPara::RULER_DOT); rulerstyle.Add(RichPara::RULER_DASH); }
NAMESPACE_UPP #define LLOG(x) // DLOG(x) #define LTIMING(x) // RTIMING(x) MenuItemBase::MenuItemBase() { accel = 0; state = 0; isenabled = true; type = 0; font = StdFont(); leftgap = Zx(16); textgap = Zy(6); accesskey = 0; NoWantFocus(); style = &MenuBar::StyleDefault(); Transparent(); maxiconsize = Size(INT_MAX, INT_MAX); }
int TopMenuItem::GetStdHeight(Font font) { return font.Info().GetHeight() + Zy(7); }
void MenuItem::Paint(Draw& w) { int q = text.Find('\t'); String txt, keydesc; if(accel) keydesc = GetKeyDesc(accel); if(q >= 0) { keydesc = text.Mid(q + 1); txt = text.Mid(0, q); } else txt = text; state = GetVisualState(); bool hl = state != NORMAL; Size sz = GetSize(); if(hl) { if(GUI_GlobalStyle() >= GUISTYLE_XP) ChPaint(w, 0, 0, sz.cx, sz.cy, style->item); else w.DrawRect(sz, SColorHighlight); } UPP::Image li = licon; if(li.IsEmpty()) { switch(type) { case CHECK0: li = CtrlImg::MenuCheck0(); break; case CHECK1: li = CtrlImg::MenuCheck1(); break; case RADIO0: li = CtrlImg::MenuRadio0(); break; case RADIO1: li = CtrlImg::MenuRadio1(); break; } } Size isz = li.GetSize(); // Size isz = min(maxiconsize, imsz); // if(isz != imsz) // li = CachedRescale(li, isz); int iy = (sz.cy - isz.cy) / 2; bool chk = false; int x = Zx(2); if(!licon.IsEmpty() && type) { chk = type == CHECK1 || type == RADIO1; if(GUI_GlobalStyle() >= GUISTYLE_XP) { if(chk && !hl) DrawXPButton(w, RectC(0, iy - Zy(2), isz.cx + Zx(4), isz.cy + Zy(4)), BUTTON_EDGE|BUTTON_CHECKED); } else { w.DrawRect(x - Zx(1), iy - Zy(1), isz.cx + Zx(2), isz.cy + Zy(2), chk ? Blend(SColorFace, SColorLight) : SColorFace); DrawBorder(w, x - Zx(2), iy - Zy(2), isz.cx + Zx(4), isz.cy + Zy(4), chk ? ThinInsetBorder : ThinOutsetBorder); } } if(isenabled) DrawHighlightImage(w, x, iy, li, hl || chk, true); else w.DrawImage(x, iy, DisabledImage(li)); x = max(isz.cx, leftgap) + textgap; isz = GetTextSize(text, StdFont()); DrawMenuText(w, x, (sz.cy - isz.cy) / 2, txt, font, isenabled, hl, style->menutext, style->itemtext); isz = ricon.GetSize(); if(isenabled) w.DrawImage(sz.cx - isz.cx, (sz.cy - isz.cy) / 2, ricon, hl ? style->itemtext : style->menutext); else w.DrawImage(sz.cx - isz.cx, (sz.cy - isz.cy) / 2, DisabledImage(ricon)); x = sz.cx - max(isz.cx, Zx(16)) - Zx(1); if(!IsEmpty(keydesc)) { isz = GetTextSize(keydesc, StdFont()); UPP::DrawMenuText(w, x - isz.cx - Zx(2), (sz.cy - isz.cy) / 2, keydesc, font, isenabled, hl, 0, SColorMenuMark(), style->itemtext, false); } }
InverseKinematicsReportPtr KinematicSolver::internalInverseSoln(const btTransform& pose, Arm* arm,const InverseKinematicsOptions& options) const { TRACER_ENTER_SCOPE("KinematicSolver::internalInverseSoln(arm@%p)",arm); InverseKinematicsReportPtr report(new InverseKinematicsReport()); Arm::IdType armId_new = arm_->id(); int armId = armIdFromSerial(arm_->id()); // desired tip position // btVector3 currentPoint = btVector3(mech->pos.x,mech->pos.y,mech->pos.z) / MICRON_PER_M; // btVector3 actualPoint = btVector3(pos_d->x,pos_d->y,pos_d->z) / MICRON_PER_M; // btMatrix3x3 actualOrientation = toBt(ori_d->R); // btTransform actualPose(actualOrientation,actualPoint); // btTransform actualPose_fk = pose; // tb_angles currentPoseAngles = tb_angles(mech->ori.R); // tb_angles actualPoseAngles = tb_angles(ori_d->R); //float grasp = GRASP_TO_IK(armId,mech->ori_d.grasp); float grasp = arm->getJointById(Joint::IdType::GRASP_)->position(); // if (print) { // log_msg("j s % 2.1f e % 2.1f r % 2.1f i % 1.3f p % 2.1f y % 2.1f g % 2.1f g1 % 2.1f g2 % 2.1f", // arm->getJointById(Joint::Type::SHOULDER_)->position() RAD2DEG, // arm->getJointById(Joint::Type::ELBOW_)->position() RAD2DEG, // arm->getJointById(Joint::Type::TOOL_ROT_)->position() RAD2DEG, // arm->getJointById(Joint::Type::INSERTION__)->position(), // arm->getJointById(Joint::Type::WRIST_)->position() RAD2DEG, // fix_angle(arm->getJointById(Joint::Type::GRIPPER1_)->position() - arm->getJointById(Joint::Type::GRIPPER2_)->position()) / 2 RAD2DEG, // (arm->getJointById(Joint::Type::GRIPPER1_)->position() + arm->getJointById(Joint::Type::GRIPPER2_)->position()) RAD2DEG, // arm->getJointById(Joint::Type::GRIPPER1_)->position() RAD2DEG,arm->getJointById(Joint::Type::GRIPPER2_)->position() RAD2DEG); // log_msg("v s % 2.1f e % 2.1f r % 2.1f i % 1.3f p % 2.1f y % 2.1f g % 2.1f g1 % 2.1f g2 % 2.1f", // arm->getJointById(Joint::Type::SHOULDER].jvel RAD2DEG, // arm->getJointById(Joint::Type::ELBOW].jvel RAD2DEG, // arm->getJointById(Joint::Type::TOOL_ROT].jvel RAD2DEG, // arm->getJointById(Joint::Type::INSERTION_].jvel, // arm->getJointById(Joint::Type::WRIST].jvel RAD2DEG, // (arm->getJointById(Joint::Type::GRASP1].jvel - arm->getJointById(Joint::Type::GRASP2].jvel) / 2 RAD2DEG, // arm->getJointById(Joint::Type::GRASP1].jvel + arm->getJointById(Joint::Type::GRASP2].jvel RAD2DEG, // arm->getJointById(Joint::Type::GRASP1].jvel RAD2DEG,arm->getJointById(Joint::Type::GRASP2].jvel RAD2DEG); // log_msg("t s % 1.3f e % 1.3f r % 1.3f i % 1.3f p % 1.3f g1 % 1.3f g2 % 1.3f",arm->getJointById(Joint::Type::SHOULDER].tau_d, // arm->getJointById(Joint::Type::ELBOW].tau_d,arm->getJointById(Joint::Type::TOOL_ROT].tau_d,arm->getJointById(Joint::Type::INSERTION_].tau_d,arm->getJointById(Joint::Type::WRIST].tau_d, // arm->getJointById(Joint::Type::GRASP1].tau_d,arm->getJointById(Joint::Type::GRASP2].tau_d); // log_msg("d s %d e %d r %d i %d p %d g1 %d g2 %d",tToDACVal(&(arm->getJointById(Joint::Type::SHOULDER])), // tToDACVal(&(arm->getJointById(Joint::Type::ELBOW])),tToDACVal(&(arm->getJointById(Joint::Type::TOOL_ROT])),tToDACVal(&(arm->getJointById(Joint::Type::INSERTION_])),tToDACVal(&(arm->getJointById(Joint::Type::WRIST])), // tToDACVal(&(arm->getJointById(Joint::Type::GRASP1])),tToDACVal(&(arm->getJointById(Joint::Type::GRASP2]))); // log_msg("cp (% 1.3f,% 1.3f,% 1.3f\typr (% 1.3f,% 1.3f,% 1.3f))", // currentPoint.x(),currentPoint.y(),currentPoint.z(), // currentPoseAngles.yaw_deg,currentPoseAngles.pitch_deg,currentPoseAngles.roll_deg); // log_msg("pt (% 1.3f,% 1.3f,% 1.3f)\typr (% 1.3f,% 1.3f,% 1.3f)\tg % 1.3f", // actualPoint.x(),actualPoint.y(),actualPoint.z(), // actualPoseAngles.yaw_deg,actualPoseAngles.pitch_deg,actualPoseAngles.roll_deg, // grasp); // // btVector3 point = actualPose_fk.getOrigin(); // tb_angles angles = tb_angles(actualPose_fk.getBasis()); // log_msg("fp (% 1.3f,% 1.3f,% 1.3f)\typr (% 2.1f,% 2.1f,% 2.1f)\tg % 1.3f", // point.x(),point.y(),point.z(), // angles.yaw_deg,angles.pitch_deg,angles.roll_deg, // grasp); // // } /* * Actual pose is in the actual world frame, so we have <actual_world to gripper> * The ik world frame is the base frame. * Therefore, we need <base to actual_world> to get <base to gripper>. * <base to actual_world> is inverse of <actual_world to base> * * Since the ik is based on the yaw frame (to which the gripper is fixed), we * take the pose of the yaw frame, not the gripper frame */ btTransform ik_pose = ik_world_to_actual_world(armId) * pose * Tg.inverse(); btMatrix3x3 ik_orientation = ik_pose.getBasis(); btVector3 ik_point = ik_pose.getOrigin(); tb_angles ikPoseAngles = tb_angles(ik_pose); // if (print) { // log_msg("ik (%0.4f,%0.4f,%0.4f)\typr (%0.4f,%0.4f,%0.4f)", // ik_point.x(),ik_point.y(),ik_point.z(), // ikPoseAngles.yaw_deg,ikPoseAngles.pitch_deg,ikPoseAngles.roll_deg); // } float ths_offset, thr_offset, thp_offset; if (arm->isGold()) { ths_offset = SHOULDER_OFFSET_GOLD; thr_offset = TOOL_ROT_OFFSET_GOLD; thp_offset = WRIST_OFFSET_GOLD; } else { ths_offset = SHOULDER_OFFSET_GREEN; thr_offset = TOOL_ROT_OFFSET_GREEN; thp_offset = WRIST_OFFSET_GREEN; } const float th12 = THETA_12; const float th23 = THETA_23; const float ks12 = sin(th12); const float kc12 = cos(th12); const float ks23 = sin(th23); const float kc23 = cos(th23); const float dw = DW; btTransform Tworld_to_gripper = ik_pose; btTransform Tgripper_to_world = ik_pose.inverse(); // if (print) { // log_msg("Tw2g: %d",PRINT_EVERY); // for (int i=0;i<3;i++) { // log_msg(" %0.4f\t%0.4f\t%0.4f\t%0.4f",ik_pose.getBasis()[i][0],ik_pose.getBasis()[i][1],ik_pose.getBasis()[i][2],ik_pose.getOrigin()[i]); // } // } btVector3 origin_in_gripper_frame = Tgripper_to_world.getOrigin(); float px = origin_in_gripper_frame.x(); float py = origin_in_gripper_frame.y(); float pz = origin_in_gripper_frame.z(); float thy = atan2f(py,-px); float thp; if (fabs(thy) < 0.001) { thp = atan2f(-pz, -px/cos(thy) - dw); // if (print) { log_msg("zero thy: %0.4f, (%0.4f, %0.4f, %0.4f)",thp,px,py,pz); } } else { thp = atan2f(-pz, py/sin(thy) - dw); } float d = -pz / sin(thp); float d_act, thp_act, thy_act, g1_act, g2_act; d_act = D_FROM_IK(armId,d); thp_act = THP_FROM_IK(armId,thp); thy_act = THY_FROM_IK(armId,thy,grasp); g1_act = FINGER1_FROM_IK(armId,thy,grasp); g2_act = FINGER2_FROM_IK(armId,thy,grasp); //check angles int validity1[4]; bool valid1 = checkJointLimits1(d_act,thp_act,g1_act,g2_act,validity1); if (!valid1) { // if (_curr_rl == 3 && !(DISABLE_ALL_PRINTING)) { // printf("ik %d invalid --1-- d [%d] % 2.4f \tp [%d] % 3.1f\ty [%d %d] % 3.1f\n", // armId, // validity1[0], d_act, // validity1[1], thp_act RAD2DEG, // validity1[2],validity1[3], thy_act RAD2DEG); // } return report; } //set joints //setJointsWithLimits1(mech,d_act,thp_act,thy_act,grasp); btVector3 z_roll_in_world = btTransform(Zi(d) * Xip * Zp(thp) * Xpy * Zy(thy) * Tg * Tgripper_to_world).invXform(btVector3(0,0,1)); btVector3 x_roll_in_world = btTransform(Zi(d) * Xip * Zp(thp) * Xpy * Zy(thy) * Tg * Tgripper_to_world).invXform(btVector3(1,0,0)); float zx = z_roll_in_world.x(); float zy = z_roll_in_world.y(); float zz = z_roll_in_world.z(); float xx = x_roll_in_world.x(); float xy = x_roll_in_world.y(); float xz = x_roll_in_world.z(); float cthe = (zy + kc12*kc23) / (ks12*ks23); float the_1 = acos(cthe); float the_2 = -acos(cthe); float the_opt[2]; the_opt[0] = the_1; the_opt[1] = the_2; float ths_opt[2]; float thr_opt[2]; bool opts_valid[2]; float validity2[2][4]; float ths_act[2]; float the_act[2]; float thr_act[2]; for (int i=0;i<2;i++) { float sthe_tmp = sin(the_opt[i]); float C1 = ks12*kc23 + kc12*ks23*cthe; float C2 = ks23 * sthe_tmp; float C3 = C2 + C1*C1 / C2; ths_opt[i] = atan2( -sgn(C3)*(zx - C1 * zz / C2), sgn(C3)*(zz + C1 * zx / C2)); float sths_tmp = sin(ths_opt[i]); float cths_tmp = cos(ths_opt[i]); float C4 = ks12 * sin(the_opt[i]); float C5 = kc12 * ks23 + ks12 * kc23 * cos(the_opt[i]); float C6 = kc23*(sthe_tmp * sths_tmp - kc12*cthe*cths_tmp) + cths_tmp*ks12*ks23; float C7 = cthe*sths_tmp + kc12*cths_tmp*sthe_tmp; thr_opt[i] = atan2( (xx - C7 * xy / C4) / (C6 + C7*C5/C4), (xx + C6 * xy / C5) / (-C6*C4/C5 - C7)); ths_act[i] = THS_FROM_IK(armId,ths_opt[i]); the_act[i] = THE_FROM_IK(armId,the_opt[i]); thr_act[i] = THR_FROM_IK(armId,thr_opt[i]); // if (print) { // log_msg("j s % 3.1f e % 3.1f r % 3.1f i % 1.3f p % 3.1f y % 3.1f g % 3.1f g1 % 3.1f g2 % 3.1f", // arm->getJointById(Joint::Type::SHOULDER_)->position() RAD2DEG, // arm->getJointById(Joint::Type::ELBOW_)->position() RAD2DEG, // arm->getJointById(Joint::Type::TOOL_ROT_)->position() RAD2DEG, // arm->getJointById(Joint::Type::INSERTION__)->position(), // arm->getJointById(Joint::Type::WRIST_)->position() RAD2DEG, // THY_MECH_FROM_FINGERS(armIdFromMechType(mech->type),arm->getJointById(Joint::Type::GRIPPER1_)->position(), arm->getJointById(Joint::Type::GRIPPER2_)->position()) RAD2DEG,//fix_angle(arm->getJointById(Joint::Type::GRIPPER2_)->position() - arm->getJointById(Joint::Type::GRIPPER1_)->position(),0) / 2 RAD2DEG, // mech->ori.grasp * 1000. RAD2DEG, // fix_angle(arm->getJointById(Joint::Type::GRIPPER1_)->position() + arm->getJointById(Joint::Type::GRIPPER2_)->position(),0) RAD2DEG, // arm->getJointById(Joint::Type::GRIPPER1_)->position() RAD2DEG, arm->getJointById(Joint::Type::GRIPPER2_)->position() RAD2DEG); // log_msg("%d s % 3.1f e % 3.1f r % 3.1f i % 1.3f p % 3.1f y % 3.1f g % 3.1f g1 % 3.1f g2 % 3.1f",i, // ths_act[i] RAD2DEG, // the_act[i] RAD2DEG, // thr_act[i] RAD2DEG, // d_act, // thp_act RAD2DEG, // thy_act RAD2DEG, // grasp RAD2DEG, // g1_act RAD2DEG, // g2_act RAD2DEG); // // if (ths_act != ths_act) { // log_msg("C1 %0.4f\tC2 %0.4f\tC3 %0.4f\tC4 %0.4f\tC5 %0.4f\tC6 %0.4f\tC7 %0.4f\t",C1,C2,C3,C4,C5,C6,C7); // log_msg("ks23 %0.4f\tsthe_tmp %0.4f",ks23 , sthe_tmp); // log_msg("cthe %0.4f\tzy %0.4f\tkc12 %0.4f\tkc23 %0.4f\tks12 %0.4f\tks23 %0.4f",cthe,zy,kc12,kc23,ks12,ks23); // } // } bool valid2 = checkJointLimits2(ths_act[i],the_act[i],thr_act[i],validity2[i]); opts_valid[i] = valid2; if (valid2) { float ths_diff, the_diff, d_diff, thr_diff, thp_diff, thg1_diff, thg2_diff; //set joints setJointsWithLimits1(arm,d_act,thp_act,g1_act,g2_act); setJointsWithLimits2(arm,ths_act[i],the_act[i],thr_act[i]); ths_diff = arm->getJointById(Joint::IdType::SHOULDER_)->position() - ths_act[i]; the_diff = arm->getJointById(Joint::IdType::ELBOW_)->position() - the_act[i]; d_diff = arm->getJointById(Joint::IdType::INSERTION_)->position() - d_act; thr_diff = arm->getJointById(Joint::IdType::ROTATION_)->position() - thr_act[i]; thp_diff = arm->getJointById(Joint::IdType::WRIST_)->position() - thp_act; thg1_diff = arm->getJointById(Joint::IdType::FINGER1_)->position() - g1_act; thg2_diff = arm->getJointById(Joint::IdType::FINGER2_)->position() - g2_act; /* ths_diff = arm->getJointById(Joint::Type::SHOULDER_)->position()_d - ths_act; the_diff = arm->getJointById(Joint::Type::ELBOW_)->position()_d - the_act; d_diff = arm->getJointById(Joint::Type::INSERTION__)->position()_d - d_act; thr_diff = arm->getJointById(Joint::Type::TOOL_ROT_)->position()_d - thr_act; thp_diff = arm->getJointById(Joint::Type::WRIST_)->position()_d - thp_act; thg1_diff = arm->getJointById(Joint::Type::GRIPPER1_)->position()_d - g1_act; thg2_diff = arm->getJointById(Joint::Type::GRIPPER2_)->position()_d - g2_act; */ // if (print) { // log_msg("%d s % 2.1f e % 2.1f r % 2.1f i % 1.3f p % 2.1f g % 2.1f g1 % 2.1f g2 % 2.1f",i, // arm->getJointById(Joint::Type::SHOULDER_)->position()_d RAD2DEG, // arm->getJointById(Joint::Type::ELBOW_)->position()_d RAD2DEG, // arm->getJointById(Joint::Type::TOOL_ROT_)->position()_d RAD2DEG, // arm->getJointById(Joint::Type::INSERTION__)->position()_d, // arm->getJointById(Joint::Type::WRIST_)->position()_d RAD2DEG, // grasp RAD2DEG, // arm->getJointById(Joint::Type::GRIPPER1_)->position()_d RAD2DEG, // arm->getJointById(Joint::Type::GRIPPER2_)->position()_d RAD2DEG); // log_msg("diff:"); // log_msg("R s %0.4f e %0.4f r %0.4f d %0.4f p %0.4f g1 %0.4f g2 %0.4f", // ths_diff,the_diff,thr_diff,d_diff,thp_diff,thg1_diff,thg2_diff); // log_msg("D s %0.4f e %0.4f r %0.4f d %0.4f p %0.4f g1 %0.4f g2 %0.4f", // ths_diff*180/M_PI,the_diff*180/M_PI,thr_diff*180/M_PI,d_diff, // thp_diff*180/M_PI,thg1_diff*180/M_PI,thg2_diff*180/M_PI); // // } // // if (i==1 && _curr_rl == 3) { // //printf("ik ok! %d\n",_ik_counter); // } } } if (opts_valid[0]) { report->success_ = true; return report; } else if (opts_valid[1]) { // bool ENABLE_PARTIAL_INVALID_IK_PRINTING = false; // if (ENABLE_PARTIAL_INVALID_IK_PRINTING && (_curr_rl == 3 || print) && !(DISABLE_ALL_PRINTING)) { // printf("ik %d ok **2** s %1.4f %1.4f\te %1.4f %1.4f\tr %1.4f %1.4f\n", // armId, // ths_act[0] RAD2DEG,validity2[0][0], // the_act[0] RAD2DEG,validity2[0][1], // thr_act[0] RAD2DEG,validity2[0][2]); // /* // printf("%7d d %0.4f \tp %0.4f \ty %0.4f\n",_ik_counter, // d_act,thp_act RAD2DEG,thy_act RAD2DEG); // printf("x (%f, %f, %f) z (%f, %f, %f) %f\n",xx,xy,xz,zx,zy,zz,cthe); // printf("norms %f %f\n",x_roll_in_world.length(),z_roll_in_world.length()); // printf("(zy + kc12*kc23): %f (%f, %f)\n",(zy + kc12*kc23),zy, kc12*kc23); // printf("(ks12*ks23): %f\n",(ks12*ks23)); // */ // } report->success_ = true; return report; } else { const float maxValidDist = 3 DEG2RAD; float valid_dist[2]; for (int i=0;i<2;i++) { float sum = 0; for (int j=0;j<3;j++) { float v = fabs(validity2[i][j]); sum += v*v; } valid_dist[i] = sqrt(sum); } bool use0 = valid_dist[0] < maxValidDist && valid_dist[0] < valid_dist[1]; bool use1 = valid_dist[1] < maxValidDist && valid_dist[0] > valid_dist[1]; printf("ik validity distances: (%s | %s) % 1.3f\t%f\n",use0?"Y":" ",use1?"Y":" ",valid_dist[0],valid_dist[1]); if (valid_dist[0] < maxValidDist && valid_dist[0] < valid_dist[1]) { printf("setting joints to ik soln 1\n"); setJointsWithLimits1(arm,d_act,thp_act,g1_act,g2_act); setJointsWithLimits2(arm,ths_act[0],the_act[0],thr_act[0]); } else if (valid_dist[1] < maxValidDist) { printf("setting joints to ik soln 2\n"); setJointsWithLimits1(arm,d_act,thp_act,g1_act,g2_act); setJointsWithLimits2(arm,ths_act[1],the_act[1],thr_act[1]); } // if ((_curr_rl == 3 || print) && !(DISABLE_ALL_PRINTING)) { // printf("ik %d invalid **2** s %1.4f % 2.1f\te %1.4f % 2.1f\tr %1.4f % 2.1f\n", // armId, // ths_act[0] RAD2DEG,validity2[0][0] RAD2DEG, // the_act[0] RAD2DEG,validity2[0][1] RAD2DEG, // thr_act[0] RAD2DEG,validity2[0][2] RAD2DEG); // printf(" s %1.4f % 2.1f\te %1.4f % 2.1f\tr %1.4f % 2.1f\n", // ths_act[1] RAD2DEG,validity2[1][0] RAD2DEG, // the_act[1] RAD2DEG,validity2[1][1] RAD2DEG, // thr_act[1] RAD2DEG,validity2[1][2] RAD2DEG); // } return report; } return report; }
void SIC_Courier(EscEscape& e) { e = EscFont(Courier(Zy(e.Int(0)))); }
void SIC_Roman(EscEscape& e) { e = EscFont(Roman(Zy(e.Int(0)))); }
void SIC_Arial(EscEscape& e) { e = EscFont(Arial(Zy(e.Int(0)))); }
void VideoFluids::trackVelocity(Matrix& Zn1,Matrix& Zn,Matrix& U,Matrix& V) { Matrix Zx(height,width),Zy(height,width),ZZx(height,width),ZZy(height,width),Zt(height,width),ZZt(height,width),ZZtx(height,width),ZZty(height,width); Matrix Au1(height,width),Au2(height,width),Av1(height,width),Av2(height,width); Matrix Z2x(height,width),Z2y(height,width),Z2(height,width); Matrix Cu(height,width),Cv(height,width); Matrix tmp(height,width),tmp1(height,width); Matrix U_old(height,width),V_old(height,width),Ux(height,width),Uy(height,width),Vx(height,width),Vy(height,width),Uax(height,width),Uay(height,width),Vax(height,width),Vay(height,width),Uxy(height,width),Vxy(height,width); Matrix Coe(height,width); Zt = Zn; Zt -= Zn1; DotMul(Zn,Zt,ZZt); Zn.output("Zn.txt"); Zn1.output("Zn1.txt"); Zt.output("Zt.txt"); Partial(ZZt,ZZtx,AXIS_X); Partial(ZZt,ZZty,AXIS_Y); Partial(Zn,Zx,AXIS_X); Partial(Zn,Zy,AXIS_Y); DotMul(Zn,Zx,ZZx); DotMul(Zn,Zy,ZZy); DotMul(Zx,Zx,Au1); Partial(ZZx,tmp,AXIS_X); Au1-=tmp; DotMul(Zn,Zn,tmp); Au1+=tmp; Au1+=2*alpha*alpha; DotMul(Zx,Zy,Au2); Partial(ZZy,tmp,AXIS_X); Au2-=tmp; DotMul(Zx,Zy,Av1); Partial(ZZx,tmp,AXIS_Y); Av1-=tmp; DotMul(Zy,Zy,Av2); Partial(ZZy,tmp,AXIS_Y); Av2-=tmp; DotMul(Zn,Zn,tmp); Av2+=tmp; Av2+=2*alpha*alpha; DotMul(Zn,Zn,Z2); Partial(Z2,Z2x,AXIS_X); Partial(Z2,Z2y,AXIS_Y); for (int i = 0;i<height;i++) for (int j = 0;j<width;j++) Coe[i][j] = 1.0/(Au1[i][j]*Av2[i][j]-Au2[i][j]*Av1[i][j]); U = 0.0; V = 0.0; for (int iter_time = 0;iter_time<iterationTime;iter_time++) { V_old = V; U_old = U; Partial(U,Ux,AXIS_X); Partial(U,Uy,AXIS_Y); Partial(V,Vx,AXIS_X); Partial(V,Vy,AXIS_Y); Partial(Vx,Vxy,AXIS_Y); Partial(Ux,Uxy,AXIS_Y); Average(U,Uax,AXIS_X); Average(U,Uay,AXIS_Y); Average(V,Vax,AXIS_X); Average(V,Vay,AXIS_Y); DotMul(Z2x,Ux,Cu); DotMul(ZZy,Vx,tmp); Cu += tmp; tmp = ZZx*-1; tmp+=Z2x; DotMul(tmp,Vy,tmp1); Cu+=tmp1; tmp = Z2; tmp+=alpha*alpha; DotMul(tmp,Uax,tmp1); Cu+=tmp1; tmp1=Uay; tmp1*=alpha*alpha; Cu+=tmp1; DotMul(Z2,Vxy,tmp1); Cu+=tmp1; DotMul(Zx,Zt,tmp); Cu-=tmp; Cu+=ZZtx; DotMul(Z2y,Vy,Cv); DotMul(ZZx,Uy,tmp); Cv += tmp; tmp = ZZy; tmp*=-1; tmp+=Z2y; DotMul(tmp,Ux,tmp1); Cv+=tmp1; tmp = Z2; tmp+=alpha*alpha; DotMul(tmp,Vay,tmp1); Cv+=tmp1; tmp1=Vax; tmp1*=alpha*alpha; Cv+=tmp1; DotMul(Z2,Uxy,tmp1); Cv+=tmp1; DotMul(Zy,Zt,tmp); Cv-=tmp; Cv+=ZZty; for (int i = 0;i<height;i++) for (int j = 0;j<width;j++) { U[i][j] = Coe[i][j]*(Av2[i][j]*Cu[i][j]-Au2[i][j]*Cv[i][j]); V[i][j] = Coe[i][j]*(-Av1[i][j]*Cu[i][j]+Au1[i][j]*Cv[i][j]); } for (int i = 0;i<height;i++) { U[i][0] = U[i][1]; U[i][width-1] = U[i][width-2]; V[i][0] = V[i][1]; V[i][width-1] =V[i][width-2]; } for (int i = 0;i<width;i++) { U[0][i] = U[1][i]; U[height-1][i] = U[height-2][i]; V[0][i] = V[1][i]; V[height-1][i] =V[height-2][i]; } FILE* fp; // Au1.output("Au1.txt"); // Au2.output("Au2.txt"); // Av1.output("Av1.txt"); // Av2.output("Av2.txt"); // Cu.output("Cu.txt"); // Cv.output("Cv.txt"); float d1 = Difference(U,U_old); float d2 = Difference(V,V_old); // U.output("U.txt"); // U_old.output("U_old.txt"); // V.output("V.txt"); cout<<d1<<' '<<d2<<endl; if (d1<iterationTorlerance && d2<iterationTorlerance) break; } U.output("U.txt"); cv::Mat showV(height,width,CV_8UC3); float lowv=10000000,lowu=10000000,highu=-10000000,highv=-1000000; for(int j=0;j<height;j++){ for(int k=0;k<width;k++){ if(U[j][k]>highu) highu=U[j][k]; if(U[j][k]<lowu) lowu=U[j][k]; if(V[j][k]>highv) highv=V[j][k]; if(V[j][k]<lowv) lowv=V[j][k]; } } for(int j=0;j<height;j++){ for(int k=0;k<width;k++){ //printf("%d %d\n",j,k); //if(sfs_list[i][j][k]<low) // showH.at<uchar>(j,k)=0; //else float u=(U[j][k]-lowu)/(highu-lowu); float v=(V[j][k]-lowv)/(highv-lowv); if(u>0.5) showV.at<cv::Vec3b>(j,k)[2]=255; else showV.at<cv::Vec3b>(j,k)[2]=255*u; if(v>0.5){ showV.at<cv::Vec3b>(j,k)[0]=255; showV.at<cv::Vec3b>(j,k)[1]=255*(1-v); } else{ showV.at<cv::Vec3b>(j,k)[1]=255; showV.at<cv::Vec3b>(j,k)[0]=255*v; } } } cv::imwrite("testV.bmp",showV); printf("show you"); }