├── nao_behaviour_ros ├── srv │ ├── __init__.py │ ├── Tired.srv │ ├── Crouch.srv │ ├── TaiChi.srv │ ├── WakeUp.srv │ ├── BallTracker.srv │ ├── Macarena.srv │ ├── Vangelis.srv │ ├── WarmHello.srv │ ├── DanceEvolution.srv │ ├── Default.srv │ ├── EyeOfTheTiger.srv │ ├── GangnamStyle.srv │ └── sayText.srv ├── thanks.sh ├── setup.py ├── CMakeLists.txt ├── package.xml └── src │ └── nao_behaviour │ ├── nao_speek_node │ └── nao_behaviour_node ├── nao_walk_ros ├── srv │ ├── Tired.srv │ ├── Crouch.srv │ ├── TaiChi.srv │ ├── WakeUp.srv │ ├── BallTracker.srv │ ├── Macarena.srv │ ├── Vangelis.srv │ ├── WarmHello.srv │ ├── DanceEvolution.srv │ ├── EyeOfTheTiger.srv │ ├── GangnamStyle.srv │ └── sayText.srv ├── action │ ├── speek.action │ └── behaviour.action ├── include │ └── nao_walk │ │ ├── joint_states_table.h │ │ ├── nao_walk_engine.h │ │ ├── socket_client.h │ │ ├── dcm_data.h │ │ └── nao_walk_ros.h ├── config │ └── robotcfg.yaml ├── launch │ ├── nao_robot_state_publisher.launch │ └── gait_control.launch ├── nodes │ └── server.cpp ├── src │ ├── joint_states_table.cpp │ ├── gait_control.cpp │ └── socket_client.cpp ├── cfg │ ├── GaitControl.cfg │ └── GaitControl.cfg.bak ├── CMakeLists.txt ├── package.xml └── cmake │ └── FindNAOqi.cmake ├── humanoid_msgs ├── humanoid_nav_msgs │ ├── srv │ │ ├── ClipFootstep.srv │ │ ├── StepTargetService.srv │ │ ├── PlanFootsteps.srv │ │ └── PlanFootstepsBetweenFeet.srv │ ├── action │ │ └── ExecFootsteps.action │ ├── msg │ │ └── StepTarget.msg │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ └── package.xml ├── README.md ├── humanoid_msgs │ ├── CMakeLists.txt │ ├── CHANGELOG.rst │ └── package.xml └── .gitignore ├── nao_walk_naoqi ├── KMat.cpp ├── SystemMutex.hpp ├── qiproject.xml ├── Logs.h ├── LeakyIntegrator.cpp ├── butterworthLPF.h ├── butterworthHPF.h ├── JointSSKF.h ├── ZMPFilter.h ├── Array.cc ├── SocketServer.h ├── FootPolygon.h ├── CoMAdmittanceControl.h ├── main.cpp ├── Kalman.h ├── MotionDefines.h ├── StepAdjustment.h ├── Stepplanner2D.h ├── DelayedObserverDCM.h ├── LeakyIntegrator.h ├── MPCDCM.h ├── behaviourMap.h ├── JointSSKF.cpp ├── ZMPDistributor.h ├── FeetEngine.h ├── matlog.h ├── PostureController.h ├── ZMPFilter.cpp ├── CMakeLists.txt ├── CoMAdmittanceControl.cpp ├── butterworthLPF.cpp ├── butterworthHPF.cpp ├── DcmData.h ├── DelayedObserverDCM.cpp ├── TrajectoryPlanner.h ├── MovingAverageFilter.h ├── RobotParameters.cpp ├── QuadProg++.hh ├── MovingAverageFilter.cpp ├── PCThread.h.bak ├── PCThread.h ├── LIPMThread.h ├── robot_consts.h ├── CoMEKF.h ├── MovingMedianFilter.h ├── SocketServer.cpp ├── Kalman.cpp ├── LockedBuffer.hpp ├── KinematicsDefines.h ├── ZMPDistributor.cpp ├── Common.hpp ├── FootPolygon.cpp ├── PostureController.cpp ├── matlog.cpp ├── RobotParameters.h ├── MPCDCM.cpp ├── FeetEngine.cpp ├── LowLevelPlanner.h ├── StepAdjustment.cpp └── Stepplanner2D.cpp └── README.md /nao_behaviour_ros/srv/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /nao_walk_ros/srv/Tired.srv: -------------------------------------------------------------------------------- 1 | --- 2 | string result -------------------------------------------------------------------------------- /nao_behaviour_ros/srv/Tired.srv: -------------------------------------------------------------------------------- 1 | --- 2 | string result -------------------------------------------------------------------------------- /nao_walk_ros/srv/Crouch.srv: -------------------------------------------------------------------------------- 1 | --- 2 | string result 3 | -------------------------------------------------------------------------------- /nao_walk_ros/srv/TaiChi.srv: -------------------------------------------------------------------------------- 1 | --- 2 | string result 3 | -------------------------------------------------------------------------------- /nao_walk_ros/srv/WakeUp.srv: -------------------------------------------------------------------------------- 1 | --- 2 | string result 3 | -------------------------------------------------------------------------------- /nao_behaviour_ros/srv/Crouch.srv: -------------------------------------------------------------------------------- 1 | --- 2 | string result 3 | -------------------------------------------------------------------------------- /nao_behaviour_ros/srv/TaiChi.srv: -------------------------------------------------------------------------------- 1 | --- 2 | string result 3 | -------------------------------------------------------------------------------- /nao_behaviour_ros/srv/WakeUp.srv: -------------------------------------------------------------------------------- 1 | --- 2 | string result 3 | -------------------------------------------------------------------------------- /nao_walk_ros/srv/BallTracker.srv: -------------------------------------------------------------------------------- 1 | --- 2 | string result 3 | -------------------------------------------------------------------------------- /nao_walk_ros/srv/Macarena.srv: -------------------------------------------------------------------------------- 1 | --- 2 | string result 3 | -------------------------------------------------------------------------------- /nao_walk_ros/srv/Vangelis.srv: -------------------------------------------------------------------------------- 1 | --- 2 | string result 3 | -------------------------------------------------------------------------------- /nao_walk_ros/srv/WarmHello.srv: -------------------------------------------------------------------------------- 1 | --- 2 | string result 3 | -------------------------------------------------------------------------------- /nao_behaviour_ros/srv/BallTracker.srv: -------------------------------------------------------------------------------- 1 | --- 2 | string result 3 | -------------------------------------------------------------------------------- /nao_behaviour_ros/srv/Macarena.srv: -------------------------------------------------------------------------------- 1 | --- 2 | string result 3 | -------------------------------------------------------------------------------- /nao_behaviour_ros/srv/Vangelis.srv: -------------------------------------------------------------------------------- 1 | --- 2 | string result 3 | -------------------------------------------------------------------------------- /nao_behaviour_ros/srv/WarmHello.srv: -------------------------------------------------------------------------------- 1 | --- 2 | string result 3 | -------------------------------------------------------------------------------- /nao_walk_ros/srv/DanceEvolution.srv: -------------------------------------------------------------------------------- 1 | --- 2 | string result 3 | -------------------------------------------------------------------------------- /nao_walk_ros/srv/EyeOfTheTiger.srv: -------------------------------------------------------------------------------- 1 | --- 2 | string result 3 | -------------------------------------------------------------------------------- /nao_walk_ros/srv/GangnamStyle.srv: -------------------------------------------------------------------------------- 1 | --- 2 | string result 3 | -------------------------------------------------------------------------------- /nao_behaviour_ros/srv/DanceEvolution.srv: -------------------------------------------------------------------------------- 1 | --- 2 | string result 3 | -------------------------------------------------------------------------------- /nao_behaviour_ros/srv/Default.srv: -------------------------------------------------------------------------------- 1 | --- 2 | string result 3 | 4 | -------------------------------------------------------------------------------- /nao_behaviour_ros/srv/EyeOfTheTiger.srv: -------------------------------------------------------------------------------- 1 | --- 2 | string result 3 | -------------------------------------------------------------------------------- /nao_behaviour_ros/srv/GangnamStyle.srv: -------------------------------------------------------------------------------- 1 | --- 2 | string result 3 | -------------------------------------------------------------------------------- /nao_walk_ros/srv/sayText.srv: -------------------------------------------------------------------------------- 1 | string request 2 | --- 3 | string result -------------------------------------------------------------------------------- /nao_behaviour_ros/srv/sayText.srv: -------------------------------------------------------------------------------- 1 | string request 2 | --- 3 | string result -------------------------------------------------------------------------------- /humanoid_msgs/humanoid_nav_msgs/srv/ClipFootstep.srv: -------------------------------------------------------------------------------- 1 | StepTarget step 2 | --- 3 | StepTarget step 4 | -------------------------------------------------------------------------------- /humanoid_msgs/README.md: -------------------------------------------------------------------------------- 1 | humanoid_msgs 2 | ============= 3 | 4 | Messages and services for humanoid robots in ROS 5 | -------------------------------------------------------------------------------- /humanoid_msgs/humanoid_nav_msgs/srv/StepTargetService.srv: -------------------------------------------------------------------------------- 1 | # Step target as service: 2 | humanoid_nav_msgs/StepTarget step 3 | --- 4 | -------------------------------------------------------------------------------- /humanoid_msgs/humanoid_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(humanoid_msgs) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /nao_behaviour_ros/thanks.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | rosservice call /nao_speak "thank you jaco." 3 | rosservice call /nao_speak "Now is time to rest." 4 | rosservice call /nao_behaviours/crouch 5 | -------------------------------------------------------------------------------- /nao_walk_ros/action/speek.action: -------------------------------------------------------------------------------- 1 | # Define the goal 2 | string text 3 | --- 4 | # Define the result 5 | int32 status 6 | --- 7 | # Define a feedback message 8 | float32 percent_complete 9 | 10 | -------------------------------------------------------------------------------- /nao_walk_ros/include/nao_walk/joint_states_table.h: -------------------------------------------------------------------------------- 1 | #ifndef JOINT_STATES_TABLE_H 2 | #define JOINT_STATES_TABLE_H 3 | 4 | #include 5 | #include 6 | 7 | extern const std::vectorjoint_state_names; 8 | 9 | #endif 10 | -------------------------------------------------------------------------------- /nao_walk_naoqi/KMat.cpp: -------------------------------------------------------------------------------- 1 | #include"KMat.hpp" 2 | 3 | const double KMath::KMat::transformations::PI=3.1415926535897932384626433832795028841971693993751058209749445923078164062862089986280348253421170679821480865132823066470938446095505822317253594081284811; 4 | -------------------------------------------------------------------------------- /humanoid_msgs/humanoid_nav_msgs/srv/PlanFootsteps.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/Pose2D start 2 | geometry_msgs/Pose2D goal 3 | --- 4 | bool result 5 | humanoid_nav_msgs/StepTarget[] footsteps 6 | float64 costs 7 | float64 final_eps 8 | float64 planning_time 9 | int64 expanded_states 10 | -------------------------------------------------------------------------------- /nao_walk_ros/config/robotcfg.yaml: -------------------------------------------------------------------------------- 1 | #RobotIP: "odysseus.local" 2 | RobotIP: "mumra.local" 3 | 4 | #RobotIP: 169.254.113.113 5 | RobotPort: 9559 6 | DriverBrokerPort: 54000 7 | DriverBrokerIP: 0.0.0.0 8 | Version: V4 9 | BodyType: H2 10 | port: 8080 11 | nao_hostname: "mumra.local" -------------------------------------------------------------------------------- /nao_walk_naoqi/SystemMutex.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SYSTEMMUTEX_HPP 2 | #define SYSTEMMUTEX_HPP 3 | 4 | #include 5 | namespace KSystem 6 | { 7 | 8 | //class SystemMutex : public boost::mutex { }; 9 | typedef boost::mutex SystemMutex; 10 | 11 | }; 12 | 13 | #endif // SYSTEMMUTEX_HPP 14 | -------------------------------------------------------------------------------- /humanoid_msgs/.gitignore: -------------------------------------------------------------------------------- 1 | # ROS package 2 | build/ 3 | 4 | # Specific ignores for ROS msgs 5 | humanoid_nav_msgs/src 6 | msg_gen 7 | srv_gen 8 | # Actionlib: 9 | humanoid_nav_msgs/msg/*Action.msg 10 | humanoid_nav_msgs/msg/*Feedback.msg 11 | humanoid_nav_msgs/msg/*Goal.msg 12 | humanoid_nav_msgs/msg/*Result.msg 13 | -------------------------------------------------------------------------------- /humanoid_msgs/humanoid_nav_msgs/action/ExecFootsteps.action: -------------------------------------------------------------------------------- 1 | # Define the goal 2 | humanoid_nav_msgs/StepTarget[] footsteps 3 | float64 feedback_frequency 4 | --- 5 | # Define the result 6 | humanoid_nav_msgs/StepTarget[] executed_footsteps 7 | --- 8 | # Define a feedback message 9 | humanoid_nav_msgs/StepTarget[] executed_footsteps 10 | -------------------------------------------------------------------------------- /humanoid_msgs/humanoid_msgs/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package humanoid_msgs 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.0 (2014-01-16) 6 | ------------------ 7 | 8 | 0.2.0 (2013-10-25) 9 | ------------------ 10 | * Initial catkinization 11 | 12 | 0.1.2 (2013-01-10) 13 | ------------------ 14 | -------------------------------------------------------------------------------- /nao_behaviour_ros/setup.py: -------------------------------------------------------------------------------- 1 | ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | setup_args = generate_distutils_setup( 7 | packages=['nao_behaviour'], 8 | package_dir={'': 'src'} 9 | ) 10 | 11 | setup(**setup_args) -------------------------------------------------------------------------------- /nao_walk_naoqi/qiproject.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 7 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /humanoid_msgs/humanoid_nav_msgs/msg/StepTarget.msg: -------------------------------------------------------------------------------- 1 | # Target for a single stepping motion of a humanoid's leg 2 | 3 | geometry_msgs/Pose2D pose # step pose as relative offset to last leg 4 | uint8 leg # which leg to use (left/right, see below) 5 | 6 | uint8 right=0 # right leg constant 7 | uint8 left=1 # left leg constant 8 | -------------------------------------------------------------------------------- /nao_walk_ros/include/nao_walk/nao_walk_engine.h: -------------------------------------------------------------------------------- 1 | #ifndef NAO_WALK_ENGINE_H 2 | #define NAO_WALK_ENGINE_H 3 | 4 | #include "socket_client.h" 5 | #include "data_msg.h" 6 | 7 | class NaoWalkEngine 8 | { 9 | public: 10 | NaoWalkEngine(std::string ip,int port); 11 | 12 | int readOdom(odom_t *odom); 13 | 14 | private: 15 | }; 16 | #endif 17 | -------------------------------------------------------------------------------- /nao_walk_ros/launch/nao_robot_state_publisher.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /nao_walk_naoqi/Logs.h: -------------------------------------------------------------------------------- 1 | #ifndef LOGS_H 2 | #define LOGS_H 3 | 4 | #define LOG_ERR(...) { fprintf(stderr,__VA_ARGS__); fprintf(stderr,"\n"); } 5 | #define LOG_INF(...) { fprintf(stdout,__VA_ARGS__) ; fprintf(stdout,"\n"); } 6 | 7 | #ifdef DEBUG 8 | #define LOG_DBG(...) { fprintf(stderr,__VA_ARGS__) ; fprintf(stderr,"\n"); } 9 | #else 10 | #define LOG_DBG(...) 11 | #endif 12 | #endif 13 | -------------------------------------------------------------------------------- /nao_walk_ros/action/behaviour.action: -------------------------------------------------------------------------------- 1 | # CROUCH = 0 2 | # WAKE_UP = 1 3 | # MAKARENA = 2 4 | # BALL, 5 | # DANCE_EVOL, 6 | # GANGNAM_ST, 7 | # VANGELIS, 8 | # TIRED, 9 | # HELLO, 10 | # TAICHI, 11 | # EYE_OF_TIGER, 12 | 13 | # Define the goal 14 | int32 behaviour_id 15 | --- 16 | # Define the result 17 | int32 status 18 | --- 19 | # Define a feedback message 20 | float32 percent_complete 21 | -------------------------------------------------------------------------------- /humanoid_msgs/humanoid_nav_msgs/srv/PlanFootstepsBetweenFeet.srv: -------------------------------------------------------------------------------- 1 | humanoid_nav_msgs/StepTarget start_left 2 | humanoid_nav_msgs/StepTarget start_right 3 | humanoid_nav_msgs/StepTarget goal_left 4 | humanoid_nav_msgs/StepTarget goal_right 5 | float64 planning_time 6 | bool until_first_solution 7 | float64 initial_eps 8 | --- 9 | bool result 10 | humanoid_nav_msgs/StepTarget[] footsteps 11 | float64 costs 12 | float64 final_eps 13 | float64 planning_time 14 | int64 expanded_states 15 | -------------------------------------------------------------------------------- /humanoid_msgs/humanoid_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | humanoid_msgs 3 | 0.3.0 4 | 5 | Messages and services for humanoid robots 6 | 7 | Armin Hornung 8 | BSD 9 | 10 | http://www.ros.org/wiki/humanoid_msgs 11 | https://github.com/ahornung/humanoid_msgs/issues 12 | https://github.com/ahornung/humanoid_msgs 13 | 14 | catkin 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /nao_walk_naoqi/LeakyIntegrator.cpp: -------------------------------------------------------------------------------- 1 | #include "LeakyIntegrator.h" 2 | 3 | 4 | LeakyIntegrator::LeakyIntegrator() 5 | { 6 | integral_ = 0.0; 7 | rate_ = 0.1; 8 | saturation_ = -1.; 9 | } 10 | void LeakyIntegrator::add(const double & value, double dt) 11 | { 12 | integral_ = (1. - rate_ * dt) * integral_ + dt * value; 13 | if (saturation_ > 0.) 14 | { 15 | saturate(); 16 | } 17 | } 18 | 19 | void LeakyIntegrator::saturate() 20 | { 21 | if (integral_ < -saturation_) 22 | { 23 | integral_ = -saturation_; 24 | } 25 | else if (integral_ > saturation_) 26 | { 27 | integral_ = saturation_; 28 | } 29 | } 30 | 31 | -------------------------------------------------------------------------------- /nao_walk_naoqi/butterworthLPF.h: -------------------------------------------------------------------------------- 1 | #ifndef __butLPF_H__ 2 | #define __butLPF_H__ 3 | #include "KMat.hpp" 4 | 5 | #include 6 | #include 7 | using namespace std; 8 | class butterworthLPF 9 | { 10 | 11 | private: 12 | float x_p, x_pp, y_p,y_pp; 13 | float a1, a2, b0, b1, b2, ff, ita, q, a; 14 | float fx, fs; 15 | int i; 16 | 17 | public: 18 | string name; 19 | void reset(); 20 | 21 | /** @fn void filter(float y) 22 | * @brief filters the measurement with a 2nd order Butterworth filter 23 | */ 24 | float filter(float y); 25 | 26 | butterworthLPF(); 27 | void init(string name_ ,float fsampling, float fcutoff); 28 | 29 | }; 30 | #endif 31 | -------------------------------------------------------------------------------- /nao_walk_naoqi/butterworthHPF.h: -------------------------------------------------------------------------------- 1 | #ifndef __butHPF_H__ 2 | #define __butHPF_H__ 3 | 4 | 5 | #include 6 | #include 7 | #include 8 | using namespace std; 9 | 10 | class butterworthHPF 11 | { 12 | 13 | private: 14 | double x_p, x_pp, y_p,y_pp; 15 | double a1, a2, b0, b1, b2, ff, ita, q, a; 16 | double fx, fs; 17 | int i; 18 | 19 | public: 20 | string name; 21 | void reset(); 22 | 23 | /** @fn void filter(double y) 24 | * @brief filters the measurement with a 2nd order Butterworth filter 25 | */ 26 | double filter(double y); 27 | 28 | butterworthHPF(); 29 | void init(string name_ ,double fsampling, double fcutoff); 30 | 31 | }; 32 | #endif -------------------------------------------------------------------------------- /nao_behaviour_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(nao_behaviour) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | std_msgs 6 | actionlib 7 | rospy 8 | message_generation 9 | ) 10 | 11 | add_service_files( 12 | FILES 13 | Default.srv 14 | sayText.srv 15 | ) 16 | 17 | catkin_python_setup() 18 | generate_messages( 19 | DEPENDENCIES 20 | std_msgs 21 | ) 22 | 23 | catkin_package( 24 | CATKIN_DEPENDS 25 | actionlib_msgs 26 | actionlib 27 | ) 28 | 29 | set( nodes 30 | src/nao_behaviour/nao_behaviour_node 31 | src/nao_behaviour/nao_speek_node 32 | ) 33 | 34 | catkin_install_python(PROGRAMS 35 | ${nodes} 36 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 37 | ) 38 | -------------------------------------------------------------------------------- /nao_walk_naoqi/JointSSKF.h: -------------------------------------------------------------------------------- 1 | #ifndef __JOINTSSKF_H__ 2 | #define __JOINTSSKF_H__ 3 | #include "KMat.hpp" 4 | #include "RobotParameters.h" 5 | class JointSSKF 6 | { 7 | 8 | private: 9 | KMath::KMat::GenMatrix K; 10 | KMath::KMat::GenMatrix F; 11 | KMath::KMat::GenMatrix x; 12 | RobotParameters NaoRobot; 13 | bool firstrun; 14 | public: 15 | float JointPosition; 16 | float JointVelocity; 17 | 18 | /** @fn void Filter(float JointPosMeasurement); 19 | * @brief filters the Joint Position using the measurement by the encoders 20 | */ 21 | 22 | void Filter(float JointPosMeasurement); 23 | void reset(); 24 | void initialize(RobotParameters &robot); 25 | 26 | }; 27 | #endif 28 | -------------------------------------------------------------------------------- /nao_walk_naoqi/ZMPFilter.h: -------------------------------------------------------------------------------- 1 | #ifndef __ZMPFILTER_H_ 2 | #define __ZMPFILTER_H_ 3 | #include 4 | #include "RobotParameters.h" 5 | #include 6 | using namespace Eigen; 7 | using namespace std; 8 | 9 | class ZMPFilter 10 | { 11 | 12 | private: 13 | float alpha, gravity, m; 14 | Matrix3f Ib; 15 | RobotParameters NaoRobot; 16 | public: 17 | void reset(); 18 | 19 | /** @fn void filter(Vector3f cop, Vector3f CoM, Vector3f Acc, Vector3f Gyro, Vector3f Gyrodot) 20 | * @brief filters the measurement with an average filter 21 | */ 22 | ZMPFilter(RobotParameters &robot); 23 | Vector3f filter(Vector3f cop, Vector3f CoM, Vector3f Acc, Vector3f Gyro, Vector3f Gyrodot, Matrix3f Rotwb); 24 | }; 25 | #endif -------------------------------------------------------------------------------- /nao_walk_naoqi/Array.cc: -------------------------------------------------------------------------------- 1 | // $Id: Array.cc 201 2008-05-18 19:47:38Z digasper $ 2 | // This file is part of QuadProg++: 3 | // Copyright (C) 2006--2009 Luca Di Gaspero. 4 | // 5 | // This software may be modified and distributed under the terms 6 | // of the MIT license. See the LICENSE file for details. 7 | 8 | #include "Array.hh" 9 | 10 | /** 11 | Index utilities 12 | */ 13 | 14 | namespace quadprogpp { 15 | 16 | std::set seq(unsigned int s, unsigned int e) 17 | { 18 | std::set tmp; 19 | for (unsigned int i = s; i <= e; i++) 20 | tmp.insert(i); 21 | 22 | return tmp; 23 | } 24 | 25 | std::set singleton(unsigned int i) 26 | { 27 | std::set tmp; 28 | tmp.insert(i); 29 | 30 | return tmp; 31 | } 32 | 33 | } // namespace quadprogpp 34 | -------------------------------------------------------------------------------- /nao_walk_naoqi/SocketServer.h: -------------------------------------------------------------------------------- 1 | #ifndef SOCKET_SERVER_H 2 | #define SOCKET_SERVER_H 3 | #include 4 | #include 5 | #include 6 | #include "DcmData.h" 7 | 8 | class SocketServer 9 | { 10 | public: 11 | SocketServer(); 12 | ~SocketServer(); 13 | int socInit(int port); 14 | int socListen(); 15 | int socAccept(); 16 | bool hasClient() const 17 | { 18 | return client_conn>0; 19 | } 20 | 21 | void socClose(); 22 | int sendToSoc(char *buf,size_t size); 23 | int receive(char *buf,size_t size); 24 | int receiveCommand(command_t &com); 25 | private: 26 | int server_fd; 27 | int client_conn; 28 | struct sockaddr_in address; 29 | }; 30 | #endif -------------------------------------------------------------------------------- /humanoid_msgs/humanoid_nav_msgs/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package humanoid_nav_msgs 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.0 (2014-01-16) 6 | ------------------ 7 | * Add service to (re)plan between feet as start and goal. 8 | * Contributors: Armin Hornung 9 | 10 | 0.2.0 (2013-10-25) 11 | ------------------ 12 | * Initial catkinization 13 | 14 | 0.1.2 (2013-01-10) 15 | ------------------ 16 | * spelling mistake corrected 17 | * added more details to PlanFootsteps srv result 18 | * action ExecFootsteps can now feedback changeable_footsteps and robot_pose (see naoqi docu for further info) 19 | * integrated a new action to communicate with the action server provided by nao_footsteps.py in the nao_driver package 20 | * service to clip footsteps 21 | * moved humanoid_nav_msgs into new humanoid_msgs stack 22 | -------------------------------------------------------------------------------- /nao_walk_naoqi/FootPolygon.h: -------------------------------------------------------------------------------- 1 | #ifndef __FOOTPOLYGON_H__ 2 | #define __FOOTPOLYGON_H__ 3 | 4 | #include "RobotParameters.h" 5 | #include 6 | #include 7 | using namespace Eigen; 8 | using namespace std; 9 | 10 | 11 | class FootPolygon 12 | { 13 | 14 | private: 15 | RobotParameters robot; 16 | float stepXF, stepXH, stepYL, stepYR; 17 | public: 18 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 19 | Vector3f footLF, footRF, footLH, footRH, footA; 20 | Matrix polygon; 21 | int numNodes; 22 | FootPolygon(RobotParameters& robot_); 23 | void setPolygon(Vector3f footA_, Affine3f T); 24 | bool checkIn(float x_, float y_); 25 | bool pnpoly(float x_, float y_); 26 | float getAngle(float x1, float y1, float x2, float y2); 27 | 28 | }; 29 | 30 | 31 | #endif -------------------------------------------------------------------------------- /nao_walk_naoqi/CoMAdmittanceControl.h: -------------------------------------------------------------------------------- 1 | #ifndef __COMADMITTANCECONTROL__ 2 | #define __COMADMITTANCECONTROL__ 3 | #include "RobotParameters.h" 4 | #include 5 | #include 6 | #include "LeakyIntegrator.h" 7 | using namespace Eigen; 8 | using namespace std; 9 | 10 | class CoMAdmittanceControl 11 | { 12 | private: 13 | 14 | RobotParameters NaoRobot; 15 | LeakyIntegrator ZMPXint, ZMPPXint,ZMPYint, ZMPPYint; 16 | Matrix2d k_f; 17 | Vector2d int_zmp, int_zmpp; 18 | double rateX, rateY; 19 | bool firstrun; 20 | 21 | public: 22 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 23 | 24 | double gain_x, gain_y; 25 | 26 | CoMAdmittanceControl(RobotParameters &robot); 27 | 28 | 29 | Vector2d com_c; 30 | Vector2d Control(Vector2d com_d,Vector2d comd_d,Vector2d vrp_d,Vector2d vrp,Vector2d com); 31 | 32 | }; 33 | 34 | 35 | #endif 36 | -------------------------------------------------------------------------------- /nao_walk_ros/include/nao_walk/socket_client.h: -------------------------------------------------------------------------------- 1 | #ifndef SOCKET_CLIENT_H 2 | #define SOCKET_CLIENT_H 3 | #include 4 | #include 5 | #include 6 | 7 | #include "dcm_data.h" 8 | 9 | class SocketClient 10 | { 11 | public: 12 | SocketClient(); 13 | ~SocketClient(); 14 | int socketConnect(std::string hostname,int port); 15 | static int hostname_to_ip(char const* hostname , char* ip); 16 | bool isConnected() const 17 | { 18 | return _isConnected; 19 | } 20 | void disconnect(); 21 | int receive(char *data,size_t size); 22 | int sendToSoc(char *buf,size_t size); 23 | int sendCommand(command_t &com); 24 | 25 | private: 26 | struct sockaddr_in server; 27 | int _sock; 28 | bool _isConnected; 29 | 30 | 31 | }; 32 | 33 | #endif 34 | -------------------------------------------------------------------------------- /nao_walk_ros/launch/gait_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /nao_walk_naoqi/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * main.cpp 3 | * 4 | * Created on: Jul 30, 2015 5 | * Author: master 6 | */ 7 | 8 | 9 | 10 | #include "LowLevelPlanner.h" 11 | 12 | 13 | 14 | #ifdef _WIN32 15 | # define ALCALL __declspec(dllexport) 16 | #else 17 | # define ALCALL 18 | #endif 19 | 20 | extern "C" 21 | { 22 | ALCALL int _createModule(boost::shared_ptr pBroker) 23 | { 24 | // init broker with the main broker instance 25 | // from the parent executable 26 | AL::ALBrokerManager::setInstance(pBroker->fBrokerManager.lock()); 27 | AL::ALBrokerManager::getInstance()->addBroker(pBroker); 28 | 29 | // create module instances 30 | AL::ALModule::createModule(pBroker,"LowLevelPlanner" ); 31 | return 0; 32 | } 33 | 34 | ALCALL int _closeModule() 35 | { 36 | return 0; 37 | } 38 | 39 | } // extern "C" 40 | -------------------------------------------------------------------------------- /nao_walk_ros/nodes/server.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | void callback(nao_walk::GaitControlConfig &config, uint32_t level) { 7 | ROS_INFO("Reconfigure Request: %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f", 8 | config.Observer_COPX, config.Observer_COPY, config.Observer_CoMX,config.Observer_CoMY, config.COP_NoiseX, config.COP_NoiseY, config.CoM_NoiseX, config.CoM_NoiseY, 9 | config.Kp_PitchT, config.Kd_PitchT, config.Kp_RollT, config.Kd_RollT, config.kfx, config.kfy, config.StepHeight); 10 | 11 | } 12 | 13 | int main(int argc, char **argv) { 14 | ros::init(argc, argv, "nao_walk_dynamic_configure"); 15 | 16 | dynamic_reconfigure::Server server; 17 | dynamic_reconfigure::Server::CallbackType f; 18 | 19 | f = boost::bind(&callback, _1, _2); 20 | server.setCallback(f); 21 | ros::spin(); 22 | return 0; 23 | } -------------------------------------------------------------------------------- /humanoid_msgs/humanoid_nav_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(humanoid_nav_msgs) 3 | 4 | #List to make rest of code more readable 5 | set( MESSAGE_DEPENDENCIES std_msgs geometry_msgs actionlib_msgs) 6 | 7 | #Declare build dependencies 8 | find_package(catkin REQUIRED 9 | COMPONENTS 10 | message_generation 11 | ${MESSAGE_DEPENDENCIES} ) 12 | 13 | #Add message files 14 | add_message_files(DIRECTORY msg FILES StepTarget.msg) 15 | 16 | #Add service files 17 | add_service_files(DIRECTORY srv 18 | FILES 19 | ClipFootstep.srv 20 | PlanFootsteps.srv 21 | PlanFootstepsBetweenFeet.srv 22 | StepTargetService.srv ) 23 | 24 | #Add action files 25 | add_action_files(DIRECTORY action 26 | FILES 27 | ExecFootsteps.action) 28 | 29 | #And now generate the messages 30 | generate_messages(DEPENDENCIES ${MESSAGE_DEPENDENCIES}) 31 | 32 | # Generate catkin/pkg-config import information 33 | catkin_package( CATKIN_DEPENDS message_runtime ${MESSAGE_DEPENDENCIES} ) 34 | 35 | -------------------------------------------------------------------------------- /nao_walk_naoqi/Kalman.h: -------------------------------------------------------------------------------- 1 | #ifndef __KALMAN_H__ 2 | #define __KALMAN_H__ 3 | #include "KMat.hpp" 4 | #include "RobotParameters.h" 5 | #include 6 | #define ZMPKALMANDELAY 5 7 | class Kalman 8 | { 9 | 10 | private: 11 | KMath::KMat::GenMatrix Ckalman; 12 | KMath::KMat::GenMatrix Kgain; 13 | KMath::KMat::GenMatrix s,MeasurementNoise; 14 | KMath::KMat::GenMatrix P, ProcessNoise,Bkalman; 15 | RobotParameters &OurRobot; 16 | public: 17 | KMath::KMat::GenMatrix StateKalman,StatePredict; 18 | std::queue uBuffer,combuffer; 19 | KMath::KMat::GenMatrix ykalman; 20 | 21 | /** @fn void Filter(float ZMPMeasured,float CoMMeasured) 22 | * @brief filters the ZMP measurement from the CoP using 23 | * also the COM measured by the encoders 24 | */ 25 | void Filter(float ZMPMeasured,float CoMMeasured); 26 | void reset(); 27 | Kalman(RobotParameters &robot); 28 | double CoM_Noise, COP_Noise; 29 | }; 30 | #endif 31 | -------------------------------------------------------------------------------- /nao_walk_naoqi/MotionDefines.h: -------------------------------------------------------------------------------- 1 | #ifndef __MOTIONDEFINES_H__ 2 | #define __MOTIONDEFINES_H__ 3 | #include "KMat.hpp" 4 | #include "robot_consts.h" 5 | #include "Common.hpp" 6 | 7 | enum { 8 | LeftFoot=0, RightFoot 9 | }; 10 | 11 | enum { 12 | double_support=0, single_support 13 | }; 14 | 15 | enum { 16 | WALK=0, STAND 17 | }; 18 | 19 | /** A class with the necessary data 20 | * for the walking procedure 21 | **/ 22 | class WalkInstruction 23 | { 24 | public: 25 | KVecFloat3 target; //2D swing foot pose 26 | KDeviceLists::SupportLeg targetSupport, targetZMP; //which IS the support foot in this instruction 27 | unsigned steps; //walk instruction steps 28 | bool phase; //DS or SS; 29 | int step_id; 30 | }; 31 | 32 | class SupportInstruction 33 | { 34 | public: 35 | KVecFloat3 pose; //2D swing foot pose 36 | KDeviceLists::SupportLeg targetSupport, targetZMP; //which IS the support foot in this instruction 37 | int step_id; 38 | }; 39 | 40 | 41 | 42 | 43 | #endif 44 | -------------------------------------------------------------------------------- /humanoid_msgs/humanoid_nav_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | humanoid_nav_msgs 3 | 0.3.0 4 | 5 | Messages and services for humanoid robot navigation 6 | 7 | Armin Hornung 8 | BSD 9 | 10 | http://ros.org/wiki/humanoid_nav_msgs 11 | https://github.com/ahornung/humanoid_msgs/issues 12 | https://github.com/ahornung/humanoid_msgs 13 | Armin Hornung 14 | 15 | catkin 16 | 17 | message_generation 18 | std_msgs 19 | geometry_msgs 20 | actionlib_msgs 21 | 22 | message_runtime 23 | std_msgs 24 | geometry_msgs 25 | actionlib_msgs 26 | 27 | -------------------------------------------------------------------------------- /nao_walk_ros/src/joint_states_table.cpp: -------------------------------------------------------------------------------- 1 | #include"nao_walk/joint_states_table.h" 2 | 3 | const std::vectorjoint_state_names 4 | { 5 | std::string("HeadYaw"), 6 | std::string("HeadPitch"), 7 | std::string("LShoulderPitch"), 8 | std::string("LShoulderRoll"), 9 | std::string("LElbowYaw"), 10 | std::string("LElbowRoll"), 11 | std::string("LWristYaw"), 12 | std::string("LHand"), 13 | std::string("LHipYawPitch"), 14 | std::string("LHipRoll"), 15 | std::string("LHipPitch"), 16 | std::string("LKneePitch"), 17 | std::string("LAnklePitch"), 18 | std::string("LAnkleRoll"), 19 | std::string("RHipYawPitch"), 20 | std::string("RHipRoll"), 21 | std::string("RHipPitch"), 22 | std::string("RKneePitch"), 23 | std::string("RAnklePitch"), 24 | std::string("RAnkleRoll"), 25 | std::string("RShoulderPitch"), 26 | std::string("RShoulderRoll"), 27 | std::string("RElbowYaw"), 28 | std::string("RElbowRoll"), 29 | std::string("RWristYaw"), 30 | std::string("RHand") 31 | }; 32 | -------------------------------------------------------------------------------- /nao_walk_naoqi/StepAdjustment.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef __STEPADJUSTMENT__ 3 | #define __STEPADJUSTMENT__ 4 | #include "RobotParameters.h" 5 | #include 6 | #include 7 | #include "QuadProg++.hh" 8 | #include 9 | #include "KMat.hpp" 10 | using namespace std; 11 | 12 | 13 | class StepAdjustment 14 | { 15 | private: 16 | RobotParameters &robot; 17 | quadprogpp::Matrix G, CE, CI; 18 | quadprogpp::Vector g0, ce0, ci0, x; 19 | KMath::KMat::GenMatrix rot, rot_T; 20 | KVecDouble2 L_max, L_min, b_nom, L_nom, tempV; 21 | int n, m, p, axis; 22 | double dt,w, T_max, T_min, T_nom, tau_min, tau_max, tau_nom, lp, bx_nom, by_nom, Lx_max, Ly_max, Lx_min, Ly_min; 23 | double a_0, a_1, a_2, a_3, a_4, a_5; 24 | double a_11, a_12, a_21, a_22; 25 | public: 26 | 27 | float step_locationx, step_locationy, step_instructions, step_duration, step_bx, step_by; 28 | StepAdjustment(RobotParameters &robot_); 29 | void solve(double ksix_0, double ksiy_0, double copx_0, double copy_0, double vrpx_0, double vrpy_0, double vrpx_ref, double vrpy_ref, double support_orientation, int RSS); 30 | }; 31 | #endif 32 | -------------------------------------------------------------------------------- /nao_behaviour_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | nao_behaviour 4 | 0.0.1 5 | The nao_behaviour package 6 | 7 | 8 | tavu 9 | BSD 10 | catkin 11 | actionlib 12 | 13 | actionlib_msgs 14 | message_generation 15 | message_runtime 16 | actionlib 17 | actionlib_msgs 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | rospy 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /nao_walk_naoqi/Stepplanner2D.h: -------------------------------------------------------------------------------- 1 | /*! \file StepPlanner.h 2 | * \brief A Monas Activity that first plans the trajectories needed by the Walk Engine 3 | and executes the desired walking gait! 4 | * 5 | */ 6 | 7 | #ifndef _STEPPLANNER2D_H 8 | #define _STEPPLANNER2D_H 9 | 10 | #include "RobotParameters.h" 11 | #include "MotionDefines.h" 12 | #include 13 | 14 | using namespace std; 15 | 16 | class Stepplanner2D 17 | { 18 | private: 19 | KVecFloat3 v_; 20 | KVecFloat2 targetXY, pivotXY, centerXY, c, h, tempV, MaxStep, MinStep; 21 | KMath::KMat::GenMatrix rot; 22 | int cmd, step_id; 23 | float eps, support_foot_x, support_foot_y, support_foot_orientation, targetTheta; 24 | RobotParameters robot; 25 | public: 26 | Stepplanner2D(RobotParameters robot_); 27 | WalkInstruction planStep2D(KVecFloat3 v, WalkInstruction si); 28 | bool planAvailable; 29 | void emptyPlan(); 30 | void plan(WalkInstruction si); 31 | boost::circular_buffer stepAnkleQ; 32 | boost::circular_buffer velocityQ; 33 | float cropStep(float f_, float max_, float min_) 34 | { 35 | return max(min_, min(f_, max_)); 36 | } 37 | }; 38 | #endif 39 | 40 | -------------------------------------------------------------------------------- /nao_walk_naoqi/DelayedObserverDCM.h: -------------------------------------------------------------------------------- 1 | #ifndef __DELAYEDOBSERVERDCM_H__ 2 | #define __DELAYEDOBSERVERDCM_H__ 3 | 4 | #include 5 | #include 6 | #include "RobotParameters.h" 7 | #include 8 | 9 | #define ZMPDELAY 3 10 | using namespace Eigen; 11 | using namespace std; 12 | 13 | typedef Matrix Matrix4_1f; 14 | typedef Matrix Matrix4_2f; 15 | typedef Matrix Matrix1_4f; 16 | class DelayedObserverDCM 17 | { 18 | 19 | private: 20 | RobotParameters &NaoRobot; 21 | Matrix4f A, I; 22 | Matrix1_4f Ccom, Czmp; 23 | Matrix4_1f B, Lcom; 24 | Matrix4_2f L; 25 | //std::queue xbuffer; 26 | std::queue xbuffer; 27 | 28 | Vector4f x; 29 | void updateVars(); 30 | 31 | public: 32 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 33 | /** @fn void Filter(float ZMPMeasured,float CoMMeasured) 34 | * @brief filters the ZMP measurement from the CoP using 35 | * also the COM measured by the encoders 36 | */ 37 | bool firstrun; 38 | void setInitialState(Vector4f x_); 39 | void update(float u_, float com_, float zmp_); 40 | DelayedObserverDCM(RobotParameters &robot); 41 | float com, vrp, dcm, dist; 42 | Vector4f getState(); 43 | }; 44 | #endif 45 | -------------------------------------------------------------------------------- /nao_behaviour_ros/src/nao_behaviour/nao_speek_node: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import roslib 3 | roslib.load_manifest('nao_behaviour') 4 | import rospy 5 | import actionlib 6 | 7 | #from ammar_bridge.msg import speekAction, speekGoal 8 | import sys 9 | 10 | from nao_walk.msg import speekAction, speekGoal 11 | 12 | rospy.init_node('nao_speek') 13 | client = actionlib.SimpleActionClient('/speed_execution', speekAction) 14 | client.wait_for_server() 15 | 16 | #client2 = actionlib.SimpleActionClient('/ammar_bridge_write/speek_server', speekAction) 17 | #print 'client started' 18 | 19 | #f=True 20 | #rate = rospy.Rate(10) 21 | while not rospy.is_shutdown(): 22 | try: 23 | text = raw_input("Say: ") 24 | except EOFError: 25 | client.wait_for_result() 26 | print('\n') 27 | quit() 28 | #print text 29 | goal = speekGoal() 30 | goal.text = text 31 | client.send_goal(goal) 32 | #rate.sleep() 33 | #client.wait_for_result(rospy.Duration.from_sec(5.0)) 34 | client.wait_for_result() 35 | #print text 36 | #goal.data = text 37 | #if f: 38 | #client.send_goal(goal) 39 | #else: 40 | #client2.send_goal(goal) 41 | #f = not f 42 | #client.wait_for_result(rospy.Duration.from_sec(5.0)) 43 | 44 | rospy.spin() -------------------------------------------------------------------------------- /nao_walk_naoqi/LeakyIntegrator.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | class LeakyIntegrator 4 | { 5 | public: 6 | /** Add constant input for a fixed duration. 7 | * 8 | * \param value Constant input. 9 | * 10 | * \param dt Fixed duration. 11 | * 12 | */ 13 | void add(const double & value, double dt); 14 | 15 | /** Evaluate the output of the integrator. 16 | * 17 | */ 18 | const double & eval() const 19 | { 20 | return integral_; 21 | } 22 | 23 | LeakyIntegrator(); 24 | /** Get leak rate. 25 | * 26 | */ 27 | inline double rate() const 28 | { 29 | return rate_; 30 | } 31 | 32 | /** Set the leak rate of the integrator. 33 | * 34 | * \param rate New leak rate. 35 | * 36 | */ 37 | inline void rate(double rate) 38 | { 39 | rate_ = rate; 40 | } 41 | 42 | /** Set output saturation. Disable by providing a negative value. 43 | * 44 | * \param s Output will saturate between -s and +s. 45 | * 46 | */ 47 | inline void saturation(double s) 48 | { 49 | saturation_ = s; 50 | } 51 | 52 | /** Reset integral to zero. 53 | * 54 | */ 55 | inline void setZero() 56 | { 57 | integral_=0.0; 58 | } 59 | 60 | private: 61 | void saturate(); 62 | double integral_; 63 | double rate_; 64 | double saturation_; 65 | }; 66 | -------------------------------------------------------------------------------- /nao_walk_ros/src/gait_control.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "nao_walk/nao_walk_ros.h" 3 | using std::string; 4 | using std::cerr; 5 | using std::endl; 6 | 7 | int main( int argc, char** argv ) 8 | { 9 | int pport; 10 | string pip; 11 | // A broker needs a name, an IP and a port: 12 | std::string broker_name = "Nao Walk ROS Wrapper Broker"; // FIXME: would be a good idea to look for a free port first 13 | int broker_port; 14 | // listen port of the broker (here an anything) 15 | string broker_ip; 16 | ros::init(argc, argv, "nao_walk"); 17 | ros::NodeHandle nh("~"); //For parameter server 18 | if(!ros::master::check()) 19 | { 20 | cerr<<"Could not contact master!\nQuitting... "<initialize(nh); 33 | // Run Nao Driver Loop 34 | nao_walk->run(); 35 | //Done here 36 | // AL::ALBrokerManager::getInstance()->killAllBroker(); 37 | // AL::ALBrokerManager::kill(); 38 | ROS_INFO( "Quitting... " ); 39 | 40 | } -------------------------------------------------------------------------------- /nao_walk_ros/cfg/GaitControl.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "nao_walk" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | gen.add("Observer_COPX", double_t, 0, "Observer_COPX ", 0.15, 0.00, 10.0) 8 | gen.add("Observer_COPY", double_t, 0, "Observer_COPY", 0.15, 0.00, 10.0) 9 | gen.add("Observer_CoMX",double_t, 0, "Observer_CoMX ", 0.65, 0, 10.0) 10 | gen.add("Observer_CoMY",double_t, 0, "Observer_CoMY ", 0.65, 0, 10.0) 11 | gen.add("COP_NoiseX",double_t, 0, "COP_NoiseX", 0.2, 0, 100.0) 12 | gen.add("COP_NoiseY",double_t, 0, "COP_NoiseY", 0.2, 0, 50.0) 13 | gen.add("CoM_NoiseX",double_t, 0, "CoM_NoiseX", 0.01, 0, 100.0) 14 | gen.add("CoM_NoiseY",double_t, 0, "CoM_NoiseY", 0.01, 0, 100.0) 15 | gen.add("Kp_PitchT",double_t, 0, "Kp_PitchT", 0.1, 0, 50.0) 16 | gen.add("Kd_PitchT",double_t, 0, "Kd_PitchT", 1.0, 0, 3.0) 17 | gen.add("Kp_RollT",double_t, 0, "Kp_RollT", 1.0, 0, 3.0) 18 | gen.add("Kd_RollT",double_t, 0, "Kd_RollT", 1.0, 0, 3.0) 19 | gen.add("kfx",double_t, 0, "ZMP Force Control X", 2.2, 0, 20.0) 20 | gen.add("kfy",double_t, 0, "ZMP Force Control Y", 4.2, 0, 20.0) 21 | gen.add("StepHeight",double_t, 0, "StepHeight", 0.016, 0.015, 0.0175) 22 | exit(gen.generate(PACKAGE, "nao_walk", "GaitControl")) 23 | -------------------------------------------------------------------------------- /nao_walk_ros/cfg/GaitControl.cfg.bak: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "nao_walk" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | gen.add("COP_Noise", double_t, 0, "Observer COPX ", 4.0, 0.00, 10.0) 8 | gen.add("CoM_Noise", double_t, 0, "Observer COPY", 4.0, 0.00, 10.0) 9 | gen.add("Observer_CoM",double_t, 0, "Observer CoMX ", 1.3, 0, 10.0) 10 | gen.add("Observer_COP",double_t, 0, "Observer CoMY ", 1.3, 0, 10.0) 11 | gen.add("Kp_Pitch",double_t, 0, "Ankle Pitch P", 0., 0, 1.0) 12 | gen.add("Kd_Pitch",double_t, 0, "Ankle Pitch D", 0., 0, 5.0) 13 | gen.add("Kp_Roll",double_t, 0, "Ankle Roll P", 0., 0, 1.0) 14 | gen.add("Kd_Roll",double_t, 0, "Ankle Roll D", 0., 0, 5.0) 15 | gen.add("amX",double_t, 0, "Angular Momentum X", 1.0, 0, 3.0) 16 | gen.add("amY",double_t, 0, "Angular Momentum Y", 1.0, 0, 3.0) 17 | gen.add("StepHeight",double_t, 0, "Max Step Height", 0.0165, 0.014, 0.02) 18 | gen.add("kfx",double_t, 0, "ZMP Force Control X", 0.0, 0, 7.0) 19 | gen.add("kfy",double_t, 0, "ZMP Force Control Y", 0.0, 0, 7.0) 20 | gen.add("kcx",double_t, 0, "CoM Force Control X", 0.000, 0.0, 15.0) 21 | gen.add("kcy",double_t, 0, "CoM Force Control Y", 0.000, 0.0, 15.0) 22 | exit(gen.generate(PACKAGE, "nao_walk", "GaitControl")) 23 | -------------------------------------------------------------------------------- /nao_walk_naoqi/MPCDCM.h: -------------------------------------------------------------------------------- 1 | #ifndef __MPCDCM_H__ 2 | #define __MPCDCM_H__ 3 | #include "RobotParameters.h" 4 | #include 5 | #include 6 | #include "KMat.hpp" 7 | #include 8 | #include "DelayedObserverDCM.h" 9 | #define Np 102 10 | using namespace Eigen; 11 | using namespace std; 12 | class MPCDCM 13 | { 14 | private: 15 | 16 | 17 | RobotParameters &NaoRobot; 18 | 19 | 20 | DelayedObserverDCM dObsDCMx, dObsDCMy; 21 | 22 | float du_x, du_y, qx, qv, u_x, u_y; 23 | Vector2f DCM_, VRP_; 24 | Vector4f x,y,x_,y_; 25 | Vector3f xe, ye; 26 | MatrixXf Fx, Fv, Fxu, Fvu, R, Qx, Qv, K_X, K_V,H , tmpb, H_inv, Ad, Ae, Cd, A, L; 27 | VectorXf K_v, VRPRefX, VRPRefY, Cksi, Ce, Cx, Be, Bd, temp, K_x, B, C; 28 | 29 | 30 | 31 | public: 32 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 33 | float vrpx_d,comx_d,vrpy_, vrpx_,comdx_d,dcmx_d,dcmdx_d, vrpy_d,comy_d,comdy_d,dcmy_d,dcmdy_d, comddx_d, comddy_d; 34 | bool firstrun; 35 | 36 | MPCDCM(RobotParameters &robot); 37 | 38 | void setInitialState(Vector2f DCM, Vector2f CoM, Vector2f ZMP); 39 | 40 | void Control(boost::circular_buffer & VRPRef, Vector2f DCM, Vector2f CoM, Vector2f ZMP); 41 | 42 | 43 | 44 | }; 45 | #endif 46 | -------------------------------------------------------------------------------- /nao_walk_naoqi/behaviourMap.h: -------------------------------------------------------------------------------- 1 | #ifndef BEHAVIOUR_MAP_H 2 | #define BEHAVIOUR_MAP_H 3 | 4 | #include 5 | #include 6 | namespace Behaviours 7 | { 8 | 9 | class Behaviour 10 | { 11 | public: 12 | Behaviour(std::string beh,std::string desc) :behaviour(beh),description(desc){} 13 | std::string behaviour; 14 | std::string description; 15 | }; 16 | 17 | //TODO use c++11 for static initialization 18 | std::vector create_beh_list() 19 | { 20 | std::vector behL; 21 | behL.push_back( Behaviour("macarena-ef2c97/behavior_1","macarena!") ); 22 | behL.push_back( Behaviour("tracking-d31feb/behavior_1","Ψάχνω το μπαλάκι παιδιά!") ); 23 | behL.push_back( Behaviour("danceevolution-faf871/behavior_1","Ώρα για Thriller!") ); 24 | behL.push_back( Behaviour("gangnam-57eec6/behavior_1","gangnam Style!") ); 25 | behL.push_back( Behaviour("vangelis-6f4014/behavior_1","Πασόκ Σώσε μας Σε παρακαλώ!") ); 26 | behL.push_back( Behaviour("koyrastika-91e979/behavior_1","") ); 27 | behL.push_back( Behaviour("warmhello-4f850e/behavior_1","") ); 28 | behL.push_back( Behaviour("taichi-6066a5/behavior_1","Είμαι το Κung-Fu Πάντα") ); 29 | behL.push_back( Behaviour("eyeofthetiger-84c49a/behavior_1","Είμαι ο Rocky!") ); 30 | 31 | 32 | return behL; 33 | 34 | }; 35 | 36 | const std::vector behaviourL =create_beh_list(); 37 | 38 | } 39 | #endif 40 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # README 2 | The code is open-source (BSD License). Please note that this work is an on-going research and thus some parts are not fully developed yet. Furthermore, the code will be subject to changes in the future which could include greater re-factoring. 3 | 4 | * nao_walk_naoqi: A C++ naoqi module that achieves real-time omni-directional walking. This module needs to be cross-compiles the cross naoqi sdk and uploaded to NAO. 5 | 6 | * nao_walk_ros: A ROS/C++ wrapper that enables real-time communication with NAO without relying on naoqi-sdk. 7 | [![NAO Outdoors Walking](https://img.youtube.com/vi/DD0I0H3wR7c/0.jpg)](https://www.youtube.com/watch?v=DD0I0H3wR7c) 8 | 9 | 10 | 11 | 12 | https://youtu.be/DD0I0H3wR7c 13 | ## Prerequisites 14 | * Ubuntu 16.04 and later 15 | * ROS kinetic and later 16 | * Eigen 3.2.0 and later 17 | 18 | ## Instalation 19 | * git clone https://github.com/mrsp/nao_walk.git in a ROS workspace. 20 | 21 | ## naoqi walk module 22 | * cd nao_walk/nao_walk_naoqi && qibuild init 23 | * qibuild configure --release -c cross-naoqi-sdk 24 | * qibuild make -c cross-naoqi-sdk 25 | * This will generate a naoqi module named "libnao_walkF.so" in: 26 | nao_walk/nao_walk_naoqi/build-cross-naoqi-sdk/sdk/lib/naoqi 27 | * scp libnao_walkF.so nao@nao.local:/home/nao/modules to copy the module onto NAO. 28 | 29 | ## nao walk ROS wrapper 30 | * catkin_make 31 | * roslaunch nao_walk gait_control.launch 32 | * roslaunch teleop_twist_keyboard velocity_cmd.launch (To make the robot walk with the keyboard) 33 | -------------------------------------------------------------------------------- /nao_walk_naoqi/JointSSKF.cpp: -------------------------------------------------------------------------------- 1 | #include "JointSSKF.h" 2 | 3 | 4 | 5 | void JointSSKF::initialize(RobotParameters &robot) 6 | { 7 | 8 | NaoRobot=robot; 9 | 10 | 11 | F.zero(); 12 | F(0,0)=1.000; 13 | F(0,1)=NaoRobot.getWalkParameter(Ts); 14 | F(1,1)=1.000; 15 | x.zero(); 16 | K.zero(); 17 | K(0)=0.4738; 18 | K(1)=0.9176; 19 | 20 | 21 | JointPosition=0.000; 22 | JointVelocity=0.000; 23 | 24 | firstrun=true; 25 | 26 | std::cout<<"Joint Steady-State Kalman Filter Initialized Successfully"< 4 | #include "RobotParameters.h" 5 | #include "FootPolygon.h" 6 | #include 7 | using namespace Eigen; 8 | using namespace std; 9 | 10 | 11 | 12 | class ZMPDistributor 13 | { 14 | 15 | private: 16 | RobotParameters robot; 17 | float maxForceReadingL, maxForceReadingR; 18 | public: 19 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 20 | ZMPDistributor(RobotParameters& robot_); 21 | FootPolygon LFoot, RFoot; 22 | Vector3f frd, fld, fl, fr, tauld, taurd, tau0, taul, taur; 23 | Vector2f pL, pR, pa; 24 | float a; 25 | void computeDistribution(Vector3f pzmp_d, Vector3f pzmp, Vector3f fl, Vector3f fr, Vector3f fext, Vector3f LfootA, Vector3f RfootA, Affine3f Til, Affine3f Tir, bool right_support, bool double_support); 26 | 27 | Vector2f computePointLineIntersection(Vector2f point_, Vector2f linepoint0_, Vector2f linepoint1_) 28 | { 29 | Vector2f res; 30 | res.setZero(); 31 | float dx = linepoint0_(0)-linepoint1_(0); 32 | float dy = linepoint0_(1)-linepoint1_(1); 33 | float mag = sqrt(dx*dx + dy*dy); 34 | dx /= mag; 35 | dy /= mag; 36 | // translate the point and get the dot product 37 | float lambda = (dx * (point_(0) - linepoint1_(0))) + (dy * (point_(1) - linepoint1_(1))); 38 | res(0) = (dx * lambda) + linepoint1_(0); 39 | res(1) = (dy * lambda) + linepoint1_(1); 40 | return res; 41 | } 42 | }; 43 | #endif 44 | -------------------------------------------------------------------------------- /nao_walk_naoqi/FeetEngine.h: -------------------------------------------------------------------------------- 1 | #ifndef __FEETENGINE_H__ 2 | #define __FEETENGINE_H__ 3 | #include "KMat.hpp" 4 | #include "Common.hpp" 5 | #include "KWalkMat.h" 6 | #include "RobotParameters.h" 7 | 8 | 9 | 10 | class FeetEngine 11 | { 12 | 13 | private: 14 | 15 | RobotParameters &NaoRobot; 16 | KWalkMat interp; 17 | 18 | 19 | public: 20 | KMath::KMat::GenMatrix startL, planL, startR, planR; 21 | 22 | //Current Desired FootHold position xytheta 23 | KMath::KMat::GenMatrix FootL, FootR; 24 | float startLz,startRz, planRz, planLz, FootLz, FootRz, StepZ_; 25 | 26 | FeetEngine(RobotParameters &robot); 27 | void reset(); 28 | void default_params(); 29 | void setFootStartXYTheta(KMath::KMat::GenMatrix xytheta, bool isRight); 30 | void setFootDestXYTheta(KMath::KMat::GenMatrix xytheta, bool isRight); 31 | void setFootXYTheta(KMath::KMat::GenMatrix xytheta, bool isRight); 32 | void setFootStartZ(float z, bool isRight); 33 | void setFootDestZ(float z, bool isRight); 34 | void setFootZ(float z, bool isRight); 35 | KVecFloat3 getFootDestXYTheta(bool isRight); 36 | float getFootDestTheta(bool isRight); 37 | float getFootDestZ(bool isRight); 38 | KVecFloat3 getFootXYTheta(bool isRight); 39 | float getFootTheta(bool isRight); 40 | float getFootZ(bool isRight); 41 | void MotionPlan(KVecFloat3 target, unsigned step, unsigned totalsteps, bool right_support, bool double_support, bool RightEarlyContact, bool LeftEarlyContact, bool RightLateContact, bool LeftLateContact); 42 | }; 43 | #endif 44 | -------------------------------------------------------------------------------- /nao_walk_naoqi/matlog.h: -------------------------------------------------------------------------------- 1 | /* 2 | * matlog.h 3 | * 4 | * Created on: Dec 5, 2012 5 | * Author: trs 6 | */ 7 | 8 | #ifndef MATLOG_H_ 9 | #define MATLOG_H_ 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #ifdef MAT_IO 16 | #include "matIO/matio.h" 17 | #include "matIO/matindex.h" 18 | #endif 19 | 20 | #define TIMEBUF 20 //string size to save the date,time 21 | 22 | typedef float num_type; 23 | 24 | using namespace std; 25 | 26 | typedef enum {RAW=0 27 | #ifdef MAT_IO 28 | ,MAT=1 29 | #endif 30 | } SAVETYPE; 31 | 32 | 33 | 34 | class mat_log 35 | { 36 | public: 37 | int log_files_counter; 38 | int counter; 39 | string log_name; 40 | map data; 41 | int autosave_period; 42 | char timebuf[TIMEBUF]; 43 | SAVETYPE type; 44 | ofstream rawfile; 45 | bool more_vars_added; 46 | public: 47 | 48 | mat_log(string base_log_name, SAVETYPE type=RAW, int autosave_period=-1); 49 | ~mat_log(); 50 | /// add a value to data structure 51 | void periodic_save(); 52 | 53 | void initilize_datalog_variable_names(vector); 54 | 55 | void insert(string name, num_type value); 56 | /// add or append a vector of values 57 | // void insert(string name, vector values); 58 | /// erase all the data 59 | void empty_all() 60 | { 61 | data.clear(); 62 | } 63 | #ifdef MAT_IO 64 | int saveMatTo(string filename); 65 | #endif 66 | int saveRawTo(string filename); 67 | 68 | int save(); 69 | }; 70 | 71 | 72 | #endif /* MATLOG_H_ */ 73 | -------------------------------------------------------------------------------- /nao_walk_naoqi/PostureController.h: -------------------------------------------------------------------------------- 1 | #ifndef __POSTURECONTROLLER_H__ 2 | #define __POSTURECONTROLLER_H__ 3 | #include "RobotParameters.h" 4 | #include "KMat.hpp" 5 | #include 6 | 7 | 8 | using namespace Eigen; 9 | 10 | class PostureController 11 | { 12 | private: 13 | 14 | RobotParameters NaoRobot; 15 | 16 | float TH; 17 | void default_params(); 18 | public: 19 | /** Ankle PD Control **/ 20 | float Kc_, Tc_, Ka_, Ta_, Kn_, Tn_, dt; 21 | float dTorso_Roll, dTorso_Pitch, dLAnkle_Roll, dLAnkle_Pitch, dRAnkle_Roll, dRAnkle_Pitch, dKnee_Pitch; 22 | /** AM Control **/ 23 | float amX, amY; 24 | 25 | float lankle_Pitch, rankle_Pitch, lankle_Roll, rankle_Roll; 26 | float lhip_Pitch, rhip_Pitch, lhip_Roll, rhip_Roll; 27 | float lknee_Pitch, rknee_Pitch; 28 | 29 | PostureController(RobotParameters &robot); 30 | 31 | 32 | KVecFloat2 soleAnguralMomentumStabilizer(float Torso_Roll,float comX, float Torso_Pitch,float comY); 33 | 34 | void kneeStabilizer(float flz, float frz, float flz_d, float frz_d, 35 | float lknee_Pitch_d, float rknee_Pitch_d); 36 | void ankleStabilizer(Vector3f tauld, Vector3f taurd, Vector3f taul, Vector3f taur, 37 | float lankle_Pitch_d, float rankle_Pitch_d,float lankle_Roll_d,float rankle_Roll_d); 38 | void torsoStabilizer(float Torso_Roll, float Torso_Pitch, float Torso_Roll_d, float Torso_Pitch_d, 39 | float lhip_Pitch_d, float rhip_Pitch_d,float lhip_Roll_d,float rhip_Roll_d); 40 | }; 41 | #endif 42 | -------------------------------------------------------------------------------- /nao_walk_naoqi/ZMPFilter.cpp: -------------------------------------------------------------------------------- 1 | #include "ZMPFilter.h" 2 | 3 | 4 | ZMPFilter::ZMPFilter(RobotParameters &robot):NaoRobot(robot) 5 | { 6 | Ib.setZero(); 7 | Ib(0,0) = NaoRobot.getWalkParameter(I_xx); 8 | Ib(1,1) = NaoRobot.getWalkParameter(I_yy); 9 | Ib(2,2) = NaoRobot.getWalkParameter(I_zz); 10 | alpha = NaoRobot.getWalkParameter(alpha_ZMPFilter); 11 | m = NaoRobot.getWalkParameter(mass); 12 | gravity = NaoRobot.getWalkParameter(g); 13 | cout<<" ZMP Filter Initialized"<0) 54 | { 55 | ZMPXint.add(vrp(0)-vrp_d(0),NaoRobot.getWalkParameter(Ts)); 56 | int_zmp(0) = ZMPXint.eval(); 57 | ZMPPXint.add(int_zmp(0),NaoRobot.getWalkParameter(Ts)); 58 | int_zmpp(0) = ZMPPXint.eval(); 59 | 60 | } 61 | 62 | if(k_f(1,1)>0) 63 | { 64 | ZMPYint.add(vrp(1)-vrp_d(1),NaoRobot.getWalkParameter(Ts)); 65 | int_zmp(1) = ZMPYint.eval(); 66 | ZMPPYint.add(int_zmp(1),NaoRobot.getWalkParameter(Ts)); 67 | int_zmpp(1) = ZMPPYint.eval(); 68 | } 69 | 70 | com_c = com_d + k_f * int_zmpp; 71 | 72 | 73 | } 74 | 75 | 76 | return com_c; 77 | 78 | 79 | } 80 | 81 | -------------------------------------------------------------------------------- /nao_walk_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(nao_walk) 3 | set (CMAKE_CXX_STANDARD 11) 4 | add_compile_options(-std=c++11) 5 | list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") 6 | 7 | find_package(catkin REQUIRED COMPONENTS 8 | geometry_msgs 9 | roscpp 10 | genmsg 11 | actionlib_msgs 12 | actionlib 13 | message_generation 14 | tf 15 | humanoid_nav_msgs 16 | dynamic_reconfigure 17 | ) 18 | 19 | 20 | add_service_files( 21 | FILES 22 | sayText.srv 23 | BallTracker.srv 24 | Tired.srv 25 | WarmHello.srv 26 | WakeUp.srv 27 | Crouch.srv 28 | DanceEvolution.srv 29 | EyeOfTheTiger.srv 30 | GangnamStyle.srv 31 | Macarena.srv 32 | TaiChi.srv 33 | Vangelis.srv 34 | WakeUp.srv 35 | ) 36 | 37 | add_action_files(DIRECTORY action FILES behaviour.action speek.action) 38 | 39 | generate_messages( 40 | DEPENDENCIES 41 | actionlib_msgs 42 | std_msgs 43 | ) 44 | 45 | generate_dynamic_reconfigure_options( 46 | cfg/GaitControl.cfg 47 | ) 48 | 49 | catkin_package( 50 | INCLUDE_DIRS include 51 | LIBRARIES nao_walk 52 | CATKIN_DEPENDS geometry_msgs roscpp actionlib actionlib_msgs message_runtime tf humanoid_nav_msgs 53 | ) 54 | 55 | include_directories( include 56 | ${catkin_INCLUDE_DIRS} 57 | ${Boost_INCLUDE_DIRS}) 58 | 59 | add_executable(nao_walk 60 | src/gait_control.cpp 61 | src/nao_walk_ros.cpp 62 | src/socket_client.cpp 63 | src/joint_states_table.cpp 64 | ) 65 | 66 | add_dependencies(nao_walk ${PROJECT_NAME}_gencfg) 67 | add_executable(nao_walk_dynamic_configure nodes/server.cpp) 68 | target_link_libraries(nao_walk 69 | ${catkin_LIBRARIES} 70 | ${Boost_LIBRARIES}) 71 | target_link_libraries(nao_walk_dynamic_configure ${catkin_LIBRARIES}) 72 | 73 | 74 | add_definitions( " -DBOOST_SIGNALS_NO_DEPRECATION_WARNING ") -------------------------------------------------------------------------------- /nao_walk_naoqi/butterworthLPF.cpp: -------------------------------------------------------------------------------- 1 | #include "butterworthLPF.h" 2 | 3 | 4 | butterworthLPF::butterworthLPF() 5 | { 6 | 7 | 8 | 9 | fx = 0; 10 | fs = 0; 11 | a1 = 0; 12 | a2 = 0; 13 | b0 = 0; 14 | b1 = 0; 15 | b2 = 0; 16 | ff = 0; 17 | ita = 0; 18 | q = 0; 19 | i = 0; 20 | y_p = 0; 21 | y_pp = 0; 22 | x_p = 0; 23 | x_pp = 0; 24 | 25 | 26 | 27 | } 28 | 29 | 30 | 31 | void butterworthLPF::reset() 32 | { 33 | 34 | 35 | fx = 0; 36 | fs = 0; 37 | a1 = 0; 38 | a2 = 0; 39 | b0 = 0; 40 | b1 = 0; 41 | b2 = 0; 42 | ff = 0; 43 | ita = 0; 44 | q = 0; 45 | i = 0; 46 | y_p = 0; 47 | y_pp = 0; 48 | x_p = 0; 49 | x_pp = 0; 50 | cout<2) 80 | out = b0 * y + b1 * y_p + b2* y_pp + a1 * x_p + a2 * x_pp; 81 | else{ 82 | out = x_p + a * (y - x_p); 83 | i++; 84 | } 85 | 86 | 87 | y_pp = y_p; 88 | y_p = y; 89 | x_pp = x_p; 90 | x_p = out; 91 | 92 | return out; 93 | 94 | 95 | /** ------------------------------------------------------------- **/ 96 | } 97 | -------------------------------------------------------------------------------- /nao_walk_naoqi/butterworthHPF.cpp: -------------------------------------------------------------------------------- 1 | #include "butterworthHPF.h" 2 | 3 | 4 | butterworthHPF::butterworthHPF() 5 | { 6 | fx = 0; 7 | fs = 0; 8 | a1 = 0; 9 | a2 = 0; 10 | b0 = 0; 11 | b1 = 0; 12 | b2 = 0; 13 | ff = 0; 14 | ita = 0; 15 | q = 0; 16 | i = 0; 17 | y_p = 0; 18 | y_pp = 0; 19 | x_p = 0; 20 | x_pp = 0; 21 | } 22 | 23 | 24 | 25 | void butterworthHPF::reset() 26 | { 27 | fx = 0; 28 | fs = 0; 29 | a1 = 0; 30 | a2 = 0; 31 | b0 = 0; 32 | b1 = 0; 33 | b2 = 0; 34 | ff = 0; 35 | ita = 0; 36 | q = 0; 37 | i = 0; 38 | y_p = 0; 39 | y_pp = 0; 40 | x_p = 0; 41 | x_pp = 0; 42 | cout<2) 76 | out = b0 * y + b1 * y_p + b2* y_pp + a1 * x_p + a2 * x_pp; 77 | else{ 78 | out = a*x_p + a * (y - y_p); 79 | i++; 80 | } 81 | 82 | 83 | y_pp = y_p; 84 | y_p = y; 85 | x_pp = x_p; 86 | x_p = out; 87 | 88 | return out; 89 | 90 | 91 | /** ------------------------------------------------------------- **/ 92 | } -------------------------------------------------------------------------------- /nao_walk_naoqi/DcmData.h: -------------------------------------------------------------------------------- 1 | #ifndef DCM_DATA_H 2 | #define DCM_DATA_H 3 | 4 | #define LEFT 0 5 | #define RIGHT 1 6 | 7 | #define WALK_ENGINE_ID 1001 8 | #define STEP_MAX_CMD 32 9 | 10 | #define STATUS_DONE 0 11 | #define STATUS_PENDING 1 12 | #define STATUS_ABORTED 2 13 | #define STATUS_INV -1 14 | #define PARAM_INV -1 15 | #define STEPS_INV -1 //invalid steps-dcm not running 16 | #define STEPS_INIT 0 17 | #define INV -1 18 | #define RECONFIG_SIZE 15 19 | enum COMMANDS 20 | { 21 | NONE=0, 22 | STEPS, 23 | SPEEK, 24 | RECONFIG, 25 | VELOCITY, 26 | COMMANDS_SIZE 27 | }; 28 | 29 | enum BEHAVIOURS 30 | { 31 | CROUCH=COMMANDS_SIZE, 32 | WAKE_UP, 33 | MAKARENA, 34 | BALL, 35 | DANCE_EVOL, 36 | GANGNAM_ST, 37 | VANGELIS, 38 | TIRED, 39 | HELLO, 40 | TAICHI, 41 | EYE_OF_TIGER, 42 | BEHAVIOURS_SIZE 43 | }; 44 | 45 | 46 | // #define NONE 0 47 | // #define STEPS 1 48 | 49 | #define STAND 1 50 | #define WALK 2 51 | 52 | #define CMD_FINISHED -1 53 | #define CMD_ERR -2 54 | 55 | typedef struct 56 | { 57 | float vx; 58 | float vy; 59 | float vw; 60 | } velocity_t; 61 | 62 | typedef struct 63 | { 64 | float trans[3]; 65 | float quat[4]; 66 | } odom_t; 67 | 68 | typedef struct 69 | { 70 | bool isValid; 71 | odom_t odom; 72 | 73 | float copl[4]; 74 | float copr[4]; 75 | float acc[3]; 76 | float gyro[3]; 77 | float joint_states[26]; 78 | 79 | int last_footstep; 80 | 81 | int last_cmd; 82 | unsigned int last_cmd_id; 83 | int last_cmd_param; 84 | int last_cmd_status; 85 | } dcm_data_t; 86 | 87 | typedef struct 88 | { 89 | dcm_data_t dcm_data; 90 | int stamp; 91 | } dcm_data_stamp_t; 92 | 93 | typedef struct 94 | { 95 | float x; 96 | float y; 97 | float theta; 98 | char leg; 99 | char cmd; 100 | } step_t; 101 | 102 | typedef struct 103 | { 104 | // int engine_id; 105 | int command; 106 | unsigned int id; 107 | int data_size; 108 | void *data; 109 | }command_t; 110 | 111 | #endif 112 | -------------------------------------------------------------------------------- /nao_walk_ros/include/nao_walk/dcm_data.h: -------------------------------------------------------------------------------- 1 | #ifndef DCM_DATA_H 2 | #define DCM_DATA_H 3 | 4 | #define LEFT 0 5 | #define RIGHT 1 6 | 7 | #define WALK_ENGINE_ID 1001 8 | #define STEP_MAX_CMD 32 9 | 10 | #define STATUS_DONE 0 11 | #define STATUS_PENDING 1 12 | #define STATUS_ABORTED 2 13 | #define STATUS_INV -1 14 | #define PARAM_INV -1 15 | #define STEPS_INV -1 //invalid steps-dcm not running 16 | #define STEPS_INIT 0 17 | #define INV -1 18 | #define RECONFIG_SIZE 15 19 | enum COMMANDS 20 | { 21 | NONE=0, 22 | STEPS, 23 | SPEEK, 24 | RECONFIG, 25 | VELOCITY, 26 | COMMANDS_SIZE 27 | }; 28 | 29 | typedef struct 30 | { 31 | float vx; 32 | float vy; 33 | float vw; 34 | } velocity_t; 35 | 36 | 37 | enum BEHAVIOURS 38 | { 39 | CROUCH=COMMANDS_SIZE, 40 | WAKE_UP, 41 | MAKARENA, 42 | BALL, 43 | DANCE_EVOL, 44 | GANGNAM_ST, 45 | VANGELIS, 46 | TIRED, 47 | HELLO, 48 | TAICHI, 49 | EYE_OF_TIGER, 50 | BEHAVIOURS_SIZE 51 | }; 52 | 53 | 54 | // #define NONE 0 55 | // #define STEPS 1 56 | 57 | #define STAND 1 58 | #define WALK 0 59 | 60 | #define CMD_FINISHED -1 61 | #define CMD_ERR -2 62 | 63 | typedef struct 64 | { 65 | float trans[3]; 66 | float quat[4]; 67 | } odom_t; 68 | 69 | 70 | typedef struct 71 | { 72 | bool isValid; 73 | odom_t odom; 74 | 75 | float copl[4]; 76 | float copr[4]; 77 | float acc[3]; 78 | float gyro[3]; 79 | float joint_states[26]; 80 | 81 | int last_footstep; 82 | 83 | int last_cmd; 84 | unsigned int last_cmd_id; 85 | int last_cmd_param; 86 | int last_cmd_status; 87 | } dcm_data_t; 88 | 89 | typedef struct 90 | { 91 | dcm_data_t dcm_data; 92 | int stamp; 93 | } dcm_data_stamp_t; 94 | 95 | typedef struct 96 | { 97 | float x; 98 | float y; 99 | float theta; 100 | char leg; 101 | char cmd; 102 | } step_t; 103 | 104 | typedef struct 105 | { 106 | // int engine_id; 107 | int command; 108 | unsigned int id; 109 | int data_size; 110 | void *data; 111 | }command_t; 112 | 113 | #endif 114 | -------------------------------------------------------------------------------- /nao_walk_naoqi/DelayedObserverDCM.cpp: -------------------------------------------------------------------------------- 1 | #include "DelayedObserverDCM.h" 2 | 3 | DelayedObserverDCM::DelayedObserverDCM(RobotParameters &robot):NaoRobot(robot) 4 | { 5 | 6 | 7 | //State is com, dcm, vrp, and ZMP offset 8 | x.setZero(); 9 | A.setZero(); 10 | B.setZero(); 11 | I.setIdentity(); 12 | 13 | 14 | Ccom.setZero(); 15 | Czmp.setZero(); 16 | 17 | Ccom(0,0) = 1.000; 18 | Czmp(0,2) = 1.000; 19 | Czmp(0,3) = 1.000; 20 | 21 | 22 | 23 | 24 | A(0,0) = -NaoRobot.getWalkParameter(omega); 25 | A(0,1) = NaoRobot.getWalkParameter(omega); 26 | A(1,1) = NaoRobot.getWalkParameter(omega); 27 | A(1,2) = -NaoRobot.getWalkParameter(omega); 28 | A *= NaoRobot.getWalkParameter(Ts); 29 | A.noalias() += I; 30 | 31 | B.setZero(); 32 | B(2) = 1.000; 33 | B *= NaoRobot.getWalkParameter(Ts); 34 | 35 | 36 | L.setZero(); 37 | L(0,0) =0.13494; 38 | L(0,1) =0.003825; 39 | L(1,0) =0.003129; 40 | L(1,1) =0.038694; 41 | L(2,0) =0.033215; 42 | L(2,1) =0.34279; 43 | L(3,0) =-0.052321; 44 | L(3,1) =0.26414; 45 | 46 | L=L/20.0; 47 | 48 | Lcom.setZero(); 49 | Lcom = L.block<4,1>(0,0); 50 | 51 | 52 | cout<<"ZMP Delayed Observer Initialized Successfully"< (int) ZMPDELAY - 1) 69 | { 70 | com_ -= Ccom*x; 71 | zmp_ -= Czmp*xbuffer.front(); 72 | x = A*x; 73 | x.noalias() += B*u_; 74 | x.noalias() += L * Vector2f(com_,zmp_); 75 | xbuffer.pop(); 76 | } 77 | else 78 | { 79 | com_ -= Ccom*x; 80 | x = A*x; 81 | x.noalias() += B*u_; 82 | x.noalias() += Lcom * com_; 83 | } 84 | 85 | 86 | 87 | xbuffer.push(x); 88 | 89 | updateVars(); 90 | } 91 | 92 | 93 | 94 | void DelayedObserverDCM::updateVars() 95 | { 96 | com = x(0); 97 | dcm = x(1); 98 | vrp = x(2); 99 | dist = x(3); 100 | } 101 | 102 | Vector4f DelayedObserverDCM::getState() 103 | { 104 | return x; 105 | } -------------------------------------------------------------------------------- /nao_behaviour_ros/src/nao_behaviour/nao_behaviour_node: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | import actionlib 5 | 6 | from std_srvs.srv import Empty 7 | from nao_behaviour.srv import Default, sayText 8 | from nao_walk.msg import behaviourAction, behaviourGoal, speekAction, speekGoal 9 | 10 | from geometry_msgs.msg import PoseStamped 11 | from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal 12 | 13 | rospy.init_node('nao_behaviours') 14 | 15 | behClient = actionlib.SimpleActionClient('/behaviour_execution', behaviourAction) 16 | behClient.wait_for_server() 17 | 18 | speekClient = actionlib.SimpleActionClient('/speed_execution', speekAction) 19 | speekClient.wait_for_server() 20 | 21 | goalClient = actionlib.SimpleActionClient('/move_base', MoveBaseAction) 22 | 23 | behaviours_str=[ 24 | 'crouch', 25 | 'wakeUp', 26 | 'macarena', 27 | 'ballTracker', 28 | 'danceEvolution', 29 | 'gangnamStyle', 30 | 'vangelis', 31 | 'tired', 32 | 'warmHello', 33 | 'taichi', 34 | 'eyeOfTheTiger' 35 | ] 36 | 37 | def speek_cb(req): 38 | print(req) 39 | goal=speekGoal() 40 | goal.text=req.request 41 | #goal.target_pose.pose=pose.pose 42 | print(req.request) 43 | speekClient.send_goal(goal) 44 | speekClient.wait_for_result() 45 | print('Done') 46 | return 'Done' 47 | 48 | 49 | def goal_cb(pose): 50 | goal=MoveBaseGoal() 51 | goal.target_pose.pose=pose.pose 52 | goalClient.send_goal(goal) 53 | 54 | def service_cb(req): 55 | srv_name = req._connection_header['service'] 56 | beh_str=srv_name.split('/')[-1] 57 | print(beh_str) 58 | beh_id = -1 59 | i = 0 60 | for beh_entry in behaviours_str: 61 | if beh_entry == beh_str: 62 | beh_id = i 63 | i = i + 1 64 | print(beh_id) 65 | 66 | goal=behaviourGoal() 67 | goal.behaviour_id = beh_id 68 | print("send goal") 69 | behClient.send_goal(goal) 70 | behClient.wait_for_result() 71 | 72 | return 'Done' 73 | 74 | 75 | 76 | services = [] 77 | 78 | #goalClient.wait_for_server(rospy.Duration.from_sec(3.0)) 79 | 80 | for beh in behaviours_str: 81 | services.append(rospy.Service("/nao_behaviours/"+beh, Default,service_cb) ) 82 | 83 | services.append(rospy.Service("/nao_speak/", sayText,speek_cb) ) 84 | 85 | rospy.Subscriber("goal", PoseStamped, goal_cb) 86 | rospy.spin() 87 | -------------------------------------------------------------------------------- /nao_walk_naoqi/TrajectoryPlanner.h: -------------------------------------------------------------------------------- 1 | #ifndef __TRAJPLAN_H__ 2 | #define __TRAJPLAN_H__ 3 | 4 | #include "KMat.hpp" 5 | #include "KWalkMat.h" 6 | #include "MotionDefines.h" 7 | #include "RobotParameters.h" 8 | #include "robot_consts.h" 9 | #include // std::queue 10 | #include 11 | #include 12 | #include "StepAdjustment.h" 13 | 14 | using namespace std; 15 | class TrajectoryPlanner 16 | { 17 | 18 | private: 19 | RobotParameters &NaoRobot; 20 | WalkInstruction planned, zmpi, ci; 21 | KWalkMat interp; 22 | float maxX,maxY,minX,minY; 23 | public: 24 | /** Walking Instruction to be executed **/ 25 | 26 | std::queue walkInstructionbuffer; 27 | KVecFloat3 target, start, startL, startR, targetR, targetL, plantargetL, plantargetR, plantarget, planstartL, planstartR; 28 | /** ZMP Buffer for the Prediction Horizon **/ 29 | boost::circular_buffer ZMPbuffer; 30 | /** Queues needed for planning **/ 31 | std::queue stepAnkleQ, zmpQ; 32 | bool planAvailable, firstrun; 33 | 34 | //Methods 35 | void planZMPTrajectory(); 36 | void computeSwingLegAndZMP(KVecFloat3 &tl, KVecFloat3 &tr, KVecFloat3 &t, KVecFloat3 sl, KVecFloat3 sr, WalkInstruction i); 37 | void generatePlan(KVecFloat2 DCM_, KVecFloat2 COP_,bool UseStepAdjustment); 38 | void init(KVecFloat3 sl, KVecFloat3 sr); 39 | void emptyPlan(); 40 | void plan(KVecFloat2 DCM_,KVecFloat2 COP_,bool UseStepAdjustment); 41 | TrajectoryPlanner(RobotParameters &robot); 42 | //Step Adjustment 43 | StepAdjustment sa; 44 | KVecFloat2 dx, tempV; 45 | KMath::KMat::GenMatrix SFoot_rot; 46 | float SFoot_angle; 47 | 48 | void computeFeetAnkleFromFeetCenter(KVecFloat3& al, KVecFloat3& ar, KVecFloat3 cl, KVecFloat3 cr); 49 | void computeFeetCenterFromFeetAnkle(KVecFloat3& cl, KVecFloat3& cr, KVecFloat3 tl, KVecFloat3 tr); 50 | WalkInstruction getCurrWalkInstruction(); 51 | void setInitialFeet(KVecFloat3 sl, KVecFloat3 sr); 52 | /** @fn float anglemean(float l, float r) const 53 | * @brief Computation of mean angle 54 | **/ 55 | 56 | float anglemean(float l, float r) const 57 | { 58 | return (float) ((double) l + KMath::anglediff2( (double) r, (double) l) / 2.0); 59 | } 60 | float cropStep(float f_, float max_, float min_) 61 | { 62 | return fmax(min_, fmin(f_, max_)); 63 | } 64 | }; 65 | #endif 66 | -------------------------------------------------------------------------------- /nao_walk_naoqi/MovingAverageFilter.h: -------------------------------------------------------------------------------- 1 | /* 2 | * humanoid_state_estimation - a complete state estimation scheme for humanoid robots 3 | * 4 | * Copyright 2017-2018 Stylianos Piperakis, Foundation for Research and Technology Hellas (FORTH) 5 | * License: BSD 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Foundation for Research and Technology Hellas (FORTH) 16 | * nor the names of its contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | #ifndef __MOVINGAVERAGEFILTER_H__ 33 | #define __MOVINGAVERAGEFILTER_H__ 34 | 35 | #include 36 | #include 37 | 38 | 39 | class MovingAverageFilter 40 | { 41 | 42 | private: 43 | int windowSize; 44 | unsigned currentstep; 45 | std::queue windowBuffer; 46 | public: 47 | float x; 48 | 49 | /** @fn void Filter(float y) 50 | * @brief filters the measurement with a cummulative moving average filter 51 | */ 52 | void setParams(int windowSize_) 53 | { 54 | windowSize=windowSize_; 55 | } 56 | void filter(double y); 57 | MovingAverageFilter(); 58 | void reset(); 59 | }; 60 | #endif 61 | -------------------------------------------------------------------------------- /nao_walk_naoqi/RobotParameters.cpp: -------------------------------------------------------------------------------- 1 | #include "RobotParameters.h" 2 | 3 | #include 4 | #include 5 | 6 | float RobotParameters::getWalkParameter(int p) 7 | { 8 | if(p>ParametersSize || p < 0) 9 | { 10 | std::cerr << "Error Invalid parameter" << std::endl; 11 | return 0; 12 | } 13 | return WalkParameters[p]; 14 | } 15 | 16 | void RobotParameters::setWalkParameter(int p, float s) 17 | { 18 | 19 | if(p>ParametersSize || p < 0) 20 | { 21 | std::cerr << "Error Invalid parameter" << std::endl; 22 | return ; 23 | } 24 | WalkParameters[p]=s; 25 | } 26 | 27 | 28 | int RobotParameters::writeWalkParametersFromFile(const char * filename) 29 | { 30 | fprintf(stderr,"Saving configuration to `%s` \n",filename); 31 | FILE * fp = fopen(filename,"w"); 32 | if (fp!=0) 33 | { 34 | fprintf(fp,"%s\n",robotName); 35 | fprintf(fp,"%u\n",ParametersSize); 36 | unsigned int i=0; 37 | for (i=0; i= 0 14 | 15 | The matrix and vectors dimensions are as follows: 16 | G: n * n 17 | g0: n 18 | 19 | CE: n * p 20 | ce0: p 21 | 22 | CI: n * m 23 | ci0: m 24 | 25 | x: n 26 | 27 | The function will return the cost of the solution written in the x vector or 28 | std::numeric_limits::infinity() if the problem is infeasible. In the latter case 29 | the value of the x vector is not correct. 30 | 31 | References: D. Goldfarb, A. Idnani. A numerically stable dual method for solving 32 | strictly convex quadratic programs. Mathematical Programming 27 (1983) pp. 1-33. 33 | 34 | Notes: 35 | 1. pay attention in setting up the vectors ce0 and ci0. 36 | If the constraints of your problem are specified in the form 37 | A^T x = b and C^T x >= d, then you should set ce0 = -b and ci0 = -d. 38 | 2. The matrices have column dimension equal to MATRIX_DIM, 39 | a constant set to 20 in this file (by means of a #define macro). 40 | If the matrices are bigger than 20 x 20 the limit could be 41 | increased by means of a -DMATRIX_DIM=n on the compiler command line. 42 | 3. The matrix G is modified within the function since it is used to compute 43 | the G = L^T L cholesky factorization for further computations inside the function. 44 | If you need the original matrix G you should make a copy of it and pass the copy 45 | to the function. 46 | 47 | Author: Luca Di Gaspero 48 | DIEGM - University of Udine, Italy 49 | luca.digaspero@uniud.it 50 | http://www.diegm.uniud.it/digaspero/ 51 | 52 | The author will be grateful if the researchers using this software will 53 | acknowledge the contribution of this function in their research papers. 54 | 55 | Copyright (c) 2007-2016 Luca Di Gaspero 56 | 57 | This software may be modified and distributed under the terms 58 | of the MIT license. See the LICENSE file for details. 59 | */ 60 | 61 | 62 | #ifndef _QUADPROGPP 63 | #define _QUADPROGPP 64 | 65 | #include "Array.hh" 66 | 67 | namespace quadprogpp { 68 | 69 | double solve_quadprog(Matrix& G, Vector& g0, 70 | const Matrix& CE, const Vector& ce0, 71 | const Matrix& CI, const Vector& ci0, 72 | Vector& x); 73 | 74 | } // namespace quadprogpp 75 | 76 | #endif // #define _QUADPROGPP 77 | -------------------------------------------------------------------------------- /nao_walk_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | nao_walk 3 | 0.0.0 4 | The nao_walk package 5 | 6 | 7 | 8 | 9 | master 10 | 11 | 12 | 13 | 14 | 15 | TODO 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | catkin 42 | geometry_msgs 43 | actionlib_msgs 44 | actionlib 45 | roscpp 46 | tf 47 | simple_stepplanner2D 48 | message_generation 49 | humanoid_nav_msgs 50 | dynamic_reconfigure 51 | 52 | 53 | geometry_msgs 54 | roscpp 55 | actionlib_msgs 56 | simple_stepplanner2D 57 | message_runtime 58 | actionlib 59 | actionlib_msgs 60 | tf 61 | humanoid_nav_msgs 62 | dynamic_reconfigure 63 | 64 | 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /nao_walk_naoqi/MovingAverageFilter.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * humanoid_state_estimation - a complete state estimation scheme for humanoid robots 3 | * 4 | * Copyright 2017-2018 Stylianos Piperakis, Foundation for Research and Technology Hellas (FORTH) 5 | * License: BSD 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Foundation for Research and Technology Hellas (FORTH) 16 | * nor the names of its contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | #include "MovingAverageFilter.h" 33 | 34 | 35 | MovingAverageFilter::MovingAverageFilter() 36 | { 37 | x = 0.000; 38 | //Window Size 39 | windowSize = 10; 40 | currentstep = 0; 41 | std::cout<<"Moving Average Filter Initialized Successfully"<0) 51 | windowBuffer.pop(); 52 | std::cout<<"Moving Average Filter Reseted"<=windowSize) 70 | windowBuffer.pop(); 71 | } 72 | windowBuffer.push(y); 73 | /** ------------------------------------------------------------- **/ 74 | } 75 | -------------------------------------------------------------------------------- /nao_walk_naoqi/PCThread.h.bak: -------------------------------------------------------------------------------- 1 | #ifndef __PCTHREAD_H__ 2 | #define __PCTHREAD_H__ 3 | 4 | 5 | #define PWindow 151 6 | 7 | 8 | #include "KMat.hpp" 9 | #include 10 | #include 11 | #include "RobotParameters.h" 12 | #include 13 | #define ZMPKALMANDELAY 5 14 | using namespace std; 15 | class Dynamics 16 | { 17 | private: 18 | 19 | KMath::KMat::GenMatrix Ad; 20 | 21 | KMath::KMat::GenMatrix Bd,temp; 22 | 23 | KMath::KMat::GenMatrix L; 24 | 25 | RobotParameters &OurRobot; 26 | 27 | 28 | public: 29 | 30 | KMath::KMat::GenMatrix Cd; 31 | 32 | KMath::KMat::GenMatrix State; 33 | 34 | float zmpstate, zmpstate_; 35 | 36 | Dynamics(RobotParameters &robot); 37 | 38 | 39 | double Observer_COP, Observer_CoM; 40 | /** @fn Update(float u, KVecFloat2 error) 41 | * @brief Update the Linear Dynamics 42 | * of the Cart and Table 43 | **/ 44 | void Update(float u,KVecFloat2 error); 45 | 46 | 47 | }; 48 | 49 | 50 | 51 | class Kalman 52 | { 53 | 54 | private: 55 | KMath::KMat::GenMatrix Ckalman; 56 | KMath::KMat::GenMatrix Kgain; 57 | KMath::KMat::GenMatrix s,MeasurementNoise; 58 | KMath::KMat::GenMatrix P, ProcessNoise,Bkalman; 59 | RobotParameters &OurRobot; 60 | public: 61 | KMath::KMat::GenMatrix StateKalman,StatePredict; 62 | std::queue uBuffer,combuffer; 63 | KMath::KMat::GenMatrix ykalman; 64 | 65 | /** @fn void Filter(float ZMPMeasured,float CoMMeasured) 66 | * @brief filters the ZMP measurement from the CoP using 67 | * also the COM measured by the encoders 68 | */ 69 | void Filter(float ZMPMeasured,float CoMMeasured); 70 | void reset(); 71 | Kalman(RobotParameters &robot); 72 | double CoM_Noise, COP_Noise; 73 | }; 74 | 75 | 76 | class PCThread 77 | { 78 | private: 79 | 80 | 81 | KMath::KMat::GenMatrix ZMPReferenceX, ZMPReferenceY; 82 | /** DMPC **/ 83 | 84 | KMath::KMat::GenMatrix Gx; 85 | 86 | KMath::KMat::GenMatrix Gd; 87 | 88 | float Gi,Integrationfbx,Statefbx,Predictionfbx,Integrationfby,Statefby,Predictionfby,uX,uY; 89 | 90 | public: 91 | 92 | RobotParameters &OurRobot; 93 | 94 | 95 | Dynamics DynamicsX, DynamicsY; 96 | Kalman KalmanX, KalmanY; 97 | PCThread(RobotParameters&); 98 | 99 | /** @fn void LIPMComPredictor(CircularBuffer & ZmpBuffer, float CoMmeasuredX, float CoMmeasuredY, float ZMPMeasuredX, float ZMPMeasuredY); 100 | * @brief Computes the desired COM 101 | */ 102 | void setInitialState(KVecFloat2 CoM, KVecFloat2 ZMP); 103 | void Control(boost::circular_buffer & ZmpBuffer, float CoMMeasuredX, float CoMMeasuredY, float CoMMeasuredZ, float ZMPMeasuredX, float ZMPMeasuredY); 104 | 105 | float vrpx_d, vrpy_d, comx_d, comy_d, comdx_d, comdy_d; 106 | float Observer_CoMX,Observer_CoMY,Observer_COPX,Observer_COPY; 107 | bool firstrun; 108 | }; 109 | 110 | 111 | 112 | 113 | 114 | #endif 115 | -------------------------------------------------------------------------------- /nao_walk_naoqi/PCThread.h: -------------------------------------------------------------------------------- 1 | #ifndef __PCTHREAD_H__ 2 | #define __PCTHREAD_H__ 3 | 4 | 5 | #define PWindow 101 6 | 7 | 8 | #include "KMat.hpp" 9 | #include 10 | #include 11 | #include "RobotParameters.h" 12 | #include 13 | #define ZMPKALMANDELAY 3 14 | using namespace std; 15 | class Dynamics 16 | { 17 | private: 18 | 19 | KMath::KMat::GenMatrix Ad; 20 | 21 | KMath::KMat::GenMatrix Bd,temp; 22 | 23 | KMath::KMat::GenMatrix L; 24 | 25 | RobotParameters &OurRobot; 26 | 27 | 28 | public: 29 | 30 | KMath::KMat::GenMatrix Cd; 31 | 32 | KMath::KMat::GenMatrix State; 33 | 34 | float zmpstate, zmpstate_; 35 | 36 | Dynamics(RobotParameters &robot); 37 | 38 | 39 | double Observer_COP, Observer_CoM; 40 | /** @fn Update(float u, KVecFloat2 error) 41 | * @brief Update the Linear Dynamics 42 | * of the Cart and Table 43 | **/ 44 | void Update(float u,KVecFloat2 error); 45 | 46 | 47 | }; 48 | 49 | 50 | 51 | class Kalman 52 | { 53 | 54 | private: 55 | KMath::KMat::GenMatrix Ckalman; 56 | KMath::KMat::GenMatrix Kgain; 57 | KMath::KMat::GenMatrix s,MeasurementNoise; 58 | KMath::KMat::GenMatrix P, ProcessNoise,Bkalman; 59 | RobotParameters &OurRobot; 60 | public: 61 | KMath::KMat::GenMatrix StateKalman,StatePredict; 62 | std::queue uBuffer,combuffer; 63 | KMath::KMat::GenMatrix ykalman; 64 | 65 | /** @fn void Filter(float ZMPMeasured,float CoMMeasured) 66 | * @brief filters the ZMP measurement from the CoP using 67 | * also the COM measured by the encoders 68 | */ 69 | void Filter(float ZMPMeasured,float CoMMeasured); 70 | void reset(); 71 | Kalman(RobotParameters &robot); 72 | double CoM_Noise, COP_Noise; 73 | }; 74 | 75 | 76 | class PCThread 77 | { 78 | private: 79 | 80 | 81 | KMath::KMat::GenMatrix ZMPReferenceX, ZMPReferenceY; 82 | /** DMPC **/ 83 | 84 | KMath::KMat::GenMatrix Gx; 85 | 86 | KMath::KMat::GenMatrix Gd; 87 | 88 | float Gi,Integrationfbx,Statefbx,Predictionfbx,Integrationfby,Statefby,Predictionfby,uX,uY; 89 | 90 | public: 91 | 92 | RobotParameters &OurRobot; 93 | 94 | 95 | Dynamics DynamicsX, DynamicsY; 96 | Kalman KalmanX, KalmanY; 97 | PCThread(RobotParameters&); 98 | 99 | /** @fn void LIPMComPredictor(CircularBuffer & ZmpBuffer, float CoMmeasuredX, float CoMmeasuredY, float ZMPMeasuredX, float ZMPMeasuredY); 100 | * @brief Computes the desired COM 101 | */ 102 | void setInitialState(KVecFloat2 CoM, KVecFloat2 ZMP); 103 | void Control(boost::circular_buffer & ZmpBuffer, float CoMMeasuredX, float CoMMeasuredY, float CoMMeasuredZ, float ZMPMeasuredX, float ZMPMeasuredY); 104 | 105 | float vrpx_d, vrpy_d, comx_d, comy_d, comdx_d, comdy_d, zmpx, zmpy, dcmx_d, dcmy_d, vrpdx_d, vrpdy_d, zmpx_ref, zmpy_ref; 106 | float Observer_CoMX,Observer_CoMY,Observer_COPX,Observer_COPY; 107 | bool firstrun; 108 | }; 109 | 110 | 111 | 112 | 113 | 114 | #endif 115 | -------------------------------------------------------------------------------- /nao_walk_naoqi/LIPMThread.h: -------------------------------------------------------------------------------- 1 | #ifndef __LIPMTHREAD_H__ 2 | #define __LIPMTHREAD_H__ 3 | 4 | 5 | #define PWindow 101 6 | 7 | 8 | #include "KMat.hpp" 9 | #include 10 | #include 11 | #include "RobotParameters.h" 12 | #include 13 | #define ZMPKALMANDELAY 3 14 | using namespace std; 15 | 16 | class DynamicsLIPM 17 | { 18 | private: 19 | 20 | KMath::KMat::GenMatrix Ad; 21 | 22 | KMath::KMat::GenMatrix Bd,temp; 23 | 24 | KMath::KMat::GenMatrix L; 25 | KMath::KMat::GenMatrix Lcom; 26 | KVecFloat2 error, y_; 27 | RobotParameters &OurRobot; 28 | 29 | 30 | public: 31 | 32 | KMath::KMat::GenMatrix Cd; 33 | 34 | KMath::KMat::GenMatrix State; 35 | 36 | float zmpstate, zmpstate_; 37 | 38 | DynamicsLIPM(RobotParameters &robot); 39 | 40 | 41 | double Observer_COP, Observer_CoM; 42 | /** @fn Update(float u, KVecFloat2 error) 43 | * @brief Update the Linear Dynamics 44 | * of the Cart and Table 45 | **/ 46 | void Update(float u,KVecFloat2 error); 47 | 48 | 49 | }; 50 | 51 | 52 | 53 | class KalmanLIPM 54 | { 55 | 56 | private: 57 | KMath::KMat::GenMatrix Ckalman; 58 | KMath::KMat::GenMatrix Kgain; 59 | KMath::KMat::GenMatrix s,MeasurementNoise; 60 | KMath::KMat::GenMatrix P, ProcessNoise,Bkalman; 61 | RobotParameters &OurRobot; 62 | public: 63 | KMath::KMat::GenMatrix StateKalman,StatePredict; 64 | std::queue uBuffer,combuffer; 65 | KMath::KMat::GenMatrix ykalman; 66 | 67 | /** @fn void Filter(float ZMPMeasured,float CoMMeasured) 68 | * @brief filters the ZMP measurement from the CoP using 69 | * also the COM measured by the encoders 70 | */ 71 | void Filter(float ZMPMeasured,float CoMMeasured); 72 | void reset(); 73 | KalmanLIPM(RobotParameters &robot); 74 | double CoM_Noise, COP_Noise; 75 | }; 76 | 77 | 78 | class LIPMThread 79 | { 80 | private: 81 | 82 | 83 | KMath::KMat::GenMatrix ZMPReferenceX, ZMPReferenceY; 84 | /** DMPC **/ 85 | 86 | KMath::KMat::GenMatrix Gx; 87 | 88 | KMath::KMat::GenMatrix Gd; 89 | 90 | float Gi,Integrationfbx,Statefbx,Predictionfbx,Integrationfby,Statefby,Predictionfby,uX,uY; 91 | 92 | public: 93 | 94 | RobotParameters &OurRobot; 95 | 96 | 97 | DynamicsLIPM DynamicsX, DynamicsY; 98 | KalmanLIPM KalmanX, KalmanY; 99 | LIPMThread(RobotParameters&); 100 | 101 | /** @fn void LIPMComPredictor(CircularBuffer & ZmpBuffer, float CoMmeasuredX, float CoMmeasuredY, float ZMPMeasuredX, float ZMPMeasuredY); 102 | * @brief Computes the desired COM 103 | */ 104 | void setInitialState(KVecFloat2 CoM, KVecFloat2 ZMP); 105 | void Control(boost::circular_buffer & ZmpBuffer, float CoMMeasuredX, float CoMMeasuredY, float CoMMeasuredZ, float ZMPMeasuredX, float ZMPMeasuredY); 106 | 107 | float vrpx_d, vrpy_d, comx_d, comy_d, comdx_d, comdy_d, zmpx, zmpy, dcmx_d, dcmy_d, vrpdx_d, vrpdy_d, zmpx_ref, zmpy_ref; 108 | float Observer_CoMX,Observer_CoMY,Observer_COPX,Observer_COPY; 109 | bool firstrun; 110 | }; 111 | 112 | 113 | 114 | 115 | 116 | #endif 117 | -------------------------------------------------------------------------------- /nao_walk_naoqi/robot_consts.h: -------------------------------------------------------------------------------- 1 | /** 2 | * robot_consts.h 3 | * 4 | * Created on: Dec 9, 2010 5 | * Author: trs 6 | */ 7 | 8 | #ifndef ROBOT_CONSTS_H 9 | #define ROBOT_CONSTS_H 10 | #include 11 | #include 12 | #include 13 | 14 | /// Returns a map from joint ids (enum Joint names) to almemory string "path" names 15 | namespace KDeviceLists 16 | { 17 | 18 | 19 | enum ChainHeadNames 20 | { 21 | YAW = 0, PITCH, HEAD_SIZE 22 | }; 23 | 24 | enum ChainArmNames 25 | { 26 | SHOULDER_PITCH = 0, SHOULDER_ROLL, ELBOW_YAW, ELBOW_ROLL, WRIST_YAW, /*HAND,*/ ARM_SIZE 27 | }; 28 | 29 | enum ChainLegNames 30 | { 31 | HIP_YAW_PITCH = 0, HIP_ROLL, HIP_PITCH, KNEE_PITCH, ANKLE_PITCH, ANKLE_ROLL, LEG_SIZE 32 | }; 33 | 34 | 35 | enum ChainsNames 36 | { 37 | CHAIN_HEAD=0, CHAIN_L_ARM, CHAIN_L_LEG, CHAIN_R_LEG,CHAIN_R_ARM, CHAINS_SIZE 38 | }; 39 | 40 | 41 | enum JointNames 42 | { 43 | HEAD = 0, L_ARM = HEAD_SIZE, L_LEG = L_ARM + ARM_SIZE, R_LEG = L_LEG + LEG_SIZE, R_ARM = R_LEG + LEG_SIZE , 44 | 45 | NUMOFJOINTS = R_ARM + ARM_SIZE 46 | 47 | }; 48 | 49 | 50 | enum ChainAccessNames 51 | { 52 | AXIS_X = 0, AXIS_Y, AXIS_Z , AXIS_SIZE 53 | }; 54 | 55 | enum ChainIniertialSizes 56 | { 57 | ACC_SIZE = AXIS_SIZE, GYR_SIZE = AXIS_SIZE, ANGLE_SIZE = AXIS_Z 58 | }; 59 | 60 | enum ChainFSRNames 61 | { 62 | FSR_FL = 0, FSR_FR, FSR_RL, FSR_RR, COP_X, COP_Y, TOTAL_WEIGHT, FSR_SIZE 63 | }; 64 | enum ChainUltraSonicNames 65 | { 66 | US_VALUE, US_VALUE1, US_VALUE2, US_VALUE3, US_VALUE4, US_VALUE5, US_VALUE6, US_VALUE7, US_VALUE8, US_VALUE9, US_SIZE 67 | }; 68 | enum RobotPositionNames 69 | { 70 | ROBOT_X = 0, ROBOT_Y, ROBOT_ANGLE, ROBOTPOSITION_SIZE 71 | }; 72 | 73 | 74 | 75 | enum ButtonNames 76 | { 77 | 78 | CHEST_BUTTON = 0, L_BUMPER_L, L_BUMPER_R, R_BUMPER_L, R_BUMPER_R, BUTTONS_SIZE, NUMOFBUTTONS = BUTTONS_SIZE 79 | }; 80 | 81 | enum SupportLeg 82 | { 83 | SUPPORT_LEG_NONE = 0, SUPPORT_LEG_LEFT, SUPPORT_LEG_RIGHT, SUPPORT_LEG_BOTH 84 | }; 85 | 86 | enum SensorNames 87 | { 88 | 89 | ACC = 0, 90 | 91 | GYR = ACC + ACC_SIZE, 92 | 93 | //ANGLE=GYR+GYR_SIZE, 94 | //L_FSR=ANGLE+ANGLE_SIZE, 95 | L_FSR = GYR + GYR_SIZE, 96 | R_FSR = L_FSR + FSR_SIZE, 97 | L_US = R_FSR + FSR_SIZE, 98 | R_US = L_US + US_SIZE, 99 | //BUTTONS=BUTTONS_SIZE, 100 | NUMOFSENSORS = R_US + US_SIZE 101 | 102 | }; 103 | 104 | enum ComputedSensorsNames 105 | { 106 | ANGLE = 0, 107 | SUPPORT_LEG = ANGLE + ANGLE_SIZE, 108 | NUMOFCOMPUTEDSENSORS = SUPPORT_LEG +1 109 | }; 110 | 111 | struct Interpret 112 | { 113 | static const float GRAVITY_PULL = 9.81f; //(m/s^2) 114 | 115 | static const float GYR_Z_REF = 1230; //expected value of GYR_Z 116 | static const float GYR_Z_RAW = -1680; //expected value of GYR_Z_RAW 117 | static const float ACC_NORM = 58; //expected value of GYR_Z_RAW 118 | static const float GYR_GAIN = (-1.0 / 2.0) * 0.017453; //(1 / (2mv/deg/sec))* gyr ref 119 | 120 | 121 | static const float BUTTON_PRESSED = 0.0; //Normally on switches 122 | static const float BUTTON_RELEASED = 1.0; //Normally on switches 123 | static const float ROBOT_WEIGHT = 4.879f; 124 | }; 125 | std::vector const& getJointNames(); 126 | 127 | std::vector const& getJointKeys(); 128 | std::vector const& getPositionActuatorKeys(); 129 | std::vector const& getHardnessActuatorKeys(); 130 | 131 | std::vector const& getSensorNames(); 132 | std::vector const& getSensorKeys(); 133 | 134 | 135 | //std::vector const& getButtonNames(); 136 | std::vector const& getButtonKeys(); 137 | 138 | std::map const& getJointIDs(); 139 | // Not Implemented yet :P 140 | //std::map const& getSensorIDs(); 141 | 142 | 143 | //std::vector< std::string> ActuatorName; 144 | 145 | }; 146 | #endif /* ROBOT_CONSTS_H_ */ 147 | -------------------------------------------------------------------------------- /nao_walk_naoqi/CoMEKF.h: -------------------------------------------------------------------------------- 1 | /* 2 | * humanoid_state_estimation - a complete state estimation scheme for humanoid robots 3 | * 4 | * Copyright 2017-2018 Stylianos Piperakis, Foundation for Research and Technology Hellas (FORTH) 5 | * License: BSD 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Foundation for Research and Technology Hellas (FORTH) 16 | * nor the names of its contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | #ifndef __COMEKF_H__ 33 | #define __COMEKF_H__ 34 | 35 | #include 36 | #include 37 | #include "butterworthLPF.h" 38 | using namespace Eigen; 39 | using namespace std; 40 | 41 | class CoMEKF { 42 | 43 | private: 44 | 45 | Matrix F, P, I, Q, Fd; 46 | 47 | Vector3f COP, fN, L, COP_p, fN_p, L_p, Acc; 48 | 49 | Matrix H; 50 | 51 | Matrix K; 52 | 53 | Matrix R, S; 54 | 55 | butterworthLPF *DCMx_LPF, *DCMy_LPF; 56 | Matrix z; 57 | 58 | float tmp; 59 | 60 | 61 | void euler(Vector3f COP_, Vector3f fN_, Vector3f L_); 62 | Matrix computeDyn(Matrix x_, Vector3f COP_, Vector3f fN_, Vector3f L_); 63 | Matrix computeTrans(Matrix x_, Vector3f COP_, Vector3f fN_, Vector3f L_); 64 | void RK4(Vector3f COP_, Vector3f fN_, Vector3f L_, Vector3f COP0, Vector3f fN0, Vector3f L0); 65 | 66 | public: 67 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 68 | Matrix x, f; 69 | 70 | float comd_q, com_q, fd_q, com_r, comdd_r; 71 | 72 | float dt, m, g, I_xx,I_yy; 73 | 74 | float bias_fx, bias_fy, bias_fz; 75 | bool firstrun; 76 | bool useEuler; 77 | void init(); 78 | void updateVars(); 79 | 80 | 81 | Vector2f DCM,DCMdot,ZMP, DCM_lp; 82 | void setdt(float dtt) { 83 | dt = dtt; 84 | DCMx_LPF->init("DCMx",1.0/dt,5.5); 85 | DCMy_LPF->init("DCMy",1.0/dt,5.5); 86 | } 87 | 88 | void setParams(float m_, float I_xx_, float I_yy_, float g_) 89 | { 90 | m = m_; 91 | I_xx = I_xx_; 92 | I_yy = I_yy_; 93 | g = g_; 94 | 95 | } 96 | 97 | void setCoMPos(Vector3f pos) { 98 | x(0) = pos(0); 99 | x(1) = pos(1); 100 | x(2) = pos(2); 101 | } 102 | void setCoMExternalForce(Vector3f force) { 103 | x(6) = force(0); 104 | x(7) = force(1); 105 | x(8) = force(2); 106 | } 107 | 108 | void predict(Vector3f COP_, Vector3f fN_, Vector3f L_); 109 | void update(Vector3f Acc_, Vector3f Pos_, Vector3f Gyro_, Vector3f Gyrodot_); 110 | 111 | float comX, comY, comZ, velX, velY, velZ, fX, 112 | fY, fZ, accX, accY, accZ; 113 | 114 | }; 115 | 116 | #endif -------------------------------------------------------------------------------- /nao_walk_ros/src/socket_client.cpp: -------------------------------------------------------------------------------- 1 | #include"nao_walk/socket_client.h" 2 | #include //memset 3 | #include //For errno - the error number 4 | #include //hostent 5 | #include 6 | #include 7 | #include 8 | #include 9 | SocketClient::SocketClient() 10 | :_sock(-1), 11 | _isConnected(false) 12 | { 13 | } 14 | 15 | SocketClient::~SocketClient() 16 | { 17 | if(_isConnected) 18 | disconnect(); 19 | } 20 | 21 | void SocketClient::disconnect() 22 | { 23 | _isConnected=false; 24 | close(_sock); 25 | _sock=-1; 26 | } 27 | 28 | int SocketClient::socketConnect(std::string hostname, int port) 29 | { 30 | // int sock; 31 | // _sock 32 | // char message[1000] , server_reply[2000]; 33 | 34 | //Create socket 35 | _sock = socket(AF_INET , SOCK_STREAM , 0); 36 | if (_sock == -1) 37 | { 38 | ROS_ERROR("Could not create socket"); 39 | // perror("Could not create socket"); 40 | return 1; 41 | } 42 | // ROS_INFO("Socket created"); 43 | 44 | char ip[64]; 45 | hostname_to_ip(hostname.c_str(),ip); 46 | 47 | server.sin_addr.s_addr = inet_addr(ip); 48 | server.sin_family = AF_INET; 49 | server.sin_port = htons( port ); 50 | 51 | //Connect to remote server 52 | // ROS_INFO("Connecting to %s:%d",ip,port); 53 | if (connect(_sock , (struct sockaddr *)&server , sizeof(server)) < 0) 54 | { 55 | // ROS_ERROR("connect failed. Error"); 56 | close(_sock); 57 | return 1; 58 | } 59 | _isConnected=true; 60 | // ROS_INFO("Connected to socket"); 61 | return 0; 62 | } 63 | 64 | int SocketClient::hostname_to_ip(char const*hostname , char* ip) 65 | { 66 | struct hostent *he; 67 | struct in_addr **addr_list; 68 | int i; 69 | 70 | if ( (he = gethostbyname( hostname ) ) == NULL) 71 | { 72 | // get the host info 73 | // herror("gethostbyname"); 74 | return 1; 75 | } 76 | 77 | addr_list = (struct in_addr **) he->h_addr_list; 78 | 79 | for(i = 0; addr_list[i] != NULL; i++) 80 | { 81 | //Return the first one; 82 | strcpy(ip , inet_ntoa(*addr_list[i]) ); 83 | return 0; 84 | } 85 | 86 | return 1; 87 | } 88 | 89 | int SocketClient::receive(char *data,size_t size) 90 | { 91 | //ROS_INFO("SIZE %lu",size); 92 | size_t red=0; 93 | int r; 94 | while(red0) 151 | return sendToSoc((char*)com.data,com.data_size); 152 | } 153 | 154 | 155 | -------------------------------------------------------------------------------- /nao_walk_naoqi/MovingMedianFilter.h: -------------------------------------------------------------------------------- 1 | //Copyright (c) 2011 ashelly.myopenid.com under 2 | 3 | #include 4 | 5 | //Customize for your data Item type 6 | typedef int Item; 7 | #define ItemLess(a,b) ((a)<(b)) 8 | #define ItemMean(a,b) (((a)+(b))/2) 9 | 10 | typedef struct Mediator_t 11 | { 12 | Item* data; //circular queue of values 13 | int* pos; //index into `heap` for each value 14 | int* heap; //max/median/min heap holding indexes into `data`. 15 | int N; //allocated size. 16 | int idx; //position in circular queue 17 | int ct; //count of items in queue 18 | } Mediator; 19 | 20 | /*--- Helper Functions ---*/ 21 | 22 | #define minCt(m) (((m)->ct-1)/2) //count of items in minheap 23 | #define maxCt(m) (((m)->ct)/2) //count of items in maxheap 24 | 25 | //returns 1 if heap[i] < heap[j] 26 | int mmless(Mediator* m, int i, int j) 27 | { 28 | return ItemLess(m->data[m->heap[i]],m->data[m->heap[j]]); 29 | } 30 | 31 | //swaps items i&j in heap, maintains indexes 32 | int mmexchange(Mediator* m, int i, int j) 33 | { 34 | int t = m->heap[i]; 35 | m->heap[i]=m->heap[j]; 36 | m->heap[j]=t; 37 | m->pos[m->heap[i]]=i; 38 | m->pos[m->heap[j]]=j; 39 | return 1; 40 | } 41 | 42 | //swaps items i&j if i1 && i < minCt(m) && mmless(m, i+1, i)) { ++i; } 53 | if (!mmCmpExch(m,i,i/2)) { break; } 54 | } 55 | } 56 | 57 | //maintains maxheap property for all items below i/2. (negative indexes) 58 | void maxSortDown(Mediator* m, int i) 59 | { 60 | for (; i >= -maxCt(m); i*=2) 61 | { if (i<-1 && i > -maxCt(m) && mmless(m, i, i-1)) { --i; } 62 | if (!mmCmpExch(m,i/2,i)) { break; } 63 | } 64 | } 65 | 66 | //maintains minheap property for all items above i, including median 67 | //returns true if median changed 68 | int minSortUp(Mediator* m, int i) 69 | { 70 | while (i>0 && mmCmpExch(m,i,i/2)) i/=2; 71 | return (i==0); 72 | } 73 | 74 | //maintains maxheap property for all items above i, including median 75 | //returns true if median changed 76 | int maxSortUp(Mediator* m, int i) 77 | { 78 | while (i<0 && mmCmpExch(m,i/2,i)) i/=2; 79 | return (i==0); 80 | } 81 | 82 | /*--- Public Interface ---*/ 83 | 84 | 85 | //creates new Mediator: to calculate `nItems` running median. 86 | //mallocs single block of memory, caller must free. 87 | Mediator* MediatorNew(int nItems) 88 | { 89 | int size = sizeof(Mediator)+nItems*(sizeof(Item)+sizeof(int)*2); 90 | Mediator* m= malloc(size); 91 | m->data= (Item*)(m+1); 92 | m->pos = (int*) (m->data+nItems); 93 | m->heap = m->pos+nItems + (nItems/2); //points to middle of storage. 94 | m->N=nItems; 95 | m->ct = m->idx = 0; 96 | while (nItems--) //set up initial heap fill pattern: median,max,min,max,... 97 | { m->pos[nItems]= ((nItems+1)/2) * ((nItems&1)?-1:1); 98 | m->heap[m->pos[nItems]]=nItems; 99 | } 100 | return m; 101 | } 102 | 103 | 104 | //Inserts item, maintains median in O(lg nItems) 105 | void MediatorInsert(Mediator* m, Item v) 106 | { 107 | int isNew=(m->ctN); 108 | int p = m->pos[m->idx]; 109 | Item old = m->data[m->idx]; 110 | m->data[m->idx]=v; 111 | m->idx = (m->idx+1) % m->N; 112 | m->ct+=isNew; 113 | if (p>0) //new item is in minHeap 114 | { if (!isNew && ItemLess(old,v)) { minSortDown(m,p*2); } 115 | else if (minSortUp(m,p)) { maxSortDown(m,-1); } 116 | } 117 | else if (p<0) //new item is in maxheap 118 | { if (!isNew && ItemLess(v,old)) { maxSortDown(m,p*2); } 119 | else if (maxSortUp(m,p)) { minSortDown(m, 1); } 120 | } 121 | else //new item is at median 122 | { if (maxCt(m)) { maxSortDown(m,-1); } 123 | if (minCt(m)) { minSortDown(m, 1); } 124 | } 125 | } 126 | 127 | //returns median item (or average of 2 when item count is even) 128 | Item MediatorMedian(Mediator* m) 129 | { 130 | Item v= m->data[m->heap[0]]; 131 | if ((m->ct&1)==0) { v= ItemMean(v,m->data[m->heap[-1]]); } 132 | return v; 133 | } 134 | 135 | 136 | -------------------------------------------------------------------------------- /nao_walk_naoqi/SocketServer.cpp: -------------------------------------------------------------------------------- 1 | #include"SocketServer.h" 2 | 3 | 4 | #include 5 | #include 6 | #include 7 | #include"Logs.h" 8 | 9 | #include 10 | #include 11 | 12 | SocketServer::SocketServer() 13 | :server_fd(-1), 14 | client_conn(-1) 15 | {} 16 | 17 | SocketServer::~SocketServer() 18 | { 19 | socClose(); 20 | } 21 | 22 | int SocketServer::socInit(int port) 23 | { 24 | int opt = 1; 25 | if ((server_fd = socket(AF_INET, SOCK_STREAM, 0)) == 0) 26 | { 27 | LOG_ERR("socket failed"); 28 | return -1; 29 | } 30 | /* 31 | * No SO_REUSEPORT 32 | if (setsockopt(server_fd, SOL_SOCKET, SO_REUSEADDR | SO_REUSEPORT, &opt, sizeof(opt))) 33 | */ 34 | if (setsockopt(server_fd, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt))) 35 | { 36 | LOG_ERR("setsockopt"); 37 | return -1; 38 | } 39 | address.sin_family = AF_INET; 40 | address.sin_addr.s_addr = INADDR_ANY; 41 | address.sin_port = htons( port ); 42 | 43 | if (bind(server_fd, (struct sockaddr *)&address, sizeof(address))<0) 44 | { 45 | LOG_ERR("bind failed"); 46 | return -1; 47 | } 48 | return 0; 49 | } 50 | 51 | int SocketServer::socListen() 52 | { 53 | if (listen(server_fd, 1) < 0) 54 | { 55 | LOG_ERR("listen"); 56 | return -1; 57 | } 58 | return 0; 59 | } 60 | 61 | int SocketServer::socAccept() 62 | { 63 | int addrlen = sizeof(address); 64 | if ((client_conn = accept(server_fd, (struct sockaddr *)&address, 65 | (socklen_t*)&addrlen))<0) 66 | { 67 | client_conn=-1; 68 | LOG_ERR("Socket accepting error"); 69 | return -1; 70 | } 71 | LOG_INF("Socket accepted"); 72 | return 0; 73 | } 74 | 75 | int SocketServer::sendToSoc(char *buf,size_t size) 76 | { 77 | size_t written=0; 78 | int r=0; 79 | while(written0) 154 | { 155 | com.data=malloc(com.data_size); 156 | return receive((char*)com.data,com.data_size); 157 | } 158 | com.data=0; 159 | return 0; 160 | } 161 | 162 | 163 | void SocketServer::socClose() 164 | { 165 | if(client_conn!=-1) 166 | { 167 | close(client_conn); 168 | client_conn=-1; 169 | } 170 | 171 | if(server_fd>0) 172 | close(server_fd); 173 | } -------------------------------------------------------------------------------- /nao_walk_naoqi/Kalman.cpp: -------------------------------------------------------------------------------- 1 | #include "Kalman.h" 2 | 3 | Kalman::Kalman(RobotParameters &robot):OurRobot(robot) 4 | { 5 | /*Akalman(0,0)=1.000; 6 | Akalman(0,1)=0;//-OurRobot.getWalkParameter(Ts); 7 | Akalman(1,0)=0.000; 8 | Akalman(1,1)=1.000;*/ 9 | Bkalman.zero(); 10 | Bkalman(0)=1.000;//OurRobot.getWalkParameter(Ts); 11 | StateKalman.zero(); 12 | ProcessNoise.zero(); 13 | ProcessNoise(0,0)=1*1e-5; //5*1e-7; 14 | s.zero(); 15 | P.zero(); 16 | Kgain.zero(); 17 | P(0,0)=1e-20; 18 | MeasurementNoise.identity(); 19 | 20 | CoM_Noise = 0.01; 21 | COP_Noise = 0.1; 22 | MeasurementNoise(0,0) = CoM_Noise; 23 | MeasurementNoise(1,1) = COP_Noise; 24 | Ckalman.zero(); 25 | Ckalman(0,0)=1.000; 26 | Ckalman(1,0)=1.000; 27 | StatePredict.zero(); 28 | ykalman.zero(); 29 | 30 | std::cout<<"ZMP Delayed Kalman Filter Initialized Successfully"<0) 67 | uBuffer.pop(); 68 | 69 | while(combuffer.size()>0) 70 | combuffer.pop(); 71 | 72 | std::cout<<"ZMP Delayed Kalman Filter Reseted"<1e-15) 98 | { 99 | P=P+ProcessNoise; 100 | doup=true; 101 | 102 | if(combuffer.size()==2) 103 | { 104 | 105 | 106 | ct2=combuffer.front(); 107 | combuffer.pop(); 108 | ct1=combuffer.front(); 109 | combuffer.pop(); 110 | combuffer.push(ct1); 111 | 112 | 113 | float comddot=(CoMMeasured-2*ct1+ct2)/(OurRobot.getWalkParameter(Ts)*OurRobot.getWalkParameter(Ts) ); 114 | zmpfromcom=CoMMeasured-(OurRobot.getWalkParameter(ComZ)/OurRobot.getWalkParameter(g))*comddot; 115 | } 116 | 117 | } 118 | 119 | combuffer.push(CoMMeasured); 120 | 121 | /** Update **/ 122 | //StateKalman.prettyPrint(); 123 | if(doup) 124 | { 125 | /** innovation value **/ 126 | ykalman=KVecFloat2(ZMPMeasured,zmpfromcom); 127 | ykalman+=(Ckalman*(StateKalman)).scalar_mult(-1.0); 128 | 129 | s=Ckalman*P*Ckalman.transp()+MeasurementNoise; 130 | s.fast_invert(); 131 | Kgain=(P*Ckalman.transp())*s; 132 | StateKalman+=Kgain*ykalman; 133 | //StateKalman.prettyPrint(); 134 | } 135 | 136 | //StateKalman.prettyPrint(); 137 | 138 | Kgain.scalar_mult(-1.0); 139 | P+=Kgain*Ckalman*P; 140 | if(combuffer.size()>2) 141 | combuffer.pop(); 142 | 143 | 144 | if(uBuffer.size()>ZMPKALMANDELAY) 145 | uBuffer.pop(); 146 | 147 | 148 | /** Getting Rid of the ZMP delay **/ 149 | StatePredict=StateKalman; 150 | unsigned bufsize=uBuffer.size(); 151 | for(unsigned i=0; i 3 | Copyright (C) 4 | 5 | This program is free software; you can redistribute it and/or modify 6 | it under the terms of the GNU General Public License as published by 7 | the Free Software Foundation; either version 2 of the License, or 8 | (at your option) any later version. 9 | 10 | This program is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | GNU General Public License for more details. 14 | 15 | You should have received a copy of the GNU General Public License along 16 | with this program; if not, write to the Free Software Foundation, Inc., 17 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 18 | 19 | */ 20 | 21 | #ifndef LOCKEDBUFFER_HPP 22 | #define LOCKEDBUFFER_HPP 23 | #include "SystemMutex.hpp" 24 | #include 25 | #include 26 | #include 27 | 28 | 29 | template 30 | class LockedBuffer 31 | { 32 | public: 33 | typedef boost::function1 *> NotifierFncPtr; 34 | 35 | explicit 36 | LockedBuffer(std::size_t nid); 37 | ~LockedBuffer(); 38 | void add( std::vector const & tuples); 39 | bool tryadd( std::vector const & tuples); 40 | std::size_t size() 41 | { 42 | KSystem::SystemMutex::scoped_lock data_lock(mutex); 43 | return msg_buf.size(); 44 | 45 | } 46 | 47 | void add(const T & t); 48 | std::vector remove(); 49 | T removeOne(); 50 | T readOne(); 51 | std::vector tryremove(); 52 | //bool operator==( MessageBuffer& other) ; 53 | std::size_t getOwnerID() const 54 | { 55 | return ownerId; 56 | }; 57 | //MessageQueue & getQueue() {return mq;}; 58 | void setNotifier(NotifierFncPtr an) 59 | { 60 | KSystem::SystemMutex::scoped_lock data_lock(mutex); 61 | n = an; 62 | }; 63 | void setCleanUp(NotifierFncPtr ac) 64 | { 65 | KSystem::SystemMutex::scoped_lock ata_lock(mutex); 66 | c = ac; 67 | }; 68 | private: 69 | 70 | std::vector msg_buf; 71 | std::size_t ownerId; 72 | KSystem::SystemMutex mutex; 73 | NotifierFncPtr n; 74 | NotifierFncPtr c; 75 | }; 76 | 77 | using namespace KSystem; 78 | 79 | 80 | template 81 | LockedBuffer::LockedBuffer(std::size_t nid): 82 | ownerId(nid), n(NULL), c(NULL) 83 | { 84 | } 85 | template 86 | LockedBuffer::~LockedBuffer() 87 | { 88 | SystemMutex::scoped_lock data_lock(mutex); 89 | 90 | if(c != NULL) 91 | c(this); 92 | } 93 | 94 | 95 | 96 | template 97 | void LockedBuffer::add( std::vector const & tuples) 98 | { 99 | SystemMutex::scoped_lock data_lock(mutex); 100 | msg_buf.reserve(msg_buf.size() + tuples.size()); 101 | msg_buf.insert(msg_buf.end(), tuples.begin(), tuples.end()); 102 | data_lock.unlock(); 103 | 104 | if(tuples.size() > 0 && n != NULL) 105 | { 106 | //mq.requestMailMan(this); 107 | n(this);//Call notifier 108 | } 109 | } 110 | 111 | template 112 | bool LockedBuffer::tryadd( std::vector const & tuples) 113 | { 114 | if(!mutex.try_lock()) 115 | return false; 116 | 117 | msg_buf.reserve(msg_buf.size() + tuples.size()); 118 | msg_buf.insert(msg_buf.end(), tuples.begin(), tuples.end()); 119 | mutex.unlock(); 120 | 121 | if(tuples.size() > 0 && n != NULL) 122 | { 123 | //mq.requestMailMan(this); 124 | n(this);//Call notifier 125 | } 126 | 127 | return true; 128 | } 129 | 130 | template 131 | void LockedBuffer::add(const T & t) 132 | { 133 | SystemMutex::scoped_lock data_lock(mutex); 134 | msg_buf.push_back(t); 135 | data_lock.unlock(); 136 | 137 | if( n != NULL) 138 | { 139 | n(this);//Call notifier 140 | //mq.requestMailMan(this); 141 | } 142 | } 143 | template 144 | std::vector LockedBuffer::remove() 145 | { 146 | SystemMutex::scoped_lock data_lock(mutex); 147 | std::vector oldtupples = msg_buf; 148 | msg_buf.clear(); 149 | return oldtupples; 150 | } 151 | 152 | template 153 | T LockedBuffer::readOne() 154 | { 155 | SystemMutex::scoped_lock data_lock(mutex); 156 | T t= msg_buf.front(); 157 | return t; 158 | } 159 | 160 | template 161 | T LockedBuffer::removeOne() 162 | { 163 | SystemMutex::scoped_lock data_lock(mutex); 164 | T t= msg_buf.front(); 165 | std::vector oldtupples = msg_buf; 166 | msg_buf.clear(); 167 | msg_buf.insert(msg_buf.begin(), ++(oldtupples.begin()), oldtupples.end()); 168 | return t; 169 | } 170 | 171 | 172 | 173 | #endif // BUFFER_H 174 | -------------------------------------------------------------------------------- /nao_walk_naoqi/KinematicsDefines.h: -------------------------------------------------------------------------------- 1 | /* FIXED TO MATCH V4 SPECIFICATIONS 2 | @author Mumra 3 | */ 4 | #ifndef KINEMATICDEFINES_H 5 | #define KINEMATICDEFINES_H 6 | 7 | //Define for the lengths of the arms foots etc 8 | #define ShoulderOffsetY 98.0 9 | #define ShoulderOffsetZ 100.0 10 | #define ElbowOffsetY 15.0 11 | #define UpperArmLength 105.0 12 | #define LowerArmLength 55.95 13 | #define HandOffsetX 57.75 14 | #define HandOffsetZ 12.31 15 | #define HipOffsetZ 85.0 16 | #define HipOffsetY 50.0 17 | #define ThighLength 100.0 18 | #define TibiaLength 102.9 19 | #define FootHeight 45.19 20 | #define NeckOffsetZ 126.5 21 | #define CameraBotomX 48.8 22 | #define CameraBotomZ 23.81 23 | #define CameraTopX 53.9 24 | #define CameraTopZ 67.9 25 | 26 | //Head Limits 27 | #define HeadYawHigh 2.0857 28 | #define HeadYawLow -2.0857 29 | #define HeadPitchHigh 0.5149 30 | #define HeadPitchLow -0.6720 31 | //Left Hand limits 32 | #define LShoulderPitchHigh 2.0857 33 | #define LShoulderPitchLow -2.0857 34 | #define LShoulderRollHigh 1.3265 35 | #define LShoulderRollLow -0.3142 36 | #define LElbowYawHigh 2.0875 37 | #define LElbowYawLow -2.0875 38 | #define LElbowRollHigh 0.00001f//Aldebaran gives this value (-0.0349f) but the hand can go further 39 | #define LElbowRollLow -1.5446 40 | #define LWristYawHigh 1.8238 41 | #define LWristYawLow -1.8238 42 | //Right Hand limits 43 | #define RShoulderPitchHigh 2.0857 44 | #define RShoulderPitchLow -2.0857 45 | #define RShoulderRollHigh 0.3142 46 | #define RShoulderRollLow -1.3265 47 | #define RElbowYawHigh 2.0875 48 | #define RElbowYawLow -2.0875 49 | #define RElbowRollHigh 1.5446 50 | #define RElbowRollLow -0.00001f//Aldebaran gives this value (0.0349f) but the hand can go further 51 | #define RWristYawHigh 1.8238 52 | #define RWristYawLow -1.8238 53 | //Left Leg limits 54 | #define LHipYawPitchHigh 0.740810 55 | #define LHipYawPitchLow -1.145303 56 | #define LHipRollHigh 0.790477 57 | #define LHipRollLow -0.379472 58 | #define LHipPitchHigh 0.484090 59 | #define LHipPitchLow -1.535889 60 | #define LKneePitchHigh 2.112528 61 | #define LKneePitchLow -0.092346 62 | #define LAnklePitchHigh 0.922747 63 | #define LAnklePitchLow -1.189516 64 | #define LAnkleRollHigh 0.769001 65 | #define LAnkleRollLow -0.397880 66 | //Left Right limits 67 | #define RHipYawPitchHigh 0.740810 68 | #define RHipYawPitchLow -1.145303 69 | #define RHipRollHigh 0.379472 70 | #define RHipRollLow -0.790477 71 | #define RHipPitchHigh 0.484090 72 | #define RHipPitchLow -1.535889 73 | #define RKneePitchHigh 2.120198 74 | #define RKneePitchLow -0.103083 75 | #define RAnklePitchHigh 0.932056 76 | #define RAnklePitchLow -1.186448 77 | #define RAnkleRollHigh 0.397935 78 | #define RAnkleRollLow -0.768992 79 | 80 | //Masses defines 81 | //Total mass + battery 82 | #define TotalMassH25 5.182530 83 | //Torso 84 | #define TorsoMass 1.0496 85 | #define TorsoX -4.13 86 | #define TorsoY 0.00 87 | #define TorsoZ 43.42 88 | 89 | //Not provided by aldebaran 90 | #define BatteryMass 0.345 91 | #define BatteryX -30.00 92 | #define BatteryY 0.00 93 | #define BatteryZ 39.00 94 | 95 | //Head 96 | #define HeadYawMass 0.06442 97 | #define HeadYawX -0.01 98 | #define HeadYawY 0.000 99 | #define HeadYawZ -27.42 100 | 101 | #define HeadPitchMass 0.60533 102 | #define HeadPitchX -1.1200 103 | #define HeadPitchY 0.000 104 | #define HeadPitchZ 52.5800 105 | 106 | //Right Hand 107 | #define RShoulderPitchMass 0.07504 108 | #define RShoulderPitchX -1.6500 109 | #define RShoulderPitchY 26.6300 110 | #define RShoulderPitchZ 0.14 111 | 112 | #define RShoulderRollMass 0.15777 113 | #define RShoulderRollX 24.5500 114 | #define RShoulderRollY -5.6300 115 | #define RShoulderRollZ 3.3 116 | 117 | #define RElbowYawMass 0.06483 118 | #define RElbowYawX -27.44 119 | #define RElbowYawY 0.00 120 | #define RElbowYawZ -0.14 121 | 122 | #define RElbowRollMass 0.07761 123 | #define RElbowRollX 25.5600 124 | #define RElbowRollY -2.8100 125 | #define RElbowRollZ 0.7600 126 | 127 | 128 | #define RWristYawMass 0.18533 129 | #define RWristYawX LowerArmLength+34.34 130 | #define RWristYawY -0.88 131 | #define RWristYawZ 3.08 132 | 133 | //Right Leg 134 | #define RHipYawPitchMass 0.06981 135 | #define RHipYawPitchX -7.8100 136 | #define RHipYawPitchY 11.14 137 | #define RHipYawPitchZ 26.61 138 | 139 | #define RHipRollMass 0.13053 140 | #define RHipRollX -15.49 141 | #define RHipRollY -0.29 142 | #define RHipRollZ -5.15 143 | 144 | #define RHipPitchMass 0.38968 145 | #define RHipPitchX 1.38 146 | #define RHipPitchY -2.21 147 | #define RHipPitchZ -53.73 148 | 149 | #define RKneePitchMass 0.29142 150 | #define RKneePitchX 4.5300 151 | #define RKneePitchY -2.2500 152 | #define RKneePitchZ -49.3600 153 | 154 | #define RAnklePitchMass 0.13416 155 | #define RAnklePitchX 0.4500 156 | #define RAnklePitchY -0.29 157 | #define RAnklePitchZ 6.85 158 | 159 | #define RAnkleRollMass 0.16184 160 | #define RAnkleRollX 25.42 161 | #define RAnkleRollY -3.30 162 | #define RAnkleRollZ -32.39 163 | 164 | #endif 165 | -------------------------------------------------------------------------------- /nao_walk_naoqi/ZMPDistributor.cpp: -------------------------------------------------------------------------------- 1 | #include "ZMPDistributor.h" 2 | /* alpha = 1.0 -> right */ 3 | 4 | ZMPDistributor::ZMPDistributor(RobotParameters &robot_):robot(robot_), LFoot(robot_), RFoot(robot_) 5 | { 6 | frd.setZero(); 7 | fld.setZero(); 8 | tauld.setZero(); 9 | taurd.setZero(); 10 | tau0.setZero(); 11 | taul.setZero(); 12 | taur.setZero(); 13 | a = 0.000; 14 | maxForceReadingL = robot.getWalkParameter(mass)*robot.getWalkParameter(g); 15 | maxForceReadingR = robot.getWalkParameter(mass)*robot.getWalkParameter(g); 16 | 17 | 18 | std::cout<<"ZMP Distributor Initialized Successfully"<1.0) 80 | a= 1.0; 81 | 82 | 83 | frd(2) = a * maxForceReadingR; 84 | fld(2) = (1.0-a) * maxForceReadingL; 85 | 86 | tau0 = -(RFoot.footA - pzmp_d).cross(frd) - (LFoot.footA - pzmp_d).cross(fld); 87 | taurd(1) = a * tau0(1); 88 | tauld(1) = (1.0-a) * tau0(1); 89 | 90 | if(tau0(0)<0) 91 | { 92 | taurd(0) = tau0(0); 93 | tauld(0) = 0.0; 94 | } 95 | else 96 | { 97 | taurd(0) = 0.0; 98 | tauld(0) = tau0(0); 99 | } 100 | } 101 | 102 | //If the measuerd ZMP is inside of one of the support polygons 103 | if(LFoot.pnpoly(pzmp(0),pzmp(1))) 104 | { 105 | a=0.0; 106 | fl = fl_ + fext; 107 | taul = (LFoot.footA - pzmp).cross(fl); 108 | } 109 | else if(RFoot.pnpoly(pzmp(0),pzmp(1))) 110 | { 111 | a=1.0; 112 | fr = fr_ + fext; 113 | taur = (RFoot.footA - pzmp).cross(fr); 114 | } 115 | else 116 | { 117 | pL = computePointLineIntersection(Vector2f(pzmp(0),pzmp(1)), Vector2f(LFoot.footRF(0),LFoot.footRF(1)), Vector2f(LFoot.footRH(0),LFoot.footRH(1))); 118 | pR = computePointLineIntersection(Vector2f(pzmp(0),pzmp(1)), Vector2f(RFoot.footLF(0),RFoot.footLF(1)), Vector2f(RFoot.footLH(0),RFoot.footLH(1))); 119 | pa = computePointLineIntersection(Vector2f(pzmp(0),pzmp(1)), pR, pL); 120 | 121 | //Compute Distribution parameter a 122 | a = (pa - pL).norm()/((pL-pR).norm()); 123 | if(a>1.0) 124 | a= 1.0; 125 | 126 | 127 | fr=fr_+a*fext; 128 | fl=fl_+(1.0-a)*fext; 129 | tau0 = -(RFoot.footA - pzmp).cross(fr) - (LFoot.footA - pzmp).cross(fl); 130 | taur(1) = a * tau0(1); 131 | taul(1) = (1.0-a) * tau0(1); 132 | 133 | if(tau0(0)<0) 134 | { 135 | taur(0) = tau0(0); 136 | taul(0) = 0.0; 137 | } 138 | else 139 | { 140 | taur(0) = 0.0; 141 | taul(0) = tau0(0); 142 | } 143 | } 144 | } 145 | 146 | 147 | } 148 | 149 | 150 | 151 | -------------------------------------------------------------------------------- /nao_walk_naoqi/Common.hpp: -------------------------------------------------------------------------------- 1 | #ifndef COMMON_HPP 2 | #define COMMON_HPP 3 | 4 | #include 5 | #include 6 | 7 | // DO NOT USE DEFINES, USE THE BLOODY FUNCTIONS !!! 8 | 9 | #define TO_RAD_SUPER_INTERNAL_DO_NOT_USE 0.01745329 10 | 11 | namespace KMath { 12 | 13 | const float INF = std::numeric_limits::infinity(); 14 | 15 | inline static double TO_RAD(double X) { return X * TO_RAD_SUPER_INTERNAL_DO_NOT_USE; } 16 | inline static double TO_DEG(double X) { return X / TO_RAD_SUPER_INTERNAL_DO_NOT_USE; } 17 | 18 | template int sign(T val) { 19 | return (T(0) < val) - (val < T(0)); 20 | } 21 | 22 | template inline T DISTANCE(T x1, T x2, T y1, T y2) { 23 | return sqrt((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2)); 24 | } 25 | 26 | template inline T norm2(T dx, T dy) { 27 | return sqrt((dx) * (dx) + (dy) * (dy)); 28 | } 29 | 30 | template inline T Min(T a, T b) { 31 | return (a < b) ? a : b; 32 | } 33 | 34 | template inline T Max(T a, T b) { 35 | return (a > b) ? a : b; 36 | } 37 | 38 | /** 39 | * @fn inline static unsigned int factorial(unsigned int x) 40 | * @brief The product of all positive integers less than or equal to x. 41 | */ 42 | inline static unsigned int factorial(unsigned int x) { 43 | unsigned int r = 1; 44 | for (unsigned int i = 1 ; i <= x ; i++) 45 | r *= i; 46 | return r; 47 | } 48 | 49 | /** 50 | * @fn inline static unsigned int decreasing_factorial(unsigned int x,unsigned int k) 51 | * @brief The product of all positive integers less than or equal to x. and greater than k. 52 | */ 53 | inline static unsigned int decreasing_factorial(unsigned int k,unsigned int x) { 54 | unsigned int r = 1; 55 | for( unsigned int i = k ; i <= x ; i++) 56 | r *= i; 57 | return r; 58 | } 59 | 60 | 61 | /** 62 | * @fn inline static unsigned int binomialCoefficient(unsigned int n, unsigned int k) 63 | * @brief For any set containing n elements, the number of distinct k-element subsets of it that can be formed 64 | * (the k-combinations of its elements) is given by the binomial coefficient. Calculated using factorials 65 | * formula. 66 | */ 67 | inline static unsigned int binomialCoefficient(unsigned int n, unsigned int k) { 68 | return (decreasing_factorial(n - k + 1, n) / factorial(k)); 69 | } 70 | 71 | /** 72 | * @fn inline static float gaussian2D(float A, float xCenter, float yCenter, float sigmaX, float sigmaY, float x, float y) 73 | * @brief 2-Dimensional Gaussian function. 74 | * @parameter A is the amplitude. 75 | * @parameter xCenter, yCenter is the center of the blob. 76 | * @parameter sigmaX, sigmaY are the x and y spreads of the blob. 77 | */ 78 | inline static float gaussian2D(float A, float xCenter, float yCenter, float sigmaX, float sigmaY, float x, float y) { 79 | return A*exp( -( pow(x - xCenter, 2)/(2*pow(sigmaX, 2)) + pow(y - yCenter, 2)/(2*pow(sigmaY, 2)) ) ); 80 | } 81 | 82 | /** 83 | * @fn template inline T FUNC1ByX(T x) 84 | * @brief function f(x)=1/|x| used to generate supporters positions. 85 | * @return the result of 1/|x| or infinity number in case x = 0. 86 | */ 87 | template inline T func1ByAbsX(T x) { 88 | if (x == 0) 89 | return INF; 90 | return 1/fabs(x); 91 | } 92 | 93 | inline static double wrapToPi(double angle) { 94 | while (angle > M_PI) 95 | angle -= 2.0 * M_PI; 96 | 97 | while (angle < -M_PI) 98 | angle += 2.0 * M_PI; 99 | 100 | return angle; 101 | } 102 | 103 | inline static double wrapTo2Pi(double angle) { 104 | while (angle > 2.0 * M_PI) 105 | angle -= 2.0 * M_PI; 106 | 107 | while (angle < -2.0 * M_PI) 108 | angle += 2.0 * M_PI; 109 | 110 | return angle; 111 | } 112 | 113 | inline static double wrapTo0_2Pi(double angle) { 114 | while (angle > 2.0 * M_PI) 115 | angle -= 2.0 * M_PI; 116 | 117 | while (angle < 0) 118 | angle += 2.0 * M_PI; 119 | 120 | return angle; 121 | } 122 | 123 | inline static double anglediff2(double a1, double a2) { 124 | return wrapToPi(wrapToPi(a1 + M_PI - a2) - M_PI); 125 | } 126 | 127 | inline static double anglediff(double a1, double a2) { 128 | return fabs(wrapTo0_2Pi(a1 + M_PI - a2) - M_PI); 129 | } 130 | 131 | inline static double toPolarD(double x, double y) { 132 | return sqrt(x * x + y * y); 133 | } 134 | 135 | inline static double toPolarT(double x, double y) { 136 | return atan2(y, x); 137 | } 138 | 139 | inline static double toCartesianX(double d, double t) { 140 | return d * cos(t); 141 | } 142 | 143 | inline static double toCartesianY(double d, double t) { 144 | return d * sin(t); 145 | } 146 | 147 | inline float fast_atan2f(const float y,const float x ) 148 | { 149 | // |error| < 0.005 150 | #define PI_FLOAT 3.14159265f 151 | #define PIBY2_FLOAT 1.5707963f 152 | if ( x == 0.0f ) 153 | { 154 | if ( y > 0.0f ) return PIBY2_FLOAT; 155 | if ( y == 0.0f ) return 0.0f; 156 | return -PIBY2_FLOAT; 157 | } 158 | float atan; 159 | float z = y/x; 160 | if ( fabsf( z ) < 1.0f ) 161 | { 162 | atan = z/(1.0f + 0.28f*z*z); 163 | if ( x < 0.0f ) 164 | { 165 | if ( y < 0.0f ) return atan - PI_FLOAT; 166 | return atan + PI_FLOAT; 167 | } 168 | } 169 | else 170 | { 171 | atan = PIBY2_FLOAT - z/(z*z + 0.28f); 172 | if ( y < 0.0f ) return atan - PI_FLOAT; 173 | } 174 | return atan; 175 | } 176 | } 177 | #endif 178 | -------------------------------------------------------------------------------- /nao_walk_naoqi/FootPolygon.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "FootPolygon.h" 3 | FootPolygon::FootPolygon(RobotParameters& robot_):robot(robot_) 4 | { 5 | footLF.setZero(); 6 | footRF.setZero(); 7 | footLH.setZero(); 8 | footRH.setZero(); 9 | footA.setZero(); 10 | polygon.setZero(); 11 | stepXF = robot.getWalkParameter(StepXF); 12 | stepXH = robot.getWalkParameter(StepXH); 13 | stepYL = robot.getWalkParameter(StepYL); 14 | stepYR = robot.getWalkParameter(StepYR); 15 | } 16 | void FootPolygon::setPolygon(Vector3f footA_, Affine3f T) 17 | { 18 | 19 | 20 | //Set Ankle 3D position w.r.t. Inertial frame of Reference 21 | footA = footA_; 22 | //Transform to Inertial frame of Reference 23 | footLF = T.linear() * Vector3f(stepXF,stepYL,0.0) + footA; 24 | footLH = T.linear() * Vector3f(stepXH,stepYL,0.0) + footA; 25 | footRF = T.linear() * Vector3f(stepXF,stepYR,0.0) + footA; 26 | footRH = T.linear() * Vector3f(stepXH,stepYR,0.0) + footA; 27 | 28 | polygon(0,0) = footLF(0); 29 | polygon(0,1) = footLF(1); 30 | polygon(1,0) = footLH(0); 31 | polygon(1,1) = footLH(1); 32 | polygon(2,0) = footRH(0); 33 | polygon(2,1) = footRH(1); 34 | polygon(3,0) = footRF(0); 35 | polygon(3,1) = footRF(1); 36 | 37 | 38 | 39 | numNodes = 4; 40 | 41 | } 42 | 43 | 44 | // Returns true if the point p lies inside the polygon[] with n vertices 45 | bool FootPolygon::checkIn(float x_, float y_) 46 | { 47 | // Check if point lies on same side of each segment. We will check each 48 | // segment in order. 49 | // Assumption: vertices are given in either clockwise or 50 | // counterclockwise manner. 51 | Vector3f current_poly; 52 | int current_sign; 53 | int check_sign; 54 | bool insidePoly=true; 55 | for(int k = 0; k0) 79 | { 80 | current_sign = 1; 81 | } 82 | else 83 | { 84 | current_sign = -1; 85 | } 86 | 87 | if(k == 0) 88 | check_sign = current_sign; 89 | else if(check_sign != current_sign) 90 | insidePoly = false; 91 | 92 | } 93 | return insidePoly; 94 | 95 | } 96 | bool FootPolygon::pnpoly(float x, float y) 97 | { 98 | // If we never cross any lines we're inside. 99 | bool inside = false; 100 | 101 | // Loop through all the edges. 102 | for (int i = 0; i < polygon.rows(); ++i) 103 | { 104 | // i is the index of the first vertex, j is the next one. 105 | // The original code uses a too-clever trick for this. 106 | int j = (i + 1) % polygon.rows(); 107 | 108 | // The vertices of the edge we are checking. 109 | double xp0 = polygon(i, 0); 110 | double yp0 = polygon(i, 1); 111 | double xp1 = polygon(j, 0); 112 | double yp1 = polygon(j, 1); 113 | 114 | // Check whether the edge intersects a line from (-inf,y) to (x,y). 115 | 116 | // First check if the line crosses the horizontal line at y in either direction. 117 | if ( ((yp0 <= y) && (yp1 > y)) || ((yp1 <= y) && (yp0 > y))) 118 | { 119 | // If so, get the point where it crosses that line. This is a simple solution 120 | // to a linear equation. Note that we can't get a division by zero here - 121 | // if yp1 == yp0 then the above if be false. 122 | double cross = (xp1 - xp0) * (y - yp0) / (yp1 - yp0) + xp0; 123 | 124 | // Finally check if it crosses to the left of our test point. You could equally 125 | // do right and it should give the same result. 126 | if (cross < x) 127 | inside = !inside; 128 | } 129 | } 130 | return inside; 131 | } -------------------------------------------------------------------------------- /nao_walk_naoqi/PostureController.cpp: -------------------------------------------------------------------------------- 1 | #include "PostureController.h" 2 | #include "KinematicsDefines.h" 3 | #include "ZMPDistributor.h" 4 | 5 | PostureController::PostureController(RobotParameters &robot): NaoRobot(robot) 6 | { 7 | dTorso_Roll = 0.0; 8 | dTorso_Pitch = 0.0; 9 | 10 | dLAnkle_Roll = 0.0; 11 | dLAnkle_Pitch = 0.0; 12 | dRAnkle_Roll = 0.0; 13 | dRAnkle_Pitch = 0.0; 14 | 15 | dKnee_Pitch = 0.0; 16 | TH = 0.0 * 3.14/180.0; 17 | default_params(); 18 | std::cout<<"Real-Time Posture Controller Initialized Successfully"<LAnklePitchHigh)lankle_Pitch=LAnklePitchHigh; 94 | if(lankle_PitchRAnklePitchHigh) rankle_Pitch = RAnklePitchHigh; 96 | if(rankle_PitchLAnkleRollHigh) lankle_Roll=LAnkleRollHigh; 99 | if(lankle_RollRAnkleRollHigh) rankle_Roll=LAnkleRollHigh; 101 | if(rankle_RollLKneePitchHigh)lknee_Pitch=LKneePitchHigh; 118 | if(lknee_PitchRKneePitchHigh)rknee_Pitch=RKneePitchHigh; 120 | if(rknee_PitchLHipPitchHigh - TH)lhip_Pitch=LHipPitchHigh - TH; 145 | if(lhip_PitchRHipPitchHigh - TH) rhip_Pitch = RHipPitchHigh - TH; 147 | if(rhip_PitchLAnkleRollHigh - TH) lhip_Roll=LHipRollHigh - TH; 150 | if(lhip_RollRAnkleRollHigh - TH) rhip_Roll=LHipRollHigh - TH; 152 | if(rhip_Roll 10 | #include 11 | #include 12 | #include 13 | #include // std::out_of_range 14 | #include 15 | 16 | template 17 | std::string _toString(T val) 18 | { 19 | std::ostringstream ost; 20 | ost << val; 21 | ost.flush(); 22 | return ost.str(); 23 | } 24 | 25 | mat_log::mat_log(string base_log_name, SAVETYPE type, int autosave_period) 26 | { 27 | log_files_counter = 0; 28 | this->log_name = base_log_name; 29 | counter = 0; 30 | std::time_t t = std::time(NULL); 31 | std::tm tm = *std::localtime(&t); 32 | std::strftime(timebuf, TIMEBUF, "%F_%H%Ms%S", &tm); 33 | this->type = type; 34 | this->autosave_period = autosave_period; 35 | more_vars_added = false; 36 | } 37 | ; 38 | 39 | void mat_log::insert(string name, num_type value) 40 | { 41 | num_type *table = 0; 42 | try 43 | { 44 | num_type *table = data.at(name); // vector::at throws an out-of-range 45 | table[counter] = value; //if name does exist in the maps then 46 | } 47 | catch (const std::out_of_range& oor) 48 | { 49 | 50 | std::clog << " Creating new key entry: " << name 51 | << "\t Array size (bytes): " 52 | << autosave_period * sizeof(num_type) << '\n'; 53 | table = new num_type[autosave_period]; 54 | memset(table, 0, autosave_period * sizeof(num_type)); 55 | table[counter] = value; 56 | data[name] = table; 57 | more_vars_added = true; 58 | } 59 | 60 | } 61 | 62 | mat_log::~mat_log() 63 | { 64 | if (type == RAW) 65 | { 66 | for (map::iterator it = data.begin(); 67 | it != data.end(); ++it) 68 | 69 | delete[] (it->second); 70 | 71 | } 72 | } 73 | 74 | #ifdef MAT_IO 75 | 76 | int mat_log::saveMatTo(string filename) 77 | { 78 | mat_t *matfp; 79 | unsigned int i; 80 | matvar_t *matvar; 81 | size_t dims[2] = { 10, 1 }; 82 | double * matdata; 83 | 84 | filename += "_" + string(timebuf); 85 | 86 | if (filename.find(".mat") == std::string::npos) 87 | filename += ".mat"; 88 | 89 | matfp = Mat_Open((filename).c_str(), MAT_ACC_RDWR | MAT_FT_MAT5); 90 | 91 | if (NULL == matfp) 92 | { 93 | std::cerr << "Error opening MAT file " << filename << std::endl; 94 | return -1; 95 | } 96 | 97 | for (map::iterator it = data.begin(); it != data.end(); 98 | ++it) 99 | { 100 | std::cout << "name " << it->first << " data size()" 101 | << sizeof(it->second) << " size" 102 | << sizeof(it->second) / sizeof(num_type) << "\n"; 103 | 104 | dims[0] = sizeof(it->second) / sizeof(num_type); 105 | //dims[0] = ((vector ) it->second).size(); 106 | matdata = it->second; //new double[dims[0]]; 107 | // for (i = 0; i < dims[0]; i++) 108 | // matdata[i] = ((vector ) it->second).at(i); 109 | 110 | matvar = Mat_VarCreate((it->first).c_str(), MAT_C_DOUBLE, MAT_T_DOUBLE, 111 | 2, dims, matdata, 0); //make variable 112 | Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_NONE); // save variable 113 | Mat_VarFree(matvar); //free mat variable 114 | 115 | delete matdata; //free data 116 | } 117 | return 0; 118 | 119 | } 120 | #endif 121 | 122 | // 123 | int mat_log::save() 124 | { 125 | int ret = -1; 126 | if (type == RAW) 127 | ret = this->saveRawTo(log_name + _toString(log_files_counter++)); // 128 | #ifdef MAT_IO 129 | else if (type == MAT) 130 | ret = this->saveMatTo(log_name + _toString(log_files_counter++)); 131 | #endif 132 | 133 | return ret; 134 | } 135 | 136 | int mat_log::saveRawTo(string filename) 137 | { 138 | 139 | if (more_vars_added) //must resave variables names.txt ? 140 | if (!rawfile.is_open()) 141 | { 142 | rawfile.open((filename + ".bin").c_str(), 143 | ios::out | ios::app | ios::binary); 144 | } 145 | 146 | if (rawfile.fail()) 147 | { 148 | std::cerr << "Error opening file for logging" << filename << std::endl; 149 | return -1; 150 | } 151 | 152 | if (more_vars_added) 153 | { 154 | ofstream namesfn; 155 | namesfn.open((filename + "_varnames.csv").c_str(), 156 | std::ofstream::out | std::ofstream::app); 157 | //precision, 158 | namesfn << typeid(num_type).name() << ',' <<_toString(sizeof(num_type)) << ',' << _toString(autosave_period) << ',' << _toString(data.size()); 159 | for (map::iterator it = data.begin(); //save all variables names 160 | it != data.end(); ++it) 161 | { 162 | namesfn << ','<< (string) (it->first) ; 163 | } 164 | namesfn.close(); 165 | 166 | } 167 | for (map::iterator it = data.begin(); it != data.end(); 168 | ++it) 169 | { 170 | //std::cout << it->second[0] << std::endl; 171 | //std::cout <(it->second), autosave_period * sizeof(num_type)); 174 | } 175 | 176 | return 0; 177 | } 178 | 179 | void mat_log::periodic_save() 180 | { 181 | if (++counter >= autosave_period) 182 | { 183 | counter = 0; 184 | save(); 185 | more_vars_added = false; 186 | } 187 | } 188 | -------------------------------------------------------------------------------- /nao_walk_ros/cmake/FindNAOqi.cmake: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2013, Miguel Sarabia 2 | # Imperial College London 3 | # 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # 7 | # # Redistributions of source code must retain the above copyright 8 | # notice, this list of conditions and the following disclaimer. 9 | # # Redistributions in binary form must reproduce the above copyright 10 | # notice, this list of conditions and the following disclaimer in the 11 | # documentation and/or other materials provided with the distribution. 12 | # # Neither the name of the Imperial College London nor the names of its 13 | # contributors may be used to endorse or promote products derived from 14 | # this software without specific prior written permission. 15 | # 16 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 20 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | # POSSIBILITY OF SUCH DAMAGE. 27 | # 28 | 29 | 30 | # - Try to find NAOqi 31 | # Once executed this script will define the following: 32 | # NAOqi_FOUND - NAOqi was succesfully found 33 | # NAOqi_INCLUDE_DIRS - NAOqi's include directories 34 | # NAOqi_LIBRARIES - NAOqi's libraries 35 | # NAOqi_DIR - Directory where NAOqi was found 36 | #------------------------------------------------------------------------------ 37 | # Users can set NAOqi_DIR to force CMake to look in a particular location, 38 | # setting the AL_DIR environment variable will have a similar effect. 39 | 40 | cmake_minimum_required(VERSION 2.8.3) 41 | 42 | #These are NAOqi's known components (ie. libraries) 43 | set(NAOqi_COMPONENTS 44 | alaudio 45 | albonjourdiscovery 46 | alextractor 47 | allog 48 | almodelutils 49 | alproject 50 | alresource 51 | altools 52 | alautomatictest 53 | alboxrary 54 | alfile 55 | almathinternal 56 | almotion 57 | alpythonbridge 58 | alserial 59 | altts 60 | albehaviorinfo 61 | alcommon 62 | allauncher 63 | almath 64 | almotionrecorder 65 | alpythontools 66 | alsoap 67 | alvalue 68 | albehavior 69 | alerror 70 | allogremote 71 | almemoryfastaccess 72 | alparammanager 73 | alremotecall 74 | althread 75 | alvision 76 | alproxies 77 | qi 78 | qitype 79 | ) 80 | 81 | 82 | #Set INCLUDE hints 83 | set(NAOqi_INCLUDE_HINTS 84 | "${NAOqi_DIR}/include" 85 | "$ENV{AL_DIR}/include" ) 86 | 87 | # Set LIBRARY hints 88 | set(NAOqi_LIBRARY_HINTS 89 | "${NAOqi_DIR}/lib" 90 | "$ENV{AL_DIR}/lib" ) 91 | 92 | # Find include directories 93 | find_path(NAOqi_INCLUDE_DIR alcommon/alproxy.h HINTS ${NAOqi_INCLUDE_HINTS} ) 94 | 95 | # Verify we know about all the components requested 96 | # and remove those we don't know about 97 | set(NAOqi_FILTERED_COMPONENTS ${NAOqi_FIND_COMPONENTS}) 98 | 99 | if ( NAOqi_FIND_COMPONENTS ) 100 | foreach(comp ${NAOqi_FIND_COMPONENTS}) 101 | list(FIND NAOqi_COMPONENTS ${comp} ${comp}_KNOWN) 102 | if (${comp}_KNOWN EQUAL -1) 103 | list(REMOVE_ITEM NAOqi_FILTERED_COMPONENTS ${comp}) 104 | message(STATUS "Unknown NAOqi component ${comp}") 105 | endif() 106 | endforeach() 107 | endif() 108 | 109 | list(LENGTH NAOqi_FILTERED_COMPONENTS NAOqi_NUMBER_OF_COMPONENTS) 110 | set(NAOqi_FOUND_COMPONENTS TRUE) 111 | 112 | # Look for components (ie. libraries) 113 | if( ${NAOqi_NUMBER_OF_COMPONENTS} ) 114 | foreach(comp ${NAOqi_FILTERED_COMPONENTS}) 115 | #Look for the actual library here 116 | find_library(${comp}_LIBRARY NAMES ${comp} HINTS ${NAOqi_LIBRARY_HINTS} NO_CMAKE_PATH NO_CMAKE_ENVIRONMENT_PATH) 117 | if ( ${${comp}_LIBRARY} STREQUAL ${comp}_LIBRARY-NOTFOUND) 118 | message(STATUS "Could not find NAOqi's ${comp}") 119 | set(NAOqi_FOUND_COMPONENTS FALSE) 120 | else() 121 | #If everything went well append this component to list of libraries 122 | list(APPEND NAOqi_LIBRARY ${${comp}_LIBRARY}) 123 | endif() 124 | endforeach() 125 | else() 126 | message(STATUS "No NAOqi components specified") 127 | endif() 128 | 129 | 130 | # Handle the QUIET and REQUIRED arguments 131 | include(FindPackageHandleStandardArgs) 132 | find_package_handle_standard_args( 133 | NAOqi #Package name 134 | DEFAULT_MSG 135 | # Variables required to evaluate as TRUE 136 | NAOqi_LIBRARY 137 | NAOqi_INCLUDE_DIR 138 | NAOqi_FOUND_COMPONENTS) 139 | 140 | # Copy the values of the advanced variables to the user-facing ones 141 | set(NAOqi_LIBRARIES ${NAOqi_LIBRARY} ) 142 | set(NAOqi_INCLUDE_DIRS ${NAOqi_INCLUDE_DIR} ) 143 | set(NAOqi_FOUND ${NAOQI_FOUND}) 144 | 145 | # If NAOqi was found, update NAOqi_DIR to show where it was found 146 | if ( NAOqi_FOUND ) 147 | get_filename_component(NAOqi_NEW_DIR "${NAOqi_INCLUDE_DIRS}/../" ABSOLUTE) 148 | endif() 149 | set(NAOqi_DIR ${NAOqi_NEW_DIR} CACHE FILEPATH "NAOqi root directory" FORCE) 150 | 151 | #Hide these variables 152 | mark_as_advanced(NAOqi_INCLUDE_DIR NAOqi_LIBRARY NAOQI_FOUND) 153 | -------------------------------------------------------------------------------- /nao_walk_naoqi/RobotParameters.h: -------------------------------------------------------------------------------- 1 | #ifndef ROBOTPARAMETERS_H_ 2 | #define ROBOTPARAMETERS_H_ 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #define MAX_TRAJECTORY_LENGTH 250 9 | 10 | enum 11 | { 12 | StepX=0, StepY, StepZ, H0, HX, HY, ComZ, Tstep,Tinit, Tds, Tss, Ts, Td, Init_instructions, SS_instructions, DS_instructions, MaxStepX, MaxStepY, MaxStepTheta, MinStepX, MinStepY, MinStepTheta, g, 13 | mass,omega,Early_Contact_threshold, Ground_Contact_threshold,LegUpThres,LegLowThres, CoM_Stiffness, CoM_Damping, PreviewWindow, 14 | StepPlanSize, com_q, comd_q, fd_q, com_r, comdd_r, I_xx, I_yy, I_zz, bias_fx, bias_fy, bias_fz, transitionSI_instructions, 15 | CoM_state_uncertainty, DCM_state_uncertainty, VRP_state_uncertainty, External_wrench_uncertainty, CoM_noise, COP_Noise, StepXF, StepXH, StepYL, StepYR, 16 | Tc, Ta, Tn, Kc, Ka, Kn, Tss_min, Tss_max, StepPlanAdjustment, velocityControl, alpha_ZMPFilter, ParametersSize 17 | }; 18 | 19 | static const char * robotParameterNames[] = 20 | { 21 | "StepX", "StepY", "StepZ", "H0", "HX", "HY", "ComZ", "Tstep","Tinit", "Tds", "Tss", "Ts", "Td", "Init_instructions", "SS_instructions", "DS_instructions", "MaxStepX", "MaxStepY", "MaxStepTheta", "MinStepX", "MinStepY", "MinStepTheta", "g", "mass","Early_Contact_threshold", 22 | "Ground_Contact_threshold","LegUpThres","LegLowThres", "CoM_Stiffness", "CoM_Damping", "PreviewWindow", 23 | "StepPlanSize" "com_q" "comd_q" "fd_q" "com_r" "comdd_r" "I_xx" "I_yy" "I_zz" "bias_fx" "bias_fy" "bias_fz" "transitionSI_instructions" 24 | "CoM_state_uncertainty" "DCM_state_uncertainty" "VRP_state_uncertainty" "External_wrench_uncertainty" "CoM_noise" "COP_Noise" "StepXF" "StepXH" "StepYL" "StepYR" 25 | "Tc" "Ta" "Tn" "Kc" "Ka" "Kn" "Tss_min" "Tss_max" "StepPlanAdjustment" "velocityControl" "alpha_ZMPFilter" "ParametersSize" 26 | }; 27 | 28 | 29 | static const char defaultFilenameForParameters[]="/home/nao/KWalkRobotParameters.ini"; 30 | 31 | 32 | class RobotParameters 33 | { 34 | 35 | private: 36 | float WalkParameters[ParametersSize]; 37 | char robotName[256]; 38 | 39 | public: 40 | RobotParameters() 41 | { 42 | snprintf(robotName,256,"default"); 43 | WalkParameters[Ts] = 0.01; 44 | WalkParameters[Td] = 0.05; 45 | WalkParameters[StepX] = 0.165; 46 | WalkParameters[StepY] = 0.085; 47 | WalkParameters[StepZ] = 0.016; 48 | WalkParameters[H0] = 0.1025/2.00; 49 | WalkParameters[HY] = 0.0; 50 | WalkParameters[HX] = -0.015; 51 | WalkParameters[ComZ] = 0.26; 52 | WalkParameters[Tstep] = 0.4; //0.28 53 | WalkParameters[Tinit]= 0.4; 54 | WalkParameters[Tss] = 0.3; //0.3 55 | WalkParameters[Tds] = 0.1; //0.05 56 | WalkParameters[MaxStepX] = 0.05; 57 | WalkParameters[MaxStepY] = 2.0*WalkParameters[H0] + 0.01; 58 | WalkParameters[MaxStepTheta] = 0.349065850401537; 59 | WalkParameters[MinStepX] = -0.025; 60 | WalkParameters[MinStepY] = 2.0*WalkParameters[H0]; 61 | WalkParameters[MinStepTheta] = -0.050614548300834; 62 | WalkParameters[g] = 9.80665; 63 | WalkParameters[mass]= 5.182530; //Weight + LIDAR 64 | WalkParameters[omega]= sqrt(WalkParameters[g]/WalkParameters[ComZ]); 65 | WalkParameters[Init_instructions]=ceil(WalkParameters[Tinit]/WalkParameters[Ts]); 66 | WalkParameters[SS_instructions]=ceil(WalkParameters[Tss]/WalkParameters[Ts]); 67 | WalkParameters[DS_instructions]=ceil(WalkParameters[Tds]/WalkParameters[Ts]); 68 | WalkParameters[Early_Contact_threshold]=80.0f; 69 | WalkParameters[Ground_Contact_threshold]=12.0f; 70 | WalkParameters[LegUpThres] = 22.0f; 71 | WalkParameters[LegLowThres] = 0.1f; 72 | WalkParameters[CoM_Stiffness] = 22000.0f; 73 | WalkParameters[CoM_Damping] = 1100.0f; 74 | WalkParameters[PreviewWindow] = 150; 75 | WalkParameters[StepPlanSize] = 30; 76 | WalkParameters[com_q] = 5.0e-02; 77 | WalkParameters[comd_q] = 1.5; 78 | WalkParameters[fd_q] = 5.0; 79 | WalkParameters[com_r] = 5.0e-04; 80 | WalkParameters[comdd_r] = 0.015; 81 | WalkParameters[I_xx] = 0.0050623407587; 82 | WalkParameters[I_yy] = 0.0048801358789; 83 | WalkParameters[I_zz] = 0.001610300038; 84 | WalkParameters[bias_fx] = 7.35705; 85 | WalkParameters[bias_fy] = -4.37976; 86 | WalkParameters[bias_fz] = 6.28909; 87 | WalkParameters[CoM_state_uncertainty] = 1.0e-2; 88 | WalkParameters[DCM_state_uncertainty] = 1.0e-1; 89 | WalkParameters[VRP_state_uncertainty] = 1.0e-1; 90 | WalkParameters[External_wrench_uncertainty] = 5.0e-1; 91 | WalkParameters[CoM_noise] = 2.0e-3; 92 | WalkParameters[COP_Noise] = 1.0e-1; 93 | WalkParameters[StepXF] = 0.07; 94 | WalkParameters[StepXH] = -0.03; 95 | WalkParameters[StepYL] = 0.02; 96 | WalkParameters[StepYR] = -0.02; 97 | WalkParameters[transitionSI_instructions] = 250; 98 | WalkParameters[Tc] = 0.1; 99 | WalkParameters[Ta] = 0.1; //0.1 100 | WalkParameters[Tn] = 0.1; //0.1 101 | WalkParameters[Kc] = 2.5; //5.0 102 | WalkParameters[Ka] = 0.05; //0.05 103 | WalkParameters[Kn] = 0.005; //0.005 104 | WalkParameters[Tss_min] = 0.28; 105 | WalkParameters[Tss_max] = 0.35; 106 | WalkParameters[StepPlanAdjustment] = false; 107 | WalkParameters[velocityControl] = false; 108 | WalkParameters[alpha_ZMPFilter] = 0.35; 109 | } 110 | 111 | float getWalkParameter(int); 112 | void setWalkParameter(int p, float s); 113 | int writeWalkParametersFromFile(const char * filename); 114 | int readWalkParametersFromFile(const char * filename); 115 | void printWalkParameters(); 116 | }; 117 | #endif 118 | -------------------------------------------------------------------------------- /nao_walk_ros/include/nao_walk/nao_walk_ros.h: -------------------------------------------------------------------------------- 1 | /*! \file nao_walk_ros.h 2 | * 3 | * 4 | */ 5 | 6 | #ifndef nao_walk_ros_h 7 | #define nao_walk_ros_h 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | // NAOqi Headers 26 | /* 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | */ 37 | // Boost Headers 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | /**/ 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | #include 55 | #include 56 | #include 57 | 58 | #include 59 | #include "nao_walk/dcm_data.h" 60 | #include 61 | #include 62 | #include 63 | #include 64 | 65 | /** 66 | * \class nao_walk_ros 67 | * 68 | * \file nao_walk_ros.h 69 | **/ 70 | 71 | using namespace Eigen; 72 | 73 | 74 | // namespace AL 75 | // { 76 | // class ALBroker; 77 | // } 78 | 79 | 80 | // // Helper definition 81 | // template 82 | // T * end(T (&ra)[N]) { 83 | // return ra + N; 84 | // } 85 | // 86 | 87 | 88 | 89 | // class nao_walk_ros : public AL::ALModule 90 | class nao_walk_ros 91 | { 92 | 93 | private: 94 | //actions 95 | typedef actionlib::ActionServer::GoalHandle FootstepsGoalH; 96 | actionlib::ActionServer *stepsActServer; 97 | FootstepsGoalH stepsGoal; 98 | bool hasStepGoal; 99 | 100 | typedef actionlib::ActionServer::GoalHandle BehaviourGoalH; 101 | actionlib::ActionServer *behaviourActServer; 102 | BehaviourGoalH behaveGoal; 103 | bool hasBehaveGoal; 104 | 105 | typedef actionlib::ActionServer::GoalHandle SpeekGoalH; 106 | actionlib::ActionServer *speekActServer; 107 | SpeekGoalH speekGoal; 108 | // bool hasSpeekGoal; 109 | 110 | //callbacks 111 | void footstepsExecutionCallback(FootstepsGoalH gh); 112 | void behaviourExecutionCallback(BehaviourGoalH gh); 113 | void speekExecutionCallback(SpeekGoalH gh); 114 | 115 | std::map speekMap; 116 | void clearCmd(); 117 | 118 | 119 | std::string nao_hostname; 120 | int port; 121 | // ROS Standard Variables 122 | // ros::NodeHandle node_handle_; 123 | // Helper 124 | ros::Subscriber sub, footstep_sub, odomSub; 125 | ros::Publisher odom_pub, lfsr_pub, rfsr_pub, copr_pub, copl_pub, imu_pub, joint_state_pub; 126 | ros::ServiceServer serv_wakeUp, serv_crouch, serv_sayText; 127 | ros::ServiceServer serv_makarena, serv_danceEvolution, serv_gangnamStyle, serv_vangelis, serv_taichi, serv_eyeofthetiger, serv_warmhello, serv_tired, serv_balltracker; 128 | ros::ServiceClient sc; 129 | boost::shared_ptr odom_msg; 130 | geometry_msgs::WrenchStamped lfsr_msg, rfsr_msg; 131 | geometry_msgs::PointStamped copl_msg, copr_msg; 132 | sensor_msgs::Imu imu_msg; 133 | sensor_msgs::JointState joint_state_msg; 134 | Vector3d copl, copr, RLegGRF, LLegGRF, RLegGRT, LLegGRT; 135 | int seq, fsr_seq; 136 | 137 | 138 | //!< Dynamic reconfigure server to allow config modifications at runtime 139 | boost::shared_ptr< dynamic_reconfigure::Server > dynamic_recfg_; 140 | void odomCallbackFromTopic(const nav_msgs::Odometry::ConstPtr &odom) ; 141 | void publishOdomToTf(const nav_msgs::Odometry::ConstPtr &odom ); 142 | 143 | bool tf_from_kimenatics; 144 | SocketClient socketClient; 145 | 146 | unsigned int cmd_id; 147 | std::deque cmd_q; 148 | command_t current_cmd; 149 | 150 | int sendData(); 151 | inline int addCmd(command_t &cmd) 152 | { 153 | cmd_id++; 154 | cmd.id=cmd_id; 155 | cmd_q.push_back(cmd); 156 | return cmd_id; 157 | } 158 | public: 159 | 160 | nao_walk_ros(); 161 | ~nao_walk_ros(); 162 | void odomCallback(); 163 | 164 | int readData(dcm_data_t &data); 165 | void generateOdomMsg(const dcm_data_t &dcm_data); 166 | void generateLFsrMsg(const dcm_data_t &dcm_data); 167 | void generateRFsrMsg(const dcm_data_t &dcm_data); 168 | void generateImuMsg(const dcm_data_t &dcm_data); 169 | void generateJointsMsg(const dcm_data_t &data); 170 | void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& twist_); 171 | 172 | 173 | bool sayTextCb(nao_walk::sayText::Request &req, nao_walk::sayText::Response &res); 174 | void initialize(ros::NodeHandle nh); 175 | int run(); 176 | 177 | void activeCb(); 178 | 179 | 180 | void reconfigureCB(nao_walk::GaitControlConfig& config, uint32_t level); 181 | bool wakeUpCb(nao_walk::WakeUp::Request &req, nao_walk::WakeUp::Response &res); 182 | bool crouchCb(nao_walk::Crouch::Request &req, nao_walk::Crouch::Response &res); 183 | }; 184 | #endif 185 | 186 | -------------------------------------------------------------------------------- /nao_walk_naoqi/MPCDCM.cpp: -------------------------------------------------------------------------------- 1 | #include "MPCDCM.h" 2 | MPCDCM::MPCDCM(RobotParameters &robot):NaoRobot(robot), dObsDCMx(robot), dObsDCMy(robot) 3 | { 4 | 5 | 6 | //State xe is Delta dcm Delta vrp vrp 7 | Ae.resize(3,3); 8 | Be.resize(3); 9 | Ce.resize(3); 10 | Cx.resize(3); 11 | 12 | 13 | 14 | //Embedded Integrator DCM VRP 15 | A.resize(2,2); 16 | A.setZero(); 17 | A(0,0) = NaoRobot.getWalkParameter(omega); 18 | A(0,1) = -NaoRobot.getWalkParameter(omega); 19 | A *= NaoRobot.getWalkParameter(Ts); 20 | A += Matrix2f::Identity(); 21 | B.resize(2); 22 | B.setZero(); 23 | B(1) = 1.000; 24 | B = B *NaoRobot.getWalkParameter(Ts); 25 | 26 | C.resize(2); 27 | C.setZero(); 28 | C(1) = 1.000; 29 | 30 | 31 | Ae.setZero(); 32 | Ae.block<2,2>(0,0) = A; 33 | Ae.block<1,2>(2,0) = C.transpose()* A; 34 | Ae(2,2) = 1.000; 35 | Be.setZero(); 36 | Be(0) = B(0); 37 | Be(1) = B(1); 38 | Be(2) = C.transpose()*B; 39 | Ce.setZero(); 40 | Ce(2) = 1.000; 41 | Cx.setZero(); 42 | Cx(0) = 1.000; 43 | 44 | 45 | 46 | 47 | 48 | 49 | x.setZero(); 50 | y.setZero(); 51 | x_.setZero(); 52 | y_.setZero(); 53 | xe.setZero(); 54 | ye.setZero(); 55 | 56 | 57 | //VRP 58 | Fv.resize(Np,3); 59 | Fv.setZero(); 60 | Fvu.resize(Np,Np); 61 | Fvu.setZero(); 62 | 63 | //DCM 64 | Fx.resize(Np,3); 65 | Fx.setZero(); 66 | Fxu.resize(Np,Np); 67 | Fxu.setZero(); 68 | 69 | tmpb.resize(1,Np-1); 70 | temp.resize(3); 71 | Fv.block<1,3>(0,0)=Ce.transpose()*Ae; 72 | Fx.block<1,3>(0,0)=Cx.transpose()*Ae; 73 | Fv.block<1,3>(1,0)=Ce.transpose()*Ae*Ae; 74 | Fx.block<1,3>(1,0)=Cx.transpose()*Ae*Ae; 75 | 76 | 77 | temp = Be; 78 | Fvu(0,0) = Ce.transpose()*Be; 79 | Fxu(0,0) = Cx.transpose()*Be; 80 | Fvu(0,1) = Ce.transpose()*Ae*temp; 81 | Fxu(0,1) = Cx.transpose()*Ae*temp; 82 | 83 | Fvu(1,1) = Ce.transpose()*Be; 84 | Fxu(1,1) = Cx.transpose()*Be; 85 | 86 | 87 | 88 | for (unsigned int i = 2; i < Np; i++) 89 | { 90 | 91 | 92 | Fv.block<1,3>(i,0) = Fv.block<1,3>(i-1,0) * Ae; 93 | tmpb = Fvu.block<1,Np-1>(i-1,0); 94 | Fvu.block<1,Np-1>(i,1) = tmpb; 95 | temp = Ae*temp; 96 | Fvu(i,0) = Ce.transpose()*temp; 97 | 98 | 99 | Fx.block<1,3>(i,0) = Fx.block<1,3>(i-1,0) * Ae; 100 | tmpb = Fxu.block<1,Np-1>(i-1,0); 101 | Fxu.block<1,Np-1>(i,1) = tmpb; 102 | Fxu(i,0) = Cx.transpose()*temp; 103 | } 104 | 105 | 106 | R.resize(Np,Np); 107 | R.setIdentity(); 108 | R*=1.0e-4; 109 | 110 | qx = 0.05; 111 | qv = 0.02; 112 | 113 | 114 | Qv.resize(Np,Np); 115 | Qv.setIdentity(); 116 | Qv = Qv*qv; 117 | 118 | Qx.resize(Np,Np); 119 | Qx.setIdentity(); 120 | Qx = Qx*qx; 121 | 122 | //Hessian Matrix 123 | H.resize(Np,Np); 124 | H_inv.resize(Np,Np); 125 | H.setZero(); 126 | H = R; 127 | H.noalias() += Fxu.transpose()*Qx*Fxu; 128 | H.noalias() += Fvu.transpose()*Qv*Fvu; 129 | 130 | //Make Symmetric 131 | H = (H+H.transpose())/2.0; 132 | //Compute the Gains 133 | H_inv = H.inverse(); 134 | 135 | 136 | 137 | K_X.resize(Np,3); 138 | K_X = -H_inv*(Fxu.transpose()*Qx*Fx + Fvu.transpose() * Qv * Fv); 139 | K_x.resize(3); 140 | K_x(0) = K_X(0,0); 141 | K_x(1) = K_X(0,1); 142 | K_x(2) = K_X(0,2); 143 | 144 | K_V.resize(Np,Np); 145 | K_V = H_inv * Fvu.transpose()*Qv; 146 | K_v.resize(Np,1); 147 | K_v = K_V.block<1,Np>(0,0); 148 | 149 | 150 | cout<<"K_x "< & VRPRef, Vector2f DCM, Vector2f CoM, Vector2f VRP) 200 | { 201 | 202 | 203 | 204 | 205 | for (unsigned int i = 0; i < Np; i++) 206 | { 207 | if (i+1 < VRPRef.size()) 208 | { 209 | VRPRefX(i) = VRPRef[i+1](0); 210 | VRPRefY(i) = VRPRef[i+1](1); 211 | } 212 | else 213 | { 214 | VRPRefX(i) = VRPRef[VRPRef.size() - 1](0); 215 | VRPRefY(i) = VRPRef[VRPRef.size() - 1](1); 216 | 217 | } 218 | } 219 | 220 | 221 | 222 | 223 | xe(0) = x(1)-x_(1); 224 | xe(1) = x(2)-x_(2); 225 | xe(2) = x(2); 226 | 227 | ye(0) = y(1)-y_(1); 228 | ye(1) = y(2)-y_(2); 229 | ye(2) = y(2); 230 | 231 | //Optimal MPC Law 232 | du_x = K_x.transpose()*xe; 233 | du_x += K_v.transpose()*VRPRefX; 234 | 235 | du_y = K_x.transpose()*ye; 236 | du_y += K_v.transpose()*VRPRefY; 237 | 238 | //Desired VRP Velocity 239 | u_x += du_x; 240 | u_y += du_y; 241 | 242 | 243 | //Observer for MPC 244 | x_ = x; 245 | y_ = y; 246 | 247 | 248 | dObsDCMx.update(u_x,VRP(0),CoM(0)); 249 | dObsDCMy.update(u_y,VRP(1),CoM(1)); 250 | x = dObsDCMx.getState(); 251 | y = dObsDCMy.getState(); 252 | 253 | cout<<"State X "< xytheta, bool isRight) 35 | { 36 | if (isRight) 37 | { 38 | startR(0)=xytheta(0); 39 | startR(1)=xytheta(1); 40 | startR(2)=xytheta(2); 41 | } 42 | else 43 | { 44 | startL(0)=xytheta(0); 45 | startL(1)=xytheta(1); 46 | startL(2)=xytheta(2); 47 | } 48 | } 49 | 50 | void FeetEngine::setFootDestXYTheta(KMath::KMat::GenMatrix xytheta, bool isRight) 51 | { 52 | if (isRight) 53 | { 54 | planR(0)=xytheta(0); 55 | planR(1)=xytheta(1); 56 | planR(2)=xytheta(2); 57 | } 58 | else 59 | { 60 | planL(0)=xytheta(0); 61 | planL(1)=xytheta(1); 62 | planL(2)=xytheta(2); 63 | } 64 | } 65 | 66 | void FeetEngine::setFootXYTheta(KMath::KMat::GenMatrix xytheta, bool isRight) 67 | { 68 | if (isRight) 69 | { 70 | FootR(0)=xytheta(0); 71 | FootR(1)=xytheta(1); 72 | FootR(2)=xytheta(2); 73 | } 74 | else 75 | { 76 | FootL(0)=xytheta(0); 77 | FootL(1)=xytheta(1); 78 | FootL(2)=xytheta(2); 79 | } 80 | } 81 | 82 | void FeetEngine::setFootZ(float z, bool isRight) 83 | { 84 | if (isRight) 85 | FootRz=z; 86 | else 87 | FootLz=z; 88 | } 89 | 90 | void FeetEngine::setFootStartZ(float z, bool isRight) 91 | { 92 | if (isRight) 93 | startRz=z; 94 | else 95 | startLz=z; 96 | } 97 | 98 | void FeetEngine::setFootDestZ(float z, bool isRight) 99 | { 100 | if (isRight) 101 | planRz=z; 102 | else 103 | planLz=z; 104 | } 105 | 106 | 107 | 108 | KVecFloat3 FeetEngine::getFootDestXYTheta(bool isRight) 109 | { 110 | KVecFloat3 res; 111 | if (isRight) 112 | { 113 | res(0)=planR(0); 114 | res(1)=planR(1); 115 | res(2)=planR(2); 116 | } 117 | else 118 | { 119 | res(0)=planL(0); 120 | res(1)=planL(1); 121 | res(2)=planL(2); 122 | } 123 | 124 | return res; 125 | 126 | } 127 | 128 | float FeetEngine::getFootDestTheta(bool isRight) 129 | { 130 | float res; 131 | if (isRight) 132 | res=planR(2); 133 | else 134 | res=planL(2); 135 | 136 | 137 | return res; 138 | 139 | } 140 | 141 | 142 | 143 | float FeetEngine::getFootDestZ(bool isRight) 144 | { 145 | float res; 146 | if (isRight) 147 | res=planRz; 148 | else 149 | res=planLz; 150 | 151 | 152 | return res; 153 | 154 | } 155 | 156 | float FeetEngine::getFootTheta(bool isRight) 157 | { 158 | float res; 159 | if (isRight) 160 | res=FootR(2); 161 | else 162 | res=FootL(2); 163 | 164 | 165 | return res; 166 | 167 | } 168 | 169 | 170 | float FeetEngine::getFootZ(bool isRight) 171 | { 172 | float res; 173 | if (isRight) 174 | res=FootRz; 175 | else 176 | res=FootLz; 177 | 178 | return res; 179 | 180 | } 181 | 182 | KVecFloat3 FeetEngine::getFootXYTheta(bool isRight) 183 | { 184 | KVecFloat3 res; 185 | if (isRight) 186 | res=FootR; 187 | else 188 | res=FootL; 189 | 190 | return res; 191 | 192 | } 193 | 194 | 195 | 196 | 197 | void FeetEngine::MotionPlan(KVecFloat3 target, unsigned step, unsigned totalsteps, bool right_support, bool double_support, bool RightEarlyContact, bool LeftEarlyContact, bool RightLateContact, bool LeftLateContact) 198 | { 199 | 200 | 201 | 202 | //Generate Feet Trajectories while swing 203 | if(!right_support && !double_support) 204 | { 205 | 206 | if (!RightEarlyContact) 207 | { 208 | 209 | /** Right Foot Interpolation **/ 210 | float diff=KMath::anglediff2(target(2),startR(2)); 211 | 212 | //if (ci.state==KICK) 213 | //dr(0)=planForwardKick( (float)currentstep, ci.target(0), startR(0), ci.steps-1); 214 | //else 215 | FootR(0)=interp.planFeetTrajectoryXY((float) step, target(0), startR(0), totalsteps-1.0); 216 | 217 | FootR(1)=interp.planFeetTrajectoryXY((float) step, target(1), startR(1), totalsteps-1.0); 218 | 219 | 220 | FootR(2)=startR(2)+interp.LinearInterpolation((float) step, diff, 0.0, totalsteps-1.0); 221 | 222 | // if(!RightLateContact) 223 | // { 224 | FootRz=interp.CubicSplineInterpolation( (float) step, 0.000, StepZ_/2.0, StepZ_, StepZ_/3.0,0.000,totalsteps-1.0); 225 | // FootRz=interp.planFeetTrajectoryZ((float) step, StepZ_,0.000, totalsteps); 226 | //FootRz=interp.BezierZ((float) step, StepZ_, totalsteps-1.0); 227 | // } 228 | // else if(FootRz > -0.03) 229 | // {3 230 | // FootRz -= 0.005; 231 | // } 232 | 233 | 234 | } 235 | 236 | } 237 | else if(right_support && !double_support) 238 | { 239 | if (!LeftEarlyContact) 240 | { 241 | /** Left Foot Interpolation **/ 242 | float diff=KMath::anglediff2(target(2),startL(2)); 243 | 244 | FootL(0)=interp.planFeetTrajectoryXY((float) step, target(0), startL(0), totalsteps-1.0); 245 | 246 | FootL(1)=interp.planFeetTrajectoryXY((float) step, target(1), startL(1), totalsteps-1.0); 247 | 248 | FootL(2)=startL(2)+interp.LinearInterpolation( (float) step, diff, 0.0, totalsteps-1.0); 249 | 250 | // if(!LeftLateContact) 251 | // { 252 | FootLz=interp.CubicSplineInterpolation((float) step, 0.000, StepZ_/2.0, StepZ_, StepZ_/3.0,0.000,totalsteps-1.0); 253 | //FootLz=interp.planFeetTrajectoryZ((float) step, StepZ_,0.000, totalsteps); 254 | //FootLz=interp.BezierZ((float) step, StepZ_, totalsteps-1.0); 255 | 256 | // } 257 | // else if(FootLz > -0.03) 258 | // { 259 | // FootLz -= 0.005; 260 | // } 261 | 262 | } 263 | } 264 | 265 | 266 | } 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | -------------------------------------------------------------------------------- /nao_walk_naoqi/LowLevelPlanner.h: -------------------------------------------------------------------------------- 1 | /*! \file LowLevelPlanner.h 2 | * \brief A Monas Activity that first plans the trajectories needed by the Walk Engine 3 | and executes the desired walking gait! 4 | * 5 | */ 6 | 7 | #ifndef LOWLEVELPLANNER_H 8 | #define LOWLEVELPLANNER_H 9 | #include 10 | #include 11 | #include 12 | #include "RobotParameters.h" 13 | #include "WalkEngine.h" 14 | #include "MotionDefines.h" 15 | // NAOqi Headers 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | //Vision/Camera 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | // Boost Headers 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | #include"SocketServer.h" 48 | #include"DcmData.h" 49 | #include 50 | 51 | enum 52 | { 53 | DCM_RUNNING, DCM_RUNNING_ONLY_FEET,DCM_NEED_STOP, DCM_READY_TO_STOP, DCM_STOP, 54 | }; 55 | 56 | 57 | 58 | 59 | /** 60 | * \class LowLevelPlanner 61 | * 62 | * \file LowLevelPlanner.h 63 | **/ 64 | 65 | namespace AL 66 | { 67 | class ALBroker; 68 | } 69 | 70 | 71 | template 72 | T * end(T (&ra)[N]) 73 | 74 | { 75 | return ra + N; 76 | } 77 | 78 | 79 | class LowLevelPlanner : public AL::ALModule 80 | { 81 | private: 82 | // bool stopEngine; 83 | std::list stepList; 84 | SocketServer sockServer; 85 | pthread_cond_t cond; 86 | pthread_mutex_t mutex; 87 | // Used for postprocess sync with the DCM 88 | ProcessSignalConnection fDCMPostProcessConnection; 89 | /** Initialise the DCM part **/ 90 | void connectToDevices(); 91 | 92 | /** Connect To the DCM loop 100Hz **/ 93 | void connectToDCMloop(); 94 | 95 | /** function bound to the DCM **/ 96 | int DCMCallback(); 97 | 98 | /** Create DCM hardness Actuator Alias **/ 99 | void createHardnessActuatorAlias(); 100 | 101 | /** Create DCM Position Actuator Alias **/ 102 | void createJointsPositionActuatorAlias(); 103 | 104 | /** Prepare Command ALValue to send command to actuator **/ 105 | void prepareJointsPositionActuatorCommand(); 106 | 107 | /** Create DCM Position Actuator Alias **/ 108 | void createJointsPositionActuatorAlias_onlyFeet(); 109 | 110 | /** Prepare Command ALValue to send command to actuator **/ 111 | void prepareJointsPositionActuatorCommand_onlyFeet(); 112 | 113 | /** Set Stiffness with the DCM **/ 114 | void setStiffnessDCM(const float &stiffnessValue); 115 | 116 | int first_step_id; 117 | int last_step_id; 118 | /** 119 | Used by DCM callbacks 120 | **/ 121 | std::vector jointPtr, sensorPtr; 122 | //boost::shared_ptr memory; 123 | AL::ALMemoryProxy *fMemoryProxy; 124 | AL::ALTextToSpeechProxy *tts; 125 | AL::DCMProxy *dcm, *dcm_onlyFeet; 126 | AL::ALMotionProxy *motion; 127 | AL::ALRobotPostureProxy *posture; 128 | std::vector alljoints, joint_states; 129 | std::vectorjoint_statesPtr, AnglePtr, AccPtr, GyroPtr; 130 | std::vectorcoprPtr, coplPtr; 131 | 132 | float *weightlPtr, *weightrPtr; 133 | 134 | AL::ALValue commands, commands_onlyFeet; 135 | // AL::ALValue odom; 136 | // AL::ALValue data_; 137 | AL::ALValue debug_data; 138 | 139 | dcm_data_stamp_t dcm_data[2]; 140 | dcm_data_stamp_t *curr_dcm_data; 141 | 142 | int last_dcm_data_stamp; 143 | int last_cmd; 144 | unsigned int last_cmd_id; 145 | int last_cmd_param; 146 | int last_cmd_status; 147 | 148 | /****/ 149 | Matrix fsrposl, fsrposr; 150 | Vector4f fsrl,fsrr; 151 | Vector3f Angle, Acc, Gyro; 152 | Vector3f copr, copl; 153 | 154 | 155 | float weightr, weightl; 156 | int dcm_state; 157 | bool firstIncomingStep; 158 | void reset(); 159 | 160 | bool readyToStop, readyToStand, readyToReset; 161 | AL::ALBehaviorManagerProxy behavior; 162 | public: 163 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 164 | 165 | /** 166 | * @brief 167 | **/ 168 | LowLevelPlanner(boost::shared_ptr broker, const std::string& name); 169 | ~LowLevelPlanner(); 170 | // int isFootStepExecuted(); 171 | AL::ALValue getFootSteps(); 172 | // int setFootSteps(AL::ALValue value); 173 | 174 | /** 175 | * Callback function every time the example event is called. 176 | */ 177 | void stepsCallback(const command_t &cmd); 178 | 179 | // void requestStepCallback(const std::string &key, const AL::ALValue &value, const AL::ALValue &msg); 180 | // void stepCallback(const std::string &key, const AL::ALValue &value, const AL::ALValue &msg); 181 | 182 | /** 183 | * A very simple function to generate the event. 184 | */ 185 | void generateDcmDataEvent(); 186 | void generateDebugDataEvent(); 187 | // void generateOdometryEvent(); 188 | // void generateSensorDataEvent(); 189 | void setRobotConfig(float* value); 190 | void velocityCallback(velocity_t *data); 191 | void init(); 192 | void start(); 193 | void stop(); 194 | // int execute(); 195 | void connectToDCM(); 196 | 197 | int sendDcmData(); 198 | int readCommand(); 199 | 200 | void setDCMState(int st_); 201 | int getDCMState(); 202 | bool sayText(AL::ALValue value); 203 | 204 | int runBehavior(const char * behaviorPath , const char * behaviorDescription); 205 | 206 | void inline addCommand(const command_t &cmd) 207 | { 208 | last_cmd=cmd.command; 209 | last_cmd_id=cmd.id; 210 | last_cmd_param=PARAM_INV; 211 | last_cmd_status=STATUS_PENDING; 212 | } 213 | /** 214 | Main object instances used by KWalk 215 | **/ 216 | RobotParameters NaoRobot; 217 | WalkEngine* engine; 218 | 219 | 220 | /** Set one hardness value to all Body joints **/ 221 | void setStiffness(const float &stiffnessValue); 222 | }; 223 | #endif 224 | 225 | -------------------------------------------------------------------------------- /nao_walk_naoqi/StepAdjustment.cpp: -------------------------------------------------------------------------------- 1 | #include "StepAdjustment.h" 2 | StepAdjustment::StepAdjustment(RobotParameters &robot_):robot(robot_) 3 | { 4 | dt = (double) robot.getWalkParameter(Ts); 5 | w = (double) robot.getWalkParameter(omega); 6 | T_max = (double) robot.getWalkParameter(Tss_max); 7 | T_min = (double) robot.getWalkParameter(Tss_min); 8 | T_nom = (double) robot.getWalkParameter(Tss); 9 | b_nom.zero(); 10 | L_nom.zero(); 11 | tau_min = exp(w*T_min); 12 | tau_max = exp(w*T_max); 13 | tau_nom = exp(w*T_nom); 14 | 15 | rot.zero(); 16 | L_max.zero(); 17 | L_min.zero(); 18 | lp = (double) 2.0*robot.getWalkParameter(H0); 19 | 20 | //Set weights 21 | a_0 = 1.0; 22 | a_1 = 1.0; 23 | a_2 = 0.1; 24 | a_3 = 100.0; 25 | a_4 = 100.0; 26 | 27 | n = 5; 28 | G.resize(n, n); 29 | G[0][0] = 2.0*a_0; 30 | G[0][1] = 0.0; 31 | G[0][2] = 0.0; 32 | G[0][3] = 0.0; 33 | G[0][4] = 0.0; 34 | 35 | 36 | G[1][0] = 0.0; 37 | G[1][1] = 2.0*a_1; 38 | G[1][2] = 0.0; 39 | G[1][3] = 0.0; 40 | G[1][4] = 0.0; 41 | 42 | 43 | G[2][0] = 0.0; 44 | G[2][1] = 0.0; 45 | G[2][2] = 2.0*a_2; 46 | G[2][3] = 0.0; 47 | G[2][4] = 0.0; 48 | 49 | 50 | G[3][0] = 0.0; 51 | G[3][1] = 0.0; 52 | G[3][2] = 0.0; 53 | G[3][3] = 2.0*a_3; 54 | G[3][4] = 0.0; 55 | 56 | 57 | G[4][0] = 0.0; 58 | G[4][1] = 0.0; 59 | G[4][2] = 0.0; 60 | G[4][3] = 0.0; 61 | G[4][4] = 2.0*a_4; 62 | 63 | 64 | g0.resize(n); 65 | g0[0] = 0.0; 66 | g0[1] = 0.0; 67 | g0[2] = -2.0*a_2*tau_nom; 68 | g0[3] = 0.0; 69 | g0[4] = 0.0; 70 | 71 | m = 2; 72 | ce0.resize(m); 73 | ce0[0] = 0.0; 74 | ce0[1] = 0.0; 75 | CE.resize(n, m); 76 | CE[0][0]= 1.0; 77 | CE[1][0]= 0.0; 78 | CE[2][0]= 0.0; 79 | CE[3][0]= 1.0; 80 | CE[4][0]= 0.0; 81 | 82 | CE[0][1]= 0.0; 83 | CE[1][1]= 1.0; 84 | CE[2][1]= 0.0; 85 | CE[3][1]= 0.0; 86 | CE[4][1]= 1.0; 87 | 88 | p = 6; 89 | CI.resize(n, p); 90 | CI[0][0] = -1.0; 91 | CI[0][1] = 1.0; 92 | CI[0][2] = 0.0; 93 | CI[0][3] = 0.0; 94 | CI[0][4] = 0.0; 95 | CI[0][5] = 0.0; 96 | 97 | CI[1][0] = 0.0; 98 | CI[1][1] = 0.0; 99 | CI[1][2] = -1.0; 100 | CI[1][3] = 1.0; 101 | CI[1][4] = 0.0; 102 | CI[1][5] = 0.0; 103 | 104 | 105 | CI[2][0] = 0.0; 106 | CI[2][1] = 0.0; 107 | CI[2][2] = 0.0; 108 | CI[2][3] = 0.0; 109 | CI[2][4] = -1.0; 110 | CI[2][5] = 1.0; 111 | 112 | 113 | CI[3][0] = 0.0; 114 | CI[3][1] = 0.0; 115 | CI[3][2] = 0.0; 116 | CI[3][3] = 0.0; 117 | CI[3][4] = 0.0; 118 | CI[3][5] = 0.0; 119 | 120 | CI[4][0] = 0.0; 121 | CI[4][1] = 0.0; 122 | CI[4][2] = 0.0; 123 | CI[4][3] = 0.0; 124 | CI[4][4] = 0.0; 125 | CI[4][5] = 0.0; 126 | 127 | ci0.resize(p); 128 | ci0[4]= tau_max; 129 | ci0[5]= -tau_min; 130 | 131 | x.resize(n); 132 | cout<<"StepAdjustment in "<std::numeric_limits::max()) //e.g. problem is infeasible, follow the nomimal gait pattern 230 | { 231 | step_locationx = vrpx_ref; 232 | step_locationy = vrpy_ref; 233 | step_duration = T_nom; 234 | step_instructions = ceil(step_duration/dt); 235 | step_bx = 0.0; 236 | step_by = 0.0; 237 | } 238 | else 239 | { 240 | 241 | step_locationx = x[0]; 242 | step_locationy = x[1]; 243 | step_duration = 1.0/w * log(x[2]); 244 | step_instructions = ceil(step_duration/dt); 245 | step_bx = x[3]; 246 | step_by = x[4]; 247 | } 248 | cout<<"Ref Location"<0) 28 | { 29 | WalkInstruction pi; 30 | if(jjj==0) 31 | { 32 | pi = planStep2D(velocityQ.front(), si); 33 | } 34 | else 35 | { 36 | pi = planStep2D(velocityQ.front(), pi); 37 | } 38 | stepAnkleQ.push_back(pi); 39 | velocityQ.pop_front(); 40 | jjj++; 41 | } 42 | planAvailable = true; 43 | } 44 | 45 | void Stepplanner2D::emptyPlan() 46 | { 47 | while(velocityQ.size()>0) 48 | velocityQ.pop_front(); 49 | 50 | while(stepAnkleQ.size()>0) 51 | stepAnkleQ.pop_front(); 52 | 53 | planAvailable = false; 54 | 55 | } 56 | 57 | 58 | WalkInstruction Stepplanner2D::planStep2D(KVecFloat3 v, WalkInstruction si) 59 | { 60 | 61 | WalkInstruction ci; 62 | 63 | if (abs(v(0))0.75) 70 | v(0) = 0.75; 71 | if(v(0)<-1.00) 72 | v(0) = -1.00; 73 | 74 | if(v(1)>1.00) 75 | v(1) = 1.00; 76 | if(v(1)<-1.00) 77 | v(1) = -1.00; 78 | 79 | if(v(2)>0.80) 80 | v(2) = 0.80; 81 | if(v(2)<-0.80) 82 | v(2) = -0.80; 83 | 84 | 85 | //Previous Swing Foot Becomes Support 86 | support_foot_x = si.target(0); 87 | support_foot_y = si.target(1); 88 | support_foot_orientation = si.target(2); 89 | //Support Leg exchange 90 | if(si.targetSupport == KDeviceLists::SUPPORT_LEG_LEFT) 91 | { 92 | si.targetSupport = KDeviceLists::SUPPORT_LEG_RIGHT; 93 | } 94 | else 95 | { 96 | si.targetSupport = KDeviceLists::SUPPORT_LEG_LEFT; 97 | } 98 | 99 | if(si.targetSupport == KDeviceLists::SUPPORT_LEG_LEFT) 100 | { 101 | h = KVecFloat2(0.0, -2.0*robot.getWalkParameter(H0)); 102 | } 103 | else 104 | { 105 | h = KVecFloat2(0.0, 2.0*robot.getWalkParameter(H0)); 106 | } 107 | 108 | 109 | 110 | KMath::KMat::transformations::makeRotation(rot,support_foot_orientation); 111 | h = rot * h; 112 | 113 | targetXY = h; 114 | targetTheta = support_foot_orientation; 115 | 116 | if(v(0)==v_(0) && v(1)==v_(1) and v(2)==v_(2) && v(0)==0 && v(1)==0 && v(2)==0) 117 | { 118 | cmd = STAND; 119 | 120 | 121 | } 122 | else 123 | { 124 | cmd = WALK; 125 | } 126 | 127 | 128 | 129 | 130 | rot.zero(); 131 | if(v(2)>0) 132 | { 133 | if(si.targetSupport == KDeviceLists::SUPPORT_LEG_RIGHT) 134 | { 135 | KMath::KMat::transformations::makeRotation(rot,v(2)*robot.getWalkParameter(MaxStepTheta)); 136 | } 137 | else 138 | { 139 | KMath::KMat::transformations::makeRotation(rot,-v(2)*robot.getWalkParameter(MinStepTheta)); 140 | } 141 | } 142 | else 143 | { 144 | if(si.targetSupport == KDeviceLists::SUPPORT_LEG_RIGHT) 145 | { 146 | KMath::KMat::transformations::makeRotation(rot,-v(2)*robot.getWalkParameter(MinStepTheta)); 147 | } 148 | else 149 | { 150 | KMath::KMat::transformations::makeRotation(rot,v(2) * robot.getWalkParameter(MaxStepTheta)); 151 | } 152 | } 153 | targetXY = rot * targetXY; 154 | 155 | //Generate Constraints 156 | tempV.zero(); 157 | if(v(0) > 0.00) 158 | { 159 | tempV(0) = v(0) * robot.getWalkParameter(MaxStepX); 160 | } 161 | else 162 | { 163 | tempV(0) = -v(0) * robot.getWalkParameter(MinStepX); 164 | } 165 | 166 | 167 | if(v(1) > 0.00) 168 | { 169 | if(si.targetSupport == KDeviceLists::SUPPORT_LEG_RIGHT) 170 | { 171 | tempV(1) = v(1) * robot.getWalkParameter(MaxStepY); 172 | } 173 | else 174 | { 175 | tempV(1) = -v(1) * robot.getWalkParameter(MinStepY) * 0.0; 176 | } 177 | } 178 | else 179 | { 180 | if(si.targetSupport == KDeviceLists::SUPPORT_LEG_RIGHT) 181 | { 182 | tempV(1) = -v(1) * robot.getWalkParameter(MinStepY) * 0.0; 183 | } 184 | else 185 | { 186 | tempV(1) = v(1) * robot.getWalkParameter(MaxStepY); 187 | } 188 | } 189 | 190 | rot.zero(); 191 | KMath::KMat::transformations::makeRotation(rot,support_foot_orientation); 192 | tempV = rot * tempV; 193 | targetXY += tempV; 194 | 195 | if(v(2) > 0.00) 196 | { 197 | if(si.targetSupport == KDeviceLists::SUPPORT_LEG_RIGHT) 198 | { 199 | targetTheta += v(2) * robot.getWalkParameter(MaxStepTheta); 200 | } 201 | else 202 | { 203 | targetTheta -= v(2) * robot.getWalkParameter(MinStepTheta); 204 | } 205 | } 206 | else 207 | { 208 | if(si.targetSupport == KDeviceLists::SUPPORT_LEG_RIGHT) 209 | { 210 | targetTheta -= v(2) * robot.getWalkParameter(MinStepTheta); 211 | } 212 | else 213 | { 214 | targetTheta += v(2) * robot.getWalkParameter(MaxStepTheta); 215 | } 216 | } 217 | 218 | //Fix Constraints 219 | MaxStep(0) = robot.getWalkParameter(MaxStepX); 220 | MinStep(0) = robot.getWalkParameter(MinStepX); 221 | 222 | if(si.targetSupport == KDeviceLists::SUPPORT_LEG_RIGHT) 223 | { 224 | MaxStep(1) = robot.getWalkParameter(MaxStepY); 225 | MinStep(1) = robot.getWalkParameter(MinStepY); 226 | 227 | } 228 | else 229 | { 230 | MaxStep(1) = -robot.getWalkParameter(MinStepY); 231 | MinStep(1) = -robot.getWalkParameter(MaxStepY); 232 | } 233 | 234 | 235 | 236 | tempV = rot.transp() * targetXY; 237 | 238 | 239 | tempV(0) = cropStep(tempV(0),MaxStep(0),MinStep(0)); 240 | tempV(1) = cropStep(tempV(1),MaxStep(1),MinStep(1)); 241 | 242 | 243 | targetXY = rot * tempV; 244 | 245 | 246 | //Output must be a Walk Instruction 247 | ci.target(0) = targetXY(0) + support_foot_x; 248 | ci.target(1) = targetXY(1) + support_foot_y; 249 | ci.target(2) = targetTheta; 250 | ci.targetSupport = si.targetSupport; 251 | ci.targetZMP = si.targetSupport; 252 | 253 | 254 | if(cmd == STAND) 255 | { 256 | ci.targetZMP = KDeviceLists::SUPPORT_LEG_BOTH; 257 | } 258 | 259 | ci.steps = robot.getWalkParameter(SS_instructions); 260 | step_id = si.step_id + 1; 261 | ci.step_id = step_id; 262 | v_ = v; 263 | return ci; 264 | } 265 | --------------------------------------------------------------------------------