Esempio n. 1
0
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))));
}
Esempio n. 2
0
void HelpWindow::SetZoom()
{
	zoom.d = 1000000 / Zy(800);
	current_link = Null;
	GoTo0(topic);
	Refresh();
}
Esempio n. 3
0
	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;
	}
Esempio n. 4
0
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;
}
Esempio n. 5
0
void SIC_StdFont(EscEscape& e)
{
	if(e.GetCount() == 1)
		e = EscFont(StdFont()(Zy(e.Int(0))));
	else
		e = EscFont(StdFont());
}
Esempio n. 6
0
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();
}
Esempio n. 7
0
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);
}
Esempio n. 8
0
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);
}
Esempio n. 9
0
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);
}
Esempio n. 10
0
int TopMenuItem::GetStdHeight(Font font)
{
	return font.Info().GetHeight() + Zy(7);
}
Esempio n. 11
0
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);
	}
}
Esempio n. 12
0
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;
}
Esempio n. 13
0
void SIC_Courier(EscEscape& e)
{
	e = EscFont(Courier(Zy(e.Int(0))));
}
Esempio n. 14
0
void SIC_Roman(EscEscape& e)
{
	e = EscFont(Roman(Zy(e.Int(0))));
}
Esempio n. 15
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");
		

}