Class for solving a set of TSRs. More...

#include <gui.h>

Inheritance diagram for robowflex::darts::TSRSolveWidget:

Classes
struct	ErrorLines
	Error plots for TSRs. More...

Public Member Functions
	TSRSolveWidget (const WorldPtr &world, const std::vector< TSRPtr > &tsrs)
	Constructor. More...

	TSRSolveWidget (const TSRSetPtr &tsrs)
	Constructor. More...

void	initialize (Window *window) override
	Initialization with window context. More...

void	prerefresh () override
	Called before window refresh. More...

void	render () override
	Render GUI. More...

void	solve ()
	Solve for a solution to the current TSR. More...

Private Attributes
TSRSetPtr	tsrs_
	TSR set. More...

GUI Values
const float	max_tolerance_ {0.1f}
	Max tolerance value. More...

const int	max_iteration_ {200}
	Max iteration value. More...

const float	drag_tolerance_ {0.01f}
	Slider drag for tolerance. More...

bool	track_tsr_ {false}
	Track the TSR by solving IK. More...

bool	use_gradient_ {false}
	Use gradient solving instead of built-in. More...

bool	need_solve_ {false}
	A solve is requested. More...

float	step_
	GUI gradient step size. More...

float	limit_
	GUI gradient limit. More...

float	damping_
	GUI SVD damping. More...

float	tolerance_
	GUI solver tolerance. More...

int	maxIter_
	GUI maximum allowed iterations. More...

int	item_ {0}
	GUI solver. More...

GUI Plots
bool	last_solve_ {false}
	Result of last TSR solve. More...

LinePlotElement	solve_time_
	Plot of TSR solve times. More...

std::vector< ErrorLines >	errors_

Detailed Description

Class for solving a set of TSRs.

Definition at line 555 of file gui.h.

Constructor & Destructor Documentation

◆ TSRSolveWidget() [1/2]

TSRSolveWidget::TSRSolveWidget	(	const WorldPtr &	world,
		const std::vector< TSRPtr > &	tsrs
	)

Constructor.

Parameters

[in]	world	World to use.
[in]	tsrs	Set of TSRs to consider.

Definition at line 765 of file gui.cpp.

   : TSRSolveWidget(std::make_shared<TSRSet>(world, tsrs, false))
 {
 }

◆ TSRSolveWidget() [2/2]

TSRSolveWidget::TSRSolveWidget ( const TSRSetPtr & tsrs )

Constructor.

Parameters

[in] tsrs Set of TSRs to consider.

Definition at line 770 of file gui.cpp.

                                                     : tsrs_(tsrs)
 {
 }

Member Function Documentation

◆ initialize()

void TSRSolveWidget::initialize ( Window * window )

overridevirtual

Initialization with window context.

Parameters

[in] window GUI window.

Reimplemented from robowflex::darts::Widget.

Definition at line 903 of file gui.cpp.

 {
     // Compute world indices for all TSRs
     const auto &tsrs = tsrs_->getTSRs();
     std::set<std::pair<std::size_t, std::size_t>> wis;
     for (const auto &tsr : tsrs)
     {
         std::vector<std::pair<std::size_t, std::size_t>> wts =
             (tsr->getNumWorldDofs()) ? tsr->getWorldIndices() : tsr->computeWorldIndices();
  
         for (const auto &index : wts)
             wis.emplace(index);
     }
  
     std::vector<std::pair<std::size_t, std::size_t>> wts;
     wts.reserve(wis.size());
     for (const auto &index : wis)
         wts.emplace_back(index);
  
     tsrs_->useWorldIndices(wts);
     tsrs_->setWorldIndices(wts);
     tsrs_->computeLimits();
  
     tsrs_->initialize();
  
     // Setup solve parameters.
     maxIter_ = tsrs_->getMaxIterations();
     tolerance_ = tsrs_->getTolerance();
     step_ = tsrs_->getStep();
     limit_ = tsrs_->getLimit();
     damping_ = tsrs_->getDamping();
  
     // Setup solve time plots.
     solve_time_.label = "Solve Time";
     solve_time_.units = "microsec.";
     solve_time_.show_min = true;
     solve_time_.show_max = true;
     solve_time_.show_avg = true;
  
     // Setup error plots.
     const std::size_t n = tsrs_->numTSRs();
     errors_.resize(n);
  
     for (std::size_t i = 0; i < n; ++i)
     {
         errors_[i].xpd.color = Eigen::Vector3d(1, 0, 0);
         errors_[i].ypd.color = Eigen::Vector3d(0, 1, 0);
         errors_[i].zpd.color = Eigen::Vector3d(0, 0, 1);
  
         errors_[i].xrd.label = "X";
         errors_[i].xrd.color = Eigen::Vector3d(1, 0, 0);
         errors_[i].yrd.label = "Y";
         errors_[i].yrd.color = Eigen::Vector3d(0, 1, 0);
         errors_[i].zrd.label = "Z";
         errors_[i].zrd.color = Eigen::Vector3d(0, 0, 1);
     }
 }

◆ prerefresh()

void TSRSolveWidget::prerefresh ( )

overridevirtual

Called before window refresh.

Reimplemented from robowflex::darts::Widget.

Definition at line 961 of file gui.cpp.

 {
     if (track_tsr_ or need_solve_)
         solve();
  
     need_solve_ = false;
  
     const auto &tsrs = tsrs_->getTSRs();
     for (std::size_t i = 0; i < tsrs.size(); ++i)
     {
         Eigen::VectorXd e = Eigen::VectorXd::Zero(6);
         tsrs[i]->getErrorWorldRaw(e);
  
         errors_[i].xrd.addPoint(e[0]);
         errors_[i].yrd.addPoint(e[1]);
         errors_[i].zrd.addPoint(e[2]);
         errors_[i].xpd.addPoint(e[3]);
         errors_[i].ypd.addPoint(e[4]);
         errors_[i].zpd.addPoint(e[5]);
     }
 }

◆ render()

void TSRSolveWidget::render ( )

override

Render GUI.

Definition at line 774 of file gui.cpp.

 {
     // Draw main window.
     ImGui::SetNextWindowBgAlpha(0.5f);
     std::string title = "TSR Solver";
     if (!ImGui::Begin(title.c_str(), nullptr, ImGuiWindowFlags_HorizontalScrollbar))
     {
         ImGui::End();
         return;
     }
  
     // Draw TSR solve interface.
     if (ImGui::TreeNodeEx("Solving", ImGuiTreeNodeFlags_DefaultOpen))
     {
         ImGui::Checkbox("Track TSR", &track_tsr_);
         ImGui::SameLine();
         ImGui::Checkbox("Use Gradient?", &use_gradient_);
  
         ImGui::Columns(2);
         if (ImGui::SliderInt("Max Iter.", &maxIter_, 1, max_iteration_))
         {
             tsrs_->setMaxIterations(maxIter_);
             tsrs_->updateSolver();
         }
         if (ImGui::SliderFloat("Step", &step_, 0.001f, 1.f))
             tsrs_->setStep(step_);
  
         if (ImGui::SliderFloat("Limit", &limit_, 0.001f, 1.f))
             tsrs_->setLimit(limit_);
  
         ImGui::NextColumn();
         if (ImGui::SliderFloat("Tol.", &tolerance_, 1e-5f, max_tolerance_, "< %.5f", 3.f))
         {
             tsrs_->setMaxIterations(tolerance_);
             tsrs_->updateSolver();
         }
  
         const char *items[] = {"dSVD", "SVD", "QR"};
         if (ImGui::Combo("combo", &item_, items, IM_ARRAYSIZE(items)))
         {
             if (item_ == 0)
             {
                 tsrs_->useSVD();
                 tsrs_->useDamping(true);
             }
             else if (item_ == 1)
             {
                 tsrs_->useSVD();
                 tsrs_->useDamping(false);
             }
             else
                 tsrs_->useQR();
         }
  
         if (ImGui::SliderFloat("Damp.", &damping_, 1e-8f, 1e-3f, "< %.8f", 10.f))
             tsrs_->setDamping(damping_);
  
         ImGui::Columns(1);
  
         if (ImGui::Button("Solve TSR"))
             need_solve_ = true;
  
         ImGui::SameLine();
  
         if (last_solve_)
             ImGui::TextColored((ImVec4)ImColor(0.0f, 1.0f, 0.0f), "Success!");
         else
             ImGui::TextColored((ImVec4)ImColor(1.0f, 0.0f, 0.0f), "Failure!");
  
         solve_time_.render();
  
         ImGui::TreePop();
     }
  
     ImGui::Separator();
     ImGui::Spacing();
  
     // Draw distance tracking information.
  
     const auto &tsrs = tsrs_->getTSRs();
     for (std::size_t i = 0; i < tsrs.size(); ++i)
     {
         const auto &tsr = tsrs[i];
         const auto &spec = tsr->getSpecification();
  
         const std::string title = "B:" + spec.base.frame + ":" + spec.base.structure +  //
                                   " > T:" + spec.target.frame + ":" + spec.target.structure;
  
         if (ImGui::TreeNodeEx(title.c_str(), ImGuiTreeNodeFlags_DefaultOpen))
         {
             ImGui::Columns(2);
             ImGui::Text("Pos. Error");
             errors_[i].xpd.render();
             errors_[i].ypd.render();
             errors_[i].zpd.render();
             ImGui::NextColumn();
  
             ImGui::Text("Rot. Error");
             errors_[i].xrd.render();
             errors_[i].yrd.render();
             errors_[i].zrd.render();
  
             ImGui::Columns(1);
  
             ImGui::TreePop();
         }
     }
 }

