Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/pyrene_actuator/pyreneCostFunction.cpp	Lines:	0	80	0.0 %
Date:	2023-06-02 15:50:43	Branches:	0	272	0.0 %


#include "ddp-actuator-solver/pyrene_actuator/pyreneCostFunction.hh"

#include <math.h>

#include <cmath>
#include <iostream>

using namespace std;

const double CostFunctionPyreneActuator::K = 10.6;
const double CostFunctionPyreneActuator::offset_m = 0.5572;

CostFunctionPyreneActuator::CostFunctionPyreneActuator() {
  Q << 500.0, 0.0, 0.0, 0.01;
  W << 1.0, 0.0, 0.0, 0.01;
  R << 0.0001;
  P << 100.0;

  lxx = Q;
  luu = R;
  lux << 0.0, 0.0;
  lxu << 0.0, 0.0;
  lx.setZero();
  final_cost = 0.0;
  running_cost = 0.0;
  tauLim = 0.0;
  lambdaLimPos = 10.0;
  lambdaLimVel = 1.0;
  alphaTau = 0.5;
}

void CostFunctionPyreneActuator::setCostGainState(const stateMat_t& Q_new) {
  Q = Q_new;
}

void CostFunctionPyreneActuator::setCostGainStateConstraint(
    const stateMat_t& W_new) {
  W = W_new;
}

void CostFunctionPyreneActuator::setCostGainCommand(const commandMat_t& R_new) {
  R = R_new;
}

void CostFunctionPyreneActuator::setCostGainTorqueConstraint(
    const commandMat_t& P_new) {
  P = P_new;
}

void CostFunctionPyreneActuator::setTauLimit(const double& limit) {
  tauLim = limit;
}

void CostFunctionPyreneActuator::setJointLimit(const double& limitUp,
                                               const double& limitDown) {
  jointLim.push_back(limitUp);
  jointLim.push_back(limitDown);
}

void CostFunctionPyreneActuator::setJointVelLimit(const double& limitUp,
                                                  const double& limitDown) {
  jointVelLim.push_back(limitUp);
  jointVelLim.push_back(limitDown);
}

void CostFunctionPyreneActuator::computeTauConstraintsAndDeriv(
    const commandVec_t& U) {
  double maxTau = 1 - alphaTau * (tauLim - (K * U[0] - offset_m));
  double minTau = 1 - alphaTau * ((K * U[0] - offset_m) + tauLim);
  TauConstraints << exp(alphaTau * maxTau) + exp(alphaTau * minTau);
  dTauConstraints << alphaTau * alphaTau * K *
                         (exp(alphaTau * maxTau) - exp(alphaTau * minTau));
  ddTauConstraints << pow(alphaTau, 4.0) * K * K *
                          (exp(alphaTau * maxTau) + exp(alphaTau * minTau));
}

void CostFunctionPyreneActuator::computeConstraintsAndDeriv(
    const stateVec_t& X) {
  double maxJoint = 1 - lambdaLimPos * (jointLim[0] - X[0]);
  double minJoint = 1 - lambdaLimPos * (X[0] - jointLim[1]);
  Constraints[0] = exp(lambdaLimPos * maxJoint) + exp(lambdaLimPos * minJoint);

  double maxJointVel = 1 - lambdaLimVel * (jointVelLim[0] - X[0]);
  double minJointVel = 1 - lambdaLimVel * (X[0] - jointVelLim[1]);
  Constraints[1] =
      exp(lambdaLimVel * maxJointVel) + exp(lambdaLimVel * minJointVel);

  double d0 = lambdaLimPos * lambdaLimPos *
              (exp(lambdaLimPos * maxJoint) - exp(lambdaLimPos * minJoint));
  double d1 =
      lambdaLimVel * lambdaLimVel *
      (exp(lambdaLimVel * maxJointVel) - exp(lambdaLimVel * minJointVel));
  dConstraints << d0, 0.0, 0.0, d1;

  double dd0 = pow(lambdaLimPos, 4.0) * Constraints[0];
  double dd1 = pow(lambdaLimVel, 4.0) * Constraints[1];
  ddConstraints << dd0, 0.0, 0.0, dd1;
}

