38 #include <std_msgs/String.h> 41 #include <kdl/chainfksolverpos_recursive.hpp> 42 #include <kdl/chainfksolvervel_recursive.hpp> 44 #include <geometry_msgs/TwistStamped.h> 46 #include <boost/assign/list_of.hpp> 70 std::string(
"odom_on_ground"));
81 std::string(
"lleg_end_coords"));
83 std::string(
"rleg_end_coords"));
87 robot_model.
initParam(
"robot_description");
89 ROS_FATAL(
"Failed to load robot_description");
125 odom_imu_sync_ = boost::make_shared<message_filters::Synchronizer<OdomImuSyncPolicy> >(100);
142 std::vector<float> temp = boost::assign::list_of(0)(0)(1)(0);
144 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
161 stat.
summary(diagnostic_msgs::DiagnosticStatus::WARN,
"On Air");
165 stat.
summary(diagnostic_msgs::DiagnosticStatus::OK,
"Left leg on the ground");
168 stat.
summary(diagnostic_msgs::DiagnosticStatus::OK,
"Right leg on the ground");
171 stat.
summary(diagnostic_msgs::DiagnosticStatus::OK,
"Both legs on the ground");
178 return prev_val +
alpha_ * (current_val - prev_val);
184 for (
size_t i = 0; i < values.size(); i++) {
185 if (values[i]->
value < threshold) {
195 for (
size_t i = 0; i < values.size(); i++) {
196 if (values[i]->
value > threshold) {
204 const geometry_msgs::WrenchStamped& rfoot,
210 ROS_ERROR(
"[Footcoords::resolveForceTf] failed to lookup transformation for sensor value");
225 lfoot_force = lfoot_transform * lfoot_local;
226 rfoot_force = rfoot_transform * rfoot_local;
232 ROS_ERROR(
"[Footcoords::resolveForceTf] transform error: %s", e.what());
236 ROS_ERROR(
"[Footcoords::resolveForceTf] transform error: %s", e.what());
240 ROS_ERROR(
"[Footcoords::resolveForceTf] transform error: %s", e.what());
244 ROS_ERROR(
"[Footcoords::resolveForceTf] transform error: %s", e.what());
272 ROS_ERROR(
"[Footcoords::resolveForceTf] transform error: %s", e.what());
275 ROS_ERROR(
"[Footcoords::resolveForceTf] transform error: %s", e.what());
278 ROS_ERROR(
"[Footcoords::resolveForceTf] transform error: %s", e.what());
281 ROS_ERROR(
"[Footcoords::resolveForceTf] transform error: %s", e.what());
299 const geometry_msgs::WrenchStamped::ConstPtr& rfoot,
300 const sensor_msgs::JointState::ConstPtr& joint_states,
301 const geometry_msgs::PointStamped::ConstPtr& zmp)
303 jsk_footstep_controller::SynchronizedForces forces;
304 forces.header = lfoot->header;
305 forces.lleg_force = *lfoot;
306 forces.rleg_force = *rfoot;
307 forces.joint_angles = *joint_states;
310 Eigen::Vector3d zmp_point;
313 if (zmp->point.z < 0) {
314 geometry_msgs::TransformStamped ros_zmp_coords;
315 ros_zmp_coords.header = zmp->header;
317 Eigen::Affine3d zmp_pose = Eigen::Affine3d::Identity() * Eigen::Translation3d(zmp_point);
319 std::vector<geometry_msgs::TransformStamped> tf_transforms;
320 tf_transforms.push_back(ros_zmp_coords);
339 if (joint_angles.find(joint_name) != joint_angles.end()) {
340 jnt_pos(j++) = joint_angles[joint_name];
345 if (fk_solver.
JntToCart(jnt_pos, pose) < 0) {
356 geometry_msgs::Pose ros_lfoot_pose, ros_rfoot_pose;
364 std::map<std::string, double>& joint_angles,
366 geometry_msgs::Twist& output)
373 if (joint_angles.find(joint_name) != joint_angles.end()) {
374 qdot(j) = (joint_angles[joint_name] -
prev_joints_[joint_name]) / dt;
375 q(j++) = joint_angles[joint_name];
381 if (fk_solver.
JntToCart(jnt_pos, vel) < 0) {
390 geometry_msgs::TwistStamped twist_stamped;
399 twist_stamped.header.stamp = stamp;
408 geometry_msgs::WrenchStamped lfoot = msg->lleg_force;
409 geometry_msgs::WrenchStamped rfoot = msg->rleg_force;
410 zmp_ = Eigen::Vector3f(msg->zmp.point.x, msg->zmp.point.y, msg->zmp.point.z);
411 sensor_msgs::JointState joint_angles = msg->joint_angles;
412 std::map<std::string, double>
angles;
413 for (
size_t i = 0; i < joint_angles.name.size(); i++) {
414 angles[joint_angles.name[i]] = joint_angles.position[i];
420 tf::Vector3 lfoot_force_vector, rfoot_force_vector;
421 if (!
resolveForceTf(lfoot, rfoot, lfoot_force_vector, rfoot_force_vector)) {
422 ROS_ERROR(
"[Footcoords::filter] failed to resolve tf of force sensor");
430 FootCoordsLowLevelInfo
info;
431 info.header = lfoot.header;
432 info.raw_lleg_force = lfoot_force_vector[2];
433 info.raw_rleg_force = rfoot_force_vector[2];
434 info.filtered_lleg_force = lfoot_force;
435 info.filtered_rleg_force = rfoot_force;
457 geometry_msgs::PoseStamped leg_odometory;
458 leg_odometory.header.stamp = msg->header.stamp;
463 std::vector<geometry_msgs::TransformStamped> tf_transforms;
464 geometry_msgs::TransformStamped odom_transform;
466 odom_transform.header = leg_odometory.header;
467 odom_transform.child_frame_id =
"leg_odom";
468 tf_transforms.push_back(odom_transform);
483 ROS_INFO(
"changed to INITIALIZING");
545 ROS_INFO(
"changed to INITIALIZING");
624 ROS_INFO(
"changed to INITIALIZING");
704 std_msgs::String state_msg;
705 state_msg.data = state;
711 GroundContactState contact_state;
712 contact_state.header.stamp = stamp;
714 contact_state.contact_state
715 = GroundContactState::CONTACT_AIR;
718 contact_state.contact_state
719 = GroundContactState::CONTACT_LLEG_GROUND;
722 contact_state.contact_state
723 = GroundContactState::CONTACT_RLEG_GROUND;
726 contact_state.contact_state
727 = GroundContactState::CONTACT_BOTH_GROUND;
736 contact_state.error_pitch_angle = std::abs(pitch);
737 contact_state.error_roll_angle = std::abs(roll);
738 contact_state.error_yaw_angle = std::abs(yaw);
739 contact_state.error_z = std::abs(foot_transform.
getOrigin().
z());
744 ROS_ERROR(
"[Footcoords::publishContactState] transform error: %s", e.what());
748 ROS_ERROR(
"[Footcoords::publishContactState] transform error: %s", e.what());
752 ROS_ERROR(
"[Footcoords::publishContactState] transform error: %s", e.what());
756 ROS_ERROR(
"[Footcoords::publishContactState] transform error: %s", e.what());
765 ROS_ERROR(
"[Footcoords::computeMidCoordsFromSingleLeg] Failed to lookup endeffector transformation");
785 ROS_ERROR(
"[Footcoords::computeMidCoordsFromSingleLeg] transform error: %s", e.what());
790 ROS_ERROR(
"[Footcoords::computeMidCoordsFromSingleLeg] transform error: %s", e.what());
795 ROS_ERROR(
"[Footcoords::computeMidCoordsFromSingleLeg] transform error: %s", e.what());
799 ROS_ERROR(
"[Footcoords::computeMidCoordsFromSingleLeg] transform error: %s", e.what());
809 ROS_ERROR(
"[Footcoords::computeMidCoords] Failed to lookup endeffector transformation");
834 ROS_ERROR(
"[Footcoords::computeMidCoords] transform error: %s", e.what());
839 ROS_ERROR(
"[Footcoords::computeMidCoords] transform error: %s", e.what());
844 ROS_ERROR(
"[Footcoords::computeMidCoords] transform error: %s", e.what());
848 ROS_ERROR(
"[Footcoords::computeMidCoords] transform error: %s", e.what());
856 const std::string& lsensor_frame,
857 const std::string& rsensor_frame)
862 ROS_ERROR(
"[Footcoords::waitForSensorFrameTransformation] failed to lookup transform between %s and %s",
863 lsensor_frame.c_str(),
869 ROS_ERROR(
"[Footcoords::waitForSensorFrameTransformation] failed to lookup transform between %s and %s",
870 rsensor_frame.c_str(),
882 ROS_ERROR(
"[Footcoords::waitForEndEffectorTrasnformation] failed to lookup transform between %s and %s",
890 ROS_ERROR(
"[Footcoords::waitForEndEffectorTrasnformation]failed to lookup transform between %s and %s",
898 ROS_ERROR(
"[Footcoords::waitForEndEffectorTrasnformation]failed to lookup transform between %s and %s",
918 Eigen::Affine3d odom_to_midcoords;
920 Eigen::Affine3d midcoords_to_odom = odom_to_midcoords.inverse();
921 Eigen::Affine3d midcoords_to_odom_on_ground = midcoords_to_odom;
922 midcoords_to_odom_on_ground.translation().z() = 0.0;
923 Eigen::Vector3d zaxis;
924 zaxis[0] = midcoords_to_odom_on_ground(0, 2);
925 zaxis[1] = midcoords_to_odom_on_ground(1, 2);
926 zaxis[2] = midcoords_to_odom_on_ground(2, 2);
927 Eigen::Quaterniond
rot;
928 rot.setFromTwoVectors(zaxis, Eigen::Vector3d(0, 0, 1));
929 midcoords_to_odom_on_ground.rotate(rot);
930 Eigen::Affine3d odom_to_odom_on_ground
931 = odom_to_midcoords * midcoords_to_odom_on_ground;
939 Eigen::Affine3f posef;
940 jsk_pcl_ros::convertEigenAffine3(pose, posef);
942 pcl::getEulerAngles(posef, roll, pitch, yaw);
947 Eigen::Affine3f posef;
948 jsk_pcl_ros::convertEigenAffine3(pose, posef);
950 pcl::getEulerAngles(posef, roll, pitch, yaw);
957 geometry_msgs::TransformStamped ros_midcoords,
958 ros_ground_coords, ros_odom_to_body_coords,
959 ros_body_on_odom_coords, ros_odom_init_coords;
965 header.stamp = stamp;
967 ros_midcoords.header.stamp = stamp;
970 ros_ground_coords.header.stamp = stamp;
973 ros_odom_to_body_coords.header.stamp = stamp;
976 ros_body_on_odom_coords.header.stamp = stamp;
979 ros_odom_init_coords.header.stamp = stamp;
987 Eigen::Affine3d identity = Eigen::Affine3d::Identity();
990 Eigen::Affine3d body_on_odom_pose = (Eigen::Translation3d(0,
993 Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()) *
994 Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()));
996 Eigen::Affine3d odom_init_pose = (Eigen::Translation3d(
odom_init_pose_.translation()[0],
1010 std::vector<geometry_msgs::TransformStamped> tf_transforms;
1011 tf_transforms.push_back(ros_midcoords);
1012 tf_transforms.push_back(ros_ground_coords);
1014 tf_transforms.push_back(ros_odom_to_body_coords);
1016 tf_transforms.push_back(ros_body_on_odom_coords);
1017 tf_transforms.push_back(ros_odom_init_coords);
1032 geometry_msgs::TransformStamped ros_odom_init_coords;
1033 geometry_msgs::PoseStamped ros_odom_init_pose_stamped;
1034 Eigen::Affine3d odom_init_pose = (Eigen::Translation3d(
odom_init_pose_.translation()[0],
1043 ros_odom_init_pose_stamped.header = ros_odom_init_coords.header;
1044 ros_odom_init_pose_stamped.pose.position.x = ros_odom_init_coords.transform.translation.x;
1045 ros_odom_init_pose_stamped.pose.position.y = ros_odom_init_coords.transform.translation.y;
1046 ros_odom_init_pose_stamped.pose.position.z = ros_odom_init_coords.transform.translation.z;
1047 ros_odom_init_pose_stamped.pose.orientation = ros_odom_init_coords.transform.rotation;
1061 ROS_ERROR(
"[Footcoords::floorCoeffsCallback] transform error: %s", e.what());
1064 ROS_ERROR(
"[Footcoords::floorCoeffsCallback] transform error: %s", e.what());
1067 ROS_ERROR(
"[Footcoords::floorCoeffsCallback] transform error: %s", e.what());
1070 ROS_ERROR(
"[Footcoords::floorCoeffsCallback] transform error: %s", e.what());
1073 Eigen::Affine3d floor_transform_eigen;
1076 tmp_plane.
transform(floor_transform_eigen);
1084 const sensor_msgs::Imu::ConstPtr& imu_msg)
1088 Eigen::Affine3d odom_pose;
1090 Eigen::Quaterniond imu_orientation;
1093 odom_pose_ = Eigen::Translation3d(odom_pose.translation()) * imu_orientation;
1097 getRollPitch(Eigen::Affine3d::Identity() * imu_orientation, roll, pitch);
1098 odom_pose_ = (Eigen::Translation3d(odom_pose.translation()) *
1099 Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()) *
1100 Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()));
void transformEigenToTF(const Eigen::Affine3d &e, tf::Transform &t)
const Segment & getSegment(unsigned int nr) const
virtual int JntToCart(const JntArray &q_in, Frame &p_out, int segmentNr=-1)
void publish(const boost::shared_ptr< M > &message) const
KDL_PARSER_PUBLIC bool treeFromUrdfModel(const urdf::ModelInterface &robot_model, KDL::Tree &tree)
void transformTFToEigen(const tf::Transform &t, Eigen::Affine3d &e)
void summary(unsigned char lvl, const std::string s)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
void transformEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Transform &m)
unsigned int getNrOfSegments() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
IMETHOD Twist GetTwist() const
virtual Plane transform(const Eigen::Affine3d &transform)
virtual void toCoefficients(std::vector< float > &output)
Quaternion slerp(const Quaternion &q, const tfScalar &t) const
ROSCPP_DECL void spin(Spinner &spinner)
TFSIMD_FORCE_INLINE Vector3 lerp(const Vector3 &v, const tfScalar &t) const
const std::string & getName() const
static void vector3MsgToTF(const geometry_msgs::Vector3 &msg_v, Vector3 &bt_v)
TFSIMD_FORCE_INLINE const tfScalar & z() const
void poseKDLToTF(const KDL::Frame &k, tf::Pose &t)
void getRPY(tfScalar &roll, tfScalar &pitch, tfScalar &yaw, unsigned int solution_number=1) const
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
void twistKDLToMsg(const KDL::Twist &k, geometry_msgs::Twist &m)
const Joint & getJoint() const
void pointMsgToEigen(const geometry_msgs::Point &m, Eigen::Vector3d &e)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
unsigned int getNrOfJoints() const
URDF_EXPORT bool initParam(const std::string ¶m)
virtual void project(const Eigen::Vector3f &p, Eigen::Vector3f &output)
bool getChain(const std::string &chain_root, const std::string &chain_tip, Chain &chain) const
virtual int JntToCart(const JntArrayVel &q_in, FrameVel &out, int segmentNr=-1)
static void poseTFToMsg(const Pose &bt, geometry_msgs::Pose &msg)
#define ROS_INFO_STREAM(args)
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
void quaternionMsgToEigen(const geometry_msgs::Quaternion &m, Eigen::Quaterniond &e)
#define ROS_INFO_THROTTLE(rate,...)
static void transformTFToMsg(const Transform &bt, geometry_msgs::Transform &msg)