Skip to content
Snippets Groups Projects
Commit a7ebe96d authored by Johannes Mey's avatar Johannes Mey
Browse files

straighten interface

parent c511bf1f
Branches
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
config/config.yaml
+ 2
− 1
View file @ a7ebe96d
panda_mqtt_connector:
server: "tcp://localhost:1883"
robot_speed_factor: .15
robot_speed_factor: .95
robot_planning_mode: "fluid_path" # "fluid_path" or "cartesian_path"
default_trajectory: "square" # "square" or "circle", everything else is ignored
topics:
robotConfig: "robotconfig"
dataConfig: "dataconfig"
......
src/TimedPlanner.cpp
+ 113
− 124
View file @ a7ebe96d
......@@ -18,41 +18,44 @@
namespace PlannerState {
std::vector <geometry_msgs::Pose> raw_trajectory;
std::vector<geometry_msgs::Pose> raw_trajectory;
TrajectoryUtil traj_util;
const double default_velocity = 1.0;
const std::string default_planning_mode = TrajectoryUtil::FLUID_PATH;
const bool useCircle = true;
bool isLooping = true;
}
void constructPlate(std::vector<moveit_msgs::CollisionObject> &collision_objects, double x_dimension, double y_dimension) {
void
constructPlate(std::vector<moveit_msgs::CollisionObject> &collision_objects, double x_dimension, double y_dimension) {
moveit_msgs::CollisionObject plate;
moveit_msgs::CollisionObject plate;
plate.header.frame_id = "panda_link0";
plate.id = "plate";
plate.header.frame_id = "panda_link0";
plate.id = "plate";
plate.primitives.resize(1);
plate.primitives[0].type = 1u;
plate.primitives[0].dimensions.resize(3);
plate.primitives[0].dimensions[0] = x_dimension;
plate.primitives[0].dimensions[1] = y_dimension;
plate.primitives[0].dimensions[2] = 0.1;
plate.primitives.resize(1);
plate.primitives[0].type = 1u;
plate.primitives[0].dimensions.resize(3);
plate.primitives[0].dimensions[0] = x_dimension;
plate.primitives[0].dimensions[1] = y_dimension;
plate.primitives[0].dimensions[2] = 0.1;
plate.primitive_poses.resize(1);
plate.primitive_poses[0].position.x = 0;
plate.primitive_poses[0].position.y = 0;
plate.primitive_poses[0].position.z = -0.1;
plate.primitive_poses.resize(1);
plate.primitive_poses[0].position.x = 0;
plate.primitive_poses[0].position.y = 0;
plate.primitive_poses[0].position.z = -0.1;
plate.operation = plate.ADD;
plate.operation = plate.ADD;
collision_objects.push_back(plate);
collision_objects.push_back(plate);
}
void initCircularTrajectory(double radius, geometry_msgs::Point origin, int granularity) {
void loadCircularTrajectory(std::vector<geometry_msgs::Pose> &waypoints, double radius, geometry_msgs::Point origin,
int granularity) {
double step_size_rad = 2 * M_PI / granularity;
......@@ -71,125 +74,68 @@ void initCircularTrajectory(double radius, geometry_msgs::Point origin, int gran
nextPose.position.y = origin.y + radius * cos(next_rad);
nextPose.position.z = origin.z;
PlannerState::raw_trajectory.push_back(nextPose);
std::cout << "added: " << nextPose << std::endl;
waypoints.push_back(nextPose);
}
}
void
initRawTrajectory(ros::NodeHandle node_handle, moveit::planning_interface::MoveGroupInterface *group, bool useDefault,
bool useCircularDefault) {
geometry_msgs::Pose current_pose = group->getCurrentPose().pose;
if (useCircularDefault) {
geometry_msgs::Point origin;
origin.z = .3;
initCircularTrajectory(0.6, origin, 20);
return;
}
if (!PlannerState::traj_util.initWaypoints(PlannerState::raw_trajectory, node_handle, current_pose) && useDefault) {
// choose a default trajectory
geometry_msgs::Pose target_pose_1 = group->getCurrentPose().pose;
geometry_msgs::Pose target_pose_2 = group->getCurrentPose().pose;
geometry_msgs::Pose target_pose_3 = group->getCurrentPose().pose;
geometry_msgs::Pose target_pose_4 = group->getCurrentPose().pose;
target_pose_2.position.z = 0.6;//0.583;
target_pose_2.position.y = -0.6;//63;
target_pose_2.position.x = 0;//-0.007;
PlannerState::raw_trajectory.push_back(target_pose_2);
bool loadWaypoints(std::vector<geometry_msgs::Pose> &waypoints, const ros::NodeHandle &node_handle,
geometry_msgs::Pose &initialPose) {
target_pose_3.position.z = 0.6;//0.691;
target_pose_3.position.y = -0.032;
target_pose_3.position.x = -0.607;
PlannerState::raw_trajectory.push_back(target_pose_3);
// regardless if the loading fails, the trajectory is no longer "new" after calling this method
node_handle.setParam("new_trajectory_available", false);
target_pose_4.position.z = 0.6;
target_pose_4.position.y = 0.6;//0.509;
target_pose_4.position.x = 0;//0.039;
PlannerState::raw_trajectory.push_back(target_pose_4);
std::vector<double> x_values;
std::vector<double> y_values;
std::vector<double> z_values;
// check if no trajectory is configured
if (!node_handle.getParam("trajectory_pos_x", x_values) ||
!node_handle.getParam("trajectory_pos_y", y_values) ||
!node_handle.getParam("trajectory_pos_z", z_values)) {
return false;
} else {
for (int i = 0; i < x_values.size(); i++) {
geometry_msgs::Pose pose = initialPose;
pose.position.x = x_values.at(i);
pose.position.y = y_values.at(i);
pose.position.z = z_values.at(i);
waypoints.push_back(pose);
}
target_pose_1.position.z = group->getCurrentPose().pose.position.z;
target_pose_1.position.y = group->getCurrentPose().pose.position.y;
target_pose_1.position.x = group->getCurrentPose().pose.position.x;
PlannerState::raw_trajectory.push_back(target_pose_1);
return true;
}
}
bool updateRawTrajectory(ros::NodeHandle node_handle, moveit::planning_interface::MoveGroupInterface *group) {
geometry_msgs::Pose current_pose = group->getCurrentPose().pose;
PlannerState::raw_trajectory.clear();
node_handle.setParam("new_trajectory_available", false);
return PlannerState::traj_util.initWaypoints(PlannerState::raw_trajectory, node_handle, current_pose);
}
void moveRobotToInitialState(ros::NodeHandle node_handle) {
ROS_INFO("Moving to initial pose of trajectory.");
moveit::planning_interface::MoveGroupInterface group("panda_arm");
ros::Publisher display_publisher = node_handle.
advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
group.setStartStateToCurrentState();
std::vector <geometry_msgs::Pose> trajectory_to_init_pose;
// start at beginning of circular movement
tf2::Quaternion orientation; // the RPY constructor is deprecated
orientation.setRPY(0, M_PI, 0);
geometry_msgs::Pose target_pose_1;
target_pose_1.orientation = tf2::toMsg(orientation);
target_pose_1.position.x = 0;
target_pose_1.position.y = .6;
target_pose_1.position.z = .3;
void doMotion(const ros::NodeHandle &node_handle, moveit::planning_interface::MoveGroupInterface &group) {
trajectory_to_init_pose.push_back(target_pose_1);
group.setPlanningTime(10.0);
bool newTrajectoryAvailable = false;
node_handle.getParam("new_trajectory_available", newTrajectoryAvailable);
moveit_msgs::RobotTrajectory trajectory;
double fraction = group.computeCartesianPath(trajectory_to_init_pose, 0.01, 0.0, trajectory);
if (newTrajectoryAvailable) {
geometry_msgs::Pose current_pose = group.getCurrentPose().pose;
PlannerState::raw_trajectory.clear();
moveit::planning_interface::MoveGroupInterface::Plan cartesian_plan;
cartesian_plan.trajectory_ = trajectory;
group.execute(cartesian_plan);
}
if (!loadWaypoints(PlannerState::raw_trajectory, node_handle, current_pose)) {
ROS_ERROR_STREAM("Unable to load new trajectory!");
}
}
void doMotion(const ros::NodeHandle &node_handle, moveit::planning_interface::MoveGroupInterface &group) {
if (PlannerState::raw_trajectory.empty()) {
return;
}
for (int i = 0; i < PlannerState::raw_trajectory.size(); i++) {
bool newTrajectoryAvailable = false;
node_handle.getParam("new_trajectory_available", newTrajectoryAvailable);
node_handle.getParam("loop_trajectory", PlannerState::isLooping);
if (newTrajectoryAvailable) {
if (!updateRawTrajectory(node_handle, &group)) {
break;
}
i = 0;
}
// wake up if we have trajectory
while (PlannerState::raw_trajectory.empty()) {
if (newTrajectoryAvailable) {
updateRawTrajectory(node_handle, &group);
i = 0;
break;
}
}
double velocity = 0.0;
std::string planning_mode;
double velocity;
if (!node_handle.getParam("robot_speed_factor", velocity)) {
velocity = PlannerState::default_velocity;
}
std::string planning_mode;
if (!node_handle.getParam("robot_planning_mode", planning_mode)) {
planning_mode = PlannerState::default_planning_mode;
}
......@@ -197,15 +143,18 @@ void doMotion(const ros::NodeHandle &node_handle, moveit::planning_interface::Mo
ROS_INFO_STREAM("Planning the next trajectory in " << planning_mode << " with velocity " << velocity);
moveit::planning_interface::MoveGroupInterface::Plan plan;
PlannerState::traj_util.computePathToPose(group, plan, PlannerState::raw_trajectory.at(i), planning_mode, 0.8, 0.8, velocity);
group.execute(plan);
if (PlannerState::traj_util.computePathToPose(group, plan, PlannerState::raw_trajectory.at(i), planning_mode, 0.8,
0.8, velocity)) {
group.execute(plan);
} else {
ROS_ERROR_STREAM("The planner was unable to compute a path to the pose with the given settings.");
}
// make sure the robot moves in an infinite circle
if (i == (PlannerState::raw_trajectory.size() - 1)) {
if (PlannerState::isLooping) {
i = -1;
} else {
return;
}
}
}
......@@ -220,27 +169,67 @@ int main(int argc, char **argv) {
spinner.start();
// wait for robot init of robot_state_initializer
ros::Duration(5.0).sleep();
ros::Duration(3.0).sleep();
ROS_INFO(">>>>>>>>>>>>>>>>> START UP FINISHED <<<<<<<<<<<<<<<< ");
node_handle.setParam("tud_planner_ready", true);
// Initialize start state of robot and target trajectory.
moveit::planning_interface::MoveGroupInterface group("panda_arm");
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
//moveRobotToInitialState(node_handle);
initRawTrajectory(node_handle, &group, true, true);
node_handle.getParam("loop_trajectory", PlannerState::isLooping);
std::string defaultTrajectory = "<none>";
node_handle.getParam("default_trajectory", defaultTrajectory);
geometry_msgs::Pose base_pose = group.getCurrentPose().pose;
if (loadWaypoints(PlannerState::raw_trajectory, node_handle, base_pose)) {
ROS_INFO_STREAM("Ignoring default waypoints because there is a trajectory defined by parameters");
} else if (defaultTrajectory == "circle") {
ROS_INFO_STREAM("loading default trajectory 'circle'");
geometry_msgs::Point origin;
origin.z = .3;
loadCircularTrajectory(PlannerState::raw_trajectory, 0.6, origin, 20);
} else if (defaultTrajectory == "square") {
ROS_INFO_STREAM("loading default trajectory 'square'");
// choose a default trajectory
base_pose.position.x = 0.4;
base_pose.position.y = 0.4;
base_pose.position.z = 0.3;
geometry_msgs::Pose target_pose_1 = base_pose;
geometry_msgs::Pose target_pose_2 = base_pose;
geometry_msgs::Pose target_pose_3 = base_pose;
geometry_msgs::Pose target_pose_4 = base_pose;
PlannerState::raw_trajectory.push_back(target_pose_1);
target_pose_2.position.y *= -1;
PlannerState::raw_trajectory.push_back(target_pose_2);
target_pose_3.position.x *= -1;
target_pose_3.position.y *= -1;
PlannerState::raw_trajectory.push_back(target_pose_3);
target_pose_4.position.x *= -1;
PlannerState::raw_trajectory.push_back(target_pose_4);
} else {
ROS_WARN_STREAM("not loading a default trajectory! provided trajectory: " + defaultTrajectory);
}
// add the ground plate
std::vector<moveit_msgs::CollisionObject> collision_objects;
constructPlate(collision_objects, 5.0, 5.0);
planning_scene_interface.applyCollisionObjects(collision_objects);
// execute the trajectory which consists of single waypoints to allow distinct planning
doMotion(node_handle, group);
while (ros::ok()) {
// execute a trajectory
doMotion(node_handle, group);
ros::spinOnce();
}
ros::shutdown();
return 0;
}
src/util/TrajectoryUtil.cpp
+ 3
− 26
View file @ a7ebe96d
......@@ -61,6 +61,7 @@ bool TrajectoryUtil::computePathToPose(moveit::planning_interface::MoveGroupInte
} else if (pathType == FLUID_PATH) {
if (group.plan(plan) != moveit::planning_interface::MoveItErrorCode::SUCCESS) {
ROS_ERROR("Computation of fluid path has failed.");
ROS_ERROR_STREAM("Path was motion to:\n" << targetPose);
}
} else {
ROS_ERROR("Invalid type of path provided.");
......@@ -68,35 +69,11 @@ bool TrajectoryUtil::computePathToPose(moveit::planning_interface::MoveGroupInte
}
manipulateVelocity(plan, motionSpeedFactor);
}
bool TrajectoryUtil::initWaypoints(std::vector<geometry_msgs::Pose> &waypoints, ros::NodeHandle &node_handle,
geometry_msgs::Pose &initialPose) {
std::vector<double> x_values;
std::vector<double> y_values;
std::vector<double> z_values;
// check if no trajectory is configured
if (!node_handle.getParam("trajectory_pos_x", x_values) ||
!node_handle.getParam("trajectory_pos_y", y_values) ||
!node_handle.getParam("trajectory_pos_z", z_values)) {
return false;
} else {
for (int i = 0; i < x_values.size(); i++) {
geometry_msgs::Pose pose = initialPose;
pose.position.x = x_values.at(i);
pose.position.y = y_values.at(i);
pose.position.z = z_values.at(i);
waypoints.push_back(pose);
}
node_handle.setParam("new_trajectory_available", false);
return true;
}
return true;
}
void TrajectoryUtil::manipulateVelocity(moveit::planning_interface::MoveGroupInterface::Plan &plan, double velocity) {
ROS_INFO_STREAM("slowing down plan with " << plan.trajectory_.joint_trajectory.points.size() << "joints");
ROS_INFO_STREAM("slowing down plan with " << plan.trajectory_.joint_trajectory.points.size() << " joints");
for (auto &point : plan.trajectory_.joint_trajectory.points) {
point.time_from_start *= 1 / velocity;
for (double &v : point.velocities) {
......
src/util/TrajectoryUtil.h
+ 1
− 4
View file @ a7ebe96d
......@@ -27,12 +27,9 @@ public:
const std::string &pathType, double maxVelocityFactor,
double maxAccelerationFactor, double motionSpeedFactor);
bool initWaypoints(std::vector<geometry_msgs::Pose> &waypoints, ros::NodeHandle &node_handle,
geometry_msgs::Pose &initialPose);
private:
void manipulateVelocity(moveit::planning_interface::MoveGroupInterface::Plan &plan, double velocity);
static void manipulateVelocity(moveit::planning_interface::MoveGroupInterface::Plan &plan, double velocity);
};
#endif //PANDA_SIMULATION_TRAJECTORYUTIL_H
\ No newline at end of file
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment