Commit 4a77a637 authored by Timothee Habra's avatar Timothee Habra

add short feet contact gcm

parent 943f0927
#include "ComputeGCM.hh"
#include "WholeRectFeet.hh"
#include "WholeShortFeet.hh"
#include "FlatGround.hh"
#include "LinearGround.hh"
......@@ -15,7 +16,9 @@ ComputeGCM::ComputeGCM(MBSdataStruct *MBSdata)
ground = new FlatGround(MBSdata);
whole_feet = new WholeRectFeet(MBSdata, ground);
#ifdef SHORT_FEET
whole_feet = new WholeShortFeet(MBSdata, ground);
#endif
}
/*! \brief destructor
......
#include "ShortFoot.hh"
#include <cstdlib>
/*
* Foot dimensions:
* x: -0.06 to 0.08 = 0.14
* y: -0.045 to 0.045 = 0.09
*/
#define NB_X_POINTS 5
#define NB_Y_POINTS 4
#define NB_POINTS (NB_X_POINTS * NB_Y_POINTS)
#define X_MIN -0.06 ///< minimal x position [m]
#define Y_MIN -0.045 ///< minimal y posiiton [m]
#define X_RES 0.035 ///< x increment [m]
#define Y_RES 0.03 ///< y increment [m]
/*! \brief constructor
*
* \param[in] mbs_data Robotran structure
* \param[in] gait_features features of the gait class
* \param[in] foot_id ID of the foot
*/
ShortFoot::ShortFoot(MBSdataStruct *MBSdata, int S_sens_id, int left_flag): ContactFoot(MBSdata, NB_POINTS, S_sens_id)
{
int index;
for (int i=0; i<NB_X_POINTS; i++)
{
for (int j=0; j<NB_Y_POINTS; j++)
{
index = j + i*NB_Y_POINTS;
rn[index][0] = X_MIN + i*X_RES; ///< x indexes
rn[index][1] = Y_MIN + j*Y_RES; ///< y indexes
rn[index][2] = 0.0; ///< z indexes
}
}
}
/*! \brief destructor
*/
ShortFoot::~ShortFoot()
{
// free already done in mother class
}
/*!
* \author Nicolas Van der Noot
* \file ShortFoot.hh
* \brief ShortFoot class
*/
#ifndef _SHORT_FOOT_HH_
#define _SHORT_FOOT_HH_
#include "ContactFoot.hh"
/*! \brief Model of a contact foot: short foot
*/
class ShortFoot: public ContactFoot
{
public:
ShortFoot(MBSdataStruct *MBSdata, int S_sens_id, int left_flag);
virtual ~ShortFoot();
};
#endif
#include "WholeShortFeet.hh"
#include "ShortFoot.hh"
#include "simu_def.h"
#define NB_FEET_BODIES 2
enum {RIGHT_ID, LEFT_ID};
/*! \brief contructor
*
* \param[in] mbs_data Robotran structure
* \param[in] gait_features features of the gait
* \param[in] sens_info information from sensors
* \param[in] ground ground model
*/
WholeShortFeet::WholeShortFeet(MBSdataStruct *MBSdata, GroundModel *ground): WholeFeet(MBSdata, ground)
{
for(int i=0; i<NB_FEET_BODIES; i++)
{
switch (i)
{
case RIGHT_ID : bodies.push_back(new ShortFoot(MBSdata, MBSdata->Nsensor + RFOOT_FSENS_ID, 0)); break;
case LEFT_ID : bodies.push_back(new ShortFoot(MBSdata, MBSdata->Nsensor + LFOOT_FSENS_ID, 1)); break;
default: break;
}
}
}
/*! \brief destructor
*/
WholeShortFeet::~WholeShortFeet()
{
// delete already done in mother class
}
/*! \brief compute the external ground reactions related to this foot
*
* \param[out] F_tot external force output [N]
* \param[out] T_tot external torque output [Nm]
* \param[in] PxF absolute position (provided by Robotran) [m]
* \param[in] RxF absolute rotation matrix (provided by Robotran) [-]
* \param[in] VxF absolute position derivative (provided by Robotran) [m/s]
* \param[in] OMxF absolute rotation derivatives (provided by Robotran) [rad/s]
* \param[in] index index of the external sensor
*/
void WholeShortFeet::compute_F_T(double F_tot[3], double T_tot[3], double PxF[4], double RxF[4][4], double VxF[4], double OMxF[4], int index)
{
ContactFoot *body;
switch ( index )
{
case RFOOT_FSENS_ID:
body = bodies[RIGHT_ID];
break;
case LFOOT_FSENS_ID:
body = bodies[LEFT_ID];
break;
default:
body = NULL;
break;
}
body->update_pose(PxF, RxF, VxF, OMxF);
ground->compute_F_T(F_tot, T_tot, body);
}
/*! \brief get feet forces
*
* \param[in] leg_id ID of the leg
* \param[in] axis axis requested
* \return force requested [N]
*/
double WholeShortFeet::get_leg_feet_forces(int leg_id, int axis)
{
switch (leg_id)
{
case RIGHT_ID:
return bodies[RIGHT_ID]->get_F_tot(axis);
break;
case LEFT_ID:
return bodies[LEFT_ID]->get_F_tot(axis);
break;
default:
return 0.0;
break;
}
}
/*! \brief get feet torques
*
* \param[in] leg_id ID of the leg
* \param[in] axis axis requested
* \return torque requested [Nm]
*/
double WholeShortFeet::get_leg_feet_torques(int leg_id, int axis)
{
switch (leg_id)
{
case RIGHT_ID:
return bodies[RIGHT_ID]->get_T_tot(axis);
break;
case LEFT_ID:
return bodies[LEFT_ID]->get_T_tot(axis);
break;
default:
return 0.0;
break;
}
}
/*!
* \author Nicolas Van der Noot
* \file WholeShortFeet.hh
* \brief WholeShortFeet class
*/
#ifndef _WHOLE_SHORT_FEET_HH_
#define _WHOLE_SHORT_FEET_HH_
#include "WholeFeet.hh"
/*! \brief Whole feet: short feet
*/
class WholeShortFeet: public WholeFeet
{
public:
WholeShortFeet(MBSdataStruct *MBSdata, GroundModel *ground);
virtual ~WholeShortFeet();
virtual void compute_F_T(double F_tot[3], double T_tot[3], double PxF[4], double RxF[4][4], double VxF[4], double OMxF[4], int index);
virtual double get_leg_feet_forces(int leg_id, int axis);
virtual double get_leg_feet_torques(int leg_id, int axis);
};
#endif
......@@ -8,6 +8,8 @@
//---------------------------
#include "simu_def.h"
#include "gcm_interface.h"
#include "contact_interface.h"
#ifdef SIMBODY
#include "simbody_functions.h"
......@@ -112,22 +114,44 @@ double* user_ExtForces(double PxF[4], double RxF[4][4],
{
#ifdef GCM_MT
// Compute thing resultant forces and moments from the ground
ground_mesh_model(PxF, RxF, VxF, OMxF, MBSdata, tsim, ixF, dxF, SWr);
//ground_mesh_model(PxF, RxF, VxF, OMxF, MBSdata, tsim, ixF, dxF, SWr);
#endif
// Limit external forces
for (i=1; i<=3; i++)
{
SWr[i] = (SWr[i] > MAX_EXT_FORCES) ? MAX_EXT_FORCES : SWr[i];
SWr[i] = (SWr[i] < -MAX_EXT_FORCES) ? -MAX_EXT_FORCES : SWr[i];
}
// update the contact primitives model: kinematics and force-torque of the shapes
if (ixF == 1)
{
update_contact_geom(MBSdata);
}
// Limit external moments
for (i=4; i<=6; i++)
{
SWr[i] = (SWr[i] > MAX_EXT_MOMENTS) ? MAX_EXT_MOMENTS : SWr[i];
SWr[i] = (SWr[i] < -MAX_EXT_MOMENTS) ? -MAX_EXT_MOMENTS : SWr[i];
}
// apply force-torque computed by the contact primitives model
apply_contact_geom(MBSdata, ixF, &Fx, &Fy, &Fz, &Mx, &My, &Mz);
SWr[1] = Fx;
SWr[2] = Fy;
SWr[3] = Fz;
SWr[4] = Mx;
SWr[5] = My;
SWr[6] = Mz;
// Compute thing resultant forces and moments from the ground
#ifdef MESH_GSM
gcm_mesh_compute_F_T(MBSdata, SWr, PxF, RxF, VxF, OMxF, ixF);
#endif
// Limit external forces
for (i=1; i<=3; i++)
{
SWr[i] = (SWr[i] > MAX_EXT_FORCES) ? MAX_EXT_FORCES : SWr[i];
SWr[i] = (SWr[i] < -MAX_EXT_FORCES) ? -MAX_EXT_FORCES : SWr[i];
}
// Limit external moments
for (i=4; i<=6; i++)
{
SWr[i] = (SWr[i] > MAX_EXT_MOMENTS) ? MAX_EXT_MOMENTS : SWr[i];
SWr[i] = (SWr[i] < -MAX_EXT_MOMENTS) ? -MAX_EXT_MOMENTS : SWr[i];
}
// Write the results in the internal variables (for HG model)
switch (ixF)
......
......@@ -8,6 +8,8 @@
//---------------------------
#include "simu_def.h"
#include "gcm_interface.h"
#include "contact_interface.h"
// Equivalent to user_DirDyn_io from Matlab
// Computes the voltage to apply on the motors
......@@ -18,6 +20,14 @@ void user_compute_output(SimStruct *S, MBSdataStruct *MBSdata, LocalDataStruct *
void user_compute_output(MBSdataStruct *MBSdata, LocalDataStruct *lds)
#endif
{
// GCM
#ifdef MESH_GSM
gcm_mesh_state_compute(MBSdata);
#endif
// 3D contact
user_state_contact_geom(MBSdata);
simu_controller_loop(MBSdata);
}
......@@ -14,12 +14,21 @@
#include "MBSdataStruct.h"
//#include "simu_def.h"
#include "gcm_interface.h"
#include "contact_interface.h"
/*
* User function called at the end of the simulation
*/
void user_finalization(MBSdataStruct *MBSdata)
{
// GCM
#ifdef MESH_GSM
free_mesh_gcm(MBSdata);
#endif
// 3D contact
close_contact_geom(MBSdata);
// controller
//controller_close_interface(MBSdata);
......
......@@ -8,6 +8,8 @@
//---------------------------
#include "simu_def.h"
#include "gcm_interface.h"
#include "contact_interface.h"
#define FORCE_VEC_LENGTH 50
......@@ -23,6 +25,14 @@ int user_initialization(MBSdataStruct *MBSdata, LocalDataStruct *lds)
cvs = MBSdata->user_IO->cvs;
// init mesh GCM (Ground Contact Model)
#ifdef MESH_GSM
init_mesh_gcm(MBSdata);
#endif
// init 3D contact
init_contact_geom(MBSdata);
// init GCM (Ground Contact Model)
init_GCM(MBSdata);
......
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