◆ solve()

void TSRSolveWidget::solve ( )

Solve for a solution to the current TSR.

Definition at line 883 of file gui.cpp.

 {
     auto start = std::chrono::steady_clock::now();
  
     if (use_gradient_)
     {
         Eigen::VectorXd world(tsrs_->getWorldIndices().size());
         tsrs_->getPositionsWorldState(world);
  
         last_solve_ = tsrs_->solveGradientWorldState(world);
     }
     else
         last_solve_ = tsrs_->solveWorld();
  
     auto end = std::chrono::steady_clock::now();
  
     auto time = std::chrono::duration_cast<std::chrono::microseconds>(end - start).count();
     solve_time_.addPoint(time);
 }

Member Data Documentation

◆ damping_

float robowflex::darts::TSRSolveWidget::damping_

private

GUI SVD damping.

Definition at line 596 of file gui.h.

◆ drag_tolerance_

const float robowflex::darts::TSRSolveWidget::drag_tolerance_ {0.01f}

private

Slider drag for tolerance.

Definition at line 588 of file gui.h.

◆ errors_

std::vector<ErrorLines> robowflex::darts::TSRSolveWidget::errors_

private

Definition at line 621 of file gui.h.

◆ item_

int robowflex::darts::TSRSolveWidget::item_ {0}

private

GUI solver.

Definition at line 599 of file gui.h.

◆ last_solve_

bool robowflex::darts::TSRSolveWidget::last_solve_ {false}

private

Result of last TSR solve.

Definition at line 606 of file gui.h.

◆ limit_

float robowflex::darts::TSRSolveWidget::limit_

private

GUI gradient limit.

Definition at line 595 of file gui.h.

◆ max_iteration_

const int robowflex::darts::TSRSolveWidget::max_iteration_ {200}

private

Max iteration value.

Definition at line 587 of file gui.h.

◆ max_tolerance_

const float robowflex::darts::TSRSolveWidget::max_tolerance_ {0.1f}

private

Max tolerance value.

Definition at line 586 of file gui.h.

◆ maxIter_

int robowflex::darts::TSRSolveWidget::maxIter_

private

GUI maximum allowed iterations.

Definition at line 598 of file gui.h.

◆ need_solve_

bool robowflex::darts::TSRSolveWidget::need_solve_ {false}

private

A solve is requested.

Definition at line 592 of file gui.h.

◆ solve_time_

LinePlotElement robowflex::darts::TSRSolveWidget::solve_time_

private

Plot of TSR solve times.

Definition at line 607 of file gui.h.

◆ step_

float robowflex::darts::TSRSolveWidget::step_

private

GUI gradient step size.

Definition at line 594 of file gui.h.

◆ tolerance_

float robowflex::darts::TSRSolveWidget::tolerance_

private

GUI solver tolerance.

Definition at line 597 of file gui.h.

◆ track_tsr_

bool robowflex::darts::TSRSolveWidget::track_tsr_ {false}

private

Track the TSR by solving IK.

Definition at line 590 of file gui.h.

◆ tsrs_

TSRSetPtr robowflex::darts::TSRSolveWidget::tsrs_

private

TSR set.

Definition at line 581 of file gui.h.

◆ use_gradient_

bool robowflex::darts::TSRSolveWidget::use_gradient_ {false}

private

Use gradient solving instead of built-in.

Definition at line 591 of file gui.h.

The documentation for this class was generated from the following files:

Classes

Public Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ TSRSolveWidget() [1/2]

◆ TSRSolveWidget() [2/2]

Member Function Documentation

◆ initialize()

◆ prerefresh()

◆ render()

◆ solve()

Member Data Documentation

◆ damping_

◆ drag_tolerance_

◆ errors_

◆ item_

◆ last_solve_

◆ limit_

◆ max_iteration_

◆ max_tolerance_

◆ maxIter_

◆ need_solve_

◆ solve_time_

◆ step_

◆ tolerance_

◆ track_tsr_

◆ tsrs_

◆ use_gradient_