Skip to content

Commit

Permalink
fix #28 - desc' srv disabled. rviz config saved.
Browse files Browse the repository at this point in the history
  • Loading branch information
@yijiangh
yijiangh committed Jun 7, 2017
1 parent 838ea05 commit 5dfce15
Show file tree
Hide file tree
Showing 2 changed files with 150 additions and 150 deletions.
278 changes: 139 additions & 139 deletions framefab_mpp/src/framefab_planner.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -101,152 +101,152 @@ bool FrameFabPlanner::testCartPlanning(
bool FrameFabPlanner::testDescartesPlanning(
framefab_msgs::TestDescartes::Request &req, framefab_msgs::TestDescartes::Response &res)
{
// Get parameters from the message and print them
ROS_WARN_STREAM("testDescartes request:" << std::endl << req);

// Get current robot pose
tf::TransformListener listener;
listener.waitForTransform("/arm_base_link", "/tool_tip", ros::Time::now(), ros::Duration(1.0));
tf::StampedTransform transform_stamp;
Eigen::Affine3d current_pose;

try
{
listener.lookupTransform("/arm_base_link", "/tool_tip", ros::Time(0), transform_stamp);
transformTFToEigen(transform_stamp, current_pose);
}
catch (tf::TransformException &ex)
{
res.success = false;
ROS_ERROR("[testDescartesPlanning] tf error.");
return false;
}

ros::Rate loop_rate(10);

// 1. Define sequence of points
double x, y, z, rx, ry, rz;
x = 0.67734;
y = 0;
z = 0.5;
rx = 0.0;
ry = M_PI;
rz = 0.0;
TrajectoryVec points;
int N_points = 10;

framefab_process_path::ProcessPathGenerator path_generator;
framefab_process_path::ProcessPathVisualizer path_visualizer;

std::vector<Eigen::Affine3d> poses;
Eigen::Affine3d startPose;
Eigen::Affine3d endPose;
startPose = descartes_core::utils::toFrame(x, y, z, rx, ry, rz, descartes_core::utils::EulerConventions::XYZ);
endPose = descartes_core::utils::toFrame(x, y + 0.5, z, rx, ry, rz, descartes_core::utils::EulerConventions::XYZ);
poses = path_generator.addLinePathPts(startPose, endPose, N_points);


for (unsigned int i = 0; i < N_points; ++i)
{
// descartes_core::TrajectoryPtPtr pt = makeTolerancedCartesianPoint(poses[i], 0.0, 0.4, M_PI);
descartes_core::TrajectoryPtPtr pt = makeTolerancedCartesianPoint(poses[i]);
points.push_back(pt);
}

// Visualize the trajectory points in RViz
// Transform the generated poses into a markerArray message that can be visualized by RViz
visualization_msgs::MarkerArray ma;
ma = path_visualizer.createMarkerArray(poses);

ros::Publisher vis_pub = node_handle_.advertise<visualization_msgs::MarkerArray>("/visualization_marker_array", 1);
if(waitForSubscribers(vis_pub, ros::Duration(0.5)))
{
vis_pub.publish(ma);
loop_rate.sleep();
}


// // Get parameters from the message and print them
// ROS_WARN_STREAM("testDescartes request:" << std::endl << req);
//
// // Get current robot pose
// tf::TransformListener listener;
// listener.waitForTransform("/arm_base_link", "/tool_tip", ros::Time::now(), ros::Duration(1.0));
// tf::StampedTransform transform_stamp;
// Eigen::Affine3d current_pose;
//
// try
// {
// listener.lookupTransform("/arm_base_link", "/tool_tip", ros::Time(0), transform_stamp);
// transformTFToEigen(transform_stamp, current_pose);
// }
// catch (tf::TransformException &ex)
// {
// res.success = false;
// ROS_ERROR("[testDescartesPlanning] tf error.");
// return false;
// }
//
// ros::Rate loop_rate(10);
//
// // 1. Define sequence of points
// double x, y, z, rx, ry, rz;
// x = 0.67734;
// y = 0;
// z = 0.5;
// rx = 0.0;
// ry = M_PI;
// rz = 0.0;
// TrajectoryVec points;
// for (unsigned int i = 0; i < 4; ++i)
// int N_points = 10;
//
// framefab_process_path::ProcessPathGenerator path_generator;
// framefab_process_path::ProcessPathVisualizer path_visualizer;
//
// std::vector<Eigen::Affine3d> poses;
// Eigen::Affine3d startPose;
// Eigen::Affine3d endPose;
// startPose = descartes_core::utils::toFrame(x, y, z, rx, ry, rz, descartes_core::utils::EulerConventions::XYZ);
// endPose = descartes_core::utils::toFrame(x, y + 0.5, z, rx, ry, rz, descartes_core::utils::EulerConventions::XYZ);
// poses = path_generator.addLinePathPts(startPose, endPose, N_points);
//
//
// for (unsigned int i = 0; i < N_points; ++i)
// {
// Eigen::Affine3d pose = current_pose.translate(Eigen::Vector3d(0, 0, 0.005*i));
// //pose = Eigen::Translation3d(0.05*i, 0.0, 0.0);
// descartes_core::TrajectoryPtPtr pt = makeTolerancedCartesianPoint(pose);
//// descartes_core::TrajectoryPtPtr pt = makeTolerancedCartesianPoint(poses[i], 0.0, 0.4, M_PI);
// descartes_core::TrajectoryPtPtr pt = makeTolerancedCartesianPoint(poses[i]);
// points.push_back(pt);
// }

// for (unsigned int i = 0; i < 4; ++i)
//
// // Visualize the trajectory points in RViz
// // Transform the generated poses into a markerArray message that can be visualized by RViz
// visualization_msgs::MarkerArray ma;
// ma = path_visualizer.createMarkerArray(poses);
//
// ros::Publisher vis_pub = node_handle_.advertise<visualization_msgs::MarkerArray>("/visualization_marker_array", 1);
// if(waitForSubscribers(vis_pub, ros::Duration(0.5)))
// {
// Eigen::Affine3d pose;
// pose = Eigen::Translation3d(0.0, 0.04 * i, 0.05);
// descartes_core::TrajectoryPtPtr pt = makeTolerancedCartesianPoint(pose);
// points.push_back(pt);
// vis_pub.publish(ma);
// loop_rate.sleep();
// }

// TODO: move model construction in constructor
// 2. Create a robot model and initialize it
descartes_core::RobotModelPtr model (new descartes_moveit::MoveitStateAdapter);

// Name of description on parameter server. Typically just "robot_description".
const std::string robot_description = "robot_description";

// name of the kinematic group you defined when running MoveitSetupAssistant
const std::string group_name = "manipulator";

// Name of frame in which you are expressing poses. Typically "world_frame" or "base_link".
const std::string world_frame = ptr_move_group_->getPlanningFrame();

// tool center point frame (name of link associated with tool)
const std::string tcp_frame = "tool_tip";

if (!model->initialize(robot_description, group_name, world_frame, tcp_frame))
{
ROS_INFO("Could not initialize robot model");
res.success = false;
return false;
}

// 3. Create a planner and initialize it with our robot model
descartes_planner::DensePlanner planner;
planner.initialize(model);

// 4. Feed the trajectory to the planner
if (!planner.planPath(points))
{
ROS_ERROR("Could not solve for a valid path");
res.success = false;
return false;
}

TrajectoryVec result;
if (!planner.getPath(result))
{
ROS_ERROR("Could not retrieve path");
res.success = false;
return false;
}

// 5. Translate the result into a type that ROS understands
// Get Joint Names
std::vector<std::string> names;
node_handle_.getParam("controller_joint_names", names);
// Generate a ROS joint trajectory with the result path, robot model, given joint names,
// a certain time delta between each trajectory point
trajectory_msgs::JointTrajectory joint_solution = toROSJointTrajectory(result, *model, names, 1.0);

// 6. Send the ROS trajectory to the robot for execution
if (!executeTrajectory(joint_solution))
{
ROS_ERROR("Could not execute trajectory!");
res.success = false;
return false;
}

// Wait till user kills the process (Control-C)
ROS_INFO("test Descartes Done!");
res.success = true;
return true;
//
//
//// // 1. Define sequence of points
//// TrajectoryVec points;
//// for (unsigned int i = 0; i < 4; ++i)
//// {
//// Eigen::Affine3d pose = current_pose.translate(Eigen::Vector3d(0, 0, 0.005*i));
//// //pose = Eigen::Translation3d(0.05*i, 0.0, 0.0);
//// descartes_core::TrajectoryPtPtr pt = makeTolerancedCartesianPoint(pose);
//// points.push_back(pt);
//// }
//
//// for (unsigned int i = 0; i < 4; ++i)
//// {
//// Eigen::Affine3d pose;
//// pose = Eigen::Translation3d(0.0, 0.04 * i, 0.05);
//// descartes_core::TrajectoryPtPtr pt = makeTolerancedCartesianPoint(pose);
//// points.push_back(pt);
//// }
//
// // TODO: move model construction in constructor
// // 2. Create a robot model and initialize it
// descartes_core::RobotModelPtr model (new descartes_moveit::MoveitStateAdapter);
//
// // Name of description on parameter server. Typically just "robot_description".
// const std::string robot_description = "robot_description";
//
// // name of the kinematic group you defined when running MoveitSetupAssistant
// const std::string group_name = "manipulator";
//
// // Name of frame in which you are expressing poses. Typically "world_frame" or "base_link".
// const std::string world_frame = ptr_move_group_->getPlanningFrame();
//
// // tool center point frame (name of link associated with tool)
// const std::string tcp_frame = "tool_tip";
//
// if (!model->initialize(robot_description, group_name, world_frame, tcp_frame))
// {
// ROS_INFO("Could not initialize robot model");
// res.success = false;
// return false;
// }
//
// // 3. Create a planner and initialize it with our robot model
// descartes_planner::DensePlanner planner;
// planner.initialize(model);
//
// // 4. Feed the trajectory to the planner
// if (!planner.planPath(points))
// {
// ROS_ERROR("Could not solve for a valid path");
// res.success = false;
// return false;
// }
//
// TrajectoryVec result;
// if (!planner.getPath(result))
// {
// ROS_ERROR("Could not retrieve path");
// res.success = false;
// return false;
// }
//
// // 5. Translate the result into a type that ROS understands
// // Get Joint Names
// std::vector<std::string> names;
// node_handle_.getParam("controller_joint_names", names);
// // Generate a ROS joint trajectory with the result path, robot model, given joint names,
// // a certain time delta between each trajectory point
// trajectory_msgs::JointTrajectory joint_solution = toROSJointTrajectory(result, *model, names, 1.0);
//
// // 6. Send the ROS trajectory to the robot for execution
// if (!executeTrajectory(joint_solution))
// {
// ROS_ERROR("Could not execute trajectory!");
// res.success = false;
// return false;
// }
//
// // Wait till user kills the process (Control-C)
// ROS_INFO("test Descartes Done!");
// res.success = true;
// return true;
}

descartes_core::TrajectoryPtPtr FrameFabPlanner::makeCartesianPoint(const Eigen::Affine3d& pose)
Expand Down
22 changes: 11 additions & 11 deletions kuka_kr6r900_with_hotend/kr6r900sixx_with_hotend_moveit_config/launch/moveit.rviz
View file
Original file line number Diff line number Diff line change
Expand Up @@ -367,40 +367,40 @@ Visualization Manager:
Views:
Current:
Class: rviz/XYOrbit
Distance: 2.50823
Distance: 2.43651
Enable Stereo Rendering:
Stereo Eye Separation: 0.06
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0.870639
Y: 0.584665
Z: 6.40751e-07
X: 0.330819
Y: 0.222343
Z: 9.98379e-07
Name: Current View
Near Clip Distance: 0.01
Pitch: 0.489796
Pitch: 0.599796
Target Frame: arm_base_link
Value: XYOrbit (rviz)
Yaw: 0.406776
Yaw: 5.56996
Saved: ~
Window Geometry:
Displays:
collapsed: false
FrameFabRvizPanel:
collapsed: false
Height: 1317
Height: 1148
Hide Left Dock: false
Hide Right Dock: false
Motion Planning:
collapsed: false
QMainWindow State: 000000ff00000000fd0000000100000000000002b1000004dffc0200000008fb000000100044006900730070006c0061007900730100000028000000dd000000dd00fffffffb0000000800480065006c00700000000342000000bb0000000000000000fb0000000a0056006900650077007300000003b0000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e0067010000010b000001d7000001cc00fffffffb00000022004600720061006d0065004600610062005200760069007a00500061006e0065006c01000002e80000021f0000021f00fffffffb0000001200530065006c0065006300740069006f006e00000003bb000000bf0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000003d8000000a20000006400ffffff0000049e000004df00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
QMainWindow State: 000000ff00000000fd0000000100000000000002b100000436fc0200000008fb000000100044006900730070006c0061007900730000000028000000dd000000dd00fffffffb0000000800480065006c00700000000342000000bb0000000000000000fb0000000a0056006900650077007300000003b0000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e0067010000002800000211000001cc00fffffffb00000022004600720061006d0065004600610062005200760069007a00500061006e0065006c010000023f0000021f0000021f00fffffffb0000001200530065006c0065006300740069006f006e00000003bb000000bf0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000003d8000000a20000006400ffffff000004a20000043600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1877
X: 39
Y: 52
Width: 1881
X: 29
Y: 14

0 comments on commit 5dfce15

Please sign in to comment.