void LCM2ROS::neckPitchHandler(const lcm::ReceiveBuffer* rbuf, const std::string &channel, const drc::neck_pitch_t* msg) { ROS_ERROR("LCM2ROS got desired neck pitch"); ihmc_msgs::HeadOrientationPacketMessage mout; Eigen::Quaterniond quat = euler_to_quat(0, msg->pitch, 0); mout.trajectory_time = 1; mout.orientation.w = quat.w(); mout.orientation.x = quat.x(); mout.orientation.y = quat.y(); mout.orientation.z = quat.z(); mout.unique_id = msg->utime; neck_orientation_pub_.publish(mout); }
void LCM2ROS::robotPlanHandler(const lcm::ReceiveBuffer* rbuf, const std::string &channel, const drc::robot_plan_t* msg) { ROS_ERROR("LCM2ROS got robot plan with %d states", msg->num_states); ihmc_msgs::WholeBodyTrajectoryPacketMessage wbt_msg; wbt_msg.unique_id = msg->utime; // 1. Insert Arm Joints std::vector<std::string> l_arm_strings; std::vector<std::string> r_arm_strings; std::vector<std::string> input_joint_names = msg->plan[0].joint_name; if (robotName_.compare("atlas") == 0) { // Remove MIT/IPAB joint names and use IHMC joint names: filterJointNamesToIHMC(input_joint_names); l_arm_strings = { "l_arm_shz", "l_arm_shx", "l_arm_ely", "l_arm_elx", "l_arm_wry", "l_arm_wrx", "l_arm_wry2"}; r_arm_strings = { "r_arm_shz", "r_arm_shx", "r_arm_ely", "r_arm_elx", "r_arm_wry", "r_arm_wrx", "r_arm_wry2"}; } else if (robotName_.compare("valkyrie") == 0) { l_arm_strings = {"leftShoulderPitch", "leftShoulderRoll", "leftShoulderYaw", "leftElbowPitch", "leftForearmYaw", "leftWristRoll", "leftWristPitch"}; r_arm_strings = {"rightShoulderPitch", "rightShoulderRoll", "rightShoulderYaw", "rightElbowPitch", "rightForearmYaw", "rightWristRoll", "rightWristPitch"}; } ihmc_msgs::ArmJointTrajectoryPacketMessage left_arm_trajectory; bool status_left = getSingleArmPlan(msg, l_arm_strings, input_joint_names, false, left_arm_trajectory); ihmc_msgs::ArmJointTrajectoryPacketMessage right_arm_trajectory; bool status_right = getSingleArmPlan(msg, r_arm_strings, input_joint_names, true, right_arm_trajectory); if (!status_left || !status_right) { ROS_ERROR("LCM2ROS: problem with arm plan, not sending"); } wbt_msg.left_arm_trajectory = left_arm_trajectory; wbt_msg.right_arm_trajectory = right_arm_trajectory; wbt_msg.num_joints_per_arm = l_arm_strings.size(); // 2. Insert Pelvis Pose for (int i = 1; i < msg->num_states; i++) // NB: skipping the first sample as it has time = 0 { drc::robot_state_t state = msg->plan[i]; geometry_msgs::Vector3 pelvis_world_position; pelvis_world_position.x = state.pose.translation.x; pelvis_world_position.y = state.pose.translation.y; pelvis_world_position.z = state.pose.translation.z; wbt_msg.pelvis_world_position.push_back(pelvis_world_position); geometry_msgs::Quaternion pelvis_world_orientation; pelvis_world_orientation.w = state.pose.rotation.w; pelvis_world_orientation.x = state.pose.rotation.x; pelvis_world_orientation.y = state.pose.rotation.y; pelvis_world_orientation.z = state.pose.rotation.z; wbt_msg.pelvis_world_orientation.push_back(pelvis_world_orientation); wbt_msg.time_at_waypoint.push_back(state.utime * 1E-6); } wbt_msg.num_waypoints = msg->num_states - 1; // NB: skipping the first sample as it has time = 0 // 3. Insert Chest Pose (in work frame) std::vector<geometry_msgs::Quaternion> chest_trajectory; bool status_chest = getChestTrajectoryPlan(msg, chest_trajectory); if (!status_chest) { ROS_ERROR("LCM2ROS: problem with chest plan, not sending"); } wbt_msg.chest_world_orientation = chest_trajectory; whole_body_trajectory_pub_.publish(wbt_msg); ROS_ERROR("LCM2ROS sent Whole Body Trajectory"); if (1==0){ if (robotName_.compare("valkyrie") == 0){ std::vector<std::string>::iterator it1 = find(input_joint_names.begin(), input_joint_names.end(), "lowerNeckPitch"); int lowerNeckPitchIndex = std::distance(input_joint_names.begin(), it1); float lowerNeckPitchAngle = msg->plan[ msg->num_states-1 ].joint_position[lowerNeckPitchIndex]; std::vector<std::string>::iterator it2 = find(input_joint_names.begin(), input_joint_names.end(), "neckYaw"); int neckYawIndex = std::distance(input_joint_names.begin(), it2); float neckYawAngle = msg->plan[ msg->num_states-1 ].joint_position[neckYawIndex]; Eigen::Quaterniond headOrientation = euler_to_quat(0, lowerNeckPitchAngle, neckYawAngle); ihmc_msgs::HeadOrientationPacketMessage mout; mout.trajectory_time = 1; // time doesn't seem to be tracked currently. mout.orientation.w = headOrientation.w(); mout.orientation.x = headOrientation.x(); mout.orientation.y = headOrientation.y(); mout.orientation.z = headOrientation.z(); mout.unique_id = msg->utime; neck_orientation_pub_.publish(mout); ROS_ERROR("LCM2ROS sent desired head orientation"); } } /* sendSingleArmPlan(msg, l_arm_strings, input_joint_names, false); ROS_ERROR("LCM2ROS sent left arm, sleeping for 1 second"); sleep(1); ROS_ERROR("LCM2ROS sent right arm"); sendSingleArmPlan(msg, r_arm_strings, input_joint_names, true); */ }
bool FilterFindfloor::getHeightPitchRoll(double height_pitch_roll[]){ // Take the floor part of the PCD: pcl::PointCloud<pcl::PointXYZ> cloudfloor; cloudfloor.width = incloud->points.size(); cloudfloor.height = 1; cloudfloor.is_dense = false; cloudfloor.points.resize (cloudfloor.width * cloudfloor.height); // TODO // OLD: seem to need a duplicate PC as XYZRGBA cannot be used as ransac input // NEW: dec2011: i think this can not be fixed and is not required. mfallon pcl::PointCloud<pcl::PointXYZRGB> cloudfloorRGB; cloudfloorRGB.width = incloud->points.size(); cloudfloorRGB.height = 1; cloudfloorRGB.is_dense = false; cloudfloorRGB.points.resize (cloudfloorRGB.width * cloudfloorRGB.height); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudfloorRGBptr (new pcl::PointCloud<pcl::PointXYZRGB> (cloudfloorRGB)); if(verbose_text>0){ std::cout << "No of points into floor filter: " << incloud->points.size() << std::endl; } int Nransac=0; for(int j3=0; j3< incloud->points.size(); j3++) { if ((incloud->points[j3].z < -0.75 )&& (incloud->points[j3].x < 5 )) { // RANSAC head filter // -0.75m for 2011_03_16_cart_stata // -1.25m for 2011_03_14_brookshire // either -1.10 -1.00m for me cloudfloor.points[Nransac].x =incloud->points[j3].x; cloudfloor.points[Nransac].y =incloud->points[j3].y; cloudfloor.points[Nransac].z =incloud->points[j3].z; //cloudfloor.points[j].rgba =cloud.points[j].rgba; cloudfloorRGB.points[Nransac].x =incloud->points[j3].x; cloudfloorRGB.points[Nransac].y =incloud->points[j3].y; cloudfloorRGB.points[Nransac].z =incloud->points[j3].z; // cloudfloorRGB.points[Nransac].rgb =incloud->points[j3].rgb; //cloudfloorRGB.points[Nransac].rgba =incloud->points[j3].rgba; Nransac++; } } cloudfloor.points.resize (Nransac); cloudfloor.width = (Nransac); cloudfloorRGB.points.resize (Nransac); cloudfloorRGB.width = (Nransac); if(verbose_text>0){ std::cout << "No of points into ransac: " << Nransac << std::endl; } pcl::ModelCoefficients coeff; pcl::PointIndices inliers; // Create the segmentation object pcl::SACSegmentation<pcl::PointXYZ> seg; // Optional seg.setOptimizeCoefficients (true); // Mandatory seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setDistanceThreshold (0.01); // was 0.01m pcl::PointCloud<pcl::PointXYZ>::Ptr cloudptr (new pcl::PointCloud<pcl::PointXYZ> (cloudfloor)); seg.setInputCloud (cloudptr); seg.segment (inliers, coeff); if (inliers.indices.size () == 0) { printf ("[FilterFindfloor::getHeightPitchRoll] Could not estimate a planar model for the given dataset.\n"); return -1; } double pitch = atan(coeff.values[0]/coeff.values[2]); double roll =- atan(coeff.values[1]/coeff.values[2]); double coeff_norm = sqrt(pow(coeff.values[0],2) + pow(coeff.values[1],2) + pow(coeff.values[2],2)); double height = (coeff.values[2]*coeff.values[3]) / coeff_norm; // the point directly under the kinect, having corrected the rotation: // double sub_pt[3]; // sub_pt[1]= -coeff.values[0]*coeff.values[3]/pow(coeff_norm,2) // sub_pt[2]= -coeff.values[1]*coeff.values[3]/pow(coeff_norm,2) // sub_pt[3]= -coeff.values[2]*coeff.values[3]/pow(coeff_norm,2) if (verbose_text>0){ cout << "\nRANSAC Floor Coefficients: " << coeff.values[0] << " " << coeff.values[1] << " " << coeff.values[2] << " " << coeff.values[3] << endl; cout << "Pitch: " << pitch << " (" << (pitch*180/M_PI) << "d). positive nose down\n"; cout << "Roll : " << roll << " (" << (roll*180/M_PI) << "d). positive right side down\n"; cout << "Height : " << height << " of device off ground [m]\n"; cout << "Total points: " << Nransac << ". inliers: " << inliers.indices.size () << endl << endl; //std::cout << std::setprecision(20) << "corresponding obj_coll " << state->bot_id // << " and kinect " << state->msg->timestamp << "\n"; } int floor_to_file =0; if(floor_to_file==1){ //pcl::io::savePCDFile ("RANSAC_floorcloud.pcd", cloudfloor, false); //std::cout << "about to publish to RANSAC_floorcloud.pcd - " << cloudfloor.points.size () << "pts" << std::endl; } // Filter Ridiculous Values from the Ransac Floor estimator: int skip_floor_estimate =0; if ((height > 3) || (height < 0.8)){ skip_floor_estimate =1; } if ((pitch > 10 ) || (height < -10)){ skip_floor_estimate =1; } if ((roll > 10 ) || (roll < -10)){ skip_floor_estimate =1; } if (skip_floor_estimate){ std::cout << "Ransac has produced ridiculous results, ignore\n"; height= last_height_pitch_roll[0]; pitch= last_height_pitch_roll[1]; roll= last_height_pitch_roll[2]; } // use a fixed prior for the DOFs estimated by the kinect: (for testing) /* int use_fixed_prior=1; if (use_fixed_prior){ roll = -0.5*M_PI/180;// 5*M_PI/180 ; pitch = 9.5*M_PI/180;// 5*M_PI/180 ; height = 1.2; } */ if(verbose_lcm>0){ // important // turn inlier points red: for (size_t i = 0; i < inliers.indices.size (); ++i){ size_t this_index = inliers.indices[i]; unsigned char red[]={255,0,0,0}; // rgba unsigned char* rgba_ptr = (unsigned char*)&cloudfloorRGB.points[this_index].rgba; (*rgba_ptr) = red[0]; (*(rgba_ptr+1)) = red[1]; (*(rgba_ptr+2)) = red[2]; (*(rgba_ptr+3)) = red[3]; } int floor_obj_collection; int64_t floor_obj_element_id; floor_obj_collection= 4; floor_obj_element_id = pose_element_id;// bot_timestamp_now(); /*Ptcoll_cfg floorcoll_cfg; floorcoll_cfg.id = 1; floorcoll_cfg.name ="Floor RANSAC"; floorcoll_cfg.reset=true; floorcoll_cfg.type =1; floorcoll_cfg.point_lists_id = pose_element_id ; floorcoll_cfg.collection = floor_obj_collection; floorcoll_cfg.element_id = floor_obj_element_id; floorcoll_cfg.npoints = Nransac; float colorm_temp0[] ={-1.0,-1.0,-1.0,-1.0}; floorcoll_cfg.rgba.assign(colorm_temp0,colorm_temp0+4*sizeof(float)); // floorcoll_cfg.rgba = {-1,-1,-1,-1}; pcdXYZRGB_to_lcm(publish_lcm,floorcoll_cfg, cloudfloorRGB); */ if (verbose_lcm > 2){ // unimportant // 0 - position (fixed) at 000 with corrected pitch and roll vs_obj_collection_t objs; objs.id = floor_obj_collection; objs.name = (char*) "Floor Pose"; // "Trajectory"; objs.type = 1; // a pose objs.reset = 0; // true will delete them from the viewer objs.nobjs = 1; vs_obj_t poses[objs.nobjs]; poses[0].id = floor_obj_element_id; poses[0].x = 0;// state->bot_pos[0] ; poses[0].y = 0;// state->bot_pos[1] ; poses[0].z = height; // state->bot_pos[2] ; poses[0].yaw = 0;// state->bot_rpy[2] ; poses[0].pitch = -pitch; //state->bot_rpy[1]; poses[0].roll = -roll; //state->bot_rpy[0]; objs.objs = poses; vs_obj_collection_t_publish(publish_lcm, "OBJ_COLLECTION", &objs); bot_core_pose_t testdata; testdata.utime = pose_element_id; testdata.pos[0]=0; testdata.pos[1]=0; testdata.pos[2]=height; Eigen::Quaterniond quat =euler_to_quat(roll,pitch,0); // rpy testdata.orientation[0] = quat.w(); testdata.orientation[1] = quat.x(); testdata.orientation[2] = quat.y(); testdata.orientation[3] = quat.z(); // double temp_rpy[] ={ roll, pitch, 0}; // no yaw estimate // bot_roll_pitch_yaw_to_quat(temp_rpy, testdata.orientation) ; testdata.vel[0] =0; testdata.vel[1] =0; testdata.vel[2] =0; testdata.rotation_rate[0] =0; testdata.rotation_rate[1] =0; testdata.rotation_rate[2] =0; testdata.accel[0] =0; testdata.accel[1] =0; testdata.accel[2] =0; bot_core_pose_t_publish(publish_lcm,"POSE_FINDGROUND", &testdata); } } height_pitch_roll[0] = height; height_pitch_roll[1] = pitch; height_pitch_roll[2] = roll; return 1; }
void des_thrusters::step(double delta) { bool softkilled; shm_get(switches, soft_kill, softkilled); shm_getg(settings_control, shm_settings); if (softkilled || !shm_settings.enabled) { f -= f; t -= t; return; } // Read PID settings & desires. // (would switching to watchers improve performance?) SHM_GET_PID(settings_heading, shm_hp, hp) SHM_GET_PID(settings_pitch, shm_pp, pp) SHM_GET_PID(settings_roll, shm_rp, rp) SHM_GET_PID(settings_velx, shm_xp, xp) SHM_GET_PID(settings_vely, shm_yp, yp) SHM_GET_PID(settings_depth, shm_dp, dp) shm_getg(desires, shm_desires); // Read current orientation, velocity & position (in the model frame). // Orientation quaternion in the model frame. Eigen::Quaterniond qm = q * mtob_rq; Eigen::Vector3d rph = quat_to_euler(qm); // Component of the model quaternion about the z-axis (heading-only rotation). Eigen::Quaterniond qh = euler_to_quat(rph[2], 0, 0); // Velocity in heading modified world space. Eigen::Vector3d vp = qh.conjugate() * v; // Step the PID loops. Eigen::Vector3d desires = quat_to_euler(euler_to_quat(shm_desires.heading * DEG_TO_RAD, shm_desires.pitch * DEG_TO_RAD, shm_desires.roll * DEG_TO_RAD)) * RAD_TO_DEG; double ho = hp.step(delta, desires[2], rph[2] * RAD_TO_DEG); double po = pp.step(delta, desires[1], rph[1] * RAD_TO_DEG); double ro = rp.step(delta, desires[0], rph[0] * RAD_TO_DEG); double xo = xp.step(delta, shm_desires.speed, vp[0]); double yo = yp.step(delta, shm_desires.sway_speed, vp[1]); double zo = dp.step(delta, shm_desires.depth, x[2]); // f is in the heading modified world frame. // t is in the model frame. // We will work with f and b in the model frame until the end of this // function. f[0] = shm_settings.velx_active ? xo : 0; f[1] = shm_settings.vely_active ? yo : 0; f[2] = shm_settings.depth_active ? zo : 0; f *= m; Eigen::Vector3d w_in; w_in[0] = shm_settings.roll_active ? ro : 0; w_in[1] = shm_settings.pitch_active ? po : 0; w_in[2] = shm_settings.heading_active ? ho : 0; // TODO Avoid this roundabout conversion from hpr frame // to world frame and back to model frame. Eigen::Quaterniond qhp = euler_to_quat(rph[2], rph[1], 0); Eigen::Vector3d w = qm.conjugate() * (Eigen::Vector3d(0, 0, w_in[2]) + qh * Eigen::Vector3d(0, w_in[1], 0) + qhp * Eigen::Vector3d(w_in[0], 0, 0)); t = btom_rm * q.conjugate() * I * q * mtob_rm * w; // Output diagnostic information. Shown by auv-control-helm. SHM_PUT_PID(control_internal_heading, ho) SHM_PUT_PID(control_internal_pitch, po) SHM_PUT_PID(control_internal_roll, ro) SHM_PUT_PID(control_internal_velx, xo) SHM_PUT_PID(control_internal_vely, yo) SHM_PUT_PID(control_internal_depth, zo) // Subtract passive forces. // (this implementation does not support discrimination!) if (shm_settings.buoyancy_forces || shm_settings.drag_forces) { // pff is in the body frame. screw ps = pff(); f -= qh.conjugate() * q * ps.first; t -= btom_rm * ps.second; } // Hyper-ellipsoid clamping. Sort of limiting to the maximum amount of // energy the sub can output (e.g. can't move forward at full speed // and pitch at full speed at the same time). // Doesn't really account for real-world thruster configurations (e.g. // it might be possible to move forward at full speed and ascend at // full speed at the same time) but it's just an approximation. for (int i = 0; i < 3; i++) { f[i] /= axes[i]; } for (int i = 0; i < 3; i++) { t[i] /= axes[3 + i]; } double m = f.dot(f) + t.dot(t); if (m > 1) { double sm = sqrt(m); f /= sm; t /= sm; } for (int i = 0; i < 3; i++) { f[i] *= axes[i]; } for (int i = 0; i < 3; i++) { t[i] *= axes[3 + i]; } // Regular min/max clamping. clamp(f, fa, fb, 3); clamp(t, ta, tb, 3); f = q.conjugate() * qh * f; t = mtob_rm * t; }