From 5648de6e50b38ec44f782f698ee53f73cb1e5f7b Mon Sep 17 00:00:00 2001
From: mnaveau <maximilien.naveau@laas.fr>
Date: Mon, 31 Oct 2016 11:52:43 +0100
Subject: [PATCH] work on stair trajectory
---
.../FootTrajectoryGenerationStandard.cpp | 4 ++--
.../AnalyticalMorisawaCompact.cpp | 14 +++++++-------
2 files changed, 9 insertions(+), 9 deletions(-)
diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
index 8e0de11f..54a39deb 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
@@ -407,7 +407,7 @@ int FootTrajectoryGenerationStandard::SetParameters(int PolynomeIndex, double Ti
else if (FinalPosition - InitPosition > epsilon )
{
ToMP.push_back(0.4*TimeInterval);
- MP.push_back(FinalPosition+WayPoint_z);
+ MP.push_back(FinalPosition+0.5*WayPoint_z);
}
else if ( sqrt((FinalPosition - InitPosition)*(FinalPosition - InitPosition)) <= epsilon )
{
@@ -417,7 +417,7 @@ int FootTrajectoryGenerationStandard::SetParameters(int PolynomeIndex, double Ti
else if (FinalPosition - InitPosition < -epsilon )
{
ToMP.push_back(0.6*TimeInterval);
- MP.push_back(InitPosition+WayPoint_z);
+ MP.push_back(InitPosition+0.5*WayPoint_z);
}
m_BsplinesZ->SetParameters(TimeInterval,InitPosition,FinalPosition,ToMP,MP,InitSpeed,InitAcc);
break;
diff --git a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
index db2e5468..d8477e58 100644
--- a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
+++ b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
@@ -547,12 +547,12 @@ computing the analytical trajectories. */
MAL_VECTOR_DIM(CurPosWICF_homogeneous,double,4) ;
m_kajitaDynamicFilter->getComAndFootRealization()->GetCurrentPositionofWaistInCOMFrame(CurPosWICF_homogeneous);
- InitLeftFootAbsolutePosition.x += lAnklePositionLeft(0) ;
- InitLeftFootAbsolutePosition.y += lAnklePositionLeft(1) ;
- InitLeftFootAbsolutePosition.z += lAnklePositionLeft(2) ;
- InitRightFootAbsolutePosition.x += lAnklePositionRight(0) ;
- InitRightFootAbsolutePosition.y += lAnklePositionRight(1) ;
- InitRightFootAbsolutePosition.z += lAnklePositionRight(2) ;
+// InitLeftFootAbsolutePosition.x += lAnklePositionLeft(0) ;
+// InitLeftFootAbsolutePosition.y += lAnklePositionLeft(1) ;
+// InitLeftFootAbsolutePosition.z += lAnklePositionLeft(2) ;
+// InitRightFootAbsolutePosition.x += lAnklePositionRight(0) ;
+// InitRightFootAbsolutePosition.y += lAnklePositionRight(1) ;
+// InitRightFootAbsolutePosition.z += lAnklePositionRight(2) ;
m_RelativeFootPositions = RelativeFootPositions;
/* This part computes the CoM and ZMP trajectory giving the foot position information.
@@ -2886,7 +2886,7 @@ new step has to be generate.
LTHROW("No foot");
vector3d corrZ ;
corrZ = aFoot->anklePosition ;
-
+ corrZ(2) = 0.0 ;
bool sinple_support = (IndexInterval % 2) == 0 ;
double moving_time = m_RelativeFootPositions[0].SStime + m_RelativeFootPositions[0].DStime;
unsigned int stepNumber = int(t/moving_time) ;
--
GitLab