diff --git a/bioloid_apps/CMakeLists.txt b/bioloid_apps/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..a91bd99a37d5c6ff0ea8c54b5324fffae992492f
--- /dev/null
+++ b/bioloid_apps/CMakeLists.txt
@@ -0,0 +1,181 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(bioloid_apps)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+## * add a build_depend tag for "message_generation"
+## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+## but can be declared for certainty nonetheless:
+## * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+## * add "message_generation" and every package in MSG_DEP_SET to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * add "message_runtime" and every package in MSG_DEP_SET to
+## catkin_package(CATKIN_DEPENDS ...)
+## * uncomment the add_*_files sections below as needed
+## and list every .msg/.srv/.action file to be processed
+## * uncomment the generate_messages entry below
+## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# Service1.srv
+# Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# std_msgs # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+## * add "dynamic_reconfigure" to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * uncomment the "generate_dynamic_reconfigure_options" section below
+## and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+# cfg/DynReconf1.cfg
+# cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+# INCLUDE_DIRS include
+# LIBRARIES bioloid_apps
+# CATKIN_DEPENDS other_catkin_pkg
+# DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+# include_directories(include)
+
+## Declare a C++ library
+# add_library(bioloid_apps
+# src/${PROJECT_NAME}/bioloid_apps.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(bioloid_apps ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+# add_executable(bioloid_apps_node src/bioloid_apps_node.cpp)
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(bioloid_apps_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(bioloid_apps_node
+# ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS bioloid_apps bioloid_apps_node
+# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+# FILES_MATCHING PATTERN "*.h"
+# PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+# # myfile1
+# # myfile2
+# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_bioloid_apps.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/bioloid_apps/launch/action_client.launch b/bioloid_apps/launch/action_client.launch
new file mode 100644
index 0000000000000000000000000000000000000000..9d1bd8e7b065451482f9978b7044fcbecb9845cf
--- /dev/null
+++ b/bioloid_apps/launch/action_client.launch
@@ -0,0 +1,32 @@
+<launch>
+
+ <include file="$(find bioloid_description)/machines/bioloid.machines" />
+
+ <include file="$(find iri_bioloid_robot)/launch/bioloid.launch">
+ <arg name="config_file" value="bioloid_action.xml"/>
+ </include>
+
+ <group ns="bioloid">
+ <!-- launch the action client node -->
+ <node name="action_client"
+ pkg="action_client"
+ type="action_client"
+ output="screen"
+ machine="bioloid">
+ <remap from="/bioloid/motion"
+ to="/bioloid/robot/motion_action"/>
+ <remap from="/bioloid/action_client/set_servo_modules"
+ to="/bioloid/robot/set_servo_modules"/>
+ </node>
+ </group>
+
+ <!-- launch dynamic reconfigure -->
+<!-- <node name="rqt_reconfigure"
+ pkg="rqt_reconfigure"
+ type="rqt_reconfigure"
+ respawn="false"
+ machine="humanoide"
+ output="screen"/>-->
+
+</launch>
+
diff --git a/bioloid_apps/launch/action_client_sim.launch b/bioloid_apps/launch/action_client_sim.launch
new file mode 100644
index 0000000000000000000000000000000000000000..245282df83d5d2f52b345f4d7fb5e0437b59ebd4
--- /dev/null
+++ b/bioloid_apps/launch/action_client_sim.launch
@@ -0,0 +1,23 @@
+<launch>
+
+ <include file="$(find bioloid_description)/launch/bioloid_sim.launch">
+ </include>
+
+ <!-- launch the action client node -->
+ <node name="action_client"
+ pkg="action_client"
+ type="action_client"
+ output="screen"
+ ns="/bioloid">
+ <remap from="/bioloid/motion"
+ to="/bioloid/robot/motion_action"/>
+ <remap from="/bioloid/action_client/set_servo_modules"
+ to="/bioloid/robot/set_servo_modules"/>
+ </node>
+
+ <!-- launch dynamic reconfigure -->
+ <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
+ output="screen"/>
+
+</launch>
+
diff --git a/bioloid_apps/launch/walk_client.launch b/bioloid_apps/launch/walk_client.launch
new file mode 100644
index 0000000000000000000000000000000000000000..c86c87547c85724b67ef7b1b49a0166bd377a912
--- /dev/null
+++ b/bioloid_apps/launch/walk_client.launch
@@ -0,0 +1,33 @@
+<launch>
+
+ <include file="$(find bioloid_description)/machines/bioloid.machines" />
+
+ <include file="$(find iri_bioloid_robot)/launch/bioloid.launch">
+ <arg name="config_file" value="bioloid_action.xml"/>
+ </include>
+
+ <group ns="bioloid">
+ <!-- launch the walk client node -->
+ <node name="walk_client"
+ pkg="walk_client"
+ type="walk_client"
+ output="screen"
+ machine="bioloid">
+ <remap from="/bioloid/walk_client/cmd_vel"
+ to="/bioloid/robot/cmd_vel"/>
+ <remap from="/bioloid/walk_client/set_walk_params"
+ to="/bioloid/robot/set_walk_params"/>
+ <remap from="/bioloid/walk_client/set_servo_modules"
+ to="/bioloid/robot/set_servo_modules"/>
+ </node>
+ </group>
+
+ <!-- launch dynamic reconfigure -->
+<!-- <node name="rqt_reconfigure"
+ pkg="rqt_reconfigure"
+ type="rqt_reconfigure"
+ respawn="false"
+ machine="humanoide"
+ output="screen"/>-->
+
+</launch>
diff --git a/bioloid_apps/launch/walk_client_sim.launch b/bioloid_apps/launch/walk_client_sim.launch
new file mode 100644
index 0000000000000000000000000000000000000000..4337448d7b1e8f76ef5b13a542c4ff45b2952c9d
--- /dev/null
+++ b/bioloid_apps/launch/walk_client_sim.launch
@@ -0,0 +1,24 @@
+<launch>
+
+ <include file="$(find bioloid_description)/launch/bioloid_sim.launch">
+ </include>
+
+ <!-- launch the walk client node -->
+ <node name="walk_client"
+ pkg="walk_client"
+ type="walk_client"
+ output="screen"
+ ns="/bioloid">
+ <remap from="/bioloid/walk_client/cmd_vel"
+ to="/bioloid/robot/cmd_vel"/>
+ <remap from="/bioloid/walk_client/set_walk_params"
+ to="/bioloid/robot/set_walk_params"/>
+ <remap from="/bioloid/walk_client/set_servo_modules"
+ to="/bioloid/robot/set_servo_modules"/>
+ </node>
+
+ <!-- launch dynamic reconfigure -->
+ <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
+ output="screen"/>
+
+</launch>
diff --git a/bioloid_apps/package.xml b/bioloid_apps/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..2635d249cb6f02dbab3f94d30d0c5946aa846f79
--- /dev/null
+++ b/bioloid_apps/package.xml
@@ -0,0 +1,50 @@
+<?xml version="1.0"?>
+<package>
+ <name>bioloid_apps</name>
+ <version>0.0.0</version>
+ <description>The bioloid_apps package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="sergi@todo.todo">sergi</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>TODO</license>
+
+
+ <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/bioloid_apps</url> -->
+
+
+ <!-- Author tags are optional, mutiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintianers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+ <!-- The *_depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use run_depend for packages you need at runtime: -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
\ No newline at end of file
diff --git a/bioloid_control/config/bioloid_control.yaml b/bioloid_control/config/bioloid_control.yaml
index b096d3a26d33dbfbc2c6167e64afa656f29bd5cb..10deae535fa075a0ef4313f318a294e647ae3cb7 100644
--- a/bioloid_control/config/bioloid_control.yaml
+++ b/bioloid_control/config/bioloid_control.yaml
@@ -27,7 +27,7 @@ bioloid:
- j_ankle_roll_l
gains:
j_shoulder_l:
- p: 5.0
+ p: 7.5
d: 0.0
i: 0.0
proxy:
@@ -35,7 +35,7 @@ bioloid:
vel_limit: 6.0
effort_limit: 1.5
j_high_arm_l:
- p: 5.0
+ p: 7.5
d: 0.0
i: 0.0
proxy:
@@ -43,7 +43,7 @@ bioloid:
vel_limit: 6.0
effort_limit: 1.5
j_low_arm_l:
- p: 5.0
+ p: 7.5
d: 0.0
i: 0.0
proxy:
@@ -51,7 +51,7 @@ bioloid:
vel_limit: 6.0
effort_limit: 1.5
j_shoulder_r:
- p: 5.0
+ p: 7.5
d: 0.0
i: 0.0
proxy:
@@ -59,7 +59,7 @@ bioloid:
vel_limit: 6.0
effort_limit: 1.5
j_high_arm_r:
- p: 5.0
+ p: 7.5
d: 0.0
i: 0.0
proxy:
@@ -67,7 +67,7 @@ bioloid:
vel_limit: 6.0
effort_limit: 1.5
j_low_arm_r:
- p: 5.0
+ p: 7.5
d: 0.0
i: 0.0
proxy:
@@ -75,7 +75,7 @@ bioloid:
vel_limit: 6.0
effort_limit: 1.5
j_pelvis_yaw_l:
- p: 5.0
+ p: 7.5
d: 0.0
i: 0.0
proxy:
@@ -83,7 +83,7 @@ bioloid:
vel_limit: 6.0
effort_limit: 1.5
j_pelvis_roll_l:
- p: 5.0
+ p: 7.5
d: 0.0
i: 0.0
proxy:
@@ -91,7 +91,7 @@ bioloid:
vel_limit: 6.0
effort_limit: 1.5
j_pelvis_pitch_l:
- p: 5.0
+ p: 7.5
d: 0.0
i: 0.0
proxy:
@@ -99,7 +99,7 @@ bioloid:
vel_limit: 6.0
effort_limit: 1.5
j_knee_l:
- p: 5.0
+ p: 7.5
d: 0.0
i: 0.0
proxy:
@@ -107,7 +107,7 @@ bioloid:
vel_limit: 6.0
effort_limit: 1.5
j_ankle_pitch_l:
- p: 5.0
+ p: 7.5
d: 0.0
i: 0.0
proxy:
@@ -115,7 +115,7 @@ bioloid:
vel_limit: 6.0
effort_limit: 1.5
j_ankle_roll_l:
- p: 5.0
+ p: 7.5
d: 0.0
i: 0.0
proxy:
@@ -123,7 +123,7 @@ bioloid:
vel_limit: 6.0
effort_limit: 1.5
j_pelvis_yaw_r:
- p: 5.0
+ p: 7.5
d: 0.0
i: 0.0
proxy:
@@ -131,7 +131,7 @@ bioloid:
vel_limit: 6.0
effort_limit: 1.5
j_pelvis_roll_r:
- p: 5.0
+ p: 7.5
d: 0.0
i: 0.0
proxy:
@@ -139,7 +139,7 @@ bioloid:
vel_limit: 6.0
effort_limit: 1.5
j_pelvis_pitch_r:
- p: 5.0
+ p: 7.5
d: 0.0
i: 0.0
proxy:
@@ -147,7 +147,7 @@ bioloid:
vel_limit: 6.0
effort_limit: 1.5
j_knee_r:
- p: 5.0
+ p: 7.5
d: 0.0
i: 0.0
proxy:
@@ -155,7 +155,7 @@ bioloid:
vel_limit: 6.0
effort_limit: 1.5
j_ankle_pitch_r:
- p: 5.0
+ p: 7.5
d: 0.0
i: 0.0
proxy:
@@ -163,7 +163,7 @@ bioloid:
vel_limit: 6.0
effort_limit: 1.5
j_ankle_roll_r:
- p: 5.0
+ p: 7.5
d: 0.0
i: 0.0
proxy:
diff --git a/bioloid_controller/CMakeLists.txt b/bioloid_controller/CMakeLists.txt
index c1a515f1605790be73cb05d7f651dae3b296040b..5b204b780b087176b212939518edd8765906b73e 100644
--- a/bioloid_controller/CMakeLists.txt
+++ b/bioloid_controller/CMakeLists.txt
@@ -4,7 +4,7 @@ project(bioloid_controller)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
-find_package(catkin REQUIRED cmake_modules roscpp urdf controller_interface hardware_interface controller_manager xacro actionlib_msgs message_generation iri_action_server)
+find_package(catkin REQUIRED cmake_modules roscpp urdf controller_interface hardware_interface controller_manager xacro actionlib_msgs iri_action_server)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
@@ -53,18 +53,17 @@ find_package(catkin REQUIRED cmake_modules roscpp urdf controller_interface hard
# )
## Generate actions in the 'action' folder
-add_action_files(
- FILES
- bioloid_motion.action
+# add_action_files(
+# FILES
# Action1.action
# Action2.action
-)
+# )
## Generate added messages and services with any dependencies listed here
-generate_messages(
- DEPENDENCIES
- std_msgs actionlib_msgs # Or other packages containing msgs
-)
+# generate_messages(
+# DEPENDENCIES
+# std_msgs actionlib_msgs # Or other packages containing msgs
+# )
###################################
## catkin specific configuration ##
@@ -78,7 +77,7 @@ generate_messages(
catkin_package(
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME}
- CATKIN_DEPENDS roscpp urdf controller_interface hardware_interface controller_manager xacro message_runtime actionlib_msgs iri_action_server
+ CATKIN_DEPENDS roscpp urdf controller_interface hardware_interface controller_manager xacro actionlib_msgs iri_action_server
# DEPENDS system_lib
)
@@ -128,7 +127,7 @@ add_library(${PROJECT_NAME} src/bioloid_controller.cpp
## Add cmake target dependencies of the executable/library
## as an example, message headers may need to be generated before nodes
-add_dependencies(bioloid_controller bioloid_controller_generate_messages_cpp)
+add_dependencies(bioloid_controller humanoid_common_msgs_generate_messages_cpp)
## Specify libraries to link a library or executable target against
# target_link_libraries(bioloid_controller_node
diff --git a/bioloid_controller/action/bioloid_motion.action b/bioloid_controller/action/bioloid_motion.action
deleted file mode 100644
index 4a28892f3f8ebb5617fd28276c6cb61722fdaf85..0000000000000000000000000000000000000000
--- a/bioloid_controller/action/bioloid_motion.action
+++ /dev/null
@@ -1,9 +0,0 @@
-#goal definition
-int32 motion_id
----
-#result definition
-bool successful
----
-#feedback
-int32 current_page
-int32 current_step
diff --git a/bioloid_controller/include/bioloid_controller.h b/bioloid_controller/include/bioloid_controller.h
index dc236968aaa88c125b9b5cab903625c579beb974..edfcd153e79edd02794d74e63026212320a1b76c 100644
--- a/bioloid_controller/include/bioloid_controller.h
+++ b/bioloid_controller/include/bioloid_controller.h
@@ -55,7 +55,10 @@
#include <control_toolbox/pid.h>
#include <iri_action_server/iri_action_server.h>
-#include <bioloid_controller/bioloid_motionAction.h>
+#include <humanoid_common_msgs/humanoid_motionAction.h>
+#include <humanoid_common_msgs/set_servo_modules.h>
+#include <humanoid_common_msgs/set_walk_params.h>
+#include <humanoid_common_msgs/get_walk_params.h>
namespace bioloid_controller
{
@@ -67,7 +70,6 @@ namespace bioloid_controller
class BioloidController : public controller_interface::Controller<HardwareInterface>
{
public:
-
BioloidController();
~BioloidController();
@@ -76,18 +78,6 @@ namespace bioloid_controller
void stopping(const ros::Time& time);
void update(const ros::Time& time, const ros::Duration& period);
- // action subscriber
- IriActionServer<bioloid_controller::bioloid_motionAction> *motion_action_aserver_;
- void motion_action_startCallback(const bioloid_controller::bioloid_motionGoalConstPtr& goal);
- void motion_action_stopCallback(void);
- bool motion_action_isFinishedCallback(void);
- bool motion_action_hasSucceedCallback(void);
- void motion_action_getResultCallback(bioloid_controller::bioloid_motionResultPtr& result);
- void motion_action_getFeedbackCallback(bioloid_controller::bioloid_motionFeedbackPtr& feedback);
-
- ros::Subscriber imu_sub;
- void imu_callback(const sensor_msgs::Imu::ConstPtr& msg);
-
private:
typedef typename HardwareInterface::ResourceHandleType JointHandle;
typedef boost::shared_ptr<control_toolbox::Pid> PidPtr;
@@ -107,6 +97,28 @@ namespace bioloid_controller
double balance_knee_left;
double balance_knee_right;
+ // motion action subscriber
+ IriActionServer<humanoid_common_msgs::humanoid_motionAction> *motion_action_aserver_;
+ void motion_action_startCallback(const humanoid_common_msgs::humanoid_motionGoalConstPtr& goal);
+ void motion_action_stopCallback(void);
+ bool motion_action_isFinishedCallback(void);
+ bool motion_action_hasSucceedCallback(void);
+ void motion_action_getResultCallback(humanoid_common_msgs::humanoid_motionResultPtr& result);
+ void motion_action_getFeedbackCallback(humanoid_common_msgs::humanoid_motionFeedbackPtr& feedback);
+
+ /* services */
+ ros::ServiceServer set_servo_modules_server_;
+ bool set_servo_modulesCallback(humanoid_common_msgs::set_servo_modules::Request &req, humanoid_common_msgs::set_servo_modules::Response &res);
+ ros::ServiceServer set_walk_params_server_;
+ bool set_walk_paramsCallback(humanoid_common_msgs::set_walk_params::Request &req, humanoid_common_msgs::set_walk_params::Response &res);
+ ros::ServiceServer get_walk_params_server_;
+ bool get_walk_paramsCallback(humanoid_common_msgs::get_walk_params::Request &req, humanoid_common_msgs::get_walk_params::Response &res);
+
+
+ /* IMU subscriber */
+ ros::Subscriber imu_sub;
+ void imu_callback(const sensor_msgs::Imu::ConstPtr& msg);
+
// ROS API
ros::NodeHandle controller_nh_;
};
diff --git a/bioloid_controller/include/bioloid_controller_impl.h b/bioloid_controller/include/bioloid_controller_impl.h
index 8064ac606d62afc0a3a33095daed94c01449e18a..880b17d27033fb5ccef2a4a901c720072a9b718d 100644
--- a/bioloid_controller/include/bioloid_controller_impl.h
+++ b/bioloid_controller/include/bioloid_controller_impl.h
@@ -18,6 +18,39 @@ extern double real_angles[PAGE_MAX_NUM_SERVOS];
// action module external variables
extern long int manager_current_angles[PAGE_MAX_NUM_SERVOS];
+// joint names
+const std::string servo_names[MANAGER_MAX_NUM_SERVOS]={std::string("Servo0"),
+ std::string("j_shoulder_pitch_r"),
+ std::string("j_shoulder_pitch_l"),
+ std::string("j_shoulder_roll_r"),
+ std::string("j_shoulder_roll_l"),
+ std::string("j_elbow_r"),
+ std::string("j_elbow_l"),
+ std::string("j_hip_yaw_r"),
+ std::string("j_hip_yaw_l"),
+ std::string("j_hip_roll_r"),
+ std::string("j_hip_roll_l"),
+ std::string("j_hip_pitch_r"),
+ std::string("j_hip_pitch_l"),
+ std::string("j_knee_r"),
+ std::string("j_knee_l"),
+ std::string("j_ankle_pitch_r"),
+ std::string("j_ankle_pitch_l"),
+ std::string("j_ankle_roll_r"),
+ std::string("j_ankle_roll_l"),
+ std::string("Servo19"),
+ std::string("Servo20"),
+ std::string("Servo21"),
+ std::string("Servo22"),
+ std::string("Servo23"),
+ std::string("Servo24"),
+ std::string("Servo25"),
+ std::string("Servo26"),
+ std::string("Servo27"),
+ std::string("Servo28"),
+ std::string("Servo29"),
+ std::string("Servo30")};
+
namespace bioloid_controller
{
namespace internal
@@ -134,7 +167,8 @@ namespace bioloid_controller
// Cache controller node handle
controller_nh_ = controller_nh;
- this->motion_action_aserver_=new IriActionServer<bioloid_controller::bioloid_motionAction>(root_nh,"robot/motion_action");
+ /* initialize actions */
+ this->motion_action_aserver_=new IriActionServer<humanoid_common_msgs::humanoid_motionAction>(root_nh,"robot/motion_action");
this->motion_action_aserver_->registerStartCallback(boost::bind(&BioloidController<HardwareInterface>::motion_action_startCallback, this, _1));
this->motion_action_aserver_->registerStopCallback(boost::bind(& BioloidController<HardwareInterface>::motion_action_stopCallback, this));
this->motion_action_aserver_->registerIsFinishedCallback(boost::bind(&BioloidController<HardwareInterface>::motion_action_isFinishedCallback, this));
@@ -143,6 +177,11 @@ namespace bioloid_controller
this->motion_action_aserver_->registerGetFeedbackCallback(boost::bind(&BioloidController<HardwareInterface>::motion_action_getFeedbackCallback, this, _1));
this->motion_action_aserver_->start();
+ /* initialize services */
+ this->set_servo_modules_server_ = root_nh.advertiseService("robot/set_servo_modules", &BioloidController<HardwareInterface>::set_servo_modulesCallback, this);
+ this->set_walk_params_server_ = root_nh.advertiseService("robot/set_walk_params", &BioloidController<HardwareInterface>::set_walk_paramsCallback, this);
+ this->get_walk_params_server_ = root_nh.advertiseService("robot/get_walk_params", &BioloidController<HardwareInterface>::get_walk_paramsCallback, this);
+
// Controller name
name_ = getLeafNamespace(controller_nh_);
@@ -207,8 +246,6 @@ namespace bioloid_controller
ram_init();
/* initialize motion modules */
manager_init(7800);// motion manager period in us
- for(unsigned int i=0;i<MANAGER_MAX_NUM_SERVOS;++i)
- manager_set_module(i,MM_WALKING);
std::cout << "motion manager initialized" << std::endl;
return true;
@@ -234,7 +271,7 @@ namespace bioloid_controller
}
template <class HardwareInterface>
- void BioloidController<HardwareInterface>::motion_action_startCallback(const bioloid_controller::bioloid_motionGoalConstPtr& goal)
+ void BioloidController<HardwareInterface>::motion_action_startCallback(const humanoid_common_msgs::humanoid_motionGoalConstPtr& goal)
{
std::cout << "Start action" << std::endl;
/* load a page and start execution */
@@ -270,17 +307,108 @@ namespace bioloid_controller
}
template <class HardwareInterface>
- void BioloidController<HardwareInterface>::motion_action_getResultCallback(bioloid_controller::bioloid_motionResultPtr& result)
+ void BioloidController<HardwareInterface>::motion_action_getResultCallback(humanoid_common_msgs::humanoid_motionResultPtr& result)
{
}
template <class HardwareInterface>
- void BioloidController<HardwareInterface>::motion_action_getFeedbackCallback(bioloid_controller::bioloid_motionFeedbackPtr& feedback)
+ void BioloidController<HardwareInterface>::motion_action_getFeedbackCallback(humanoid_common_msgs::humanoid_motionFeedbackPtr& feedback)
{
feedback->current_page=action_get_current_page();
feedback->current_step=action_get_current_step();
}
+ template <class HardwareInterface>
+ bool BioloidController<HardwareInterface>::set_servo_modulesCallback(humanoid_common_msgs::set_servo_modules::Request &req, humanoid_common_msgs::set_servo_modules::Response &res)
+ {
+ unsigned int i=0,j=0;
+
+ for(i=0;i<req.names.size();i++)
+ {
+ for(j=0;j<MANAGER_MAX_NUM_SERVOS;j++)
+ {
+ if(req.names[i]==servo_names[j])
+ {
+ if(req.modules[i]=="action")
+ manager_set_module(j,MM_ACTION);
+ else if(req.modules[i]=="walking")
+ manager_set_module(j,MM_WALKING);
+ else if(req.modules[i]=="joints")
+ manager_set_module(j,MM_JOINTS);
+ else
+ manager_set_module(j,MM_NONE);
+ break;
+ }
+ }
+ }
+ res.ret=true;
+
+ return true;
+ }
+
+ template <class HardwareInterface>
+ bool BioloidController<HardwareInterface>::set_walk_paramsCallback(humanoid_common_msgs::set_walk_params::Request &req, humanoid_common_msgs::set_walk_params::Response &res)
+ {
+ ram_write_byte(BIOLOID_WALK_SWING_RIGHT_LEFT,(unsigned char)req.params.Y_SWAP_AMPLITUDE);
+ ram_write_byte(BIOLOID_WALK_SWING_TOP_DOWN,(unsigned char)req.params.Z_SWAP_AMPLITUDE);
+ ram_write_byte(BIOLOID_WALK_ARM_SWING_GAIN,(unsigned char)(req.params.ARM_SWING_GAIN*32.0));
+ ram_write_byte(BIOLOID_WALK_PELVIS_OFFSET,(unsigned char)(req.params.PELVIS_OFFSET*8.0));
+ ram_write_word(BIOLOID_WALK_HIP_PITCH_OFF_L,(unsigned short int)(req.params.HIP_PITCH_OFFSET*1024.0));
+ ram_write_byte(BIOLOID_WALK_X_OFFSET,(unsigned char)(req.params.X_OFFSET));
+ ram_write_byte(BIOLOID_WALK_Y_OFFSET,(unsigned char)(req.params.Y_OFFSET));
+ ram_write_byte(BIOLOID_WALK_Z_OFFSET,(unsigned char)(req.params.Z_OFFSET));
+ ram_write_byte(BIOLOID_WALK_YAW_OFFSET,(unsigned char)(req.params.A_OFFSET*8.0));
+ ram_write_byte(BIOLOID_WALK_PITCH_OFFSET,(unsigned char)(req.params.P_OFFSET*8.0));
+ ram_write_byte(BIOLOID_WALK_ROLL_OFFSET,(unsigned char)(req.params.R_OFFSET*8.0));
+ ram_write_word(BIOLOID_WALK_PERIOD_TIME_L,(unsigned short int)(req.params.PERIOD_TIME));
+ ram_write_byte(BIOLOID_WALK_DSP_RATIO,(unsigned char)(req.params.DSP_RATIO*256.0));
+ ram_write_byte(BIOLOID_WALK_STEP_FW_BW_RATIO,(unsigned char)(req.params.STEP_FB_RATIO*256.0));
+ ram_write_byte(BIOLOID_WALK_MAX_VEL,(unsigned char)(req.params.MAX_ACC));
+ ram_write_byte(BIOLOID_WALK_MAX_ROT_VEL,(unsigned char)(req.params.MAX_ROT_ACC*8.0));
+
+ res.ret=true;
+ }
+
+ template <class HardwareInterface>
+ bool BioloidController<HardwareInterface>::get_walk_paramsCallback(humanoid_common_msgs::get_walk_params::Request &req, humanoid_common_msgs::get_walk_params::Response &res)
+ {
+ unsigned char byte_value;
+ unsigned short int word_value;
+
+ ram_read_byte(BIOLOID_WALK_SWING_RIGHT_LEFT,&byte_value);
+ res.params.Y_SWAP_AMPLITUDE=byte_value;
+ ram_read_byte(BIOLOID_WALK_SWING_TOP_DOWN,&byte_value);
+ res.params.Z_SWAP_AMPLITUDE=byte_value;
+ ram_read_byte(BIOLOID_WALK_ARM_SWING_GAIN,&byte_value);
+ res.params.ARM_SWING_GAIN=((float)byte_value)/32.0;
+ ram_read_byte(BIOLOID_WALK_PELVIS_OFFSET,&byte_value);
+ res.params.PELVIS_OFFSET=((float)byte_value)/8.0;
+ ram_read_word(BIOLOID_WALK_HIP_PITCH_OFF_L,&word_value);
+ res.params.HIP_PITCH_OFFSET=((float)word_value)/1024.0;
+ ram_read_byte(BIOLOID_WALK_X_OFFSET,&byte_value);
+ res.params.X_OFFSET=byte_value;
+ ram_read_byte(BIOLOID_WALK_Y_OFFSET,&byte_value);
+ res.params.Y_OFFSET=byte_value;
+ ram_read_byte(BIOLOID_WALK_Z_OFFSET,&byte_value);
+ res.params.Z_OFFSET=byte_value;
+ ram_read_byte(BIOLOID_WALK_YAW_OFFSET,&byte_value);
+ res.params.A_OFFSET=((float)byte_value)/8.0;;
+ ram_read_byte(BIOLOID_WALK_PITCH_OFFSET,&byte_value);
+ res.params.P_OFFSET=((float)byte_value)/8.0;
+ ram_read_byte(BIOLOID_WALK_ROLL_OFFSET,&byte_value);
+ res.params.R_OFFSET=((float)byte_value)/8.0;
+ ram_read_word(BIOLOID_WALK_PERIOD_TIME_L,&word_value);
+ res.params.PERIOD_TIME=word_value;
+ ram_read_byte(BIOLOID_WALK_DSP_RATIO,&byte_value);
+ res.params.DSP_RATIO=((float)byte_value)/256.0;
+ ram_read_byte(BIOLOID_WALK_STEP_FW_BW_RATIO,&byte_value);
+ res.params.STEP_FB_RATIO=((float)byte_value)/256.0;
+ ram_read_byte(BIOLOID_WALK_MAX_VEL,&byte_value);
+ res.params.MAX_ACC=byte_value;
+ ram_read_byte(BIOLOID_WALK_MAX_ROT_VEL,&byte_value);
+ res.params.MAX_ROT_ACC=((float)byte_value)/8.0;
+ }
+
template <class HardwareInterface>
void BioloidController<HardwareInterface>::imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
{
diff --git a/bioloid_controller/package.xml b/bioloid_controller/package.xml
index 43c44905b6da82add91e20a76a89d4c1d7dadf7b..6a470c8243e69d1c4f3c9d05f6bdf235e86abd0e 100644
--- a/bioloid_controller/package.xml
+++ b/bioloid_controller/package.xml
@@ -46,6 +46,7 @@
<build_depend>iri_action_server</build_depend>
<build_depend>message_generation</build_depend>
<build_depend>actionlib_msgs</build_depend>
+ <build_depend>humanoid_common_msgs</build_depend>
<build_depend>controller_interface</build_depend>
<build_depend>hardware_interface</build_depend>
@@ -58,6 +59,7 @@
<run_depend>iri_action_server</run_depend>
<run_depend>message_runtime</run_depend>
<run_depend>actionlib_msgs</run_depend>
+ <run_depend>humanoid_common_msgs</run_depend>
<run_depend>controller_interface</run_depend>
<run_depend>hardware_interface</run_depend>
diff --git a/bioloid_robot/package.xml b/bioloid_robot/package.xml
index 31503bcf44801d4729b08e7a40e34a4fdf795dfa..e7cfb1e50c1a1d57b0cd5a8953a49b4ac82e6a64 100644
--- a/bioloid_robot/package.xml
+++ b/bioloid_robot/package.xml
@@ -45,7 +45,6 @@
<run_depend>bioloid_control</run_depend>
<run_depend>bioloid_controller</run_depend>
-
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- You can specify that this package is a metapackage here: -->