Commit a2ac1247 authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Merge branch 'devel' into origin-master

parents 72b0f9fe ab2ed89d
Pipeline #4527 failed with stage
in 8 minutes and 4 seconds
......@@ -94,7 +94,7 @@ namespace dynamicgraph {
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
sendMsg("["+name+"] "+msg, t, file, line);
Entity::sendMsg("["+name+"] "+msg, t, file, line);
}
protected:
......
......@@ -41,8 +41,8 @@ public:
private:
double m_dt; /// sampling timestep of the input signal
int m_x_size;
Eigen::Index m_filter_order_m;
Eigen::Index m_filter_order_n;
Eigen::VectorXd::Index m_filter_order_m;
Eigen::VectorXd::Index m_filter_order_n;
Eigen::VectorXd m_filter_numerator;
Eigen::VectorXd m_filter_denominator;
......
......@@ -108,6 +108,10 @@ FOREACH(plugin ${plugins})
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} pinocchio)
IF(DDP_ACTUATOR_SOLVER_FOUND)
SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake/find-external/qpOASES")
FIND_PACKAGE("qpOASES" REQUIRED)
INCLUDE_DIRECTORIES(SYSTEM ${qpOASES_INCLUDE_DIRS})
TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${qpOASES_LIBRARIES})
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} ddp-actuator-solver )
ENDIF(DDP_ACTUATOR_SOLVER_FOUND)
......
......@@ -574,8 +574,8 @@ namespace dynamicgraph
const Eigen::VectorXd& qj= m_joint_positionsSIN(iter); //n+6
const Eigen::VectorXd& dq= m_joint_velocitiesSIN(iter);
assert(qj.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_positions");
assert(dq.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_velocities");
assert(qj.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_positions");
assert(dq.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_velocities");
/* convert sot to pinocchio joint order */
m_robot_util->joints_sot_to_urdf(qj, m_q_pin.tail(m_robot_util->m_nbJoints));
......@@ -602,7 +602,7 @@ namespace dynamicgraph
SEND_WARNING_STREAM_MSG("Cannot compute signal q before initialization!");
return s;
}
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6))
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints+6))
s.resize(m_robot_util->m_nbJoints+6);
const Eigen::VectorXd & qj = m_joint_positionsSIN(iter); //n+6
......@@ -631,7 +631,7 @@ namespace dynamicgraph
wR = 0.0;
}
assert(qj.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_positions");
assert(qj.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_positions");
// if both weights are zero set them to a small positive value to avoid division by zero
if(wR==0.0 && wL==0.0)
......@@ -820,7 +820,7 @@ namespace dynamicgraph
SEND_WARNING_STREAM_MSG("Cannot compute signal q_lf before initialization!");
return s;
}
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6))
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints+6))
s.resize(m_robot_util->m_nbJoints+6);
const Eigen::VectorXd & q = m_qSOUT(iter);
......@@ -837,7 +837,7 @@ namespace dynamicgraph
SEND_WARNING_STREAM_MSG("Cannot compute signal q_rf before initialization!");
return s;
}
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6))
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints+6))
s.resize(m_robot_util->m_nbJoints+6);
const Eigen::VectorXd & q = m_qSOUT(iter);
......@@ -854,7 +854,7 @@ namespace dynamicgraph
SEND_WARNING_STREAM_MSG("Cannot compute signal q_imu before initialization!");
return s;
}
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6))
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints+6))
s.resize(m_robot_util->m_nbJoints+6);
const Eigen::VectorXd & q = m_qSOUT(iter);
......@@ -968,7 +968,7 @@ namespace dynamicgraph
SEND_WARNING_STREAM_MSG("Cannot compute signal v before initialization!");
return s;
}
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6))
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints+6))
s.resize(m_robot_util->m_nbJoints+6);
m_kinematics_computationsSINNER(iter);
......@@ -981,7 +981,7 @@ namespace dynamicgraph
const Eigen::Vector3d& gyr_imu = m_gyroscopeSIN(iter);
const Vector6 & dftrf = m_dforceRLEGSIN(iter);
const Vector6 & dftlf = m_dforceLLEGSIN(iter);
assert(dq.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_velocities");
assert(dq.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_velocities");
// if the weights are not specified by the user through the input signals w_lf, w_rf
// then compute them
......
......@@ -83,7 +83,7 @@ void CausalFilter::get_x_dx_ddx(const Eigen::VectorXd& base_x,
x_output_dx_ddx.head(m_x_size) = (input_buffer*b-output_buffer*a)/m_filter_denominator[0];
//Finite Difference
Eigen::Index pt_denominator_prev = (pt_denominator == 0) ? m_filter_order_n-2 : pt_denominator-1;
Eigen::VectorXd::Index pt_denominator_prev = (pt_denominator == 0) ? m_filter_order_n-2 : pt_denominator-1;
x_output_dx_ddx.segment(m_x_size,m_x_size) = (x_output_dx_ddx.head(m_x_size)-output_buffer.col(pt_denominator))/m_dt;
x_output_dx_ddx.tail(m_x_size) = (x_output_dx_ddx.head(m_x_size)-2*output_buffer.col(pt_denominator)+output_buffer.col(pt_denominator_prev))/m_dt/m_dt;
......@@ -98,8 +98,8 @@ void CausalFilter::get_x_dx_ddx(const Eigen::VectorXd& base_x,
void CausalFilter::switch_filter(const Eigen::VectorXd& filter_numerator,
const Eigen::VectorXd& filter_denominator)
{
Eigen::Index filter_order_m = filter_numerator.size();
Eigen::Index filter_order_n = filter_denominator.size();
Eigen::VectorXd::Index filter_order_m = filter_numerator.size();
Eigen::VectorXd::Index filter_order_n = filter_denominator.size();
Eigen::VectorXd current_x(input_buffer.col(pt_numerator));
......
......@@ -156,6 +156,11 @@ namespace dynamicgraph
docCommandVoid2("Set the Frame Name for the Foot Name.",
"(string) Foot name",
"(string) Frame name")));
addCommand("setHandFrameName",
makeCommandVoid2(*this, &ControlManager::setHandFrameName,
docCommandVoid2("Set the Frame Name for the Hand Name.",
"(string) Hand name",
"(string) Frame name")));
addCommand("setImuJointName",
makeCommandVoid1(*this, &ControlManager::setImuJointName,
docCommandVoid1("Set the Joint Name wich IMU is attached to.",
......@@ -182,28 +187,28 @@ namespace dynamicgraph
void ControlManager::init(const double & dt,
const std::string & urdfFile,
const std::string &robotRef)
const std::string & robotRef)
{
if(dt<=0.0)
return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
m_dt = dt;
m_emergency_stop_triggered = false;
m_emergency_stop_triggered = false;
m_initSucceeded = true;
vector<string> package_dirs;
m_robot = new robots::RobotWrapper(urdfFile, package_dirs, pinocchio::JointModelFreeFlyer());
std::string localName(robotRef);
if (!isNameInRobotUtil(localName))
{
m_robot_util = createRobotUtil(localName);
m_robot_util = createRobotUtil(localName);
SEND_MSG("createRobotUtil success\n", MSG_TYPE_INFO);
}
else
{
m_robot_util = getRobotUtil(localName);
m_robot_util = getRobotUtil(localName);
SEND_MSG("getRobotUtil success\n",MSG_TYPE_INFO);
}
SEND_MSG( m_robot_util->m_urdf_filename,MSG_TYPE_INFO);
m_robot_util->m_urdf_filename = urdfFile;
addCommand("getJointsUrdfToSot",
makeDirectGetter(*this, &m_robot_util->m_dgv_urdf_to_sot,
docDirectSetter("Display map Joints From URDF to SoT.",
......@@ -254,7 +259,7 @@ namespace dynamicgraph
}
const dynamicgraph::Vector& ctrl = (*m_ctrlInputsSIN[cm_id])(iter);
assert(ctrl.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
assert(ctrl.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
if(m_jointCtrlModesCountDown[i]==0)
s(i) = ctrl(i);
......@@ -262,7 +267,7 @@ namespace dynamicgraph
{
cm_id_prev = m_jointCtrlModes_previous[i].id;
const dynamicgraph::Vector& ctrl_prev = (*m_ctrlInputsSIN[cm_id_prev])(iter);
assert(ctrl_prev.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
assert(ctrl_prev.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
double alpha = m_jointCtrlModesCountDown[i]/CTRL_MODE_TRANSITION_TIME_STEP;
// SEND_MSG("Joint "+toString(i)+" changing ctrl mode from "+toString(cm_id_prev)+
......@@ -312,7 +317,7 @@ namespace dynamicgraph
for(std::size_t i=0;i<m_emergencyStopSIN.size();i++)
{
if ((*m_emergencyStopSIN[i]).isPlugged() && (*m_emergencyStopSIN[i])(iter))
if ((*m_emergencyStopSIN[i]).isPlugged() && (*m_emergencyStopSIN[i])(iter))
{
m_emergency_stop_triggered = true;
SEND_MSG("Emergency Stop has been triggered by an external entity", MSG_TYPE_ERROR);
......@@ -403,7 +408,7 @@ namespace dynamicgraph
CtrlMode cm;
if(convertStringToCtrlMode(ctrlMode,cm)==false)
return;
if(jointName=="all")
{
for(unsigned int i=0; i<m_robot_util->m_nbJoints; i++)
......@@ -497,7 +502,7 @@ namespace dynamicgraph
getClassName()+"("+getName()+")::input(bool)::emergencyStop_"+name));
// register the new signals and add the new signal dependecy
Eigen::Index i = m_emergencyStopSIN.size()-1;
Eigen::VectorXd::Index i = m_emergencyStopSIN.size()-1;
m_u_safeSOUT.addDependency(*m_emergencyStopSIN[i]);
Entity::signalRegistration(*m_emergencyStopSIN[i]);
}
......@@ -510,7 +515,7 @@ namespace dynamicgraph
SEND_WARNING_STREAM_MSG("Cannot set joint name from joint id before initialization!");
return;
}
m_robot_util->set_name_to_id(jointName,static_cast<Eigen::Index>(jointId));
m_robot_util->set_name_to_id(jointName,static_cast<Eigen::VectorXd::Index>(jointId));
}
void ControlManager::setJointLimitsFromId( const double &jointId,
......@@ -522,7 +527,7 @@ namespace dynamicgraph
SEND_WARNING_STREAM_MSG("Cannot set joints limits from joint id before initialization!");
return;
}
m_robot_util->set_joint_limits_for_id((Index)jointId,lq,uq);
}
......@@ -548,7 +553,7 @@ namespace dynamicgraph
return;
}
m_robot_util->m_force_util.set_name_to_force_id(forceName,static_cast<Eigen::Index>(forceId));
m_robot_util->m_force_util.set_name_to_force_id(forceName,static_cast<Eigen::VectorXd::Index>(forceId));
}
void ControlManager::setJoints(const dg::Vector & urdf_to_sot)
......@@ -569,7 +574,7 @@ namespace dynamicgraph
return;
}
m_robot_util->m_foot_util.m_Right_Foot_Sole_XYZ = xyz;
m_robot_util->m_foot_util.m_Right_Foot_Sole_XYZ = xyz;
}
void ControlManager::setRightFootForceSensorXYZ(const dynamicgraph::Vector &xyz)
......@@ -595,10 +600,26 @@ namespace dynamicgraph
m_robot_util->m_foot_util.m_Left_Foot_Frame_Name = FrameName;
else if (FootName=="Right")
m_robot_util->m_foot_util.m_Right_Foot_Frame_Name = FrameName;
else
else
SEND_WARNING_STREAM_MSG("Did not understand the foot name !" + FootName);
}
void ControlManager::setHandFrameName( const std::string &HandName,
const std::string &FrameName)
{
if(!m_initSucceeded)
{
SEND_WARNING_STREAM_MSG("Cannot set hand frame name!");
return;
}
if (HandName=="Left")
m_robot_util->m_hand_util.m_Left_Hand_Frame_Name = FrameName;
else if (HandName=="Right")
m_robot_util->m_hand_util.m_Right_Hand_Frame_Name = FrameName;
else
SEND_WARNING_STREAM_MSG("Did not understand the hand name !" + HandName);
}
void ControlManager::setImuJointName(const std::string &JointName)
{
if(!m_initSucceeded)
......@@ -608,7 +629,7 @@ namespace dynamicgraph
}
m_robot_util->m_imu_joint_name = JointName;
}
void ControlManager::displayRobotUtil()
{
m_robot_util->display(std::cout);
......@@ -708,7 +729,7 @@ namespace dynamicgraph
}
catch (ExceptionSignal e) {}
}
} // namespace torquecontrol
} // namespace sot
} // namespace dynamicgraph
......@@ -226,7 +226,7 @@ namespace dynamicgraph
const dynamicgraph::Vector& i_real = m_i_realSOUT(iter);
const dynamicgraph::Vector& in_out_gain = m_in_out_gainSIN(iter);
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints))
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints))
s.resize(m_robot_util->m_nbJoints);
for(unsigned int i=0; i<m_robot_util->m_nbJoints; i++)
......
......@@ -20,6 +20,65 @@
#include <sot/torque_control/commands-helper.hh>
#include <sot/torque_control/ddp-actuator-solver.hh>
#include <ddp-actuator-solver/examples/dctemp.hh>
#include <ddp-actuator-solver/examples/costtemp.hh>
#if DEBUG
#define ODEBUG(x) std::cout << x << std::endl
#else
#define ODEBUG(x)
#endif
#define ODEBUG3(x) std::cout << x << std::endl
#define DBGFILE "/tmp/debug-ddp_actuator_solver.dat"
#define RESETDEBUG5() { std::ofstream DebugFile; \
DebugFile.open(DBGFILE,std::ofstream::out); \
DebugFile.close();}
#define ODEBUG5FULL(x) { std::ofstream DebugFile; \
DebugFile.open(DBGFILE,std::ofstream::app); \
DebugFile << __FILE__ << ":" \
<< __FUNCTION__ << "(#" \
<< __LINE__ << "):" << x << std::endl; \
DebugFile.close();}
#define ODEBUG5(x) { std::ofstream DebugFile; \
DebugFile.open(DBGFILE,std::ofstream::app); \
DebugFile << x << std::endl; \
DebugFile.close();}
#define RESETDEBUG4()
#define ODEBUG4FULL(x)
#define ODEBUG4(x)
#include <ddp-actuator-solver/examples/dctemp.hh>
#include <ddp-actuator-solver/examples/costtemp.hh>
#if DEBUG
#define ODEBUG(x) std::cout << x << std::endl
#else
#define ODEBUG(x)
#endif
#define ODEBUG3(x) std::cout << x << std::endl
#define DBGFILE "/tmp/debug-ddp_actuator_solver.dat"
#define RESETDEBUG5() { std::ofstream DebugFile; \
DebugFile.open(DBGFILE,std::ofstream::out); \
DebugFile.close();}
#define ODEBUG5FULL(x) { std::ofstream DebugFile; \
DebugFile.open(DBGFILE,std::ofstream::app); \
DebugFile << __FILE__ << ":" \
<< __FUNCTION__ << "(#" \
<< __LINE__ << "):" << x << std::endl; \
DebugFile.close();}
#define ODEBUG5(x) { std::ofstream DebugFile; \
DebugFile.open(DBGFILE,std::ofstream::app); \
DebugFile << x << std::endl; \
DebugFile.close();}
#define RESETDEBUG4()
#define ODEBUG4FULL(x)
#define ODEBUG4(x)
namespace dynamicgraph
{
......@@ -32,7 +91,7 @@ namespace dynamicgraph
using namespace dynamicgraph;
using namespace dynamicgraph::command;
using namespace Eigen;
/// Define EntityClassName here rather than in the header file
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
typedef DdpActuatorSolver EntityClassName;
......@@ -48,16 +107,18 @@ namespace dynamicgraph
CONSTRUCT_SIGNAL_IN (pos_joint_measure, dynamicgraph::Vector),
CONSTRUCT_SIGNAL_IN (dx_measure, dynamicgraph::Vector),
CONSTRUCT_SIGNAL_IN (tau_measure, dynamicgraph::Vector),
CONSTRUCT_SIGNAL_IN (tau_des, dynamicgraph::Vector),
CONSTRUCT_SIGNAL_IN (temp_measure, dynamicgraph::Vector),
CONSTRUCT_SIGNAL_OUT(tau, dynamicgraph::Vector, m_pos_desSIN),
m_T(3000),
m_dt(1e-3),
m_iterMax(100),
m_stopCrit(1e-5),
m_solver(m_model,m_cost,
DISABLE_FULLDDP,
DISABLE_QPBOX)
DISABLE_QPBOX),
m_T(3000),
m_stopCrit(1e-5),
m_iterMax(100)
{
RESETDEBUG5();
Entity::signalRegistration( ALL_INPUT_SIGNALS << ALL_OUTPUT_SIGNALS );
m_zeroState.setZero();
......@@ -79,8 +140,8 @@ namespace dynamicgraph
/// ---- Get the information -----
/// Desired position
const dynamicgraph::Vector &
pos_des = m_pos_desSIN(iter);
/// Measured position
pos_des = m_pos_desSIN(iter);
/// Measured position
const dynamicgraph::Vector &
pos_joint_measure = m_pos_joint_measureSIN(iter);
/// Measured speed
......@@ -91,8 +152,11 @@ namespace dynamicgraph
temp_measure = m_temp_measureSIN(iter);
/// Measured torque
const dynamicgraph::Vector &
tau_measure = m_tau_measureSIN(iter);
tau_measure = m_tau_measureSIN(iter);
/// Desired torque
const dynamicgraph::Vector &
tau_des = m_tau_desSIN(iter);
DDPSolver<double,5,1>::stateVec_t xinit,xDes;
/// --- Initialize solver ---
......@@ -100,16 +164,41 @@ namespace dynamicgraph
dx_measure(0),
temp_measure(0),
tau_measure(0),
m_ambiant_temperature;
//m_ambiant_temperature;
25.0;
xDes << pos_des, 0.0, 25.0, tau_des, 25.0;
ODEBUG5(xinit);
ODEBUG5("");
ODEBUG5(xDes);
DCTemp model;
CostTemp cost;
DDPSolver<double,5,1> m_solver(model,cost,0,0);
m_solver.FirstInitSolver(xinit,xDes,m_T,m_dt,m_iterMax, m_stopCrit);
ODEBUG5("FirstInitSolver");
/// --- Solve the DDP ---
m_solver.solveTrajectory();
ODEBUG5("Trajectory solved");
xDes << m_pos_desSIN(0), 0.0, 0.0, 0.0, 0.0;
/// --- Get the command ---
DDPSolver<double,5,1>::traj lastTraj;
lastTraj = m_solver.getLastSolvedTrajectory();
ODEBUG5("getLastSolvedTrajectory");
m_solver.initSolver(xinit,xDes);
DDPSolver<double,5,1>::commandVecTab_t uList;
uList = lastTraj.uList;
ODEBUG5("uList");
/// --- Solve the DDP ---
s = m_solver.solveTrajectory();
return s;
//s = uList[0];
s.resize(32);
s << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, uList[0], 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
//s.setZero(32);
ODEBUG5(s);
return s;
}
void DdpActuatorSolver::
......@@ -137,4 +226,3 @@ namespace dynamicgraph
} // namespace torque_control
} // namespace sot
} // namespace dynamicgraph
......@@ -281,7 +281,7 @@ void DeviceTorqueCtrl::computeForwardDynamics()
m_dvBar, m_numericalDamping);
// compute base of null space of constraint Jacobian
Eigen::Index r = (m_Jc_svd.singularValues().array()>1e-8).count();
Eigen::VectorXd::Index r = (m_Jc_svd.singularValues().array()>1e-8).count();
m_Z = m_Jc_svd.matrixV().rightCols(m_nj+6-r);
// compute constrained accelerations ddq_c = (Z^T*M*Z)^{-1}*Z^T*(S^T*tau - h - M*ddqBar)
......
......@@ -147,8 +147,8 @@ namespace dynamicgraph
const Eigen::VectorXd& q= m_base6d_encodersSIN(iter); //n+6
const Eigen::VectorXd& dq= m_joint_velocitiesSIN(iter);
assert(q.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6) && "Unexpected size of signal base6d_encoder");
assert(dq.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_velocities");
assert(q.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints+6) && "Unexpected size of signal base6d_encoder");
assert(dq.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_velocities");
/* convert sot to pinocchio joint order */
m_robot_util->joints_sot_to_urdf(q.tail(m_robot_util->m_nbJoints), m_q_pin.tail(m_robot_util->m_nbJoints));
......@@ -168,7 +168,7 @@ namespace dynamicgraph
SEND_WARNING_STREAM_MSG("Cannot compute signal base6dFromFoot_encoders before initialization!");
return s;
}
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6))
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints+6))
s.resize(m_robot_util->m_nbJoints+6);
m_kinematics_computationsSINNER(iter);
......@@ -176,7 +176,7 @@ namespace dynamicgraph
getProfiler().start(PROFILE_FREE_FLYER_COMPUTATION);
{
const Eigen::VectorXd& q= m_base6d_encodersSIN(iter); //n+6
assert(q.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6) && "Unexpected size of signal base6d_encoder");
assert(q.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints+6) && "Unexpected size of signal base6d_encoder");
/* Compute kinematic and return q with freeflyer */
const pinocchio::SE3 iMo1(m_data->oMf[m_left_foot_id].inverse());
......@@ -232,7 +232,7 @@ namespace dynamicgraph
SEND_WARNING_STREAM_MSG("Cannot compute signal v before initialization!");
return s;
}
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6))
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints+6))
s.resize(m_robot_util->m_nbJoints+6);
m_kinematics_computationsSINNER(iter);
......@@ -240,7 +240,7 @@ namespace dynamicgraph
getProfiler().start(PROFILE_FREE_FLYER_VELOCITY_COMPUTATION);
{
const Eigen::VectorXd& dq= m_joint_velocitiesSIN(iter);
assert(dq.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_velocities");
assert(dq.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_velocities");
/* Compute foot velocities */
const Frame & f_lf = m_model->frames[m_left_foot_id];
......
This diff is collapsed.
......@@ -258,7 +258,7 @@ namespace dynamicgraph
if(m_firstIter)
{
const dynamicgraph::Vector& base6d_encoders = m_base6d_encodersSIN(iter);
if(base6d_encoders.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6))
if(base6d_encoders.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints+6))
{
SEND_ERROR_STREAM_MSG("Unexpected size of signal base6d_encoder " +
toString(base6d_encoders.size()) + " " +
......@@ -271,7 +271,7 @@ namespace dynamicgraph
m_firstIter = false;
}
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints))
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints))
s.resize(m_robot_util->m_nbJoints);
if(m_status[0]==JTG_TEXT_FILE)
......@@ -321,7 +321,7 @@ namespace dynamicgraph
}
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints))
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints))
s.resize(m_robot_util->m_nbJoints);
if(m_status[0]==JTG_TEXT_FILE)
{
......@@ -344,7 +344,7 @@ namespace dynamicgraph
}
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints))
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints))
s.resize(m_robot_util->m_nbJoints);
if(m_status[0]==JTG_TEXT_FILE)
......
......@@ -151,16 +151,16 @@ namespace dynamicgraph
const VectorN& qRef = m_qRefSIN(iter); // n
const VectorN& dqRef = m_dqRefSIN(iter); // n
assert(q.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6) && "Unexpected size of signal base6d_encoder");
assert(dq.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal dq");
assert(qRef.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal qRef");
assert(dqRef.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal dqRef");
assert(Kp.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal Kd");
assert(Kd.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal Kd");
assert(q.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints+6) && "Unexpected size of signal base6d_encoder");
assert(dq.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal dq");
assert(qRef.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal qRef");
assert(dqRef.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal dqRef");
assert(Kp.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal Kd");
assert(Kd.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal Kd");
m_pwmDes = Kp.cwiseProduct(qRef-q.tail(m_robot_util->m_nbJoints)) + Kd.cwiseProduct(dqRef-dq);
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints))
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints))
s.resize(m_robot_util->m_nbJoints);
s = m_pwmDes;
}
......@@ -179,10 +179,10 @@ namespace dynamicgraph
const VectorN6& q = m_base6d_encodersSIN(iter); //n+6
const VectorN& qRef = m_qRefSIN(iter); // n
assert(q