Commit 520ed959 authored by Olivier Stasse's avatar Olivier Stasse
Browse files

[pinocchio-device] Checked unit test for one rotation joint.

parent da269cd8
Pipeline #2055 failed with stage
in 6 minutes and 27 seconds
......@@ -131,7 +131,10 @@ namespace dynamicgraph {
return CLASS_NAME;
}
static const double TIMESTEP_DEFAULT;
/// Maps of joint devices.
std::map<std::string,dgsot::JointSoTHWControlType> jointPinocchioDevices_;
protected:
/// \brief Current integration step.
double timestep_;
......@@ -159,8 +162,6 @@ namespace dynamicgraph {
std::map<std::string,ControlType> sotControlType_;
std::map<std::string,ControlType> hwControlType_;
/// Maps of joint devices.
std::map<std::string,JointSoTHWControlType> jointPinocchioDevices_;
///
PeriodicCall periodicCallBefore_;
PeriodicCall periodicCallAfter_;
......
......@@ -29,7 +29,7 @@ using namespace std;
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
#define DBGFILE "/tmp/sot-talos-device.txt"
#define DBGFILE "/tmp/pinocchio-device.txt"
#if 0
#define RESETDEBUG5() { std::ofstream DebugFile; \
......@@ -295,8 +295,17 @@ setURDFModel(const std::string &aURDFModel)
/// Build the map between pinocchio and the alphabetical order.
for(unsigned int i=0;i<model_.names.size();i++)
jointPinocchioDevices_[model_.names[i]].pinocchio_index=i;
{
if (model_.joints[i].id()<model_.nq)
{
jointPinocchioDevices_[model_.names[i]].pinocchio_index=i;
std::cout << "jointPinocchioDevices_ index: " << i
<< " model_.joints[i].id(): " << model_.joints[i].id()
<< " model_.names[i]: " << model_.names[i]
<< " model_.joints[i].shortname() : "<< model_.joints[i].shortname()
<< std::endl;
}
}
// Initialize pinocchio vector.
position_.resize(model_.nq);
velocity_.resize(model_.nv);
......@@ -400,6 +409,14 @@ void PinocchioDevice::integrate( const double & dt )
int lctl_index = it_control_type->second.control_index;
int pino_index = it_control_type->second.pinocchio_index;
if (lctl_index==-1)
{
if (debug_mode_>1)
std::cerr << "No control index for joint "
<< model_.names[pino_index] << std::endl;
break;
}
if (pino_index!=-1)
{
int lpos_index = model_.joints[pino_index].idx_q();
......@@ -410,6 +427,8 @@ void PinocchioDevice::integrate( const double & dt )
double lvelocityLimit = model_.velocityLimit[lvel_index];
// Integration.
// Set acceleration from control and integrates to find velocity.
if (it_control_type->second.SoTcontrol==ACCELERATION)
{
acceleration_[lvel_index] = controlIN[lctl_index];
......@@ -423,6 +442,7 @@ void PinocchioDevice::integrate( const double & dt )
shouldIntegrateVelocity=true;
}
// Set velocity from control and set boolean to perform velocity integration later on.
else if (it_control_type->second.SoTcontrol==VELOCITY)
{
acceleration_[lvel_index]=0;
......@@ -434,6 +454,7 @@ void PinocchioDevice::integrate( const double & dt )
shouldIntegrateVelocity=true;
}
// Position from control is directly provided.
else if (it_control_type->second.SoTcontrol==POSITION)
{
acceleration_[lvel_index]=0;
......@@ -509,6 +530,7 @@ ParseYAMLString(const std::string & aYamlString)
if (r<0) return r;
}
}
UpdateSignals();
return 0;
}
......@@ -720,20 +742,25 @@ void PinocchioDevice::CreateAnImuSignal(const std::string &imu_sensor_name)
int PinocchioDevice::
UpdateSignals()
{
pseudoTorqueSOUT_ = new Signal<Vector, int>
("PinocchioDevice("+getName()+")::output(vector)::ptorque" );
currentsSOUT_ = new Signal<Vector, int>
("PinocchioDevice("+getName()+")::output(vector)::currents" );
if ((torque_index_!=0) && (pseudoTorqueSOUT_!=0))
pseudoTorqueSOUT_ = new Signal<Vector, int>
("PinocchioDevice("+getName()+")::output(vector)::ptorque" );
temperatureSOUT_ = new Signal<Vector, int>
("PinocchioDevice("+getName()+")::output(vector)::temperatures");
if ((current_index_!=0) && (currentsSOUT_!=0))
currentsSOUT_ = new Signal<Vector, int>
("PinocchioDevice("+getName()+")::output(vector)::currents" );
motor_anglesSOUT_ = new Signal<Vector, int>
("PinocchioDevice("+getName()+")::output(vector)::motor_angles");
if ((temperature_index_!=0) && (temperatureSOUT_!=0))
temperatureSOUT_ = new Signal<Vector, int>
("PinocchioDevice("+getName()+")::output(vector)::temperatures");
joint_anglesSOUT_ = new Signal<Vector, int>
("PinocchioDevice("+getName()+")::output(vector)::joint_angles");
if ((motor_angle_index_!=0) && (motor_anglesSOUT_!=0))
motor_anglesSOUT_ = new Signal<Vector, int>
("PinocchioDevice("+getName()+")::output(vector)::motor_angles");
if ((joint_angle_index_!=0) && (joint_anglesSOUT_!=0))
joint_anglesSOUT_ = new Signal<Vector, int>
("PinocchioDevice("+getName()+")::output(vector)::joint_angles");
return 0;
}
......@@ -1000,8 +1027,9 @@ void PinocchioDevice::getControl(map<string,dgsot::ControlValues> &controlOut)
{
ODEBUG5FULL("start");
sotDEBUGIN(25) ;
vector<double> anglesOut;
anglesOut.resize(state_.size());
const Vector & controlIN = controlSIN.accessCopy();
vector<double> lcontrolOut;
lcontrolOut.resize(controlIN.size());
// Integrate control
increment(timestep_);
......@@ -1010,13 +1038,24 @@ void PinocchioDevice::getControl(map<string,dgsot::ControlValues> &controlOut)
ODEBUG5FULL("state = "<< state_);
// Specify the joint values for the controller.
if ((int)anglesOut.size()!=state_.size()-6)
anglesOut.resize(state_.size()-6);
for(unsigned int i=6; i < state_.size();++i)
anglesOut[i-6] = state_(i);
controlOut["control"].setValues(anglesOut);
JointSHWControlType_iterator it_control_type;
for(it_control_type = jointPinocchioDevices_.begin();
it_control_type != jointPinocchioDevices_.end();
it_control_type++)
{
int lctl_index = it_control_type->second.control_index;
if (it_control_type->second.HWcontrol==TORQUE)
lcontrolOut[lctl_index] = controlIN[lctl_index];
else if (it_control_type->second.HWcontrol==POSITION)
{
int pino_index = it_control_type->second.pinocchio_index;
int lpos_index = model_.joints[pino_index].idx_q();
lcontrolOut[lctl_index] = position_[lpos_index];
}
}
controlOut["control"].setValues(lcontrolOut);
ODEBUG5FULL("end");
sotDEBUGOUT(25) ;
}
......
......@@ -29,220 +29,188 @@ void CreateYAMLFILE()
{
YAML::Emitter yaml_out;
YAML::Node aNode,yn_map_hw_sot_c,yn_map_sensors;
unsigned int index_vec_ctl=0;
yn_map_hw_sot_c = aNode["map_hardware_sot_control"];
yn_map_sensors = aNode["sensors"];
/*
yn_map_hw_sot_c["waist"];
yn_map_hw_sot_c["waist"]["hw"] = "POSITION";
yn_map_hw_sot_c["waist"]["sot"] = "POSITION";
yn_map_hw_sot_c["waist"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["waist"]["controlPos"] = 0;
yn_map_hw_sot_c["waist"]["sensors"] = "";
index_vec_ctl+=6;
*/
yn_map_hw_sot_c["LLEG_HIP_P"];
yn_map_hw_sot_c["LLEG_HIP_P"]["hw"] = "POSITION";
yn_map_hw_sot_c["LLEG_HIP_P"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["LLEG_HIP_P"]["controlPos"] = 6;
yn_map_hw_sot_c["LLEG_HIP_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
yn_map_hw_sot_c["LLEG_HIP_R"];
yn_map_hw_sot_c["LLEG_HIP_R"]["hw"] = "POSITION";
yn_map_hw_sot_c["LLEG_HIP_R"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["LLEG_HIP_R"]["controlPos"] = 7;
yn_map_hw_sot_c["LLEG_HIP_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
yn_map_hw_sot_c["LLEG_HIP_Y"];
yn_map_hw_sot_c["LLEG_HIP_Y"]["hw"] = "POSITION";
yn_map_hw_sot_c["LLEG_HIP_Y"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["LLEG_HIP_Y"]["controlPos"] = 8;
yn_map_hw_sot_c["LLEG_HIP_Y"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
yn_map_hw_sot_c["LLEG_KNEE"];
yn_map_hw_sot_c["LLEG_KNEE"]["hw"] = "POSITION";
yn_map_hw_sot_c["LLEG_KNEE"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["LLEG_KNEE"]["controlPos"] = 9;
yn_map_hw_sot_c["LLEG_KNEE"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
yn_map_hw_sot_c["LLEG_ANKLE_P"];
yn_map_hw_sot_c["LLEG_ANKLE_P"]["hw"] = "POSITION";
yn_map_hw_sot_c["LLEG_ANKLE_P"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["LLEG_ANKLE_P"]["controlPos"] = 10;
yn_map_hw_sot_c["LLEG_ANKLE_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
yn_map_hw_sot_c["LLEG_ANKLE_R"];
yn_map_hw_sot_c["LLEG_ANKLE_R"]["hw"] = "POSITION";
yn_map_hw_sot_c["LLEG_ANKLE_R"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["LLEG_ANKLE_R"]["controlPos"] = 11;
yn_map_hw_sot_c["LLEG_ANKLE_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
yn_map_hw_sot_c["RLEG_HIP_P"];
yn_map_hw_sot_c["RLEG_HIP_P"]["hw"] = "POSITION";
yn_map_hw_sot_c["RLEG_HIP_P"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["RLEG_HIP_P"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["RLEG_HIP_P"]["controlPos"] = 12;
yn_map_hw_sot_c["RLEG_HIP_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
yn_map_hw_sot_c["RLEG_HIP_R"];
yn_map_hw_sot_c["RLEG_HIP_R"]["hw"] = "POSITION";
yn_map_hw_sot_c["RLEG_HIP_R"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["RLEG_HIP_R"]["controlPos"] = 13;
yn_map_hw_sot_c["RLEG_HIP_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
yn_map_hw_sot_c["RLEG_HIP_Y"];
yn_map_hw_sot_c["RLEG_HIP_Y"]["hw"] = "POSITION";
yn_map_hw_sot_c["RLEG_HIP_Y"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["RLEG_HIP_Y"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["RLEG_HIP_Y"]["controlPos"] = 14;
yn_map_hw_sot_c["RLEG_HIP_Y"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
yn_map_hw_sot_c["RLEG_KNEE"];
yn_map_hw_sot_c["RLEG_KNEE"]["hw"] = "POSITION";
yn_map_hw_sot_c["RLEG_KNEE"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["RLEG_KNEE"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["RLEG_KNEE"]["controlPos"] = 15;
yn_map_hw_sot_c["RLEG_KNEE"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
yn_map_hw_sot_c["RLEG_ANKLE_P"];
yn_map_hw_sot_c["RLEG_ANKLE_P"]["hw"] = "POSITION";
yn_map_hw_sot_c["RLEG_ANKLE_P"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["RLEG_ANKLE_P"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["RLEG_ANKLE_P"]["controlPos"] = 16;
yn_map_hw_sot_c["RLEG_ANKLE_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
yn_map_hw_sot_c["RLEG_ANKLE_R"];
yn_map_hw_sot_c["RLEG_ANKLE_R"]["hw"] = "POSITION";
yn_map_hw_sot_c["RLEG_ANKLE_R"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["RLEG_ANKLE_R"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["RLEG_ANKLE_R"]["controlPos"] = 17;
yn_map_hw_sot_c["RLEG_ANKLE_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
yn_map_hw_sot_c["LLEG_HIP_P"];
yn_map_hw_sot_c["LLEG_HIP_P"]["hw"] = "POSITION";
yn_map_hw_sot_c["LLEG_HIP_P"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["LLEG_HIP_P"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["LLEG_HIP_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
yn_map_hw_sot_c["LLEG_HIP_R"];
yn_map_hw_sot_c["LLEG_HIP_R"]["hw"] = "POSITION";
yn_map_hw_sot_c["LLEG_HIP_R"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["LLEG_HIP_R"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["LLEG_HIP_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
yn_map_hw_sot_c["LLEG_HIP_Y"];
yn_map_hw_sot_c["LLEG_HIP_Y"]["hw"] = "POSITION";
yn_map_hw_sot_c["LLEG_HIP_Y"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["LLEG_HIP_Y"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["LLEG_HIP_Y"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
yn_map_hw_sot_c["LLEG_KNEE"];
yn_map_hw_sot_c["LLEG_KNEE"]["hw"] = "POSITION";
yn_map_hw_sot_c["LLEG_KNEE"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["LLEG_KNEE"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["LLEG_KNEE"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
yn_map_hw_sot_c["LLEG_ANKLE_P"];
yn_map_hw_sot_c["LLEG_ANKLE_P"]["hw"] = "POSITION";
yn_map_hw_sot_c["LLEG_ANKLE_P"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["LLEG_ANKLE_P"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["LLEG_ANKLE_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
yn_map_hw_sot_c["WAIST_P"];
yn_map_hw_sot_c["WAIST_P"]["hw"] = "POSITION";
yn_map_hw_sot_c["WAIST_P"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["WAIST_P"]["controlPos"] = 18;
yn_map_hw_sot_c["WAIST_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
yn_map_hw_sot_c["LLEG_ANKLE_R"];
yn_map_hw_sot_c["LLEG_ANKLE_R"]["hw"] = "POSITION";
yn_map_hw_sot_c["LLEG_ANKLE_R"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["LLEG_ANKLE_R"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["LLEG_ANKLE_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
yn_map_hw_sot_c["WAIST_R"];
yn_map_hw_sot_c["WAIST_R"]["hw"] = "POSITION";
yn_map_hw_sot_c["WAIST_R"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["WAIST_R"]["controlPos"] = 19;
yn_map_hw_sot_c["WAIST_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
yn_map_hw_sot_c["CHEST"];
yn_map_hw_sot_c["CHEST"]["hw"] = "POSITION";
yn_map_hw_sot_c["CHEST"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["CHEST"]["controlPos"] = 20;
yn_map_hw_sot_c["CHEST"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
yn_map_hw_sot_c["RARM_SHOULDER_P"];
yn_map_hw_sot_c["RARM_SHOULDER_P"]["hw"] = "POSITION";
yn_map_hw_sot_c["RARM_SHOULDER_P"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["RARM_SHOULDER_P"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["RARM_SHOULDER_P"]["controlPos"] = 21;
yn_map_hw_sot_c["RARM_SHOULDER_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
yn_map_hw_sot_c["RARM_SHOULDER_R"];
yn_map_hw_sot_c["RARM_SHOULDER_R"]["hw"] = "POSITION";
yn_map_hw_sot_c["RARM_SHOULDER_R"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["RARM_SHOULDER_R"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["RARM_SHOULDER_R"]["controlPos"] = 22;
yn_map_hw_sot_c["RARM_SHOULDER_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
yn_map_hw_sot_c["RARM_SHOULDER_Y"];
yn_map_hw_sot_c["RARM_SHOULDER_Y"]["hw"] = "POSITION";
yn_map_hw_sot_c["RARM_SHOULDER_Y"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["RARM_SHOULDER_Y"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["RARM_SHOULDER_Y"]["controlPos"] = 23;
yn_map_hw_sot_c["RARM_SHOULDER_Y"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
yn_map_hw_sot_c["RARM_ELBOW"];
yn_map_hw_sot_c["RARM_ELBOW"]["hw"] = "POSITION";
yn_map_hw_sot_c["RARM_ELBOW"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["RARM_ELBOW"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["RARM_ELBOW"]["controlPos"] = 24;
yn_map_hw_sot_c["RARM_ELBOW"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
yn_map_hw_sot_c["RARM_WRIST_Y"];
yn_map_hw_sot_c["RARM_WRIST_Y"]["hw"] = "POSITION";
yn_map_hw_sot_c["RARM_WRIST_Y"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["RARM_WRIST_Y"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["RARM_WRIST_Y"]["controlPos"] = 25;
yn_map_hw_sot_c["RARM_WRIST_Y"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
yn_map_hw_sot_c["RARM_WRIST_P"];
yn_map_hw_sot_c["RARM_WRIST_P"]["hw"] = "POSITION";
yn_map_hw_sot_c["RARM_WRIST_P"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["RARM_WRIST_P"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["RARM_WRIST_P"]["controlPos"] = 26;
yn_map_hw_sot_c["RARM_WRIST_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
yn_map_hw_sot_c["RARM_WRIST_R"];
yn_map_hw_sot_c["RARM_WRIST_R"]["hw"] = "POSITION";
yn_map_hw_sot_c["RARM_WRIST_R"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["RARM_WRIST_R"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["RARM_WRIST_R"]["controlPos"] = 27;
yn_map_hw_sot_c["RARM_WRIST_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current]";
index_vec_ctl+=1;
yn_map_hw_sot_c["LARM_SHOULDER_P"];
yn_map_hw_sot_c["LARM_SHOULDER_P"]["hw"] = "POSITION";
yn_map_hw_sot_c["LARM_SHOULDER_P"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["LARM_SHOULDER_P"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["LARM_SHOULDER_P"]["controlPos"] = 28;
yn_map_hw_sot_c["LARM_SHOULDER_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
yn_map_hw_sot_c["LARM_SHOULDER_R"];
yn_map_hw_sot_c["LARM_SHOULDER_R"]["hw"] = "POSITION";
yn_map_hw_sot_c["LARM_SHOULDER_R"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["LARM_SHOULDER_R"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["LARM_SHOULDER_R"]["controlPos"] = 29;
yn_map_hw_sot_c["LARM_SHOULDER_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
yn_map_hw_sot_c["LARM_SHOULDER_Y"];
yn_map_hw_sot_c["LARM_SHOULDER_Y"]["hw"] = "POSITION";
yn_map_hw_sot_c["LARM_SHOULDER_Y"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["LARM_SHOULDER_Y"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["LARM_SHOULDER_Y"]["controlPos"] = 30;
yn_map_hw_sot_c["LARM_SHOULDER_Y"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
yn_map_hw_sot_c["LARM_ELBOW"];
yn_map_hw_sot_c["LARM_ELBOW"]["hw"] = "POSITION";
yn_map_hw_sot_c["LARM_ELBOW"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["LARM_ELBOW"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["LARM_ELBOW"]["controlPos"] = 31;
yn_map_hw_sot_c["LARM_ELBOW"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
yn_map_hw_sot_c["LARM_WRIST_Y"];
yn_map_hw_sot_c["LARM_WRIST_Y"]["hw"] = "POSITION";
yn_map_hw_sot_c["LARM_WRIST_Y"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["LARM_WRIST_Y"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["LARM_WRIST_Y"]["controlPos"] = 32;
yn_map_hw_sot_c["LARM_WRIST_Y"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
yn_map_hw_sot_c["LARM_WRIST_P"];
yn_map_hw_sot_c["LARM_WRIST_P"]["hw"] = "POSITION";
yn_map_hw_sot_c["LARM_WRIST_P"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["LARM_WRIST_P"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["LARM_WRIST_P"]["controlPos"] = 33;
yn_map_hw_sot_c["LARM_WRIST_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
yn_map_hw_sot_c["LARM_WRIST_R"];
yn_map_hw_sot_c["LARM_WRIST_R"]["hw"] = "POSITION";
yn_map_hw_sot_c["LARM_WRIST_R"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["LARM_WRIST_R"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["LARM_WRIST_R"]["controlPos"] = 34;
yn_map_hw_sot_c["LARM_WRIST_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current]";
index_vec_ctl+=1;
yn_map_hw_sot_c["WAIST_P"];
yn_map_hw_sot_c["WAIST_P"]["hw"] = "POSITION";
yn_map_hw_sot_c["WAIST_P"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["WAIST_P"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["WAIST_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
yn_map_hw_sot_c["WAIST_R"];
yn_map_hw_sot_c["WAIST_R"]["hw"] = "POSITION";
yn_map_hw_sot_c["WAIST_R"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["WAIST_R"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["WAIST_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
yn_map_hw_sot_c["CHEST"];
yn_map_hw_sot_c["CHEST"]["hw"] = "POSITION";
yn_map_hw_sot_c["CHEST"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["CHEST"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["CHEST"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
yn_map_sensors["force_torque"];
yn_map_sensors["force_torque"]["left_ankle_ft"];
......@@ -353,7 +321,7 @@ int ReadYAMLFILE(dg::sot::PinocchioDevice &aDevice, unsigned int debug_mode)
namespace dg = dynamicgraph;
int main(int, char **)
{
unsigned int debug_mode=0;
unsigned int debug_mode=5;
std::string robot_description;
ifstream urdfFile;
......@@ -395,16 +363,33 @@ int main(int, char **)
const se3::Model & aModel = aDevice.getModel();
const dg::Vector & aPosition = aDevice.stateSOUT_(2001);
double diff=0;
for (unsigned int j=0;j<aPosition.size();j++)
double diff=0,ldiff;
dgsot::JointSHWControlType_iterator it_control_type;
for(it_control_type = aDevice.jointPinocchioDevices_.begin();
it_control_type != aDevice.jointPinocchioDevices_.end();
it_control_type++)
{
diff += fabs(aPosition(j) - aModel.lowerPositionLimit[j]);
if (debug_mode>1)
std::cout << diff << " " << aModel.names[j] << " "
<< aPosition(j) << " "<< aModel.lowerPositionLimit[j]
<< std::endl;
int lctl_index = it_control_type->second.control_index;
if (it_control_type->second.HWcontrol==dgsot::POSITION)
{
int pino_index = it_control_type->second.pinocchio_index;
if (pino_index!=-1)
{
int lpos_index = aModel.joints[pino_index].idx_q();
int lvel_index = aModel.joints[pino_index].idx_v();
ldiff = (aPosition[lpos_index] - aModel.lowerPositionLimit[lpos_index]);
diff += ldiff;
std::cout << ldiff << " " << aModel.names[pino_index] << " "
<< aPosition[lpos_index] << " "
<< aModel.lowerPositionLimit[lpos_index] << " "
<< -aModel.velocityLimit[lvel_index]
<< std::endl;
}
}
}
if (diff>1e-3)
return -1;
return 1;
return 0;
}
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