Commit 87972e93 authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Change related to abstract-robot-dynamics.

parent 39c7820a
......@@ -35,7 +35,7 @@
#define _PATTERN_GENERATOR_INTERFACE_H_
#include <deque>
#include <robotDynamics/jrlHumanoidDynamicRobot.h>
#include <abstract-robot-dynamics/jrlHumanoidDynamicRobot.h>
#include <jrl/walkgen/pgtypes.hh>
namespace PatternGeneratorJRL
......
......@@ -39,7 +39,7 @@
#include <jrl/mal/matrixabstractlayer.hh>
/* dynamics JRL Japan related inclusions */
#include <robotDynamics/jrlFoot.h>
#include <abstract-robot-dynamics/jrlFoot.h>
/* Walking pattern generation related inclusions */
......
......@@ -33,7 +33,7 @@
#include <jrl/mal/matrixabstractlayer.hh>
/* abstractRobotDynamics inclusion */
#include <robotDynamics/jrlFoot.h>
#include <abstract-robot-dynamics/jrlFoot.h>
/* Walking Pattern Generator inclusion */
......
......@@ -33,7 +33,7 @@
#include <jrl/mal/matrixabstractlayer.hh>
// Dynamics
#include <robotDynamics/jrlHumanoidDynamicRobot.h>
#include <abstract-robot-dynamics/jrlHumanoidDynamicRobot.h>
// PG
......
......@@ -38,7 +38,7 @@
#include <jrl/mal/matrixabstractlayer.hh>
#include <robotDynamics/jrlHumanoidDynamicRobot.h>
#include <abstract-robot-dynamics/jrlHumanoidDynamicRobot.h>
#include <jrl/walkgen/pgtypes.hh>
......
......@@ -39,7 +39,7 @@
#include <jrl/mal/matrixabstractlayer.hh>
#include <robotDynamics/jrlHumanoidDynamicRobot.h>
#include <abstract-robot-dynamics/jrlHumanoidDynamicRobot.h>
#include <jrl/walkgen/pgtypes.hh>
......
......@@ -29,7 +29,7 @@
#ifndef _COM_AND_FOOT_REALIZATION_H_
#define _COM_AND_FOOT_REALIZATION_H_
#include <robotDynamics/jrlHumanoidDynamicRobot.h>
#include <abstract-robot-dynamics/jrlHumanoidDynamicRobot.h>
#include <SimplePlugin.h>
#include <StepStackHandler.h>
......
......@@ -170,23 +170,16 @@ Initialization()
RightFoot->getAnklePositionInLocalFrame(lAnklePositionRight);
LeftFoot->getAnklePositionInLocalFrame(lAnklePositionLeft);
vector3d RightFootSoleCenter,LeftFootSoleCenter;
RightFoot->getProjectionCenterLocalFrameInSole(RightFootSoleCenter);
LeftFoot->getProjectionCenterLocalFrameInSole(LeftFootSoleCenter);
double lWidth,lHeight,lDepth;
lDepth = lAnklePositionRight[2];
LeftFoot->getSoleSize(lWidth,lHeight);
ODEBUG4("RightFootSoleCenter:"<<RightFootSoleCenter, "DebugDataStartingCOM.dat");
ODEBUG4("LeftFootSoleCenter:"<<LeftFootSoleCenter, "DebugDataStartingCOM.dat");
m_AnklePositionRight[0] = lAnklePositionRight[0] - RightFootSoleCenter[0];
m_AnklePositionLeft[0] = lAnklePositionLeft[0] - LeftFootSoleCenter[0];
m_AnklePositionRight[1] = lAnklePositionRight[1] - RightFootSoleCenter[1];
m_AnklePositionLeft[1] = lAnklePositionLeft[1] - LeftFootSoleCenter[1];
m_AnklePositionRight[2] = lAnklePositionRight[2] - RightFootSoleCenter[2];
m_AnklePositionLeft[2] = lAnklePositionRight[2] - LeftFootSoleCenter[2];
m_AnklePositionRight[0] = lAnklePositionRight[0];
m_AnklePositionLeft[0] = lAnklePositionLeft[0];
m_AnklePositionRight[1] = lAnklePositionRight[1];
m_AnklePositionLeft[1] = lAnklePositionLeft[1];
m_AnklePositionRight[2] = lAnklePositionRight[2];
m_AnklePositionLeft[2] = lAnklePositionRight[2];
ODEBUG4("m_AnklePositionRight: "<< m_AnklePositionRight[0] << " " <<
m_AnklePositionRight[1] << " " <<
......@@ -434,8 +427,6 @@ InitializationCoM(MAL_VECTOR(,double) &BodyAnglesIni,
// Initialise the right foot position.
CjrlFoot * RightFoot = aDMB->rightFoot();
matrix4d lFootPose = RightFoot->associatedAnkle()->currentTransformation();
vector3d RightFootSoleCenter;
RightFoot->getProjectionCenterLocalFrameInSole(RightFootSoleCenter);
MAL_S3_VECTOR_ACCESS(m_COGInitialAnkles,0) = MAL_S4x4_MATRIX_ACCESS_I_J(lFootPose,0,3);
MAL_S3_VECTOR_ACCESS(m_COGInitialAnkles,1) = MAL_S4x4_MATRIX_ACCESS_I_J(lFootPose,1,3);
......@@ -465,7 +456,7 @@ InitializationCoM(MAL_VECTOR(,double) &BodyAnglesIni,
WaistPosition[0] = CurrentConfig(0); // 0.0
WaistPosition[1] = CurrentConfig(1); // 0.0
WaistPosition[2] = CurrentConfig(2) -lFootPosition[2]-RightFootSoleCenter[2];
WaistPosition[2] = CurrentConfig(2) -lFootPosition[2];
ODEBUG("WaistPosition: " << WaistPosition);
InitRightFootPosition.x = lFootPosition[0];
......@@ -551,7 +542,7 @@ InitializationCoM(MAL_VECTOR(,double) &BodyAnglesIni,
ODEBUG( lStartingCOMPosition[0] << " "
<< lStartingCOMPosition[1] << " "
<< lStartingCOMPosition[2]);
lStartingCOMPosition[2] += -lFootPosition[2]-RightFootSoleCenter[2];
lStartingCOMPosition[2] += -lFootPosition[2];
// Vector to go from CoM to Waist.
m_DiffBetweenComAndWaist[0] = WaistPosition[0] - lStartingCOMPosition[0];
m_DiffBetweenComAndWaist[1] = WaistPosition[1] - lStartingCOMPosition[1];
......
......@@ -44,7 +44,7 @@
#include <jrl/mal/matrixabstractlayer.hh>
/*! Abstract Interface for dynamic robot. */
#include <robotDynamics/jrlHumanoidDynamicRobot.h>
#include <abstract-robot-dynamics/jrlHumanoidDynamicRobot.h>
/*! Humanoid Walking Pattern Generator */
......
......@@ -171,7 +171,7 @@ int LinearizedInvertedPendulum2D::Interpolation(deque<COMState> &COMStates,
int lCurrentPosition = CurrentPosition;
// Fill the queues with the interpolated CoM values.
for(int lk=0;lk<m_InterpolationInterval;lk++,lCurrentPosition++)
for(int lk=0;lk<=m_InterpolationInterval;lk++,lCurrentPosition++)
{
ODEBUG("lCurrentPosition: "<< lCurrentPosition);
COMState & aCOMPos = COMStates[lCurrentPosition];
......
......@@ -41,7 +41,7 @@
/*! Framework includes */
#include <jrl/walkgen/pgtypes.hh>
#include <robotDynamics/jrlJoint.h>
#include <abstract-robot-dynamics/jrlJoint.h>
namespace PatternGeneratorJRL
{
......
......@@ -45,7 +45,7 @@
using namespace::std;
/*! Abstract robot dynamics includes */
#include <robotDynamics/jrlHumanoidDynamicRobot.h>
#include <abstract-robot-dynamics/jrlHumanoidDynamicRobot.h>
/*! Framework includes */
#include <Mathematics/PolynomeFoot.h>
......
......@@ -1313,6 +1313,11 @@ void ZMPVelocityReferencedQP::CallMethod(std::string & Method, std::istringstrea
{
strm >> m_SupportFSM->m_NbOfStepsSSDS;
}
if (Method==":comheight")
{
strm >> m_ComHeight;
m_2DLIPM->SetComHeight(m_ComHeight);
}
ZMPRefTrajectoryGeneration::CallMethod(Method,strm);
}
......@@ -1380,7 +1385,7 @@ int ZMPVelocityReferencedQP::InitOnLine(deque<ZMPPosition> & FinalZMPPositions,
{
// Smooth ramp
FinalZMPPositions[CurrentZMPindex].px =lStartingZMPPosition(0);
FinalZMPPositions[CurrentZMPindex].px = lStartingZMPPosition(0);
FinalZMPPositions[CurrentZMPindex].py = lStartingZMPPosition(1);
FinalZMPPositions[CurrentZMPindex].pz = lStartingZMPPosition(2);
FinalZMPPositions[CurrentZMPindex].theta = 0.0;
......@@ -1389,7 +1394,6 @@ int ZMPVelocityReferencedQP::InitOnLine(deque<ZMPPosition> & FinalZMPPositions,
// Set CoM positions.
FinalCoMPositions[CurrentZMPindex] = lStartingCOMState;
// Set Left Foot positions.
FinalLeftFootAbsolutePositions[CurrentZMPindex] = CurrentLeftFootAbsPos;
FinalRightFootAbsolutePositions[CurrentZMPindex] = CurrentRightFootAbsPos;
......
Supports Markdown
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