I'm translating a script form matlab to C++ and I'm given a code snipet in c++ so I can fill it. My BIG issue is that my knowledge about C++ is almost nothing, I know how classes work in a basic way so now I'm stuck in a variable initialization problem.
So, coming from matlab I have some simple variables to start but, The last one depends on a function for the inizialization, so I dont know how to proceed. Here my matlab code:
For those who are not use to matlab, qs, dqs, ddqs, centrqds, Corrqds are vector of vectors of 6xn size.
function [PIDOUT,qsinit,dqsinit,ddqsinit,PosError, PosErrorSum,VelErrorSum,IntErrorSum, qs,dqs,ddqs,E2,mRobot,mControl,EM,torque]=initializeLoopVars(fregCentr, fregCor, fregFric,fregG,qc,dqc, ddqc, menu)
PIDOUT=zeros(6,length(qc));
qsinit=qc(:,1);
dqsinit=zeros(6,1);
ddqsinit=zeros(6,1);
mRobot=zeros(6,6,length(qc));
mControl=zeros(6,6,length(qc));
EM=zeros(6,length(qc));
PosError=zeros(6,1);
PosErrorSum=zeros(6,1);
VelErrorSum=zeros(6,1);
IntErrorSum=zeros(6,1);
qs=zeros(6,length(qc));
dqs=zeros(6,length(dqc));
ddqs=zeros(6,length(ddqc));
Centrqds = [dqsinit(1,:);dqsinit(2,:);dqsinit(3,:);dqsinit(4,:);dqsinit(5,:);dqsinit(6,:)];
Corrqds = prodDqiDqj([dqsinit(1,:);dqsinit(2,:);dqsinit(3,:);dqsinit(4,:);dqsinit(5,:);dqsinit(6,:)]);
torque=zeros(6,length(qc));
E2=zeros(6,length(qc));
%initial dynamic compensation
if (menu == 0)
% V+G+fr
E2(:,1)=fregCentr(qc(2,1),qc(3,1),qc(4,1),qc(5,1),qc(6,1))*Centrqds+fregCor(qc(2,1),qc(3,1),qc(4,1),qc(5,1),qc(6,1))*Corrqds+fregG(qc(2,1),qc(3,1),qc(4,1),qc(5,1),qc(6,1))+fregFric(dqsinit(1,1),dqsinit(2,1),dqsinit(3,1),dqsinit(4,1),dqsinit(5,1),dqsinit(6,1));
% V+G
%E2(:,1)=fregCentr(qc(2,1),qc(3,1),qc(4,1),qc(5,1),qc(6,1))*Centrqds+fregCor(qc(2,1),qc(3,1),qc(4,1),qc(5,1),qc(6,1))*Corrqds+fregG(qc(2,1),qc(3,1),qc(4,1),qc(5,1),qc(6,1));
% V
%E2(:,1)=fregCentr(qc(2,1),qc(3,1),qc(4,1),qc(5,1),qc(6,1))*Centrqds+fregCor(qc(2,1),qc(3,1),qc(4,1),qc(5,1),qc(6,1))*Corrqds;
% G
%E2(:,1)=fregG(qc(2,1),qc(3,1),qc(4,1),qc(5,1),qc(6,1));
elseif (menu == 1)
% V+G+fr
E2(:,1)=fregCentr(qc(2,1),qc(3,1),qc(4,1),qc(5,1),qc(6,1))*Centrqds+fregCor(qc(2,1),qc(3,1),qc(4,1),qc(5,1),qc(6,1))*Corrqds+fregG(qc(2,1),qc(3,1),qc(4,1),qc(5,1),qc(6,1))+fregFric(dqsinit(1,1),dqsinit(2,1),dqsinit(3,1),dqsinit(4,1),dqsinit(5,1),dqsinit(6,1));
% V+G
%E2(:,1)=fregCentr(qc(2,1),qc(3,1),qc(4,1),qc(5,1),qc(6,1))*Centrqds+fregCor(qc(2,1),qc(3,1),qc(4,1),qc(5,1),qc(6,1))*Corrqds+fregG(qc(2,1),qc(3,1),qc(4,1),qc(5,1),qc(6,1));
% V
%E2(:,1)=fregCentr(qc(2,1),qc(3,1),qc(4,1),qc(5,1),qc(6,1))*Centrqds+fregCor(qc(2,1),qc(3,1),qc(4,1),qc(5,1),qc(6,1))*Corrqds;
% G
%E2(:,1)=fregG(qc(2,1),qc(3,1),qc(4,1),qc(5,1),qc(6,1));
end
I'm in process of translating so some variables are missing in the c++ code!!
Notice that Corrqds depends on prodDqiDqj function to be inizialized, that is a simple algebraic calculation.
function [dqidqj] = prodDqiDqj(dq)
dqidqj = zeros(15, 1);
cont = 1;
for i = 1:6
for j = i + 1:6
dqidqj(cont, 1) = dq(i) * dq(j);
cont = cont + 1;
end
end
end
Now the c++ part. I have a RobotParameters class in RobotParameters.h where my variables are public. I started creating those variables (and some more, names are maybe changed by convenience):
#pragma once
#include <vector>
class RobotParameters
{
public:
std::vector<std::vector<double>> posDesired{ 6 };
std::vector<std::vector<double>> velDesired{ 6 };
std::vector<std::vector<double>> posRobot{ 6 };
std::vector<std::vector<double>> torqueRobot{ 6 };
std::vector<std::vector<double>> ErrorRobot{ 6 };
std::vector<std::vector<double>> velRobot{ 6 };
std::vector<std::vector<double>> qc{ 6 };
std::vector<std::vector<double>> dqc{ 6 };
std::vector<std::vector<double>> ddqc{ 6 };
std::vector<double> errorAnt = { 0.0,0.0, 0.0, 0.0, 0.0, 0.0 };
std::vector<std::vector<double>> P{ 6 };
std::vector<std::vector<double>> D{ 6 };
std::vector<std::vector<double>> I{ 6 };
std::vector<std::vector<double>> Tc{ 6 };
std::vector<double> errorInt = { 0.0,0.0, 0.0, 0.0, 0.0, 0.0 };
std::vector<double> posRobotAnt = { 0.0,0.0, 0.0, 0.0, 0.0, 0.0 };
std::vector<double> Centrqds = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
RobotParameters();
};
My issue is, for Corrqds I need to somehow declare the latter prodDqiDqj function somewhere but I'm not sure how to do it.
Someone could help me and give an explanation about how should I do it? As i told, I'm new to C++ and I'm struggling here.
edit: Added more code as requested!