25 : control_cycle_msec_(8),
145 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
152 ros_node.
param<std::string>(
"walking_param_path",
param_path_, default_param_path);
188 robotis_controller_msgs::StatusMsg status_msg;
190 status_msg.type = type;
191 status_msg.module_name =
"Walking";
192 status_msg.status_msg = msg;
201 ROS_WARN(
"walking module is not ready.");
205 if (msg->data ==
"start")
207 else if (msg->data ==
"stop")
209 else if (msg->data ==
"balance on")
211 else if (msg->data ==
"balance off")
213 else if (msg->data ==
"save")
223 op3_walking_module_msgs::GetWalkingParam::Response &res)
232 return mag *
sin(2 * M_PI / period * time - period_shift) + mag_shift;
240 double thigh_length = 93.0 * 0.001;
241 double calf_length = 93.0 * 0.001;
242 double ankle_length = 33.5 * 0.001;
243 double leg_length = 219.5 * 0.001;
245 Eigen::MatrixXd transformation_ad, transformation_da, transformation_cd, transformation_dc, transformation_ac;
246 Eigen::Vector3d vector;
247 double r_ac, acos_value, atan_value, value_k, value_l, value_m, value_n, value_s, value_c, theta;
252 vector << pos_x + transformation_ad.coeff(0, 2) * ankle_length, pos_y + transformation_ad.coeff(1, 2) * ankle_length, (pos_z
253 - leg_length) + transformation_ad.coeff(2, 2) * ankle_length;
256 r_ac = vector.norm();
258 (r_ac * r_ac - thigh_length * thigh_length - calf_length * calf_length) / (2 * thigh_length * calf_length));
259 if (std::isnan(acos_value) == 1)
261 *(out + 3) = acos_value;
265 double tda_y = transformation_da.coeff(1, 3);
266 double tda_z = transformation_da.coeff(2, 3);
267 value_k =
sqrt(tda_y * tda_y + tda_z * tda_z);
268 value_l =
sqrt(tda_y * tda_y + (tda_z - ankle_length) * (tda_z - ankle_length));
269 value_m = (value_k * value_k - value_l * value_l - ankle_length * ankle_length) / (2 * value_l * ankle_length);
272 else if (value_m < -1.0)
274 acos_value =
acos(value_m);
275 if (std::isnan(acos_value) == 1)
278 *(out + 5) = -acos_value;
280 *(out + 5) = acos_value;
285 transformation_ac = transformation_ad * transformation_dc;
286 atan_value =
atan2(-transformation_ac.coeff(0, 1), transformation_ac.coeff(1, 1));
287 if (std::isinf(atan_value) == 1)
292 atan_value =
atan2(transformation_ac.coeff(2, 1),
293 -transformation_ac.coeff(0, 1) *
sin(*(out)) + transformation_ac.coeff(1, 1) *
cos(*(out)));
294 if (std::isinf(atan_value) == 1)
296 *(out + 1) = atan_value;
299 atan_value =
atan2(transformation_ac.coeff(0, 2) *
cos(*(out)) + transformation_ac.coeff(1, 2) *
sin(*(out)),
300 transformation_ac.coeff(0, 0) *
cos(*(out)) + transformation_ac.coeff(1, 0) *
sin(*(out)));
301 if (std::isinf(atan_value) == 1)
304 value_k =
sin(*(out + 3)) * calf_length;
305 value_l = -thigh_length -
cos(*(out + 3)) * calf_length;
306 value_m =
cos(*(out)) * vector.x() +
sin(*(out)) * vector.y();
307 value_n =
cos(*(out + 1)) * vector.z() +
sin(*(out)) *
sin(*(out + 1)) * vector.x()
308 -
cos(*(out)) *
sin(*(out + 1)) * vector.y();
309 value_s = (value_k * value_n + value_l * value_m) / (value_k * value_k + value_l * value_l);
310 value_c = (value_n - value_k * value_s) / value_l;
311 atan_value =
atan2(value_s, value_c);
312 if (std::isinf(atan_value) == 1)
314 *(out + 2) = atan_value;
315 *(out + 4) = theta - *(out + 3) - *(out + 2);
409 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_INFO,
"Start walking");
415 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_INFO,
"Stop walking");
425 std::map<std::string, double> sensors)
431 int joint_size =
result_.size();
432 double angle[joint_size];
433 double balance_angle[joint_size];
438 for (
int id = 1;
id <=
result_.size();
id++)
453 for (std::map<std::string, robotis_framework::DynamixelState *>::iterator state_iter =
result_.begin();
454 state_iter !=
result_.end(); state_iter++)
456 std::string _joint_name = state_iter->first;
460 std::map<std::string, robotis_framework::Dynamixel*>::iterator dxl_it = dxls.find(_joint_name);
461 if (dxl_it != dxls.end())
462 dxl = dxl_it->second;
471 bool get_angle =
false;
476 double rl_gyro_err = 0.0 - sensors[
"gyro_x"];
477 double fb_gyro_err = 0.0 - sensors[
"gyro_y"];
481 double err_total = 0.0, err_max = 0.0;
483 for (
int idx = 0; idx < 14; idx++)
485 double goal_position = 0.0;
486 if (get_angle ==
false && idx < 12)
489 goal_position =
init_position_.coeff(0, idx) + angle[idx] + balance_angle[idx];
503 std::cout <<
"Check Err : " << err_max << std::endl;
506 int mov_time = err_max / 30;
542 for (std::map<std::string, robotis_framework::DynamixelState *>::iterator state_it =
result_.begin();
543 state_it !=
result_.end(); state_it++)
545 std::string joint_name = state_it->first;
628 double pelvis_offset_r, pelvis_offset_l;
795 left_leg_move.
roll = 0;
796 left_leg_move.
pitch = 0;
797 right_leg_move.
roll = 0;
798 right_leg_move.
pitch = 0;
805 ep[2] = swap.
z + right_leg_move.
z +
z_offset_ - leg_length;
811 ep[8] = swap.
z + left_leg_move.
z +
z_offset_ - leg_length;
832 printf(
"IK not Solved EPR : %f %f %f %f %f %f\n", ep[0], ep[1], ep[2], ep[3], ep[4], ep[5]);
838 printf(
"IK not Solved EPL : %f %f %f %f %f %f\n", ep[6], ep[7], ep[8], ep[9], ep[10], ep[11]);
843 for (
int i = 0; i < 12; i++)
857 leg_angle[i] += offset;
886 double internal_gain = 0.05;
899 * internal_gain * fbGyroErr *
walking_param_.balance_ankle_pitch_gain;
901 * internal_gain * fbGyroErr *
walking_param_.balance_ankle_pitch_gain;
916 doc = YAML::LoadFile(path.c_str());
917 }
catch (
const std::exception& e)
932 walking_param_.period_time = doc[
"period_time"].as<
double>() * 0.001;
934 walking_param_.step_fb_ratio = doc[
"step_forward_back_ratio"].as<
double>();
938 walking_param_.z_move_amplitude = doc[
"foot_height"].as<
double>();
944 walking_param_.balance_hip_roll_gain = doc[
"balance_hip_roll_gain"].as<
double>();
945 walking_param_.balance_knee_gain = doc[
"balance_knee_gain"].as<
double>();
946 walking_param_.balance_ankle_roll_gain = doc[
"balance_ankle_roll_gain"].as<
double>();
947 walking_param_.balance_ankle_pitch_gain = doc[
"balance_ankle_pitch_gain"].as<
double>();
948 walking_param_.y_swap_amplitude = doc[
"swing_right_left"].as<
double>();
949 walking_param_.z_swap_amplitude = doc[
"swing_top_down"].as<
double>();
951 walking_param_.arm_swing_gain = doc[
"arm_swing_gain"].as<
double>();
961 YAML::Emitter out_emitter;
963 out_emitter << YAML::BeginMap;
964 out_emitter << YAML::Key <<
"x_offset" << YAML::Value <<
walking_param_.init_x_offset;
965 out_emitter << YAML::Key <<
"y_offset" << YAML::Value <<
walking_param_.init_y_offset;
966 out_emitter << YAML::Key <<
"z_offset" << YAML::Value <<
walking_param_.init_z_offset;
971 out_emitter << YAML::Key <<
"period_time" << YAML::Value <<
walking_param_.period_time * 1000;
972 out_emitter << YAML::Key <<
"dsp_ratio" << YAML::Value <<
walking_param_.dsp_ratio;
973 out_emitter << YAML::Key <<
"step_forward_back_ratio" << YAML::Value <<
walking_param_.step_fb_ratio;
974 out_emitter << YAML::Key <<
"foot_height" << YAML::Value <<
walking_param_.z_move_amplitude;
975 out_emitter << YAML::Key <<
"swing_right_left" << YAML::Value <<
walking_param_.y_swap_amplitude;
976 out_emitter << YAML::Key <<
"swing_top_down" << YAML::Value <<
walking_param_.z_swap_amplitude;
978 out_emitter << YAML::Key <<
"arm_swing_gain" << YAML::Value <<
walking_param_.arm_swing_gain;
979 out_emitter << YAML::Key <<
"balance_hip_roll_gain" << YAML::Value <<
walking_param_.balance_hip_roll_gain;
980 out_emitter << YAML::Key <<
"balance_knee_gain" << YAML::Value <<
walking_param_.balance_knee_gain;
981 out_emitter << YAML::Key <<
"balance_ankle_roll_gain" << YAML::Value <<
walking_param_.balance_ankle_roll_gain;
982 out_emitter << YAML::Key <<
"balance_ankle_pitch_gain" << YAML::Value <<
walking_param_.balance_ankle_pitch_gain;
984 out_emitter << YAML::Key <<
"p_gain" << YAML::Value <<
walking_param_.p_gain;
985 out_emitter << YAML::Key <<
"i_gain" << YAML::Value <<
walking_param_.i_gain;
986 out_emitter << YAML::Key <<
"d_gain" << YAML::Value <<
walking_param_.d_gain;
987 out_emitter << YAML::EndMap;
990 std::ofstream fout(path.c_str());
991 fout << out_emitter.c_str();
1009 int all_time_steps = int(mov_time / smp_time) + 1;
1012 for (
int id = 0;
id <=
result_.size();
id++)
1017 Eigen::MatrixXd tra;
1025 std::cout <<
"Generate Trajecotry : " << mov_time <<
"s [" << all_time_steps <<
"]" << std::endl;
std::map< std::string, DynamixelState * > result_
bool computeLegAngle(double *leg_angle)
double z_swap_period_time_
Eigen::MatrixXd calcMinimumJerkTra(double pos_start, double vel_start, double accel_start, double pos_end, double vel_end, double accel_end, double smp_time, double mov_time)
void publish(const boost::shared_ptr< M > &message) const
OP3KinematicsDynamics * op3_kd_
void initialize(const int control_cycle_msec, robotis_framework::Robot *robot)
double z_move_period_time_
void walkingCommandCallback(const std_msgs::String::ConstPtr &msg)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
double a_move_phase_shift_
Eigen::MatrixXd target_position_
double x_move_phase_shift_
void publishStatusMsg(unsigned int type, std::string msg)
double y_swap_period_time_
Eigen::Matrix4d getInverseTransformation(const Eigen::MatrixXd &transform)
double z_move_amplitude_shift_
void updateMovementParam()
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
Eigen::MatrixXi joint_axis_direction_
double x_move_amplitude_shift_
void process(std::map< std::string, robotis_framework::Dynamixel * > dxls, std::map< std::string, double > sensors)
Eigen::MatrixXd init_position_
ControlMode control_mode_
Eigen::Matrix4d getTransformationXYZRPY(double position_x, double position_y, double position_z, double roll, double pitch, double yaw)
double y_swap_phase_shift_
double a_move_period_time_
double previous_x_move_amplitude_
op3_walking_module_msgs::WalkingParam walking_param_
double y_move_period_time_
double getJointDirection(const std::string link_name)
double z_move_phase_shift_
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
void setCallbackQueue(CallbackQueueInterface *queue)
double y_swap_amplitude_shift_
double wSin(double time, double period, double period_shift, double mag, double mag_shift)
#define ROS_WARN_STREAM_COND(cond, args)
Eigen::MatrixXd goal_position_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
double x_move_period_time_
void walkingParameterCallback(const op3_walking_module_msgs::WalkingParam::ConstPtr &msg)
double y_move_amplitude_shift_
void saveWalkingParam(std::string &path)
boost::thread queue_thread_
ROSLIB_DECL std::string getPath(const std::string &package_name)
double x_swap_period_time_
void loadWalkingParam(const std::string &path)
double y_move_phase_shift_
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
void processPhase(const double &time_unit)
double a_move_amplitude_shift_
DynamixelState * dxl_state_
bool getWalkigParameterCallback(op3_walking_module_msgs::GetWalkingParam::Request &req, op3_walking_module_msgs::GetWalkingParam::Response &res)
double x_swap_phase_shift_
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
double z_swap_amplitude_shift_
bool computeIK(double *out, double x, double y, double z, double a, double b, double c)
Eigen::MatrixXd calc_joint_tra_
bool calcInverseKinematicsForRightLeg(double *out, double x, double y, double z, double roll, double pitch, double yaw)
bool calcInverseKinematicsForLeftLeg(double *out, double x, double y, double z, double roll, double pitch, double yaw)
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
void swap(scoped_ptr< T > &, scoped_ptr< T > &)
std::map< std::string, int > joint_table_
double x_swap_amplitude_shift_
void computeArmAngle(double *arm_angle)
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
void sensoryFeedback(const double &rlGyroErr, const double &fbGyroErr, double *balance_angle)
double z_swap_phase_shift_
void iniPoseTraGene(double mov_time)
ros::Publisher status_msg_pub_