From 084475a90d159d53b9b333d805adaa21850835eb Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A1ndez?= <shernand@iri.upc.edu>
Date: Tue, 5 Apr 2016 21:52:14 +0000
Subject: [PATCH] Removed the pitch offset from the kinematics computation.
---
include/darwin_kinematics.h | 1 -
src/walking.c | 28 ++++++++++++++--------------
2 files changed, 14 insertions(+), 15 deletions(-)
diff --git a/include/darwin_kinematics.h b/include/darwin_kinematics.h
index 7f67ffa..783781e 100755
--- a/include/darwin_kinematics.h
+++ b/include/darwin_kinematics.h
@@ -11,7 +11,6 @@ extern "C" {
#define DARWIN_LEG_SIDE_OFFSET 37.0 //mm
#define DARWIN_THIGH_LENGTH 93.0 //mm
#define DARWIN_CALF_LENGTH 93.0 //mm
-#define DARWIN_PITCH_OFFSET 0.1910 //rad
#define DARWIN_ANKLE_LENGTH 33.5 //mm
#define DARWIN_LEG_LENGTH (DARWIN_THIGH_LENGTH + DARWIN_CALF_LENGTH + DARWIN_ANKLE_LENGTH) //mm (THIGH_LENGTH + CALF_LENGTH + ANKLE_LENGTH)
diff --git a/src/walking.c b/src/walking.c
index 192efbd..1bfbd1e 100755
--- a/src/walking.c
+++ b/src/walking.c
@@ -445,33 +445,33 @@ void walking_process(void)
// Compute motor value
if(manager_get_module(R_HIP_YAW)==MM_WALKING)
- manager_current_angles[R_HIP_YAW]=((180.0*(angle[0]-PI/4.0))/PI)*65536.0;
+ manager_current_angles[R_HIP_YAW]=((-180.0*angle[0])/PI)*65536.0;
if(manager_get_module(R_HIP_ROLL)==MM_WALKING)
- manager_current_angles[R_HIP_ROLL]=((180.0*(angle[1]+pelvis_offset_r))/PI)*65536.0;
+ manager_current_angles[R_HIP_ROLL]=((-180.0*(angle[1]+pelvis_offset_r))/PI)*65536.0;
if(manager_get_module(R_HIP_PITCH)==MM_WALKING)
- manager_current_angles[R_HIP_PITCH]=((180.0*(angle[2]-DARWIN_PITCH_OFFSET))/PI-m_Hip_Pitch_Offset)*65536.0;
+ manager_current_angles[R_HIP_PITCH]=((180.0*angle[2])/PI-m_Hip_Pitch_Offset)*65536.0;
if(manager_get_module(R_KNEE)==MM_WALKING)
- manager_current_angles[R_KNEE]=((-180.0*angle[3])/PI)*65536.0;
+ manager_current_angles[R_KNEE]=((180.0*angle[3])/PI)*65536.0;
if(manager_get_module(R_ANKLE_PITCH)==MM_WALKING)
- manager_current_angles[R_ANKLE_PITCH]=((-180.0*(angle[4]+DARWIN_PITCH_OFFSET))/PI)*65536.0;
+ manager_current_angles[R_ANKLE_PITCH]=((-180.0*angle[4])/PI)*65536.0;
if(manager_get_module(R_ANKLE_ROLL)==MM_WALKING)
- manager_current_angles[R_ANKLE_ROLL]=((-180.0*angle[5])/PI)*65536.0;
+ manager_current_angles[R_ANKLE_ROLL]=((180.0*angle[5])/PI)*65536.0;
if(manager_get_module(L_HIP_YAW)==MM_WALKING)
- manager_current_angles[L_HIP_YAW]=((180.0*(angle[6]+PI/4.0))/PI)*65536.0;
+ manager_current_angles[L_HIP_YAW]=((-180.0*angle[6])/PI)*65536.0;
if(manager_get_module(L_HIP_ROLL)==MM_WALKING)
- manager_current_angles[L_HIP_ROLL]=((180.0*(angle[7]+pelvis_offset_l))/PI)*65536.0;
+ manager_current_angles[L_HIP_ROLL]=((-180.0*(angle[7]+pelvis_offset_l))/PI)*65536.0;
if(manager_get_module(L_HIP_PITCH)==MM_WALKING)
- manager_current_angles[L_HIP_PITCH]=((-180.0*(angle[8]-DARWIN_PITCH_OFFSET))/PI+m_Hip_Pitch_Offset)*65536.0;
+ manager_current_angles[L_HIP_PITCH]=((-180.0*angle[8])/PI+m_Hip_Pitch_Offset)*65536.0;
if(manager_get_module(L_KNEE)==MM_WALKING)
- manager_current_angles[L_KNEE]=((180.0*angle[9])/PI)*65536.0;
+ manager_current_angles[L_KNEE]=((-180.0*angle[9])/PI)*65536.0;
if(manager_get_module(L_ANKLE_PITCH)==MM_WALKING)
- manager_current_angles[L_ANKLE_PITCH]=((180.0*(angle[10]+DARWIN_PITCH_OFFSET))/PI)*65536.0;
+ manager_current_angles[L_ANKLE_PITCH]=((180.0*angle[10])/PI)*65536.0;
if(manager_get_module(L_ANKLE_ROLL)==MM_WALKING)
- manager_current_angles[L_ANKLE_ROLL]=((-180.0*angle[11])/PI)*65536.0;
+ manager_current_angles[L_ANKLE_ROLL]=((180.0*angle[11])/PI)*65536.0;
if(manager_get_module(R_SHOULDER_PITCH)==MM_WALKING)
- manager_current_angles[R_SHOULDER_PITCH]=(angle[12]-90.0)*65536.0;
+ manager_current_angles[R_SHOULDER_PITCH]=(angle[12]-48.345)*65536.0;
if(manager_get_module(L_SHOULDER_PITCH)==MM_WALKING)
- manager_current_angles[L_SHOULDER_PITCH]=(-angle[13]+90.0)*65536.0;
+ manager_current_angles[L_SHOULDER_PITCH]=(-angle[13]+41.313)*65536.0;
}
// operation functions
--
GitLab