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;
}