void CostFunctionPyreneActuator::computeCostAndDeriv(const stateVec_t& X,
                                                     const stateVec_t& Xdes,
                                                     const commandVec_t& U) {
  computeConstraintsAndDeriv(X);
  computeTauConstraintsAndDeriv(U);
  running_cost =
      ((X - Xdes).transpose() * Q * (X - Xdes) + U.transpose() * R * U +
       Constraints.transpose() * W * Constraints +
       TauConstraints.transpose() * P * TauConstraints)(0, 0);

  lx = 2 * Q * (X - Xdes) + (2.0 * dConstraints.transpose() * W * Constraints);
  Eigen::Matrix<double, 2, 1> tempDD;
  tempDD = ddConstraints * W * Constraints;
  Eigen::Matrix<double, 2, 2> dd;
  dd << tempDD[0], 0.0, 0.0, tempDD[1];
  lxx = 2 * Q + (2.0 * (dConstraints.transpose() * W * dConstraints + dd));
  lu = 2 * R * U + (2.0 * dTauConstraints.transpose() * P * TauConstraints);
  luu = 2 * R + (2.0 * (ddTauConstraints.transpose() * P * TauConstraints +
                        dTauConstraints.transpose() * P * dTauConstraints));
}

void CostFunctionPyreneActuator::computeFinalCostAndDeriv(
    const stateVec_t& X, const stateVec_t& Xdes) {
  computeConstraintsAndDeriv(X);
  lx = Q * (X - Xdes) + (2.0 * dConstraints.transpose() * W * Constraints);
  Eigen::Matrix<double, 2, 1> tempDD;
  tempDD = ddConstraints * W * Constraints;
  Eigen::Matrix<double, 2, 2> dd;
  dd << tempDD[0], 0.0, 0.0, tempDD[1];
  lxx = Q + (2.0 * (dConstraints.transpose() * W * dConstraints + dd));
}


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			#include "ddp-actuator-solver/pyrene_actuator/pyreneCostFunction.hh"
2
3			#include <math.h>
4
5			#include <cmath>
6			#include <iostream>
7
8			using namespace std;
9
10			const double CostFunctionPyreneActuator::K = 10.6;
11			const double CostFunctionPyreneActuator::offset_m = 0.5572;
12
13			CostFunctionPyreneActuator::CostFunctionPyreneActuator() {
14			Q << 500.0, 0.0, 0.0, 0.01;
15			W << 1.0, 0.0, 0.0, 0.01;
16			R << 0.0001;
17			P << 100.0;
18
19			lxx = Q;
20			luu = R;
21			lux << 0.0, 0.0;
22			lxu << 0.0, 0.0;
23			lx.setZero();
24			final_cost = 0.0;
25			running_cost = 0.0;
26			tauLim = 0.0;
27			lambdaLimPos = 10.0;
28			lambdaLimVel = 1.0;
29			alphaTau = 0.5;
30			}
31
32			void CostFunctionPyreneActuator::setCostGainState(const stateMat_t& Q_new) {
33			Q = Q_new;
34			}
35
36			void CostFunctionPyreneActuator::setCostGainStateConstraint(
37			const stateMat_t& W_new) {
38			W = W_new;
39			}
40
41			void CostFunctionPyreneActuator::setCostGainCommand(const commandMat_t& R_new) {
42			R = R_new;
43			}
44
45			void CostFunctionPyreneActuator::setCostGainTorqueConstraint(
46			const commandMat_t& P_new) {
47			P = P_new;
48			}
49
50			void CostFunctionPyreneActuator::setTauLimit(const double& limit) {
51			tauLim = limit;
52			}
53
54			void CostFunctionPyreneActuator::setJointLimit(const double& limitUp,
55			const double& limitDown) {
56			jointLim.push_back(limitUp);
57			jointLim.push_back(limitDown);
58			}
59
60			void CostFunctionPyreneActuator::setJointVelLimit(const double& limitUp,
61			const double& limitDown) {
62			jointVelLim.push_back(limitUp);
63			jointVelLim.push_back(limitDown);
64			}
65
66			void CostFunctionPyreneActuator::computeTauConstraintsAndDeriv(
67			const commandVec_t& U) {
68			double maxTau = 1 - alphaTau * (tauLim - (K * U[0] - offset_m));
69			double minTau = 1 - alphaTau * ((K * U[0] - offset_m) + tauLim);
70			TauConstraints << exp(alphaTau * maxTau) + exp(alphaTau * minTau);
71			dTauConstraints << alphaTau * alphaTau * K *
72			(exp(alphaTau * maxTau) - exp(alphaTau * minTau));
73			ddTauConstraints << pow(alphaTau, 4.0) * K * K *
74			(exp(alphaTau * maxTau) + exp(alphaTau * minTau));
75			}
76
77			void CostFunctionPyreneActuator::computeConstraintsAndDeriv(
78			const stateVec_t& X) {
79			double maxJoint = 1 - lambdaLimPos * (jointLim[0] - X[0]);
80			double minJoint = 1 - lambdaLimPos * (X[0] - jointLim[1]);
81			Constraints[0] = exp(lambdaLimPos * maxJoint) + exp(lambdaLimPos * minJoint);
82
83			double maxJointVel = 1 - lambdaLimVel * (jointVelLim[0] - X[0]);
84			double minJointVel = 1 - lambdaLimVel * (X[0] - jointVelLim[1]);
85			Constraints[1] =
86			exp(lambdaLimVel * maxJointVel) + exp(lambdaLimVel * minJointVel);
87
88			double d0 = lambdaLimPos * lambdaLimPos *
89			(exp(lambdaLimPos * maxJoint) - exp(lambdaLimPos * minJoint));
90			double d1 =
91			lambdaLimVel * lambdaLimVel *
92			(exp(lambdaLimVel * maxJointVel) - exp(lambdaLimVel * minJointVel));
93			dConstraints << d0, 0.0, 0.0, d1;
94
95			double dd0 = pow(lambdaLimPos, 4.0) * Constraints[0];
96			double dd1 = pow(lambdaLimVel, 4.0) * Constraints[1];
97			ddConstraints << dd0, 0.0, 0.0, dd1;
98			}
99
100			void CostFunctionPyreneActuator::computeCostAndDeriv(const stateVec_t& X,
101			const stateVec_t& Xdes,
102			const commandVec_t& U) {
103			computeConstraintsAndDeriv(X);
104			computeTauConstraintsAndDeriv(U);
105			running_cost =
106			((X - Xdes).transpose() * Q * (X - Xdes) + U.transpose() * R * U +
107			Constraints.transpose() * W * Constraints +
108			TauConstraints.transpose() * P * TauConstraints)(0, 0);
109
110			lx = 2 * Q * (X - Xdes) + (2.0 * dConstraints.transpose() * W * Constraints);
111			Eigen::Matrix<double, 2, 1> tempDD;
112			tempDD = ddConstraints * W * Constraints;
113			Eigen::Matrix<double, 2, 2> dd;
114			dd << tempDD[0], 0.0, 0.0, tempDD[1];
115			lxx = 2 * Q + (2.0 * (dConstraints.transpose() * W * dConstraints + dd));
116			lu = 2 * R * U + (2.0 * dTauConstraints.transpose() * P * TauConstraints);
117			luu = 2 * R + (2.0 * (ddTauConstraints.transpose() * P * TauConstraints +
118			dTauConstraints.transpose() * P * dTauConstraints));
119			}
120
121			void CostFunctionPyreneActuator::computeFinalCostAndDeriv(
122			const stateVec_t& X, const stateVec_t& Xdes) {
123			computeConstraintsAndDeriv(X);
124			lx = Q * (X - Xdes) + (2.0 * dConstraints.transpose() * W * Constraints);
125			Eigen::Matrix<double, 2, 1> tempDD;
126			tempDD = ddConstraints * W * Constraints;
127			Eigen::Matrix<double, 2, 2> dd;
128			dd << tempDD[0], 0.0, 0.0, tempDD[1];
129			lxx = Q + (2.0 * (dConstraints.transpose() * W * dConstraints + dd));
130			}