Commit 194cef0b authored by Olivier Stasse's avatar Olivier Stasse

[clang] Format

parent 9ce21d25
Pipeline #8286 failed with stage
in 96 minutes and 28 seconds
......@@ -151,7 +151,7 @@ public:
virtual void display(std::ostream &os) const;
public:
void servoCurrentPosition(const int& time);
void servoCurrentPosition(const int &time);
private:
MatrixHomogeneous &computefaMfb(MatrixHomogeneous &res, int time);
......
......@@ -12,9 +12,9 @@
#include "sot/core/api.hh"
#include "sot/core/feature-abstract.hh"
#include <dynamic-graph/value.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/signal-time-dependent.h>
#include <dynamic-graph/value.h>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
......
......@@ -11,8 +11,8 @@
#define __SOT_MEMORY_TASK_HH
#include "sot/core/api.hh"
#include <sot/core/task-abstract.hh>
#include <sot/core/matrix-svd.hh>
#include <sot/core/task-abstract.hh>
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
......@@ -24,8 +24,10 @@ namespace dg = dynamicgraph;
class SOT_CORE_EXPORT MemoryTaskSOT : public TaskAbstract::MemoryTaskAbstract {
public: // protected:
typedef Eigen::Map<Matrix, Eigen::internal::traits<Matrix>::Alignment> Kernel_t;
typedef Eigen::Map<const Matrix, Eigen::internal::traits<Matrix>::Alignment> KernelConst_t;
typedef Eigen::Map<Matrix, Eigen::internal::traits<Matrix>::Alignment>
Kernel_t;
typedef Eigen::Map<const Matrix, Eigen::internal::traits<Matrix>::Alignment>
KernelConst_t;
/* Internal memory to reduce the dynamic allocation at task resolution. */
dg::Vector err, tmpTask, tmpVar;
......@@ -36,17 +38,16 @@ public: // protected:
SVD_t svd;
Kernel_t kernel;
void resizeKernel(const Matrix::Index r, const Matrix::Index c)
{
void resizeKernel(const Matrix::Index r, const Matrix::Index c) {
if (kernel.rows() != r || kernel.cols() != c) {
if (kernelMem.size() < r*c) kernelMem.resize(r, c);
if (kernelMem.size() < r * c)
kernelMem.resize(r, c);
new (&kernel) Kernel_t(kernelMem.data(), r, c);
}
}
Kernel_t& getKernel(const Matrix::Index r, const Matrix::Index c)
{
resizeKernel(r,c);
Kernel_t &getKernel(const Matrix::Index r, const Matrix::Index c) {
resizeKernel(r, c);
return kernel;
}
......
......@@ -104,12 +104,12 @@ FeaturePose<representation>::FeaturePose(const string &pointName)
//
{
using namespace dynamicgraph::command;
addCommand(
"keep",
makeCommandVoid1(
*this, &FeaturePose<representation>::servoCurrentPosition,
docCommandVoid1(
"modify the desired position to servo at current pos.", "time")));
addCommand("keep",
makeCommandVoid1(
*this, &FeaturePose<representation>::servoCurrentPosition,
docCommandVoid1(
"modify the desired position to servo at current pos.",
"time")));
}
}
......@@ -319,7 +319,7 @@ Vector &FeaturePose<representation>::computeErrorDot(Vector &errordot,
* to the current position. The effect on the servo is to maintain the
* current position and correct any drift. */
template <Representation_t representation>
void FeaturePose<representation>::servoCurrentPosition(const int& time) {
void FeaturePose<representation>::servoCurrentPosition(const int &time) {
check(*this);
const MatrixHomogeneous &_oMja = (oMja.isPlugged() ? oMja.access(time) : Id),
......
......@@ -84,8 +84,8 @@ dg::Vector &FeaturePosture::computeError(dg::Vector &res, int t) {
}
dg::Matrix &FeaturePosture::computeJacobian(dg::Matrix &, int) {
throw std::runtime_error ("jacobian signal should be constant."
" This function should never be called");
throw std::runtime_error("jacobian signal should be constant."
" This function should never be called");
}
dg::Vector &FeaturePosture::computeErrorDot(dg::Vector &res, int t) {
......@@ -135,7 +135,7 @@ void FeaturePosture::selectDof(unsigned dofId, bool control) {
}
}
// recompute jacobian
Matrix J (Matrix::Zero(nbActiveDofs_, dim));
Matrix J(Matrix::Zero(nbActiveDofs_, dim));
std::size_t index = 0;
for (std::size_t i = 0; i < activeDofs_.size(); ++i) {
......
......@@ -5,15 +5,14 @@
#include <sot/core/matrix-svd.hh>
namespace dynamicgraph {
using Eigen::ComputeFullV;
using Eigen::ComputeThinU;
using Eigen::ComputeThinV;
using Eigen::ComputeFullV;
void pseudoInverse(Matrix &_inputMatrix, Matrix &_inverseMatrix,
const double threshold) {
SVD_t svd(_inputMatrix, ComputeThinU | ComputeThinV);
SVD_t::SingularValuesType m_singularValues =
svd.singularValues();
SVD_t::SingularValuesType m_singularValues = svd.singularValues();
SVD_t::SingularValuesType singularValues_inv;
singularValues_inv.resizeLike(m_singularValues);
for (long i = 0; i < m_singularValues.size(); ++i) {
......@@ -56,4 +55,4 @@ void dampedInverse(const Matrix &_inputMatrix, Matrix &_inverseMatrix,
dampedInverse(svd, _inverseMatrix, threshold);
}
} // namespace Eigen
} // namespace dynamicgraph
......@@ -480,7 +480,8 @@ struct MatrixToRPY : public UnaryOpHeader<MatrixRotation, VectorRollPitchYaw> {
};
REGISTER_UNARY_OP(MatrixToRPY, MatrixToRPY);
struct RPYToQuaternion : public UnaryOpHeader<VectorRollPitchYaw, VectorQuaternion> {
struct RPYToQuaternion
: public UnaryOpHeader<VectorRollPitchYaw, VectorQuaternion> {
void operator()(const VectorRollPitchYaw &r, VectorQuaternion &res) {
res = (Eigen::AngleAxisd(r(2), Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(r(1), Eigen::Vector3d::UnitY()) *
......@@ -490,7 +491,8 @@ struct RPYToQuaternion : public UnaryOpHeader<VectorRollPitchYaw, VectorQuaterni
};
REGISTER_UNARY_OP(RPYToQuaternion, RPYToQuaternion);
struct QuaternionToRPY : public UnaryOpHeader<VectorQuaternion, VectorRollPitchYaw> {
struct QuaternionToRPY
: public UnaryOpHeader<VectorQuaternion, VectorRollPitchYaw> {
void operator()(const VectorQuaternion &r, VectorRollPitchYaw &res) {
res = (r.toRotationMatrix().eulerAngles(2, 1, 0)).reverse();
}
......
......@@ -14,7 +14,7 @@ using namespace dynamicgraph::sot;
using namespace dynamicgraph;
MemoryTaskSOT::MemoryTaskSOT(const Matrix::Index nJ, const Matrix::Index mJ)
: kernel(NULL, 0, 0) {
: kernel(NULL, 0, 0) {
initMemory(nJ, mJ);
}
......@@ -28,7 +28,8 @@ void MemoryTaskSOT::initMemory(const Matrix::Index nJ, const Matrix::Index mJ) {
svd = SVD_t(nJ, mJ, Eigen::ComputeThinU | Eigen::ComputeFullV);
// If the constraint is well conditioned, the kernel can be pre-allocated.
if (mJ > nJ) kernelMem.resize(mJ-nJ, mJ);
if (mJ > nJ)
kernelMem.resize(mJ - nJ, mJ);
JK.setZero();
Jt.setZero();
......
This diff is collapsed.
......@@ -25,6 +25,8 @@ typedef Switch<bool, int> SwitchBool;
template <> DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SwitchBool, "SwitchBoolean");
typedef Switch<MatrixHomogeneous, int> SwitchMatrixHomogeneous;
template <> DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SwitchMatrixHomogeneous, "SwitchMatrixHomogeneous");
template <>
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SwitchMatrixHomogeneous,
"SwitchMatrixHomogeneous");
} // namespace sot
} // namespace dynamicgraph
......@@ -144,27 +144,28 @@ BOOST_AUTO_TEST_CASE(test_matrix_selector) {
BOOST_AUTO_TEST_SUITE(test_rotation_conversions)
template<typename type> type random();
template<> VectorRollPitchYaw random<VectorRollPitchYaw>()
{ return VectorRollPitchYaw::Random(); }
template<> VectorQuaternion random<VectorQuaternion>()
{ return VectorQuaternion(Eigen::Vector4d::Random().normalized()); }
template<> MatrixRotation random<MatrixRotation>()
{ return MatrixRotation(random<VectorQuaternion>()); }
template<typename type> bool compare(const type& a, const type& b)
{
return a.isApprox (b);
template <typename type> type random();
template <> VectorRollPitchYaw random<VectorRollPitchYaw>() {
return VectorRollPitchYaw::Random();
}
template<> bool compare<VectorQuaternion>(const VectorQuaternion& a, const VectorQuaternion& b)
{
return a.isApprox (b) || a.coeffs().isApprox(-b.coeffs());
template <> VectorQuaternion random<VectorQuaternion>() {
return VectorQuaternion(Eigen::Vector4d::Random().normalized());
}
template <> MatrixRotation random<MatrixRotation>() {
return MatrixRotation(random<VectorQuaternion>());
}
template<typename AtoB, typename BtoA>
void test_impl ()
{
typedef typename AtoB::Tin A;
template <typename type> bool compare(const type &a, const type &b) {
return a.isApprox(b);
}
template <>
bool compare<VectorQuaternion>(const VectorQuaternion &a,
const VectorQuaternion &b) {
return a.isApprox(b) || a.coeffs().isApprox(-b.coeffs());
}
template <typename AtoB, typename BtoA> void test_impl() {
typedef typename AtoB::Tin A;
typedef typename AtoB::Tout B;
AtoB a2b;
......@@ -174,16 +175,14 @@ void test_impl ()
A ain = random<A>(), aout;
B b;
a2b (ain, b);
b2a (b, aout);
a2b(ain, b);
b2a(b, aout);
BOOST_CHECK(compare(ain, aout));
}
}
BOOST_AUTO_TEST_CASE(matrix_rpy) {
test_impl<MatrixToRPY, RPYToMatrix>();
}
BOOST_AUTO_TEST_CASE(matrix_rpy) { test_impl<MatrixToRPY, RPYToMatrix>(); }
BOOST_AUTO_TEST_CASE(quaternion_rpy) {
test_impl<QuaternionToRPY, RPYToQuaternion>();
}
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment