-
Sergi Hernandez authoredSergi Hernandez authored
darwin_controller_impl.h 37.90 KiB
#ifndef DARWIN_CONTROLLER_IMP_H
#define DARWIN_CONTROLLER_IMP_H
#include "stm32f1xx.h"
#include "motion_manager.h"
#include "action.h"
#include "action_id.h"
#include "walking.h"
#include "joint_motion.h"
#include "head_tracking.h"
#include "ram.h"
#include "imu.h"
#include "darwin_math.h"
#ifdef _HAVE_XSD
#include "../src/xml/darwin_config.hxx"
#endif
extern "C" {
// motion_manager private function declaration
void TIM5_IRQHandler(void);
}
// motion manager external variables
extern int num_servos;
extern double real_angles[PAGE_MAX_NUM_SERVOS];
// action module external variables
extern TServoInfo manager_servos[MANAGER_MAX_NUM_SERVOS];
//IMU module external variables
extern int32_t gyro_data[3];
extern int32_t accel_data[3];
//parameters default values
#define DEFAULT_ENABLE_SMART_CHARGER false
#define DEFAULT_INITIAL_CHARGE 2200.0
#define DEFAULT_DISCHARGE_RATE 6000.0
#define DEFAULT_CHARGE_CURRENT 2200.0
#define DEFAULT_CHARGE_STATION_LINK "none"
#define DEFAULT_CHARGE_ROBOT_LINK "none"
namespace darwin_controller
{
namespace internal
{
std::vector<std::string> getStrings(const ros::NodeHandle& nh, const std::string& param_name)
{
using namespace XmlRpc;
XmlRpcValue xml_array;
if (!nh.getParam(param_name, xml_array))
{
ROS_ERROR_STREAM("CDarwinSim : Could not find '" << param_name << "' parameter (namespace: " << nh.getNamespace() << ").");
return std::vector<std::string>();
}
if (xml_array.getType() != XmlRpcValue::TypeArray)
{
ROS_ERROR_STREAM("CDarwinSim : The '" << param_name << "' parameter is not an array (namespace: " <<
nh.getNamespace() << ").");
return std::vector<std::string>();
}
std::vector<std::string> out;
for (int i = 0; i < xml_array.size(); ++i)
{
if (xml_array[i].getType() != XmlRpcValue::TypeString)
{
ROS_ERROR_STREAM("CDarwinSim : The '" << param_name << "' parameter contains a non-string element (namespace: " <<
nh.getNamespace() << ").");
return std::vector<std::string>();
}
out.push_back(static_cast<std::string>(xml_array[i]));
}
return out;
}
boost::shared_ptr<urdf::Model> getUrdf(const ros::NodeHandle& nh, const std::string& param_name)
{
boost::shared_ptr<urdf::Model> urdf(new urdf::Model);
std::string urdf_str;
// Check for robot_description in proper namespace
if (nh.getParam(param_name, urdf_str))
{
if (!urdf->initString(urdf_str))
{
ROS_ERROR_STREAM("CDarwinSim : Failed to parse URDF contained in '" << param_name << "' parameter (namespace: " <<
nh.getNamespace() << ").");
return boost::shared_ptr<urdf::Model>();
}
}
// Check for robot_description in root
else if (!urdf->initParam("robot_description"))
{
ROS_ERROR_STREAM("CDarwinSim : Failed to parse URDF contained in '" << param_name << "' parameter");
return boost::shared_ptr<urdf::Model>();
}
return urdf;
}
typedef boost::shared_ptr<const urdf::Joint> UrdfJointConstPtr;
std::vector<UrdfJointConstPtr> getUrdfJoints(const urdf::Model& urdf, const std::vector<std::string>& joint_names)
{
std::vector<UrdfJointConstPtr> out;
for (unsigned int i = 0; i < joint_names.size(); ++i)
{
UrdfJointConstPtr urdf_joint = urdf.getJoint(joint_names[i]);
if (urdf_joint)
{
out.push_back(urdf_joint);
}
else
{
ROS_ERROR_STREAM("CDarwinSim : Could not find joint '" << joint_names[i] << "' in URDF model.");
return std::vector<UrdfJointConstPtr>();
}
}
return out;
}
std::string getLeafNamespace(const ros::NodeHandle& nh)
{
const std::string complete_ns = nh.getNamespace();
std::size_t id = complete_ns.find_last_of("/");
return complete_ns.substr(id + 1);
}
} // namespace
template <class HardwareInterface>
inline void DarwinController<HardwareInterface>::starting(const ros::Time& time)
{
for(unsigned int i = 0; i <this->joints_.size(); ++i)
{
pids_[i]->reset();
this->joints_[i].setCommand(0.0);
}
}
template <class HardwareInterface>
inline void DarwinController<HardwareInterface>::stopping(const ros::Time& time)
{
}
template <class HardwareInterface>
DarwinController<HardwareInterface>::DarwinController()
{
this->motion_action_aserver_=NULL;
}
template <class HardwareInterface>
bool DarwinController<HardwareInterface>::init(HardwareInterface* hw,ros::NodeHandle& root_nh,ros::NodeHandle& controller_nh)
{
using namespace internal;
// Cache controller node handle
controller_nh_ = controller_nh;
// smart charger interface
this->enable_smart_charger=DEFAULT_ENABLE_SMART_CHARGER;
controller_nh_.getParam("enable_smart_charger", this->enable_smart_charger);
ROS_DEBUG_STREAM_NAMED(name_, "Smart charger is: " << this->enable_smart_charger);
if(this->enable_smart_charger)
{
// initialize gazebo interfaces
this->gazebo_node=gazebo::transport::NodePtr(new gazebo::transport::Node());
this->gazebo_node->Init();
this->contacts_subs=this->gazebo_node->Subscribe("/gazebo/default/physics/contacts",&DarwinController<HardwareInterface>::OnContacts,this);
// initialize publisher
this->smart_charger_data_publisher_ = root_nh.advertise<humanoid_common_msgs::smart_charger_data>("robot/smart_charger_data", 1);
// initialize service servers
this->get_smart_charger_config_server_=root_nh.advertiseService("robot/get_smart_charger_config", &DarwinController<HardwareInterface>::get_smart_charger_configCallback, this);
this->set_smart_charger_config_server_=root_nh.advertiseService("robot/set_smart_charger_config", &DarwinController<HardwareInterface>::set_smart_charger_configCallback, this);
// read parameters
this->initial_charge=DEFAULT_INITIAL_CHARGE;
controller_nh_.getParam("initial_charge", this->initial_charge);
ROS_DEBUG_STREAM_NAMED(name_, "Initial charge set to: " << this->initial_charge);
this->current_charge=this->initial_charge;
this->discharge_rate=DEFAULT_DISCHARGE_RATE;
controller_nh_.getParam("discharge_rate", this->discharge_rate);
ROS_DEBUG_STREAM_NAMED(name_, "Discharge rate set to: " << this->discharge_rate);
this->charge_current=DEFAULT_CHARGE_CURRENT;
controller_nh_.getParam("charge_current", this->charge_current);
ROS_DEBUG_STREAM_NAMED(name_, "Charge current set to: " << this->charge_current);
this->charge_station_link=DEFAULT_CHARGE_STATION_LINK;
controller_nh_.getParam("charge_station_link", this->charge_station_link);
ROS_DEBUG_STREAM_NAMED(name_, "Charger station link set to: " << this->charge_station_link);
this->charge_robot_link=DEFAULT_CHARGE_ROBOT_LINK;
controller_nh_.getParam("charge_robot_link", this->charge_robot_link);
ROS_DEBUG_STREAM_NAMED(name_, "Charger robot link set to: " << this->charge_robot_link);
this->charger_connected=false;
}
/* initialize actions */
this->motion_action_aserver_=new IriActionServer<humanoid_common_msgs::humanoid_motionAction>(root_nh,"robot/motion_action");
this->motion_action_aserver_->registerStartCallback(boost::bind(&DarwinController<HardwareInterface>::motion_action_startCallback, this, _1));
this->motion_action_aserver_->registerStopCallback(boost::bind(& DarwinController<HardwareInterface>::motion_action_stopCallback, this));
this->motion_action_aserver_->registerIsFinishedCallback(boost::bind(&DarwinController<HardwareInterface>::motion_action_isFinishedCallback, this));
this->motion_action_aserver_->registerHasSucceedCallback(boost::bind(&DarwinController<HardwareInterface>::motion_action_hasSucceedCallback, this));
this->motion_action_aserver_->registerGetResultCallback(boost::bind(&DarwinController<HardwareInterface>::motion_action_getResultCallback, this, _1));
this->motion_action_aserver_->registerGetFeedbackCallback(boost::bind(&DarwinController<HardwareInterface>::motion_action_getFeedbackCallback, this, _1));
this->motion_action_aserver_->start();
this->joint_trajectory_aserver_=new IriActionServer<control_msgs::JointTrajectoryAction>(root_nh,"robot/joint_trajectory");
this->joint_trajectory_aserver_->registerStartCallback(boost::bind(&DarwinController<HardwareInterface>::joint_trajectory_startCallback, this, _1));
this->joint_trajectory_aserver_->registerStopCallback(boost::bind(& DarwinController<HardwareInterface>::joint_trajectory_stopCallback, this));
this->joint_trajectory_aserver_->registerIsFinishedCallback(boost::bind(&DarwinController<HardwareInterface>::joint_trajectory_isFinishedCallback, this));
this->joint_trajectory_aserver_->registerHasSucceedCallback(boost::bind(&DarwinController<HardwareInterface>::joint_trajectory_hasSucceedCallback, this));
this->joint_trajectory_aserver_->registerGetResultCallback(boost::bind(&DarwinController<HardwareInterface>::joint_trajectory_getResultCallback, this, _1));
this->joint_trajectory_aserver_->registerGetFeedbackCallback(boost::bind(&DarwinController<HardwareInterface>::joint_trajectory_getFeedbackCallback, this, _1));
this->joint_trajectory_aserver_->start();
this->follow_target_aserver_=new IriActionServer<humanoid_common_msgs::humanoid_follow_targetAction>(root_nh,"robot/head_follow_target");
this->follow_target_aserver_->registerStartCallback(boost::bind(&DarwinController<HardwareInterface>::follow_target_startCallback, this, _1));
this->follow_target_aserver_->registerStopCallback(boost::bind(& DarwinController<HardwareInterface>::follow_target_stopCallback, this));
this->follow_target_aserver_->registerIsFinishedCallback(boost::bind(&DarwinController<HardwareInterface>::follow_target_isFinishedCallback, this));
this->follow_target_aserver_->registerHasSucceedCallback(boost::bind(&DarwinController<HardwareInterface>::follow_target_hasSucceedCallback, this));
this->follow_target_aserver_->registerGetResultCallback(boost::bind(&DarwinController<HardwareInterface>::follow_target_getResultCallback, this, _1));
this->follow_target_aserver_->registerGetFeedbackCallback(boost::bind(&DarwinController<HardwareInterface>::follow_target_getFeedbackCallback, this, _1));
this->follow_target_aserver_->start();
/* initialize services */
this->set_servo_modules_server_=root_nh.advertiseService("robot/set_servo_modules", &DarwinController<HardwareInterface>::set_servo_modulesCallback, this);
this->set_walk_params_server_=root_nh.advertiseService("robot/set_walk_params", &DarwinController<HardwareInterface>::set_walk_paramsCallback, this);
this->get_walk_params_server_=root_nh.advertiseService("robot/get_walk_params", &DarwinController<HardwareInterface>::get_walk_paramsCallback, this);
this->set_pan_pid_server_=root_nh.advertiseService("robot/set_pan_pid", &DarwinController<HardwareInterface>::set_pan_pidCallback, this);
this->get_pan_pid_server_=root_nh.advertiseService("robot/get_pan_pid", &DarwinController<HardwareInterface>::get_pan_pidCallback, this);
this->set_tilt_pid_server_=root_nh.advertiseService("robot/set_tilt_pid", &DarwinController<HardwareInterface>::set_tilt_pidCallback, this);
this->get_tilt_pid_server_=root_nh.advertiseService("robot/get_tilt_pid", &DarwinController<HardwareInterface>::get_tilt_pidCallback, this);
// initialize topics
this->gyro_sub=root_nh.subscribe("sensors/raw_gyro", 1, &DarwinController<HardwareInterface>::gyro_callback,this);
this->accel_sub=root_nh.subscribe("sensors/raw_accel", 1, &DarwinController<HardwareInterface>::accel_callback,this);
this->cmd_vel_sub=root_nh.subscribe("robot/cmd_vel", 1, &DarwinController<HardwareInterface>::cmd_vel_callback,this);
this->head_target_sub=root_nh.subscribe("robot/head_target", 1, &DarwinController<HardwareInterface>::head_target_callback,this);
// initialize publisher
this->fallen_state_publisher_ = root_nh.advertise<std_msgs::Int8>("robot/fallen_state", 1);
// Controller name
name_=getLeafNamespace(controller_nh_);
// List of controlled joints
joint_names_=getStrings(controller_nh_,"joints");
if(joint_names_.empty())
return false;
const unsigned int n_joints=joint_names_.size();
// URDF joints
boost::shared_ptr<urdf::Model> urdf=getUrdf(root_nh, "robot_description");
if(!urdf)
return false;
std::vector<UrdfJointConstPtr> urdf_joints=getUrdfJoints(*urdf, joint_names_);
if(urdf_joints.empty())
return false;
assert(n_joints==urdf_joints.size());
num_servos=n_joints;// set the number of servos to the number of joint of the urdf model
// Initialize members
this->joints_.resize(n_joints);
this->pids_.resize(n_joints);
for(unsigned int i=0;i<n_joints;++i)
{
// Joint handle
try {
joints_[i]=hw->getHandle(joint_names_[i]);
}catch(...){
ROS_ERROR_STREAM("CDarwinSim : Could not find joint '" << joint_names_[i] << "' in '" <<
this->getHardwareInterfaceType() << "'.");
return false;
}
// Node handle to PID gains
ros::NodeHandle joint_nh(this->controller_nh_,std::string("gains/")+joint_names_[i]);
// Init PID gains from ROS parameter server
this->pids_[i].reset(new control_toolbox::Pid());
if(!this->pids_[i]->init(joint_nh))
{
ROS_WARN_STREAM("CDarwinSim : Failed to initialize PID gains from ROS parameter server.");
return false;
}
// Whether a joint is continuous (ie. has angle wraparound)
const std::string not_if = urdf_joints[i]->type == urdf::Joint::CONTINUOUS ? "" : "non-";
ROS_DEBUG_STREAM("CDarwinSim : Found " << not_if << "continuous joint '" << joint_names_[i] << "' in '" << this->getHardwareInterfaceType() << "'.");
}
ROS_DEBUG_STREAM("CDarwinSim : Initialized controller '" << name_ << "' with:" <<
"\n- Number of joints: " << joints_.size() <<
"\n- Hardware interface type: '" << this->getHardwareInterfaceType() << "'");
/* initialize the RAM */
ram_init();
/* initialize the IMU */
imu_init();
/* initialize motion modules */
manager_init(7800);// motion manager period in us
ROS_INFO("CDarwinSim : motion manager initialized");
/* go to the walk ready position */
for(unsigned int i=0;i<n_joints;++i)
{
manager_set_module(i+1,MM_ACTION);
manager_enable_servo(i+1);
}
manager_enable_balance();// enable balance
action_load_page(WALK_READY);
action_start_page();
return true;
}
template <class HardwareInterface>
void DarwinController<HardwareInterface>::update(const ros::Time& time, const ros::Duration& period)
{
static bool gyro_calibrated=false;
static ros::Time last_time=ros::Time(0,0);
ros::Time current_time=ros::Time::now();
std::vector<double> target_angles(joints_.size());
/* get the actual simulation angles */
for(unsigned int i=0;i<this->joints_.size();++i)
real_angles[i]=joints_[i].getPosition();
__HAL_TIM_SET_FLAG(TIM5,TIM_FLAG_CC1);
__HAL_TIM_SET_IT_SOURCE(TIM5,TIM_IT_CC1);
TIM5_IRQHandler();
for (unsigned int i = 0; i < this->joints_.size(); ++i)
{
target_angles[i] = ((manager_servos[i+1].current_angle*3.14159)/180.0)/128.0;
const double command = pids_[i]->computeCommand(target_angles[i]-real_angles[i],period);
this->joints_[i].setCommand(command);
}
if(is_walking() && ((current_time-this->walk_timeout).toSec()>1.0))
walking_stop();
if(this->enable_smart_charger)
{
this->current_charge-=this->discharge_rate*period.toSec()/3600.0;
this->smart_charger_data_smart_charger_data_msg_.header.stamp=ros::Time::now();
if(this->charger_connected)
{
this->smart_charger_data_smart_charger_data_msg_.avg_time_empty=(uint16_t)-1;
this->smart_charger_data_smart_charger_data_msg_.avg_time_full=(uint16_t)(((this->initial_charge-this->current_charge)/this->charge_current)*60.0);
this->smart_charger_data_smart_charger_data_msg_.batt_status="Connected and charging";
}
else
{
this->smart_charger_data_smart_charger_data_msg_.avg_time_empty=(uint16_t)((this->current_charge/this->discharge_rate)*60.0);
this->smart_charger_data_smart_charger_data_msg_.avg_time_full=(uint16_t)-1;
}
this->smart_charger_data_publisher_.publish(this->smart_charger_data_smart_charger_data_msg_);
}
// detect fall condition
this->fallen_state_msg_.data=(unsigned char)manager_get_fallen_position();
this->fallen_state_publisher_.publish(this->fallen_state_msg_);
}
template <class HardwareInterface>
void DarwinController<HardwareInterface>::OnContacts(ConstContactsPtr &msg)
{
for(unsigned int i=0;i<msg->contact_size();i++)
{
if(msg->contact(i).collision1()==this->charge_station_link)
{
if(msg->contact(i).collision2()==this->charge_robot_link)
{
this->charger_connected=true;
break;
}
else
this->charger_connected=false;
}
else if(msg->contact(i).collision1()==this->charge_robot_link)
{
if(msg->contact(i).collision2()==this->charge_station_link)
{
this->charger_connected=true;
break;
}
else
this->charger_connected=false;
}
else
this->charger_connected=false;
}
if(this->charger_connected)
ROS_INFO("Charger connected");
}
/************************************ motion_action *********************************************/
template <class HardwareInterface>
void DarwinController<HardwareInterface>::motion_action_startCallback(const humanoid_common_msgs::humanoid_motionGoalConstPtr& goal)
{
ROS_INFO_STREAM("CDarwinSim : Start motion action " << goal->motion_id);
/* load a page and start execution */
if(!action_load_page(goal->motion_id))
ROS_WARN("CDarwinSim : Impossible to load the desired page");
else
{
ROS_INFO("CDarwinSim : Page loaded successfully");
action_start_page();
}
}
template <class HardwareInterface>
void DarwinController<HardwareInterface>::motion_action_stopCallback(void)
{
ROS_INFO("CDarwinSim : Stop motion action");
action_stop_page();
}
template <class HardwareInterface>
bool DarwinController<HardwareInterface>::motion_action_isFinishedCallback(void)
{
if(action_is_running())
return false;
else
return true;
}
template <class HardwareInterface>
bool DarwinController<HardwareInterface>::motion_action_hasSucceedCallback(void)
{
return true;
}
template <class HardwareInterface>
void DarwinController<HardwareInterface>::motion_action_getResultCallback(humanoid_common_msgs::humanoid_motionResultPtr& result)
{
}
template <class HardwareInterface>
void DarwinController<HardwareInterface>::motion_action_getFeedbackCallback(humanoid_common_msgs::humanoid_motionFeedbackPtr& feedback)
{
feedback->current_page=action_get_current_page();
feedback->current_step=action_get_current_step();
}
/************************************ motion_action *********************************************/
/************************************ joint_trajectory_action *********************************************/
template <class HardwareInterface>
void DarwinController<HardwareInterface>::joint_trajectory_startCallback(const control_msgs::JointTrajectoryGoalConstPtr& goal)
{
unsigned int i=0,j=0;
uint16_t angle;
uint8_t speed,accel;
uint32_t servos=0;
ROS_INFO("CDarwinSim : Start joint trajectory action");
if(goal->trajectory.points.size()==1)
{
for(i=0;i<goal->trajectory.joint_names.size();i++)
{
for(j=0;j<MANAGER_MAX_NUM_SERVOS;j++)
{
if(servo_names[j]==goal->trajectory.joint_names[i])
{
angle=(goal->trajectory.points[0].positions[i]*180.0/3.14159)*128.0;
ram_write_word(DARWIN_JOINT_SERVO0_ANGLE_L+j*2,angle);
speed=(goal->trajectory.points[0].velocities[i]*180.0/3.14159);
ram_write_byte(DARWIN_JOINT_SERVO0_SPEED+j,speed);
accel=(goal->trajectory.points[0].accelerations[i]*180.0/3.14159);
ram_write_byte(DARWIN_JOINT_SERVO0_ACCEL+j,accel);
servos|=0x00000001<<j;
}
}
}
joint_motion_set_group_servos(0,servos);
joint_motion_start(0);
}
}
template <class HardwareInterface>
void DarwinController<HardwareInterface>::joint_trajectory_stopCallback(void)
{
ROS_INFO("CDarwinSim : Stop joint trajectory action");
joint_motion_stop(0);
}
template <class HardwareInterface>
bool DarwinController<HardwareInterface>::joint_trajectory_isFinishedCallback(void)
{
if(are_joints_moving(0))
return false;
else
return true;
}
template <class HardwareInterface>
bool DarwinController<HardwareInterface>::joint_trajectory_hasSucceedCallback(void)
{
return true;
}
template <class HardwareInterface>
void DarwinController<HardwareInterface>::joint_trajectory_getResultCallback(control_msgs::JointTrajectoryResultPtr& result)
{
}
template <class HardwareInterface>
void DarwinController<HardwareInterface>::joint_trajectory_getFeedbackCallback(control_msgs::JointTrajectoryFeedbackPtr& feedback)
{
}
/************************************ joint_trajectory_action *********************************************/
/************************************ follow_target_action *********************************************/
template <class HardwareInterface>
void DarwinController<HardwareInterface>::follow_target_startCallback(const humanoid_common_msgs::humanoid_follow_targetGoalConstPtr& goal)
{
ROS_INFO("CDarwinSim : Start head tracking action");
if(goal->pan_range.size()==2)
head_tracking_set_pan_range(((goal->pan_range[0]*180.0)/3.14159)*128.0,((goal->pan_range[1]*180.0)/3.14159)*128.0);
if(goal->tilt_range.size()==2)
head_tracking_set_tilt_range(((goal->tilt_range[0]*180.0)/3.14159)*128.0,((goal->tilt_range[1]*180.0)/3.14159)*128.0);
head_tracking_set_target_pan(((goal->target_pan*180.0)/3.14159)*128.0);
head_tracking_set_target_tilt(((goal->target_tilt*180.0)/3.14159)*128.0);
head_tracking_start();
}
template <class HardwareInterface>
void DarwinController<HardwareInterface>::follow_target_stopCallback(void)
{
ROS_INFO("CDarwinSim : Stop head_tracking action");
head_tracking_stop();
}
template <class HardwareInterface>
bool DarwinController<HardwareInterface>::follow_target_isFinishedCallback(void)
{
if(head_is_tracking())
return false;
else
return true;
}
template <class HardwareInterface>
bool DarwinController<HardwareInterface>::follow_target_hasSucceedCallback(void)
{
return true;
}
template <class HardwareInterface>
void DarwinController<HardwareInterface>::follow_target_getResultCallback(humanoid_common_msgs::humanoid_follow_targetResultPtr& result)
{
}
template <class HardwareInterface>
void DarwinController<HardwareInterface>::follow_target_getFeedbackCallback(humanoid_common_msgs::humanoid_follow_targetFeedbackPtr& feedback)
{
feedback->current_pan=((manager_current_angles[L_PAN]*3.14159)/180.0)/65536.0;
feedback->current_tilt=((manager_current_angles[L_TILT]*3.14159)/180.0)/65536.0;
}
/************************************ follow_target_action *********************************************/
/************************************ set_walk_params *******************************************/
template <class HardwareInterface>
bool DarwinController<HardwareInterface>::set_walk_paramsCallback(humanoid_common_msgs::set_walk_params::Request &req, humanoid_common_msgs::set_walk_params::Response &res)
{
ROS_INFO("CDarwinSim : Setting walk parameters ...");
this->walk_access.enter();
ram_write_byte(DARWIN_WALK_SWING_RIGHT_LEFT,(unsigned char)(req.params.Y_SWAP_AMPLITUDE*1000.0));
ram_write_byte(DARWIN_WALK_SWING_TOP_DOWN,(unsigned char)(req.params.Z_SWAP_AMPLITUDE*1000.0));
ram_write_byte(DARWIN_WALK_ARM_SWING_GAIN,(unsigned char)(req.params.ARM_SWING_GAIN*32.0));
ram_write_byte(DARWIN_WALK_PELVIS_OFFSET,(unsigned char)((req.params.PELVIS_OFFSET*180.0/PI)*8.0));
ram_write_word(DARWIN_WALK_HIP_PITCH_OFF_L,(unsigned short int)((req.params.HIP_PITCH_OFFSET*180.0/PI)*1024.0));
ram_write_byte(DARWIN_WALK_X_OFFSET,(unsigned char)(req.params.X_OFFSET*1000.0));
ram_write_byte(DARWIN_WALK_Y_OFFSET,(unsigned char)(req.params.Y_OFFSET*1000.0));
ram_write_byte(DARWIN_WALK_Z_OFFSET,(unsigned char)(req.params.Z_OFFSET*1000.0));
ram_write_byte(DARWIN_WALK_YAW_OFFSET,(unsigned char)((req.params.A_OFFSET*180/PI)*8.0));
ram_write_byte(DARWIN_WALK_PITCH_OFFSET,(unsigned char)((req.params.P_OFFSET*180/PI)*8.0));
ram_write_byte(DARWIN_WALK_ROLL_OFFSET,(unsigned char)((req.params.R_OFFSET*180/PI)*8.0));
ram_write_word(DARWIN_WALK_PERIOD_TIME_L,(unsigned short int)(req.params.PERIOD_TIME*1000.0));
ram_write_byte(DARWIN_WALK_DSP_RATIO,(unsigned char)(req.params.DSP_RATIO*256.0));
ram_write_byte(DARWIN_WALK_STEP_FW_BW_RATIO,(unsigned char)(req.params.STEP_FB_RATIO*256.0));
ram_write_byte(DARWIN_WALK_FOOT_HEIGHT,(unsigned char)(req.params.FOOT_HEIGHT*1000.0));
ram_write_byte(DARWIN_WALK_MAX_VEL,(unsigned char)(req.params.MAX_VEL*1000.0));
ram_write_byte(DARWIN_WALK_MAX_ROT_VEL,(unsigned char)((req.params.MAX_ROT_VEL*1440.0)/PI));
this->walk_access.exit();
res.ret=true;
}
/************************************ set_walk_params *******************************************/
/************************************ get_walk_params *******************************************/
template <class HardwareInterface>
bool DarwinController<HardwareInterface>::get_walk_paramsCallback(humanoid_common_msgs::get_walk_params::Request &req, humanoid_common_msgs::get_walk_params::Response &res)
{
unsigned char byte_value;
unsigned short int word_value;
ROS_INFO("CDarwinSim : Getting current walk parameters ...");
this->walk_access.enter();
ram_read_byte(DARWIN_WALK_SWING_RIGHT_LEFT,&byte_value);
res.params.Y_SWAP_AMPLITUDE=((double)byte_value)/1000.0;
ram_read_byte(DARWIN_WALK_SWING_TOP_DOWN,&byte_value);
res.params.Z_SWAP_AMPLITUDE=((double)byte_value)/1000.0;
ram_read_byte(DARWIN_WALK_ARM_SWING_GAIN,&byte_value);
res.params.ARM_SWING_GAIN=((double)byte_value)/32.0;
ram_read_byte(DARWIN_WALK_PELVIS_OFFSET,&byte_value);
res.params.PELVIS_OFFSET=(((double)byte_value)*PI/180.0)/8.0;
ram_read_word(DARWIN_WALK_HIP_PITCH_OFF_L,&word_value);
res.params.HIP_PITCH_OFFSET=(((double)word_value)*PI/180.0)/1024.0;
ram_read_byte(DARWIN_WALK_X_OFFSET,&byte_value);
res.params.X_OFFSET=((double)byte_value)/1000.0;
ram_read_byte(DARWIN_WALK_Y_OFFSET,&byte_value);
res.params.Y_OFFSET=((double)byte_value)/1000.0;
ram_read_byte(DARWIN_WALK_Z_OFFSET,&byte_value);
res.params.Z_OFFSET=((double)byte_value)/1000.0;
ram_read_byte(DARWIN_WALK_YAW_OFFSET,&byte_value);
res.params.A_OFFSET=(((double)byte_value)*PI/180.0)/8.0;;
ram_read_byte(DARWIN_WALK_PITCH_OFFSET,&byte_value);
res.params.P_OFFSET=(((double)byte_value)*PI/180.0)/8.0;
ram_read_byte(DARWIN_WALK_ROLL_OFFSET,&byte_value);
res.params.R_OFFSET=(((double)byte_value)*PI/180.0)/8.0;
ram_read_word(DARWIN_WALK_PERIOD_TIME_L,&word_value);
res.params.PERIOD_TIME=((double)word_value)/1000.0;
ram_read_byte(DARWIN_WALK_DSP_RATIO,&byte_value);
res.params.DSP_RATIO=((double)byte_value)/256.0;
ram_read_byte(DARWIN_WALK_STEP_FW_BW_RATIO,&byte_value);
res.params.STEP_FB_RATIO=((double)byte_value)/256.0;
ram_read_byte(DARWIN_WALK_FOOT_HEIGHT,&byte_value);
res.params.STEP_FB_RATIO=((double)byte_value)/1000.0;
ram_read_byte(DARWIN_WALK_FOOT_HEIGHT,&byte_value);
res.params.FOOT_HEIGHT=((double)byte_value)/1000.0;
ram_read_byte(DARWIN_WALK_MAX_VEL,&byte_value);
res.params.MAX_VEL=((double)byte_value)/1000.0;
ram_read_byte(DARWIN_WALK_MAX_ROT_VEL,&byte_value);
res.params.MAX_ROT_VEL=(((double)byte_value)*PI)/1440.0;
this->walk_access.exit();
return true;
}
/************************************ get_walk_params *******************************************/
/************************************ set_servo_modules *****************************************/
template <class HardwareInterface>
bool DarwinController<HardwareInterface>::set_servo_modulesCallback(humanoid_common_msgs::set_servo_modules::Request &req, humanoid_common_msgs::set_servo_modules::Response &res)
{
unsigned int i=0,j=0;
ROS_INFO("CDarwinSim : Setting servo modules ...");
for(i=0;i<req.names.size();i++)
{
for(j=0;j<MANAGER_MAX_NUM_SERVOS;j++)
{
if(req.names[i]==servo_names[j])
{
if(req.modules[i]=="action")
manager_set_module(j,MM_ACTION);
else if(req.modules[i]=="walking")
manager_set_module(j,MM_WALKING);
else if(req.modules[i]=="joints")
manager_set_module(j,MM_JOINTS);
else if(req.modules[i]=="head")
manager_set_module(j,MM_HEAD);
else
manager_set_module(j,MM_NONE);
break;
}
}
}
res.ret=true;
return true;
}
/************************************ set_servo_modules *****************************************/
/************************************ set_pan_pid *****************************************/
template <class HardwareInterface>
bool DarwinController<HardwareInterface>::set_pan_pidCallback(humanoid_common_msgs::set_pid::Request &req, humanoid_common_msgs::set_pid::Response &res)
{
uint16_t kp,ki,kd,i_clamp;
ROS_INFO("CDarwinSim : Setting pan pid ...");
kp=req.p*65536.0;
ki=req.i*65536.0;
kd=req.d*65536.0;
i_clamp=((req.i_clamp*180.0)/3.14159)*128.0;
head_tracking_set_pan_pid(kp,ki,kd,i_clamp);
res.ret=true;
return true;
}
/************************************ set_pan_pid *****************************************/
/************************************ get_pan_pid *****************************************/
template <class HardwareInterface>
bool DarwinController<HardwareInterface>::get_pan_pidCallback(humanoid_common_msgs::get_pid::Request &req, humanoid_common_msgs::get_pid::Response &res)
{
uint16_t kp,ki,kd,i_clamp;
ROS_INFO("CDarwinSim : Getting pan pid ...");
head_tracking_get_pan_pid(&kp,&ki,&kd,&i_clamp);
res.p=((float)kp)/65536.0;
res.i=((float)ki)/65536.0;
res.d=((float)kd)/65536.0;
res.i_clamp=((((float)i_clamp)*3.14159)/180.0)/128.0;
res.ret=true;
return true;
}
/************************************ get_pan_pid *****************************************/
/************************************ set_tilt_pid *****************************************/
template <class HardwareInterface>
bool DarwinController<HardwareInterface>::set_tilt_pidCallback(humanoid_common_msgs::set_pid::Request &req, humanoid_common_msgs::set_pid::Response &res)
{
uint16_t kp,ki,kd,i_clamp;
ROS_INFO("CDarwinSim : Setting tilt pid ...");
kp=req.p*65536.0;
ki=req.i*65536.0;
kd=req.d*65536.0;
i_clamp=((req.i_clamp*180.0)/3.14159)*128.0;
head_tracking_set_tilt_pid(kp,ki,kd,i_clamp);
res.ret=true;
return true;
}
/************************************ set_tilt_pid *****************************************/
/************************************ get_tilt_pid *****************************************/
template <class HardwareInterface>
bool DarwinController<HardwareInterface>::get_tilt_pidCallback(humanoid_common_msgs::get_pid::Request &req, humanoid_common_msgs::get_pid::Response &res)
{
uint16_t kp,ki,kd,i_clamp;
ROS_INFO("CDarwinSim : Getting tilt pid ...");
head_tracking_get_tilt_pid(&kp,&ki,&kd,&i_clamp);
res.p=((float)kp)/65536.0;
res.i=((float)ki)/65536.0;
res.d=((float)kd)/65536.0;
res.i_clamp=((((float)i_clamp)*3.14159)/180.0)/128.0;
res.ret=true;
return true;
}
/************************************ get_tilt_pid *****************************************/
/****************************** get_smart_charger_config ***********************************/
template <class HardwareInterface>
bool DarwinController<HardwareInterface>::get_smart_charger_configCallback(humanoid_common_msgs::get_smart_charger_config::Request &req, humanoid_common_msgs::get_smart_charger_config::Response &res)
{
}
/************************************ get_tilt_pid *****************************************/
/****************************** set_smart_charger_config ***********************************/
template <class HardwareInterface>
bool DarwinController<HardwareInterface>::set_smart_charger_configCallback(humanoid_common_msgs::set_smart_charger_config::Request &req, humanoid_common_msgs::set_smart_charger_config::Response &res)
{
}
/************************************ get_tilt_pid *****************************************/
/****************************************** imu *************************************************/
/****************************************** imu *************************************************/
template <class HardwareInterface>
void DarwinController<HardwareInterface>::gyro_callback(const sensor_msgs::Imu::ConstPtr& msg)
{
double gyro_x,gyro_y;
// saturate to +/- 500 dps;
gyro_x=msg->angular_velocity.y*180.0/3.14159;
if(gyro_x>500.0)
gyro_x=500.0;
else if(gyro_x<-500.0)
gyro_x=-500.0;
gyro_data[0]=(int32_t)(gyro_x*1000.0);
gyro_y=msg->angular_velocity.z*180.0/3.14159;
if(gyro_y>500.0)
gyro_y=500.0;
else if(gyro_y<-500.0)
gyro_y=-500.0;
gyro_data[1]=-(int32_t)(gyro_y*1000.0);
}
template <class HardwareInterface>
void DarwinController<HardwareInterface>::accel_callback(const sensor_msgs::Imu::ConstPtr& msg)
{
accel_data[0]=(int32_t)msg->linear_acceleration.x*1000.0;
accel_data[1]=(int32_t)msg->linear_acceleration.y*1000.0;
accel_data[2]=(int32_t)msg->linear_acceleration.z*1000.0;
}
/****************************************** imu *************************************************/
/****************************************** cmd_vel *********************************************/
template <class HardwareInterface>
void DarwinController<HardwareInterface>::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
{
unsigned short int period;
unsigned char step;
this->walk_access.enter();
ram_read_word(DARWIN_WALK_PERIOD_TIME_L,&period);
ram_write_byte(DARWIN_WALK_STEP_FW_BW,(unsigned char)(msg->linear.x*period));
ram_write_byte(DARWIN_WALK_STEP_LEFT_RIGHT,(unsigned char)(msg->linear.y*period));
ram_write_byte(DARWIN_WALK_STEP_DIRECTION,(unsigned char)(((msg->angular.z*1.44)/PI)*period));
if(!is_walking())
{
ROS_INFO("CDarwinSim : Start walking");
walking_start();
}
this->walk_timeout=ros::Time::now();
this->walk_access.exit();
}
/****************************************** cmd_vel *********************************************/
/****************************************** head_target *************************************************/
template <class HardwareInterface>
void DarwinController<HardwareInterface>::head_target_callback(const trajectory_msgs::JointTrajectoryPoint::ConstPtr& msg)
{
ROS_INFO("CDarwinSim : New head target ...");
if(msg->positions.size()==2)
{
head_tracking_set_target_pan(((msg->positions[0]*180.0)/3.14159)*128.0);
head_tracking_set_target_tilt(((msg->positions[1]*180.0)/3.14159)*128.0);
}
}
/****************************************** head_target *************************************************/
template <class HardwareInterface>
DarwinController<HardwareInterface>::~DarwinController()
{
if(this->motion_action_aserver_!=NULL)
{
delete this->motion_action_aserver_;
this->motion_action_aserver_=NULL;
}
if(this->joint_trajectory_aserver_!=NULL)
{
delete this->joint_trajectory_aserver_;
this->joint_trajectory_aserver_=NULL;
}
if(this->follow_target_aserver_!=NULL)
{
delete this->follow_target_aserver_;
this->follow_target_aserver_=NULL;
}
}
/****************************************** LEDS *************************************************/
template <class HardwareInterface>
void DarwinController<HardwareInterface>::leds_callback(const humanoid_common_msgs::leds::ConstPtr& msg)
{
unsigned int i=0;
for(i=0;i<msg->name.size();i++)
{
if(msg->blink[i])
ROS_INFO_STREAM("CDarwinSim : blink led " << msg->name[i] << " with period " << msg->period_ms[i] << " ms");
else if(msg->toggle[i])
ROS_INFO_STREAM("CDarwinSim : toggle led " << msg->name[i]);
else if(msg->value[i])
ROS_INFO_STREAM("CDarwinSim : set led " << msg->name[i]);
else
ROS_INFO_STREAM("CDarwinSim : clear led " << msg->name[i]);
}
}
/****************************************** LEDS *************************************************/
} // namespace
#endif // header guard