Skip to content

Commit 340f885

Browse files
markusgftmarkusgft
committed
ran clang-format on ct_rbd
1 parent 5d644d6 commit 340f885

36 files changed

+283
-340
lines changed

‎ct_rbd/include/ct/rbd/internal/CppADCodegenTrait.h

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -30,8 +30,7 @@ class CppADCodegenTrait
3030
inline static Scalar tanh(const Scalar& x) { return CppAD::tanh(x); }
3131
inline static Scalar exp(const Scalar& x) { return CppAD::exp(x); }
3232
inline static Scalar fabs(const Scalar& x) { return CppAD::fabs(x); }
33-
inline static Scalar sqrt(const Scalar& x) {return CppAD::sqrt(x); }
34-
33+
inline static Scalar sqrt(const Scalar& x) { return CppAD::sqrt(x); }
3534
template <int Dims>
3635
inline static Eigen::Matrix<Scalar, Dims, 1> solve(const Eigen::Matrix<Scalar, Dims, Dims>& A,
3736
const Eigen::Matrix<Scalar, Dims, 1>& b)

‎ct_rbd/include/ct/rbd/internal/CppADDoubleTrait.h

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -27,8 +27,7 @@ struct CppADDoubleTrait
2727
inline static Scalar tanh(const Scalar& x) { return CppAD::tanh(x); }
2828
inline static Scalar exp(const Scalar& x) { return CppAD::exp(x); }
2929
inline static Scalar fabs(const Scalar& x) { return CppAD::fabs(x); }
30-
inline static Scalar sqrt(const Scalar& x) {return CppAD::sqrt(x); }
31-
30+
inline static Scalar sqrt(const Scalar& x) { return CppAD::sqrt(x); }
3231
template <int Dims>
3332
inline static Eigen::Matrix<Scalar, Dims, 1> solve(const Eigen::Matrix<Scalar, Dims, Dims>& A,
3433
const Eigen::Matrix<Scalar, Dims, 1>& b)

‎ct_rbd/include/ct/rbd/nloc/FixBaseNLOC-impl.h

Lines changed: 12 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -26,26 +26,27 @@ FixBaseNLOC<FIX_BASE_FD_SYSTEM>::FixBaseNLOC(
2626
}
2727

2828
template <class FIX_BASE_FD_SYSTEM>
29-
FixBaseNLOC<FIX_BASE_FD_SYSTEM>::FixBaseNLOC(std::shared_ptr<ct::optcon::CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>> costFun,
30-
std::shared_ptr<ct::optcon::LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>> boxConstraints,
31-
std::shared_ptr<ct::optcon::LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>> generalConstraints,
29+
FixBaseNLOC<FIX_BASE_FD_SYSTEM>::FixBaseNLOC(
30+
std::shared_ptr<ct::optcon::CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>> costFun,
31+
std::shared_ptr<ct::optcon::LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>> boxConstraints,
32+
std::shared_ptr<ct::optcon::LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>> generalConstraints,
3233
const typename NLOptConSolver::Settings_t& nlocSettings,
3334
std::shared_ptr<FBSystem> system,
3435
bool verbose,
3536
std::shared_ptr<LinearizedSystem> linearizedSystem)
36-
: system_(system),
37+
: system_(system),
3738
linearizedSystem_(linearizedSystem),
38-
boxConstraints_(boxConstraints),
39-
generalConstraints_(generalConstraints),
39+
boxConstraints_(boxConstraints),
40+
generalConstraints_(generalConstraints),
4041
costFunction_(costFun),
4142
optConProblem_(system_, costFunction_, linearizedSystem_),
4243
iteration_(0)
4344
{
44-
if(boxConstraints_ != nullptr)
45-
optConProblem_.setBoxConstraints(boxConstraints_);
45+
if (boxConstraints_ != nullptr)
46+
optConProblem_.setBoxConstraints(boxConstraints_);
4647

47-
if(generalConstraints_ != nullptr)
48-
optConProblem_.setGeneralConstraints(generalConstraints_);
48+
if (generalConstraints_ != nullptr)
49+
optConProblem_.setGeneralConstraints(generalConstraints_);
4950

5051
optConProblem_.verify();
5152
nlocSolver_ = std::shared_ptr<NLOptConSolver>(new NLOptConSolver(optConProblem_, nlocSettings));
@@ -206,8 +207,7 @@ FixBaseNLOC<FIX_BASE_FD_SYSTEM>::getControlVectorArray()
206207
}
207208

208209
template <class FIX_BASE_FD_SYSTEM>
209-
const typename FixBaseNLOC<FIX_BASE_FD_SYSTEM>::StateVectorArray&
210-
FixBaseNLOC<FIX_BASE_FD_SYSTEM>::getStateVectorArray()
210+
const typename FixBaseNLOC<FIX_BASE_FD_SYSTEM>::StateVectorArray& FixBaseNLOC<FIX_BASE_FD_SYSTEM>::getStateVectorArray()
211211
{
212212
return nlocSolver_->getSolution().x_ref();
213213
}

‎ct_rbd/include/ct/rbd/nloc/FixBaseNLOC.h

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -30,7 +30,7 @@ class FixBaseNLOC
3030
using SCALAR = typename FBSystem::SCALAR;
3131

3232
using LinearizedSystem = ct::core::LinearSystem<STATE_DIM, CONTROL_DIM, SCALAR>;
33-
// using SystemLinearizer = ct::rbd::RbdLinearizer<FBSystem>;
33+
// using SystemLinearizer = ct::rbd::RbdLinearizer<FBSystem>;
3434

3535
//! @ todo: introduce templates for P_DIM and V_DIM
3636
using NLOptConSolver = ct::optcon::NLOptConSolver<STATE_DIM, CONTROL_DIM, STATE_DIM / 2, STATE_DIM / 2, SCALAR>;
@@ -60,8 +60,8 @@ class FixBaseNLOC
6060

6161
//! constructor which directly takes a cost function and constraints, mind the order of the constraints
6262
FixBaseNLOC(std::shared_ptr<ct::optcon::CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>> costFun,
63-
std::shared_ptr<ct::optcon::LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>> boxConstraints,
64-
std::shared_ptr<ct::optcon::LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>> generalConstraints,
63+
std::shared_ptr<ct::optcon::LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>> boxConstraints,
64+
std::shared_ptr<ct::optcon::LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>> generalConstraints,
6565
const typename NLOptConSolver::Settings_t& nlocSettings,
6666
std::shared_ptr<FBSystem> system = std::shared_ptr<FBSystem>(new FBSystem),
6767
bool verbose = false,
@@ -120,8 +120,8 @@ class FixBaseNLOC
120120
std::shared_ptr<FBSystem> system_;
121121
std::shared_ptr<LinearizedSystem> linearizedSystem_;
122122
std::shared_ptr<CostFunction> costFunction_;
123-
std::shared_ptr<ct::optcon::LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>> boxConstraints_;
124-
std::shared_ptr<ct::optcon::LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>> generalConstraints_;
123+
std::shared_ptr<ct::optcon::LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>> boxConstraints_;
124+
std::shared_ptr<ct::optcon::LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>> generalConstraints_;
125125

126126
optcon::ContinuousOptConProblem<STATE_DIM, CONTROL_DIM, SCALAR> optConProblem_;
127127

‎ct_rbd/include/ct/rbd/robot/RobCoGenContainer.h

Lines changed: 2 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -139,8 +139,7 @@ class RobCoGenContainer
139139
* @param jointPosition current joint position
140140
* @return Jacobian of the endeffector expressed in the base frame
141141
*/
142-
Jacobian getJacobianBaseEEbyId(size_t eeId,
143-
const typename JointState<NJOINTS, SCALAR>::Position& jointPosition)
142+
Jacobian getJacobianBaseEEbyId(size_t eeId, const typename JointState<NJOINTS, SCALAR>::Position& jointPosition)
144143
{
145144
return UTILS::getJacobianBaseEEbyId(jacobians(), eeId, jointPosition);
146145
}
@@ -165,8 +164,7 @@ class RobCoGenContainer
165164
* @param jointPosition current joint position
166165
* @return position of the endeffector expressed in the base frame
167166
*/
168-
Position3Tpl getEEPositionInBase(size_t eeId,
169-
const typename JointState<NJOINTS, SCALAR>::Position& jointPosition)
167+
Position3Tpl getEEPositionInBase(size_t eeId, const typename JointState<NJOINTS, SCALAR>::Position& jointPosition)
170168
{
171169
return Position3Tpl(getHomogeneousTransformBaseEEById(eeId, jointPosition).template topRightCorner<3, 1>());
172170
}

‎ct_rbd/include/ct/rbd/robot/actuator/SEADynamicsFirstOrder-impl.h

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -26,8 +26,7 @@ SEADynamicsFirstOrder<NJOINTS, SCALAR>* SEADynamicsFirstOrder<NJOINTS, SCALAR>::
2626
}
2727

2828
template <size_t NJOINTS, typename SCALAR>
29-
void SEADynamicsFirstOrder<NJOINTS, SCALAR>::computeActuatorDynamics(
30-
const JointState<NJOINTS, SCALAR>& robotJointState,
29+
void SEADynamicsFirstOrder<NJOINTS, SCALAR>::computeActuatorDynamics(const JointState<NJOINTS, SCALAR>& robotJointState,
3130
const ct::core::StateVector<NJOINTS, SCALAR>& state,
3231
const SCALAR& t,
3332
const ct::core::ControlVector<NJOINTS, SCALAR>& control,

‎ct_rbd/include/ct/rbd/robot/costfunction/TermTaskspaceGeometricJacobian.hpp

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -147,12 +147,12 @@ class TermTaskspaceGeometricJacobian : public optcon::TermBase<STATE_DIM, CONTRO
147147
// for the intuition behind, consider the following posts:
148148
// https://math.stackexchange.com/a/87698
149149
// https://math.stackexchange.com/a/773635
150-
// Eigen::Matrix<double, 3, 3> ee_R_diff = w_R_ref.transpose() * ee_rot;
150+
// Eigen::Matrix<double, 3, 3> ee_R_diff = w_R_ref.transpose() * ee_rot;
151151

152152
// compute rotation penalty using the squared Frobenius norm of (R_diff-I)
153-
// double rot_cost = Q_rot_ * (ee_R_diff - Eigen::Matrix<double, 3, 3>::Identity()).squaredNorm();
153+
// double rot_cost = Q_rot_ * (ee_R_diff - Eigen::Matrix<double, 3, 3>::Identity()).squaredNorm();
154154

155-
return pos_cost; // + rot_cost;
155+
return pos_cost; // + rot_cost;
156156
}
157157

158158

@@ -333,7 +333,7 @@ class TermTaskspaceGeometricJacobian : public optcon::TermBase<STATE_DIM, CONTRO
333333
double Q_rot_;
334334

335335
Eigen::Matrix<double, 3, 1> x_ref_; // ref position
336-
Eigen::Matrix<double, 3, 3> R_ref_; // ref rotation
336+
Eigen::Matrix<double, 3, 3> R_ref_; // ref rotation
337337
};
338338

339339

‎ct_rbd/include/ct/rbd/robot/costfunction/TermTaskspacePoseCG.hpp

Lines changed: 4 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -124,8 +124,8 @@ class TermTaskspacePoseCG : public optcon::TermBase<STATE_DIM, CONTROL_DIM, doub
124124
bool evalControlDerivatives = false)
125125
: BASE(name), eeInd_(eeInd), Q_pos_(Qpos), Q_rot_(Qrot), evalControlDerivatives_(evalControlDerivatives)
126126
{
127-
// arbitrary dummy values
128-
Eigen::Quaterniond w_q_des (0.0, 0.0, 0.0, 1.0);
127+
// arbitrary dummy values
128+
Eigen::Quaterniond w_q_des(0.0, 0.0, 0.0, 1.0);
129129
setReferenceOrientation(w_q_des);
130130
setReferencePosition(core::StateVector<3>::Zero());
131131
setup();
@@ -148,8 +148,7 @@ class TermTaskspacePoseCG : public optcon::TermBase<STATE_DIM, CONTROL_DIM, doub
148148
adParameterVector_(arg.adParameterVector_),
149149
evalControlDerivatives_(arg.evalControlDerivatives_)
150150
{
151-
derivativesCppadJIT_ =
152-
std::shared_ptr<DerivativesCppadJIT>(arg.derivativesCppadJIT_->clone());
151+
derivativesCppadJIT_ = std::shared_ptr<DerivativesCppadJIT>(arg.derivativesCppadJIT_->clone());
153152
}
154153

155154
//! destructor
@@ -459,7 +458,7 @@ class TermTaskspacePoseCG : public optcon::TermBase<STATE_DIM, CONTROL_DIM, doub
459458
SC rot_cost = (SC)Q_rot_ * (ee_R_diff - Eigen::Matrix<SC, 3, 3>::Identity()).squaredNorm();
460459

461460
Eigen::Matrix<SC, 1, 1> result;
462-
result(0,0) = (pos_cost + rot_cost);
461+
result(0, 0) = (pos_cost + rot_cost);
463462
return result;
464463
}
465464

‎ct_rbd/include/ct/rbd/robot/kinematics/InverseKinematicsBase.h

Lines changed: 7 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -14,15 +14,10 @@ namespace rbd {
1414

1515
struct InverseKinematicsSettings
1616
{
17-
InverseKinematicsSettings():
18-
maxNumTrials_(1),
19-
randomizeInitialGuess_(true),
20-
validationTol_(1e-4)
21-
{}
22-
23-
size_t maxNumTrials_;
24-
bool randomizeInitialGuess_;
25-
double validationTol_;
17+
InverseKinematicsSettings() : maxNumTrials_(1), randomizeInitialGuess_(true), validationTol_(1e-4) {}
18+
size_t maxNumTrials_;
19+
bool randomizeInitialGuess_;
20+
double validationTol_;
2621
};
2722

2823

@@ -40,10 +35,7 @@ class InverseKinematicsBase
4035
InverseKinematicsBase() = default;
4136

4237
//! constructor with additional settings
43-
InverseKinematicsBase(const InverseKinematicsSettings& settings):
44-
settings_(settings)
45-
{}
46-
38+
InverseKinematicsBase(const InverseKinematicsSettings& settings) : settings_(settings) {}
4739
//! default destructor
4840
virtual ~InverseKinematicsBase() = default;
4941

@@ -127,15 +119,9 @@ class InverseKinematicsBase
127119
ikSolution, eeBasePose, identityWorldPose, queryJointPositions, freeJoints);
128120
}
129121

