Commit 747b37e4 authored by Olivier Stasse's avatar Olivier Stasse
Browse files

First compilation with Eigen.

Not tested yet.
parent a2398869
......@@ -63,6 +63,8 @@ IF((BUILD_MULTI_MODEL_BENCHMARK OR BUILD_SINGLE_MODEL_BENCHMARKS)
SEARCH_FOR_BOOST()
ENDIF()
SEARCH_FOR_LAPACK()
# Search for Eigen.
ADD_REQUIRED_DEPENDENCY("eigen3 >= 3.0.5")
# Eigen (at least version 3.0.5) makes gcc report conversion warnings
......
......@@ -89,8 +89,8 @@ namespace PatternGeneratorJRL
@param[in] ClearStepStackHandler: Clean the stack of steps after copy.
*/
virtual void CommonInitializationOfWalking(COMState & lStartingCOMState,
MAL_S3_VECTOR_TYPE(double) & lStartingZMPPosition,
MAL_VECTOR( & ,double) BodyAnglesIni,
Eigen::Vector3d & lStartingZMPPosition,
Eigen::VectorXd & BodyAnglesIni,
FootAbsolutePosition & InitLeftFootAbsPos,
FootAbsolutePosition & InitRightFootAbsPos,
std::deque<RelativeFootPosition> & lRelativeFootPositions,
......@@ -113,10 +113,10 @@ namespace PatternGeneratorJRL
@param[out] ZMPTarget The target ZMP in the waist reference frame.
@return True is there is still some data to send, false otherwise.
*/
virtual bool RunOneStepOfTheControlLoop(MAL_VECTOR_TYPE(double) & CurrentConfiguration,
MAL_VECTOR_TYPE(double) & CurrentVelocity,
MAL_VECTOR_TYPE(double) & CurrentAcceleration,
MAL_VECTOR_TYPE(double) & ZMPTarget)=0;
virtual bool RunOneStepOfTheControlLoop(Eigen::VectorXd & CurrentConfiguration,
Eigen::VectorXd & CurrentVelocity,
Eigen::VectorXd & CurrentAcceleration,
Eigen::VectorXd & ZMPTarget)=0;
/*! \brief Run One Step of the global control loop aka The Main Method To Be Used.
@param[out] CurrentConfiguration The current configuration of the robot according to
......@@ -132,10 +132,10 @@ namespace PatternGeneratorJRL
@param[out] RightFootPosition: Absolute position of the right foot.
@return True is there is still some data to send, false otherwise.
*/
virtual bool RunOneStepOfTheControlLoop(MAL_VECTOR_TYPE(double) & CurrentConfiguration,
MAL_VECTOR_TYPE(double) & CurrentVelocity,
MAL_VECTOR_TYPE(double) & CurrentAcceleration,
MAL_VECTOR_TYPE(double) &ZMPTarget,
virtual bool RunOneStepOfTheControlLoop(Eigen::VectorXd & CurrentConfiguration,
Eigen::VectorXd & CurrentVelocity,
Eigen::VectorXd & CurrentAcceleration,
Eigen::VectorXd &ZMPTarget,
COMPosition &COMPosition,
FootAbsolutePosition &LeftFootPosition,
FootAbsolutePosition &RightFootPosition)=0;
......@@ -154,10 +154,10 @@ namespace PatternGeneratorJRL
@param[out] RightFootPosition: Absolute position of the right foot.
@return True is there is still some data to send, false otherwise.
*/
virtual bool RunOneStepOfTheControlLoop(MAL_VECTOR_TYPE(double) & CurrentConfiguration,
MAL_VECTOR_TYPE(double) & CurrentVelocity,
MAL_VECTOR_TYPE(double) & CurrentAcceleration,
MAL_VECTOR_TYPE(double) &ZMPTarget,
virtual bool RunOneStepOfTheControlLoop(Eigen::VectorXd & CurrentConfiguration,
Eigen::VectorXd & CurrentVelocity,
Eigen::VectorXd & CurrentAcceleration,
Eigen::VectorXd &ZMPTarget,
COMState &COMState,
FootAbsolutePosition &LeftFootPosition,
FootAbsolutePosition &RightFootPosition)=0;
......@@ -182,14 +182,14 @@ namespace PatternGeneratorJRL
It also updates the state of the robot if other control mechanisms
modifies the upper body part and if this should be taken into account
into the pattern generator in the second loop of control. */
virtual void SetCurrentJointValues(MAL_VECTOR( &lCurrentJointValues,double))=0;
virtual void SetCurrentJointValues(Eigen::VectorXd &lCurrentJointValues)=0;
/*! \brief Returns the walking mode. */
virtual int GetWalkMode() const=0;
/*! \brief Get the leg joint velocity */
virtual void GetLegJointVelocity(MAL_VECTOR( &dqr,double),
MAL_VECTOR( &dql,double)) const=0;
virtual void GetLegJointVelocity(Eigen::VectorXd &dqr,
Eigen::VectorXd &dql) const=0;
/*! \brief Read a sequence of steps. */
virtual void ReadSequenceOfSteps(std::istringstream &strm)=0;
......@@ -252,16 +252,16 @@ namespace PatternGeneratorJRL
double &omega) const=0;
/*! \brief An other method to get the waist position using a matrix. */
virtual void getWaistPositionMatrix(MAL_S4x4_MATRIX( &lWaistAbsPos,double)) const=0;
virtual void getWaistPositionMatrix(Eigen::Matrix4d &lWaistAbsPos) const=0;
/*!@} */
/*! \brief Set the initial ZMP reference point. */
virtual void setZMPInitialPoint(MAL_S3_VECTOR(&,double) lZMPInitialPoint)=0;
virtual void setZMPInitialPoint(Eigen::Vector3d & lZMPInitialPoint)=0;
/*! \brief Get the initial ZMP reference point. */
virtual void getZMPInitialPoint(MAL_S3_VECTOR(&,double) lZMPInitialPoint) const=0;
virtual void getZMPInitialPoint(Eigen::Vector3d & lZMPInitialPoint) const=0;
/*! \name System to call a given method based on registration of a method.
@{
......@@ -278,8 +278,8 @@ namespace PatternGeneratorJRL
/*! \brief Returns the ZMP, CoM, left foot absolute position, and right foot absolute position
for the initiale pose.*/
virtual void EvaluateStartingState(COMState & lStartingCOMState,
MAL_S3_VECTOR_TYPE(double) & lStartingZMPPosition,
MAL_VECTOR_TYPE(double) & lStartingWaistPose,
Eigen::Vector3d & lStartingZMPPosition,
Eigen::Matrix<double, 6, 1> & lStartingWaistPose,
FootAbsolutePosition & InitLeftFootAbsPos,
FootAbsolutePosition & InitRightFootAbsPos)=0;
......
......@@ -30,6 +30,7 @@ frame work */
#define PinocchioRobot_HH
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include <jrl/mal/matrixabstractlayer.hh>
......
......@@ -205,6 +205,8 @@ ENDIF ( '${CMAKE_EXTRA_GENERATOR}' STREQUAL 'CodeBlocks' )
ADD_LIBRARY(${PROJECT_NAME} SHARED ${SOURCES} ${${PROJECT_NAME}_ABSOLUTE_HEADERS})
TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${LAPACK_LIBRARIES})
# Define dependencies
SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES COMPILE_FLAGS "-msse -msse2 -msse3 -march=core2 -mfpmath=sse -fivopts -ftree-loop-im -fipa-pta ")
PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME} pinocchio)
......
......@@ -271,7 +271,7 @@ void
double LocalInterpolationStartTime = Time-(CurrentSupport.TimeLimit-(m_TDouble+m_TSingle));
int StepType = 1;
unsigned int CurrentIndex = FinalLeftFootTraj_deq.size()-1;
unsigned int CurrentIndex = (unsigned int)(FinalLeftFootTraj_deq.size()-1);
FootAbsolutePosition * LastSFP; //LastSwingFootPosition
......
......@@ -127,7 +127,7 @@ int CoMAndFootOnlyStrategy::OneGlobalStepOfControl(FootAbsolutePosition &LeftFoo
int CoMAndFootOnlyStrategy::EvaluateStartingState(Eigen::VectorXd & BodyAngles,
COMState & aStartingCOMState,
Eigen::Vector3d & aStartingZMPPosition,
Eigen::VectorXd & lStartingWaistPose,
Eigen::Matrix<double, 6,1> & lStartingWaistPose,
FootAbsolutePosition & InitLeftFootPosition,
FootAbsolutePosition & InitRightFootPosition)
{
......@@ -143,8 +143,9 @@ int CoMAndFootOnlyStrategy::EvaluateStartingState(Eigen::VectorXd & BodyAngles,
// here we use the analytical forward kinematics to initialise the position of the CoM of mass according to
// the articular position of the robot.
(*itCFR)->InitializationCoM(BodyAngles,lStartingCOMState,
lStartingWaistPose,
InitLeftFootPosition, InitRightFootPosition);
lStartingWaistPose,
InitLeftFootPosition,
InitRightFootPosition);
ODEBUG("EvaluateStartingCOM: m_StartingCOMState: " << lStartingCOMState);
aStartingCOMState.x[0] = lStartingCOMState(0);
......@@ -163,7 +164,7 @@ int CoMAndFootOnlyStrategy::EvaluateStartingState(Eigen::VectorXd & BodyAngles,
// The altitude of the zmp depend on the altitude of the support foot.
aStartingZMPPosition(2) = 0.5 * (InitLeftFootPosition.z + InitRightFootPosition.z) ;
// cerr << "YOU SHOULD INITIALIZE PROPERLY aStartingZMPosition in \
// cerr << "YOU SHOULD INITIALIZE PROPERLY aStartingZMPosition in \
// CoMAndFootOnlyStrategy::EvaluateStartingState" <<endl;
// cout << "com = " << aStartingCOMState << endl ;
......
......@@ -95,7 +95,7 @@ namespace PatternGeneratorJRL
int EvaluateStartingState(Eigen::VectorXd & BodyAngles,
COMState & aStartingCOMState,
Eigen::Vector3d & aStartingZMPPosition,
Eigen::VectorXd & aStartingWaistPose,
Eigen::Matrix<double,6,1> & aStartingWaistPose,
FootAbsolutePosition & InitLeftFootPosition,
FootAbsolutePosition & InitRightFootPosition);
......
......@@ -33,7 +33,6 @@ using namespace std;
#include <Debug.hh>
#include <GlobalStrategyManagers/DoubleStagePreviewControlStrategy.hh>
using namespace PatternGeneratorJRL;
DoubleStagePreviewControlStrategy::DoubleStagePreviewControlStrategy(SimplePluginManager * aSPM)
......@@ -190,12 +189,13 @@ int DoubleStagePreviewControlStrategy::OneGlobalStepOfControl(FootAbsolutePositi
return 0;
}
int DoubleStagePreviewControlStrategy::EvaluateStartingState(Eigen::VectorXd & BodyAngles,
COMState & aStartingCOMState,
Eigen::Vector3d & aStartingZMPPosition,
Eigen::VectorXd & aStartingWaistPose,
FootAbsolutePosition & InitLeftFootPosition,
FootAbsolutePosition & InitRightFootPosition)
int DoubleStagePreviewControlStrategy::
EvaluateStartingState(Eigen::VectorXd & BodyAngles,
COMState & aStartingCOMState,
Eigen::Vector3d & aStartingZMPPosition,
Eigen::Matrix<double, 6, 1> & aStartingWaistPose,
FootAbsolutePosition & InitLeftFootPosition,
FootAbsolutePosition & InitRightFootPosition)
{
Eigen::Vector3d lStartingCOMState;
lStartingCOMState(0) = aStartingCOMState.x[0];
......@@ -206,7 +206,8 @@ int DoubleStagePreviewControlStrategy::EvaluateStartingState(Eigen::VectorXd & B
lStartingCOMState,
aStartingZMPPosition,
aStartingWaistPose,
InitLeftFootPosition,InitRightFootPosition);
InitLeftFootPosition,
InitRightFootPosition);
aStartingCOMState.x[0] = lStartingCOMState(0);
aStartingCOMState.y[0] = lStartingCOMState(1);
......
......@@ -90,7 +90,7 @@ namespace PatternGeneratorJRL
int EvaluateStartingState(Eigen::VectorXd & BodyAngles,
COMState & aStartingCOMState,
Eigen::Vector3d & aStartingZMPPosition,
Eigen::VectorXd & aStartingWaistPose,
Eigen::Matrix<double, 6, 1> & aStartingWaistPose,
FootAbsolutePosition & InitLeftFootPosition,
FootAbsolutePosition & InitRightFootPosition);
......
......@@ -108,7 +108,7 @@ namespace PatternGeneratorJRL
virtual int EvaluateStartingState(Eigen::VectorXd & BodyAngles,
COMState & aStartingCOMState,
Eigen::Vector3d & aStartingZMPPosition,
Eigen::VectorXd & aStartingWaistPose,
Eigen::Matrix<double, 6, 1> & aStartingWaistPose,
FootAbsolutePosition & InitLeftFootPosition,
FootAbsolutePosition & InitRightFootPosition)=0;
......
......@@ -268,7 +268,7 @@ RelativeFeetInequalities::compute_linear_system ( convex_hull_t & ConvexHull,
const support_state_t & PrwSupport ) const
{
double dx,dy,dc,x1,y1,x2,y2;
unsigned nbRows = ConvexHull.X_vec.size();
unsigned int nbRows = (unsigned int)ConvexHull.X_vec.size();
double sign;
if( PrwSupport.Foot == LEFT )
......
......@@ -60,7 +60,7 @@ void CollisionDetector::SetObstacleCoordinates(ObstaclePar aObstacleInfo)
//double ObstacleWidth, ObstacleHeight, ObstacleDepth;
double c,s;
MAL_MATRIX_RESIZE(m_ObstaclePoints,3,4);
m_ObstaclePoints.resize(3,4);
m_ObstaclePosition(0) = aObstacleInfo.x;
m_ObstaclePosition(1) = aObstacleInfo.y;
......@@ -110,13 +110,13 @@ void CollisionDetector::SetObstacleCoordinates(ObstaclePar aObstacleInfo)
//cout << "I have set the obstacle info in the colission detection class" << endl;
}
void CollisionDetector::WorldFrameToObstacleFrame(MAL_S3_VECTOR(& WorldFrameCoord,double),
MAL_S3_VECTOR(&ObstacleFrameCoord,double))
void CollisionDetector::WorldFrameToObstacleFrame(Eigen::Vector3d &WorldFrameCoord,
Eigen::Vector3d &ObstacleFrameCoord)
{
// This function transforms the coordinates of a point
// expressed in the world frame to the local coordinates in the obstacle frame
ObstacleFrameCoord = MAL_S3x3_RET_A_by_B(m_ObstacleRotInv , (WorldFrameCoord - m_ObstaclePosition));
ObstacleFrameCoord = m_ObstacleRotInv * (WorldFrameCoord - m_ObstaclePosition);
/*
cout << "X WorldFrameCoord: " << WorldFrameCoord(0,0) << endl;
cout << "Y WorldFrameCoord: " << WorldFrameCoord(1,0) << endl;
......@@ -132,16 +132,16 @@ void CollisionDetector::WorldFrameToObstacleFrame(MAL_S3_VECTOR(& WorldFrameCoor
*/
}
void CollisionDetector::CalcCoordShankLowerLegPoint(MAL_S3_VECTOR( RelCoord,double),
MAL_S3_VECTOR( &AbsCoord,double),
MAL_VECTOR( LegAngles,double),
MAL_S3x3_MATRIX( WaistRot,double),
MAL_S3_VECTOR( WaistPos,double),
void CollisionDetector::CalcCoordShankLowerLegPoint(Eigen::Vector3d RelCoord,
Eigen::Vector3d &AbsCoord,
Eigen::VectorXd LegAngles,
Eigen::Matrix3d WaistRot,
Eigen::Vector3d WaistPos,
int WhichLeg)
{
MAL_S3x3_MATRIX(Rotation,double);
MAL_S3_VECTOR(TempCoord,double);
MAL_S3_VECTOR(Translation,double);
Eigen::Matrix3d Rotation;
Eigen::Vector3d TempCoord;
Eigen::Vector3d Translation;
double c,s;
......@@ -154,7 +154,7 @@ void CollisionDetector::CalcCoordShankLowerLegPoint(MAL_S3_VECTOR( RelCoord,doub
Rotation(1,0) = 0; Rotation(1,1) = 1; Rotation(1,2) = 0;
Rotation(2,0) =-s; Rotation(2,1) = 0; Rotation(2,2) = c;
TempCoord = MAL_S3x3_RET_A_by_B(Rotation , RelCoord);
TempCoord = Rotation*RelCoord;
//translation from knee to hip along Z axis
......@@ -175,7 +175,7 @@ void CollisionDetector::CalcCoordShankLowerLegPoint(MAL_S3_VECTOR( RelCoord,doub
Rotation(1,0) = 0; Rotation(1,1) = 1; Rotation(1,2) = 0;
Rotation(2,0) =-s; Rotation(2,1) = 0; Rotation(2,2) = c;
TempCoord = MAL_S3x3_RET_A_by_B(Rotation , TempCoord);
TempCoord = Rotation*TempCoord;
//rotation from hip to hip angle 1 frame 1 -> 0
......@@ -186,7 +186,7 @@ void CollisionDetector::CalcCoordShankLowerLegPoint(MAL_S3_VECTOR( RelCoord,doub
Rotation(1,0) = 0; Rotation(1,1) = c; Rotation(1,2) =-s;
Rotation(2,0) = 0; Rotation(2,1) = s; Rotation(2,2) = c;
TempCoord = MAL_S3x3_RET_A_by_B(Rotation , TempCoord);
TempCoord = Rotation*TempCoord;
//rotation from hip to hip angle 0 frame 0 -> waistframe orientation
......@@ -198,7 +198,7 @@ void CollisionDetector::CalcCoordShankLowerLegPoint(MAL_S3_VECTOR( RelCoord,doub
Rotation(1,0) = s; Rotation(1,1) = c; Rotation(1,2) = 0;
Rotation(2,0) = 0; Rotation(2,1) = 0; Rotation(2,2) = 1;
TempCoord = MAL_S3x3_RET_A_by_B(Rotation , TempCoord);
TempCoord = Rotation*TempCoord;
//translation from hip to waist along x axis
......@@ -218,7 +218,7 @@ void CollisionDetector::CalcCoordShankLowerLegPoint(MAL_S3_VECTOR( RelCoord,doub
//translation and rotation of the waist in the world frame
TempCoord = MAL_S3x3_RET_A_by_B(WaistRot, TempCoord);
TempCoord = WaistRot*TempCoord;
/*
cout << "X TempCoord coordinate: " << TempCoord(0,0) << endl;
cout << "Y TempCoord coordinate: " << TempCoord(1,0) << endl;
......@@ -262,8 +262,8 @@ bool CollisionDetector::CollisionTwoLines(vector<double> p1,
}
bool CollisionDetector::CollisionLineObstaclePlane(MAL_S3_VECTOR(&LegPoint1,double),
MAL_S3_VECTOR(&LegPoint2,double),
bool CollisionDetector::CollisionLineObstaclePlane(Eigen::Vector3d &LegPoint1,
Eigen::Vector3d &LegPoint2,
int PlaneNumber)
{
/*! This function checks for intersection of a linesegment p1p2
......@@ -367,8 +367,8 @@ bool CollisionDetector::CollisionLineObstaclePlane(MAL_S3_VECTOR(&LegPoint1,doub
}
bool CollisionDetector::CollisionLineObstacleComplete(MAL_S3_VECTOR(&Point1,double),
MAL_S3_VECTOR(&Point2,double))
bool CollisionDetector::CollisionLineObstacleComplete(Eigen::Vector3d &Point1,
Eigen::Vector3d &Point2)
{
if ((((Point1(0)>0.0)
......
......@@ -38,6 +38,7 @@
#include <vector>
#include <string>
#include <Eigen/Dense>
namespace PatternGeneratorJRL
{
......
......@@ -113,7 +113,7 @@ namespace PatternGeneratorJRL
@param IterationNumber: Number of iteration.
@param Stage: indicates which stage is reach by the Pattern Generator.
*/
virtual bool ComputePostureForGivenCoMAndFeetPosture(Eigen::VectorXd &CoMPosition,
virtual bool ComputePostureForGivenCoMAndFeetPosture(Eigen::VectorXd &COMPOSITION,
Eigen::VectorXd &CoMSpeed,
Eigen::VectorXd &CoMAcc,
Eigen::VectorXd &LeftFoot,
......@@ -153,7 +153,7 @@ namespace PatternGeneratorJRL
*/
virtual bool InitializationCoM(Eigen::VectorXd &BodyAnglesIni,
Eigen::Vector3d & lStartingCOMPosition,
Eigen::VectorXd & lStartingWaistPose,
Eigen::Matrix<double,6,1> & lStartingWaistPose,
FootAbsolutePosition & InitLeftFootAbsPos,
FootAbsolutePosition & InitRightFootAbsPos)=0;
......
......@@ -93,8 +93,8 @@ namespace PatternGeneratorJRL
*/
bool ComputePostureForGivenCoMAndFeetPosture(Eigen::VectorXd &CoMPosition,
Eigen::VectorXd & aCoMSpeed,
Eigen::VectorXd & aCoMAcc,
Eigen::VectorXd &aCoMSpeed,
Eigen::VectorXd &aCoMAcc,
Eigen::VectorXd &LeftFoot,
Eigen::VectorXd &RightFoot,
Eigen::VectorXd & CurrentConfiguration,
......@@ -114,7 +114,7 @@ namespace PatternGeneratorJRL
\param[out] lStartingWaistPose: The waist pose according to the user configuration vector.
*/
bool InitializationHumanoid(Eigen::VectorXd &BodyAnglesIni,
Eigen::VectorXd &lStartingWaistPose);
Eigen::Matrix<double, 6, 1> &lStartingWaistPose);
/*! \brief Initialize the foot position.
\param[in] aFoot: Pointer to the foot to be updated.
......@@ -137,7 +137,7 @@ namespace PatternGeneratorJRL
*/
bool InitializationCoM(Eigen::VectorXd &BodyAnglesIni,
Eigen::Vector3d & lStartingCOMPosition,
Eigen::VectorXd & lStartingWaistPosition,
Eigen::Matrix<double,6,1> & lStartingWaistPosition,
FootAbsolutePosition & InitLeftFootAbsPos,
FootAbsolutePosition & InitRightFootAbsPos);
......@@ -174,7 +174,7 @@ namespace PatternGeneratorJRL
int EvaluateStartingCoM(Eigen::VectorXd &BodyAngles,
Eigen::Vector3d &aStartingCOMPosition,
Eigen::VectorXd &aWaistPose,
Eigen::Matrix<double, 6, 1> &aWaistPose,
FootAbsolutePosition & InitLeftFootPosition,
FootAbsolutePosition & InitRightFootPosition);
......
......@@ -31,7 +31,7 @@
#include <iostream>
#include <fstream>
#include <jrl/mal/matrixabstractlayer.hh>
#include <PreviewControl/PreviewControl.hh>
#include <ZMPRefTrajectoryGeneration/ZMPDiscretization.hh>
......@@ -194,8 +194,8 @@ namespace PatternGeneratorJRL
void GenerateMotionFromKineoWorks::CreateBufferFirstPreview(deque<ZMPPosition> &ZMPRefBuffer)
{
deque<ZMPPosition> aFIFOZMPRefPositions;
MAL_MATRIX(aPC1x,double);
MAL_MATRIX(aPC1y,double);
Eigen::MatrixXd aPC1x;
Eigen::MatrixXd aPC1y;
double aSxzmp, aSyzmp;
double aZmpx2, aZmpy2;
......@@ -209,7 +209,7 @@ namespace PatternGeneratorJRL
aSxzmp = 0.0;//m_sxzmp;
aSyzmp = 0.0;//m_syzmp;
MAL_MATRIX_RESIZE(aPC1x,3,1); MAL_MATRIX_RESIZE(aPC1y,3,1);
aPC1x.resize(3,1); aPC1y.resize(3,1);
aPC1x(0,0)= 0; aPC1x(1,0)= 0; aPC1x(2,0)= 0;
aPC1y(0,0)= 0; aPC1y(1,0)= 0; aPC1y(2,0)= 0;
......
......@@ -45,7 +45,7 @@ StepOverPlanner::StepOverPlanner(ObstaclePar &ObstacleParameters,
m_PR = aPR;
// Get information specific to the humanoid.
double lWidth,lHeight;
vector3d AnklePosition;
Eigen::Vector3d AnklePosition;
if (m_PR!=0)
{
......@@ -141,7 +141,7 @@ StepOverPlanner::StepOverPlanner(ObstaclePar &ObstacleParameters,
Angle1=60.0/180.0*M_PI;
Angle2=30.0/180.0*M_PI;
MAL_MATRIX_RESIZE(m_LegLayoutPoint,3,7);
m_LegLayoutPoint.resize(3,7);
//point1
m_LegLayoutPoint(0,0)= RadiusKnee*cos(Angle1);
......@@ -362,34 +362,34 @@ void StepOverPlanner::DoubleSupportFeasibility()
int EvaluationNumber = 10;
double IncrementStepLenght, IncrementCOMHeight;
MAL_S3x3_MATRIX(Body_R,double);
MAL_S3_VECTOR(Body_P,double);
MAL_S3x3_MATRIX(Foot_R,double);
MAL_S3_VECTOR(Foot_P,double);
Eigen::Matrix3d Body_R;
Eigen::Vector3d Body_P;
Eigen::Matrix3d Foot_R;
Eigen::Vector3d Foot_P;
double c,s,co,so;
MAL_S3_VECTOR(ToTheHip,double);
MAL_VECTOR_DIM(LeftLegAngles,double,6);
MAL_VECTOR_DIM(RightLegAngles,double,6);
Eigen::Vector3d ToTheHip;
Eigen::Matrix<double,6,1> LeftLegAngles;
Eigen::Matrix<double,6,1> RightLegAngles;
MAL_S3_VECTOR(AnkleBeforeObst,double);
MAL_S3_VECTOR(AnkleAfterObst,double);
MAL_S3_VECTOR(TempCOMState,double);
MAL_MATRIX_DIM(Temp,double,3,3);
Eigen::Vector3d AnkleBeforeObst;
Eigen::Vector3d AnkleAfterObst;
Eigen::Vector3d TempCOMState;
Eigen::Matrix<double,3,3> Temp;;
COMState aCOMState;
MAL_S3_VECTOR(PointOnLeg,double);
MAL_S3_VECTOR(AbsCoord,double);
MAL_S3_VECTOR(AbsCoord1,double);
MAL_S3_VECTOR(AbsCoord2,double);
MAL_MATRIX_DIM(LegAngles,double,6,1);
MAL_S3x3_MATRIX(WaistRot,double);
MAL_S3_VECTOR(WaistPos,double);
MAL_S3_VECTOR(ObstFrameCoord,double);
MAL_S3_VECTOR(ObstFrameCoord1,double);
MAL_S3_VECTOR(ObstFrameCoord2,double);
Eigen::Vector3d PointOnLeg;
Eigen::Vector3d AbsCoord;
Eigen::Vector3d AbsCoord1;
Eigen::Vector3d AbsCoord2;
Eigen::Matrix<double,6,1> LegAngles;;
Eigen::Matrix3d WaistRot;
Eigen::Vector3d WaistPos;
Eigen::Vector3d ObstFrameCoord;
Eigen::Vector3d ObstFrameCoord1;
Eigen::Vector3d ObstFrameCoord2;
bool CollisionStatus, FinalCollisionStatus;
......@@ -445,7 +445,7 @@ void StepOverPlanner::DoubleSupportFeasibility()
//position left foot in front of the obstacle to world frame coordinates
Foot_P = m_ObstaclePosition + MAL_S3x3_RET_A_by_B(m_ObstacleRot, AnkleBeforeObst);
Foot_P = m_ObstaclePosition + m_ObstacleRot*AnkleBeforeObst;
TempCOMState(0) = AnkleBeforeObst(0)+ DoubleSupportCOMPosFactor * StepOverStepLenght;
......@@ -453,7 +453,7 @@ void StepOverPlanner::DoubleSupportFeasibility()
TempCOMState(2) = StepOverCOMHeight;
//to worldframe
TempCOMState = m_ObstaclePosition + MAL_S3x3_RET_A_by_B(m_ObstacleRot , TempCOMState);
TempCOMState = m_ObstaclePosition + m_ObstacleRot*TempCOMState;
aCOMState.x[0] = TempCOMState(0);
aCOMState.y[0] = TempCOMState(1);
......@@ -471,7 +471,7 @@ void StepOverPlanner::DoubleSupportFeasibility()
Body_R(2,0) = 0; Body_R(2,1) = 0; Body_R(2,2) = 1;
// COM position
MAL_S3x3_C_eq_A_by_B(ToTheHip,Body_R, m_StaticToTheLeftHip);
ToTheHip=Body_R*m_StaticToTheLeftHip;
Body_P(0) = aCOMState.x[0] + ToTheHip(0) ;
Body_P(1) = aCOMState.y[0] + ToTheHip(1);
Body_P(2) = aCOMState.z[0] + ToTheHip(2);
......@@ -518,11 +518,11 @@ void StepOverPlanner::DoubleSupportFeasibility()
Foot_R(2,0) = -so; Foot_R(2,1) = 0; Foot_R(2,2) = co;
// position
Foot_P = m_ObstaclePosition + MAL_S3x3_RET_A_by_B(m_ObstacleRot, AnkleAfterObst);
Foot_P = m_ObstaclePosition + m_ObstacleRot*AnkleAfterObst;
// COM position
MAL_S3x3_C_eq_A_by_B(ToTheHip,Body_R, m_StaticToTheRightHip);
ToTheHip=Body_R*m_StaticToTheRightHip;
Body_P(0) = aCOMState.x[0] + ToTheHip(0) ;
Body_P(1) = aCOMState.y[0] + ToTheHip(1);
Body_P(2) = aCOMState.z[0] + ToTheHip(2);
......@@ -755,10 +755,10 @@ void StepOverPlanner::PolyPlanner(deque<COMState> &aCOMBuffer,
void StepOverPlanner::PolyPlannerFirstStep(deque<FootAbsolutePosition> &aStepOverFootBuffer)
{
MAL_VECTOR_DIM(aBoundCondZ,double,8);