Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
uam_task_ctrl
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Model registry
Operate
Environments
Monitor
Incidents
Service Desk
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
Pep Martí Saumell
uam_task_ctrl
Commits
8a4c78cb
Commit
8a4c78cb
authored
8 years ago
by
asantamaria
Browse files
Options
Downloads
Patches
Plain Diff
Commented PINV stuff to work in RR laptop
parent
0bda60f7
No related branches found
Branches containing commit
No related tags found
No related merge requests found
Changes
5
Hide whitespace changes
Inline
Side-by-side
Showing
5 changed files
src/common_fc.cpp
+4
-3
4 additions, 3 deletions
src/common_fc.cpp
src/common_fc.h
+4
-3
4 additions, 3 deletions
src/common_fc.h
src/kinematics.cpp
+2
-1
2 additions, 1 deletion
src/kinematics.cpp
src/tasks/ir.cpp
+2
-1
2 additions, 1 deletion
src/tasks/ir.cpp
src/tasks/jl.cpp
+4
-2
4 additions, 2 deletions
src/tasks/jl.cpp
with
16 additions
and
10 deletions
src/common_fc.cpp
+
4
−
3
View file @
8a4c78cb
...
...
@@ -29,9 +29,10 @@ Eigen::MatrixXd CCommon_Fc::WeightInvJac(const Eigen::MatrixXd& J, const Eigen::
}
// weighting matrix
Eigen
::
MatrixXd
Winv
=
CalcPinv
(
W
);
Eigen
::
MatrixXd
temp
=
J
*
Winv
*
J
.
transpose
();
Eigen
::
MatrixXd
J_inverse
=
Winv
*
J
.
transpose
()
*
CalcPinv
(
temp
);
// Eigen::MatrixXd Winv = CalcPinv(W);
// Eigen::MatrixXd temp = J*Winv*J.transpose();
// Eigen::MatrixXd J_inverse = Winv*J.transpose()*CalcPinv(temp);
J_inverse
=
J
.
transpose
();
return
J_inverse
;
}
...
...
This diff is collapsed.
Click to expand it.
src/common_fc.h
+
4
−
3
View file @
8a4c78cb
...
...
@@ -61,9 +61,10 @@ class CCommon_Fc
*/
template
<
typename
_Matrix_Type_
>
static
_Matrix_Type_
CalcPinv
(
const
_Matrix_Type_
&
a
,
double
epsilon
=
std
::
numeric_limits
<
double
>::
epsilon
())
{
Eigen
::
JacobiSVD
<
_Matrix_Type_
>
svdd
(
a
,
Eigen
::
ComputeThinU
|
Eigen
::
ComputeThinV
);
double
tolerance
=
epsilon
*
std
::
max
(
a
.
cols
(),
a
.
rows
())
*
svdd
.
singularValues
().
array
().
abs
()(
0
);
_Matrix_Type_
mpinv
=
svdd
.
matrixV
()
*
(
svdd
.
singularValues
().
array
().
abs
()
>
tolerance
).
select
(
svdd
.
singularValues
().
array
().
inverse
(),
0
).
matrix
().
asDiagonal
()
*
svdd
.
matrixU
().
adjoint
();
//Eigen::JacobiSVD< _Matrix_Type_ > svdd(a, Eigen::ComputeThinU | Eigen::ComputeThinV);
//double tolerance = epsilon * std::max(a.cols(), a.rows()) *svdd.singularValues().array().abs()(0);
//_Matrix_Type_ mpinv = svdd.matrixV() * (svdd.singularValues().array().abs() > tolerance).select(svdd.singularValues().array().inverse(), 0).matrix().asDiagonal() * svdd.matrixU().adjoint();
_Matrix_Type_
mpinv
=
a
.
transpose
();
return
mpinv
;
}
...
...
This diff is collapsed.
Click to expand it.
src/kinematics.cpp
+
2
−
1
View file @
8a4c78cb
...
...
@@ -25,7 +25,8 @@ void CKinematics::UAMKine(const UAM::CQuad& quad, UAM::CArm& arm, UAM::CHT& HT,
Eigen
::
MatrixXd
Jb2i
=
QuadJac
(
quad
.
pos
);
// Inertial to body frames
Eigen
::
MatrixXd
Ji2b
=
CCommon_Fc
::
CalcPinv
(
Jb2i
);
// Eigen::MatrixXd Ji2b = CCommon_Fc::CalcPinv(Jb2i);
Eigen
::
MatrixXd
Ji2b
=
Jb2i
.
inverse
();
// Jacobian from quadrotor body to camera
Eigen
::
MatrixXd
Jb2c
=
Eigen
::
MatrixXd
::
Zero
(
6
,
6
);
...
...
This diff is collapsed.
Click to expand it.
src/tasks/ir.cpp
+
2
−
1
View file @
8a4c78cb
...
...
@@ -44,7 +44,8 @@ void CTaskIR::TaskErrorJac(const int& arm_dof, const double& inf_rad, const Eige
}
// }
Eigen
::
MatrixXd
Hinv
=
UAM
::
CCommon_Fc
::
CalcPinv
(
H
);
// Eigen::MatrixXd Hinv = UAM::CCommon_Fc::CalcPinv(H);
Eigen
::
MatrixXd
Hinv
=
H
.
transpose
();
// weighting matrix
Eigen
::
MatrixXd
temp
=
JIR
*
Hinv
*
JIR
.
transpose
();
...
...
This diff is collapsed.
Click to expand it.
src/tasks/jl.cpp
+
4
−
2
View file @
8a4c78cb
...
...
@@ -47,7 +47,8 @@ void CTaskJL::TaskErrorJac(UAM::CArm& arm, const Eigen::MatrixXd& jntlim_pos_des
JL
.
block
(
0
,
4
,
1
,
arm
.
nj
)
=
pow_n
*
Jtemp
.
transpose
();
// Task Jacobian Pseudo-inverse
JL_pseudo
=
UAM
::
CCommon_Fc
::
CalcPinv
(
JL
);
//JL_pseudo = UAM::CCommon_Fc::CalcPinv(JL);
JL_pseudo
=
JL
.
transpose
();
}
void
CTaskJL
::
TaskErrorJacFull
(
UAM
::
CArm
&
arm
,
const
Eigen
::
MatrixXd
&
jntlim_pos_des
,
Eigen
::
MatrixXd
&
jntlim_pos_error
,
Eigen
::
MatrixXd
&
sigmaL
,
Eigen
::
MatrixXd
&
JL
,
Eigen
::
MatrixXd
&
JL_pseudo
)
...
...
@@ -69,7 +70,8 @@ void CTaskJL::TaskErrorJacFull(UAM::CArm& arm, const Eigen::MatrixXd& jntlim_pos
}
// Task Jacobian Pseudo-inverse
JL_pseudo
=
UAM
::
CCommon_Fc
::
CalcPinv
(
JL
);
//JL_pseudo = UAM::CCommon_Fc::CalcPinv(JL);
JL_pseudo
=
JL
.
transpose
();
}
}
// End of UAM namespace
\ No newline at end of file
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment