From 06d42530e2eb3811213038a009f229b31e7f0168 Mon Sep 17 00:00:00 2001
From: Andrei <andrei@checkmoruk.(none)>
Date: Mon, 11 Jul 2011 18:24:19 +0200
Subject: [PATCH] Add computation of second derivative
---
.../FootTrajectoryGenerationStandard.cpp | 38 +++++++++++++++++++
.../FootTrajectoryGenerationStandard.h | 5 ++-
2 files changed, 42 insertions(+), 1 deletion(-)
diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
index 23219c1d..f6e43424 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
@@ -325,6 +325,44 @@ double FootTrajectoryGenerationStandard::Compute(unsigned int PolynomeIndex, dou
return r;
}
+double FootTrajectoryGenerationStandard::ComputeSecDerivative(unsigned int PolynomeIndex, double Time)
+{
+ double r=0.0;
+
+ switch (PolynomeIndex)
+ {
+
+ case X_AXIS:
+ r=m_PolynomeX->ComputeSecDerivative(Time);
+ break;
+
+ case Y_AXIS:
+ r=m_PolynomeY->ComputeSecDerivative(Time);
+ break;
+
+ case Z_AXIS:
+ r=m_PolynomeZ->ComputeSecDerivative(Time);
+ break;
+
+ case THETA_AXIS:
+ r=m_PolynomeTheta->ComputeSecDerivative(Time);
+ break;
+
+ case OMEGA_AXIS:
+ r=m_PolynomeOmega->ComputeSecDerivative(Time);
+ break;
+
+ case OMEGA2_AXIS:
+ r=m_PolynomeOmega2->ComputeSecDerivative(Time);
+ break;
+
+ default:
+ return -1.0;
+ break;
+ }
+ return r;
+}
+
void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosition> &SupportFootAbsolutePositions,
deque<FootAbsolutePosition> &NoneSupportFootAbsolutePositions,
int CurrentAbsoluteIndex,
diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.h b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.h
index 36b46acc..2a895f4c 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.h
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationStandard.h
@@ -52,7 +52,7 @@ namespace PatternGeneratorJRL
@{ */
/*! \brief along the frontal direction */
- const static unsigned int X_AXIS =0;
+ const static unsigned int X_AXIS = 0;
/*! \brief along the left of the robot */
const static unsigned int Y_AXIS = 1;
/*! \brief along the vertical axis of the robot. */
@@ -155,6 +155,9 @@ namespace PatternGeneratorJRL
/*! Compute the value for a given polynome. */
double Compute(unsigned int PolynomeIndex, double Time);
+ /*! Compute the value for a given polynome's second derivative. */
+ double ComputeSecDerivative(unsigned int PolynomeIndex, double Time);
+
/*! Compute the absolute foot position from the queue of relative positions.
There is not direct dependency with time.
*/
--
GitLab