Added the functions to access the walking interface of the darwin robot.

src/darwin_robot.h

@@ -44,8 +44,12 @@ class CDarwinRobot
     std::string robot_name;
     unsigned char robot_id;
     // motion manager variables
-    unsigned char num_servos;
+    unsigned char manager_num_servos;
     unsigned char manager_status;
+    /* action status */
+    unsigned char action_status;
+    /* walk status */
+    unsigned char walk_status;
   public:
     CDarwinRobot(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id);
     // GPIO interface
@@ -108,28 +112,50 @@ class CDarwinRobot
     void action_start(void);
     void action_stop(void);
     bool action_is_page_running(void);
-    // walking interface
-    void walking_set_speeds(double fw_step,double side_step,double turn);
-    void walking_start(void);
-    void walking_stop(void);
+    /* walking interface */
+    void walk_set_x_offset(double offset_m);
+    double walk_get_x_offset(void);
+    void walk_set_y_offset(double offset_m);
+    double walk_get_y_offset(void);
+    void walk_set_z_offset(double offset_m);
+    double walk_get_z_offset(void);
+    void walk_set_roll_offset(double offset_rad);
+    double walk_get_roll_offset(void);
+    void walk_set_pitch_offset(double offset_rad);
+    double walk_get_pitch_offset(void);
+    void walk_set_yaw_offset(double offset_rad);
+    double walk_get_yaw_offset(void);
+    void walk_set_hip_picth_offset(double offset_rad);
+    double walk_get_hip_pitch_offset(void);
+    void walk_set_period(double period_ms);
+    double walk_get_period(void);
+    void walk_set_ssp_dsp_ratio(double ratio);
+    double walk_get_ssp_dsp_ratio(void);
+    void walk_set_fwd_bwd_ratio(double ratio);
+    double walk_get_fwd_bwd_ratio(void);
+    void walk_set_foot_height(double height_m);
+    double walk_get_foot_height(void);
+    void walk_set_left_right_swing(double swing_m);
+    double walk_get_left_right_swing(void);
+    void walk_set_top_down_swing(double swing_m);
+    double walk_get_top_down_swing(void);
+    void walk_set_pelvis_offset(double offset_rad);
+    double walk_get_pelvis_offset(void);
+    void walk_set_arm_swing_gain(double gain);
+    double walk_get_arm_swing_gain(void);
+    void walk_set_trans_speed(double speed_m_s);
+    double walk_get_trans_speed(void);
+    void walk_set_rot_speed(double speed_rad_s);
+    double walk_get_rot_speed(void);
+    void walk_start(void);
+    void walk_stop(void);
     bool is_walking(void);
-    void walking_set_param(int param_id,double value);
-    double walking_get_param(int param_id);
-    // joint motion interface
-    void joints_load(std::vector<int> &servos,std::vector<double> &angles,std::vector<double> &speeds,std::vector<double> &accels);
-    void joints_start(void);
-    void joints_stop(void);
-    bool are_joints_moving(void);
-    // head tracking interface
-    void head_tracking_set_target(double pan,double tilt);
-    void head_tracking_set_pan_pid(double p,double i,double d);
-    void head_tracking_set_tilt_pid(double p,double i,double d);
-    void head_tracking_start(void);
-    void head_tracking_stop(void);
-    bool is_head_tracking(void);
-    // gripper interface
-    void gripper_open(grippers_t gripper);
-    void gripper_close(grippers_t gripper);
+    void walk_set_x_step(double step_m);
+    double walk_get_x_step(void);
+    void walk_set_y_step(double step_m);
+    double walk_get_y_step(void);
+    void walk_set_turn_step(double step_rad);
+    double walk_get_turn_step(void);
     ~CDarwinRobot();
 };
 

src/examples/CMakeLists.txt

@@ -19,36 +19,17 @@ ADD_EXECUTABLE(darwin_imu_test darwin_imu_test.cpp)
 TARGET_LINK_LIBRARIES(darwin_imu_test darwin_robot)
 
 # create an example application
-#ADD_EXECUTABLE(darwin_joint_motion_test darwin_joint_motion_test.cpp)
+ADD_EXECUTABLE(darwin_action_test darwin_action_test.cpp)
 # link necessary libraries
-#TARGET_LINK_LIBRARIES(darwin_joint_motion_test darwin_robot)
+TARGET_LINK_LIBRARIES(darwin_action_test darwin_robot)
 
 # create an example application
-#ADD_EXECUTABLE(darwin_action_test darwin_action_test.cpp)
+ADD_EXECUTABLE(darwin_walking_test darwin_walking_test.cpp)
 # link necessary libraries
-#TARGET_LINK_LIBRARIES(darwin_action_test darwin_robot)
-
-# create an example application
-#ADD_EXECUTABLE(darwin_walking_test darwin_walking_test.cpp)
-# link necessary libraries
-#TARGET_LINK_LIBRARIES(darwin_walking_test darwin_robot)
-
-# create an example application
-#ADD_EXECUTABLE(darwin_imu_test darwin_imu_test.cpp)
-# link necessary libraries
-#TARGET_LINK_LIBRARIES(darwin_imu_test darwin_robot)
-
-# create an example application
-#ADD_EXECUTABLE(darwin_head_tracking_test darwin_head_tracking_test.cpp)
-# link necessary libraries
-#TARGET_LINK_LIBRARIES(darwin_head_tracking_test darwin_robot)
+TARGET_LINK_LIBRARIES(darwin_walking_test darwin_robot)
 
 # create an example application
 #ADD_EXECUTABLE(darwin_kin darwin_kin.cpp)
 # link necessary libraries
 #TARGET_LINK_LIBRARIES(darwin_kin darwin_arm_kinematics)
 
-# create an example application
-#ADD_EXECUTABLE(darwin_gripper_test darwin_gripper_test.cpp)
-# link necessary libraries
-#TARGET_LINK_LIBRARIES(darwin_gripper_test darwin_robot)

src/examples/darwin_action_test.cpp

 #include "darwin_robot.h"
 #include "darwin_robot_exceptions.h"
+#include "action_id.h"
 
 #include <iostream>
 
-std::string robot_device="A603LOBS";
+//std::string robot_device="A603LOBS";
+std::string robot_device="A4008atn";
 
 int main(int argc, char *argv[])
 {
-  int i=0,num_v1_servos,num_v2_servos;
+  int i=0,num_servos;
 
   try{
-    CDarwinRobot darwin("Darwin",robot_device,1000000,0xC0);
+    CDarwinRobot darwin("Darwin",robot_device,926100,0x02);
 
-    darwin.mm_get_num_servos(&num_v1_servos,&num_v2_servos);
-    std::cout << "Found " << num_v1_servos << " v1 servos and " << num_v2_servos << " v2 servos" << std::endl;
+    num_servos=darwin.mm_get_num_servos();
+    std::cout << "Found " << num_servos << " servos " << std::endl;
     // enable all servos and assign them to the action module
     darwin.mm_enable_power();
-    for(i=1;i<=num_v1_servos;i++)
+    for(i=1;i<=num_servos;i++)
     {
       darwin.mm_enable_servo(i);
       darwin.mm_assign_module(i,DARWIN_MM_ACTION);
     }
     darwin.mm_start();
     // execute an action
-    darwin.action_load_page(9);
+    darwin.action_load_page(WALK_READY);
     darwin.action_start();
     while(darwin.action_is_page_running())
       usleep(100000);

src/examples/darwin_walking_test.cpp

 #include "darwin_robot.h"
 #include "darwin_robot_exceptions.h"
+#include "action_id.h"
 
 #include <iostream>
 
-std::string robot_device="A603LOBS";
+//std::string robot_device="A603LOBS";
+std::string robot_device="A4008atn";
 
 int main(int argc, char *argv[])
 {
-  int i=0,num_v1_servos,num_v2_servos;
+  int i=0,num_servos;
   std::vector<double> angles;
 
   try{
-    CDarwin_Robot darwin("Darwin",robot_device,1000000,0xC0);
-    darwin.mm_get_num_servos(&num_v1_servos,&num_v2_servos);
-    std::cout << "Found " << num_v1_servos << " v1 servos and " << num_v2_servos << " v2 servos" << std::endl;
+    CDarwinRobot darwin("Darwin",robot_device,926100,0x02);
+    num_servos=darwin.mm_get_num_servos();
+    std::cout << "Found " << num_servos << " servos" << std::endl;
     // enable all servos and assign them to the action module
     darwin.mm_enable_power();
-    for(i=1;i<=num_v1_servos;i++)
+    for(i=1;i<=num_servos;i++)
     {
       darwin.mm_enable_servo(i);
       darwin.mm_assign_module(i,DARWIN_MM_ACTION);
     }
     darwin.mm_start();
     // execute an action
-    darwin.action_load_page(9);
+    darwin.action_load_page(WALK_READY);
     darwin.action_start();
     while(darwin.action_is_page_running())
       usleep(100000);
-    for(i=1;i<=num_v1_servos;i++)
+    for(i=1;i<=num_servos;i++)
       darwin.mm_assign_module(i,DARWIN_MM_WALKING);
     std::cout << "Start walking ..." << std::endl;
-    darwin.walking_set_speeds(20,0,0);
-    darwin.walking_start();
+    darwin.walk_set_x_step(0.02);
+    darwin.walk_set_y_step(0.0);
+    darwin.walk_set_turn_step(0.0);
+    darwin.walk_start();
     sleep(2);
     std::cout << "Stop walking ..." << std::endl;
-    darwin.walking_stop();
+    darwin.walk_stop();
     while(darwin.is_walking())
     {
       std::cout << "Walking ..." << std::endl;