Skip to content
Snippets Groups Projects
Commit 084475a9 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Removed the pitch offset from the kinematics computation.

parent 87e4cff1
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
include/darwin_kinematics.h
+ 0
− 1
View file @ 084475a9
...@@ -11,7 +11,6 @@ extern "C" { ...@@ -11,7 +11,6 @@ extern "C" {
#define DARWIN_LEG_SIDE_OFFSET 37.0 //mm #define DARWIN_LEG_SIDE_OFFSET 37.0 //mm
#define DARWIN_THIGH_LENGTH 93.0 //mm #define DARWIN_THIGH_LENGTH 93.0 //mm
#define DARWIN_CALF_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_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) #define DARWIN_LEG_LENGTH (DARWIN_THIGH_LENGTH + DARWIN_CALF_LENGTH + DARWIN_ANKLE_LENGTH) //mm (THIGH_LENGTH + CALF_LENGTH + ANKLE_LENGTH)
......
src/walking.c
+ 14
− 14
View file @ 084475a9
...@@ -445,33 +445,33 @@ void walking_process(void) ...@@ -445,33 +445,33 @@ void walking_process(void)
// Compute motor value // Compute motor value
if(manager_get_module(R_HIP_YAW)==MM_WALKING) 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) 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) 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) 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) 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) 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) 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) 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) 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) 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) 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) 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) 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) 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 // operation functions
......
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