diff --git a/include/darwin_kinematics.h b/include/darwin_kinematics.h index 7f67ffa092131a028b2f3ca17c120f1c7bb24d2e..783781e8a49a95ab0aecd55691071913b5096959 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 192efbdad77bc1766f0469ad5fdd15fefb575182..1bfbd1ecad91d81b4aca7861869de1627725a565 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