From 6bde056541beaf0b66d532112221103ec2647920 Mon Sep 17 00:00:00 2001
From: mnaveau <maximilien.naveau@laas.fr>
Date: Thu, 10 Jul 2014 11:32:16 +0200
Subject: [PATCH] adding the upper part posture
---
.../AnalyticalMorisawaCompact.cpp | 23 ++++++++++++++++
.../DynamicFilter.cpp | 2 +-
tests/TestMorisawa2007.cpp | 27 ++++++++++++++++++-
3 files changed, 50 insertions(+), 2 deletions(-)
diff --git a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
index 422d4835..25f7a5a7 100644
--- a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
+++ b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
@@ -584,6 +584,29 @@ computing the analytical trajectories. */
lStartingCOMState.z[0],
InitLeftFootAbsolutePosition,
lStartingCOMState );
+ MAL_VECTOR_TYPE(double) UpperConfig = m_kajitaDynamicFilter->getComAndFootRealization()
+ ->getHumanoidDynamicRobot()->currentConfiguration() ;
+ UpperConfig(18)= 0.0 ; // CHEST_JOINT0
+ UpperConfig(19)= 0.015 ; // CHEST_JOINT1
+ UpperConfig(20)= 0.0 ; // HEAD_JOINT0
+ UpperConfig(21)= 0.0 ; // HEAD_JOINT1
+ UpperConfig(22)= -0.108210414 ; // RARM_JOINT0
+ UpperConfig(23)= 0.0383972435 ; // RARM_JOINT1
+ UpperConfig(24)= 0.474729557 ; // RARM_JOINT2
+ UpperConfig(25)= -1.41720735 ; // RARM_JOINT3
+ UpperConfig(26)= 1.45385927 ; // RARM_JOINT4
+ UpperConfig(27)= 0.509636142 ; // RARM_JOINT5
+ UpperConfig(28)= 0.034906585 ; // RARM_JOINT6
+ UpperConfig(29)= -0.200712864 ; // LARM_JOINT0
+ UpperConfig(30)= -0.129154365 ; // LARM_JOINT1
+ UpperConfig(31)= -0.333357887 ; // LARM_JOINT2
+ UpperConfig(32)= -1.39277274 ; // LARM_JOINT3
+ UpperConfig(33)= 1.18856922 ; // LARM_JOINT4
+ UpperConfig(34)= -0.193731547 ; // LARM_JOINT5
+ UpperConfig(35)= 0.034906585 ; // LARM_JOINT6
+
+
+ m_kajitaDynamicFilter->setRobotUpperPart(UpperConfig);
/*! Add KajitaPCpreviewWindow second to the buffers for fitering */
ZMPPosition lastZMP = ZMPPositions.back();
diff --git a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
index 6251d93c..69366fa5 100644
--- a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
+++ b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
@@ -184,7 +184,7 @@ void DynamicFilter::init(
deltay_(j,0) = 0 ;
}
- upperPartIndex.resize(2+2+6+6);
+ upperPartIndex.resize(2+2+7+7);
for (unsigned int i = 0 ; i < upperPartIndex.size() ; ++i )
{
upperPartIndex[i]=i+18;
diff --git a/tests/TestMorisawa2007.cpp b/tests/TestMorisawa2007.cpp
index 04b49797..4b4964cf 100644
--- a/tests/TestMorisawa2007.cpp
+++ b/tests/TestMorisawa2007.cpp
@@ -171,6 +171,14 @@ public:
/*! Fill the debug files with appropriate information. */
//ComparingZMPs();
+ dynamicfilter_->InverseKinematics(
+ m_OneStep.finalCOMPosition,
+ m_OneStep.LeftFootPosition,
+ m_OneStep.RightFootPosition,
+ m_CurrentConfiguration,
+ m_CurrentVelocity,
+ m_CurrentAcceleration,
+ 0.005,0,m_OneStep.NbOfIt);
fillInDebugFiles();
}
else
@@ -342,7 +350,24 @@ protected:
aof.close();
}
-
+ m_CurrentConfiguration(18)= 0.0 ; // CHEST_JOINT0
+ m_CurrentConfiguration(19)= 0.0 ; // CHEST_JOINT1
+ m_CurrentConfiguration(20)= 0.0 ; // HEAD_JOINT0
+ m_CurrentConfiguration(21)= 0.0 ; // HEAD_JOINT1
+ m_CurrentConfiguration(22)= -0.108210414 ; // RARM_JOINT0
+ m_CurrentConfiguration(23)= 0.0383972435 ; // RARM_JOINT1
+ m_CurrentConfiguration(24)= 0.474729557 ; // RARM_JOINT2
+ m_CurrentConfiguration(25)= -1.41720735 ; // RARM_JOINT3
+ m_CurrentConfiguration(26)= 1.45385927 ; // RARM_JOINT4
+ m_CurrentConfiguration(27)= 0.509636142 ; // RARM_JOINT5
+ m_CurrentConfiguration(28)= 0.034906585 ; // RARM_JOINT6
+ m_CurrentConfiguration(29)= -0.200712864 ; // LARM_JOINT0
+ m_CurrentConfiguration(30)= -0.129154365 ; // LARM_JOINT1
+ m_CurrentConfiguration(31)= -0.333357887 ; // LARM_JOINT2
+ m_CurrentConfiguration(32)= -1.39277274 ; // LARM_JOINT3
+ m_CurrentConfiguration(33)= 1.18856922 ; // LARM_JOINT4
+ m_CurrentConfiguration(34)= -0.193731547 ; // LARM_JOINT5
+ m_CurrentConfiguration(35)= 0.034906585 ; // LARM_JOINT6
/// \brief Create file .hip .pos .zmp
/// --------------------
ofstream aof;
--
GitLab