130-
const InverseKinematicsSettings& getSettings() const {return settings_;}
131-
132-
void updateSettings(const InverseKinematicsSettings& settings)
133-
{
134-
settings_ = settings;
135-
}
136-
122+
const InverseKinematicsSettings& getSettings() const { return settings_; }
123+
void updateSettings(const InverseKinematicsSettings& settings) { settings_ = settings; }
137124
protected:
138-
139125
InverseKinematicsSettings settings_;
140126
};
141127

‎ct_rbd/include/ct/rbd/robot/kinematics/ik_nlp/IKConstraintContainer.h

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -29,7 +29,7 @@ class IKConstraintsContainer final : public ct::optcon::tpl::DiscreteConstraintC
2929
const JointPosition& upperBound)
3030
: optVector_(optVector)
3131
{
32-
// set up joint limit constraints
32+
// set up joint limit constraints
3333
auto jointLimits = std::shared_ptr<JointLimitConstraints<KINEMATICS, SCALAR>>(
3434
new JointLimitConstraints<KINEMATICS, SCALAR>(optVector_, lowerBound, upperBound));
3535
this->constraints_.push_back(jointLimits);
@@ -47,7 +47,7 @@ class IKConstraintsContainer final : public ct::optcon::tpl::DiscreteConstraintC
4747
*/
4848
const std::shared_ptr<JointLimitConstraints<KINEMATICS, SCALAR>> getJointLimitConstraints() const
4949
{
50-
return std::static_pointer_cast<JointLimitConstraints<KINEMATICS, SCALAR>>(this->constraints_.front());
50+
return std::static_pointer_cast<JointLimitConstraints<KINEMATICS, SCALAR>>(this->constraints_.front());
5151
}
5252

5353
private:

0 commit comments

Comments
 (0)