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

initial test to reduce speed properly

parent bd6408ed
Branches
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
config/config.yaml
+ 3
− 1
View file @ c511bf1f
panda_mqtt_connector:
server: "tcp://localhost:1883"
robot_speed_factor: .15
robot_planning_mode: "fluid_path" # "fluid_path" or "cartesian_path"
topics:
robotConfig: "robotconfig"
dataConfig: "dataconfig"
zone_size: 0.5
zones:
- "1 1"
- "-1 -1 1"
- "0 1"
parts:
panda:
Link0: "panda::panda_link0"
......
src/TimedPlanner.cpp
+ 3
− 4
View file @ c511bf1f
......@@ -24,7 +24,6 @@ namespace PlannerState {
const double default_velocity = 1.0;
const std::string default_planning_mode = TrajectoryUtil::FLUID_PATH;
bool isInitialized = false;
bool isLooping = true;
}
......@@ -176,7 +175,7 @@ void doMotion(const ros::NodeHandle &node_handle, moveit::planning_interface::Mo
}
// wake up if we have trajectory
while (PlannerState::raw_trajectory.size() == 0) {
while (PlannerState::raw_trajectory.empty()) {
if (newTrajectoryAvailable) {
updateRawTrajectory(node_handle, &group);
i = 0;
......@@ -185,7 +184,7 @@ void doMotion(const ros::NodeHandle &node_handle, moveit::planning_interface::Mo
}
double velocity = 0.0;
std::string planning_mode = "";
std::string planning_mode;
if (!node_handle.getParam("robot_speed_factor", velocity)) {
velocity = PlannerState::default_velocity;
......@@ -198,7 +197,7 @@ 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, velocity);
PlannerState::traj_util.computePathToPose(group, plan, PlannerState::raw_trajectory.at(i), planning_mode, 0.8, 0.8, velocity);
group.execute(plan);
// make sure the robot moves in an infinite circle
......
src/util/TrajectoryUtil.cpp
+ 80
− 94
View file @ c511bf1f
......@@ -8,42 +8,70 @@ const std::string TrajectoryUtil::CARTESIAN_PATH = "cartesian_path";
const std::string TrajectoryUtil::FLUID_PATH = "fluid_path";
bool TrajectoryUtil::computePathToPose(moveit::planning_interface::MoveGroupInterface &group,
moveit::planning_interface::MoveGroupInterface::Plan &plan, geometry_msgs::Pose targetPose,
std::string pathType, double velocity)
{
if(velocity < 0.1 || velocity > 1.0){
ROS_ERROR("Invalid velocity configured. ");
moveit::planning_interface::MoveGroupInterface::Plan &plan,
geometry_msgs::Pose targetPose,
const std::string& pathType, double maxVelocityFactor,
double maxAccelerationFactor, double motionSpeedFactor) {
if (maxVelocityFactor <= 0 || maxVelocityFactor > 1.0) {
ROS_ERROR_STREAM("Invalid maximum velocity scaling factor " << maxVelocityFactor << ". Must be in (0,1].");
return false;
}
if (pathType.compare(CARTESIAN_PATH) == 0)
{
if(!computeCartesianPath(group, plan, targetPose))
{
if (maxAccelerationFactor <= 0 || maxAccelerationFactor > 1.0) {
ROS_ERROR_STREAM("Invalid maximum acceleration scaling factor " << maxAccelerationFactor << ". Must be in (0,1].");
return false;
}else{
manipulateVelocity(velocity, group, plan, CARTESIAN_PATH);
return true;
}
if (motionSpeedFactor <= 0 || motionSpeedFactor > 1.0) {
ROS_ERROR_STREAM("Invalid motion speed factor " << motionSpeedFactor << ". Must be in (0,1].");
return false;
}
if(pathType.compare(FLUID_PATH) == 0)
{
group.setStartStateToCurrentState();
group.setPlanningTime(10.0);
group.setPoseTarget(targetPose);
manipulateVelocity(velocity, group, plan, FLUID_PATH);
return (group.plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
group.setMaxVelocityScalingFactor(maxVelocityFactor);
group.setMaxAccelerationScalingFactor(maxAccelerationFactor);
if (pathType == CARTESIAN_PATH) {
std::vector<geometry_msgs::Pose> single_traj{targetPose};
moveit_msgs::RobotTrajectory trajectory_msg;
double fraction = group.computeCartesianPath(single_traj, 0.01, 0.0, trajectory_msg, true);
ROS_INFO("Planned cartesian plan (%.2f%% achieved)", fraction * 100.0);
robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), "panda_arm");
rt.setRobotTrajectoryMsg(*group.getCurrentState(), trajectory_msg);
trajectory_processing::IterativeParabolicTimeParameterization iptp{};
ROS_ERROR_STREAM("Before time parameterization, the trajectory takes " << rt.getDuration() << "s");
if (iptp.computeTimeStamps(rt)) {
ROS_ERROR_STREAM("After first time parameterization, the trajectory takes " << rt.getDuration() << "s");
}
ROS_ERROR("Invalid type of path provided.");
if (iptp.computeTimeStamps(rt, maxVelocityFactor)) {
ROS_INFO("Computed time stamps.");
rt.getRobotTrajectoryMsg(trajectory_msg);
plan.trajectory_ = trajectory_msg;
ROS_ERROR_STREAM("After second time parameterization, the trajectory takes " << rt.getDuration() << "s");
} else {
ROS_ERROR("Computation of cartesian path has failed.");
return false;
}
} else if (pathType == FLUID_PATH) {
if (group.plan(plan) != moveit::planning_interface::MoveItErrorCode::SUCCESS) {
ROS_ERROR("Computation of fluid path has failed.");
}
} else {
ROS_ERROR("Invalid type of path provided.");
return false;
}
manipulateVelocity(plan, motionSpeedFactor);
}
bool TrajectoryUtil::initWaypoints(std::vector<geometry_msgs::Pose> &waypoints, ros::NodeHandle &node_handle, geometry_msgs::Pose &initialPose)
{
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;
......@@ -51,12 +79,10 @@ bool TrajectoryUtil::initWaypoints(std::vector<geometry_msgs::Pose> &waypoints,
// 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) )
{
!node_handle.getParam("trajectory_pos_z", z_values)) {
return false;
} else {
for(int i = 0; i < x_values.size(); i++)
{
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);
......@@ -69,57 +95,17 @@ bool TrajectoryUtil::initWaypoints(std::vector<geometry_msgs::Pose> &waypoints,
}
}
bool TrajectoryUtil::computeCartesianPath(moveit::planning_interface::MoveGroupInterface &group,
moveit::planning_interface::MoveGroupInterface::Plan &plan,
const geometry_msgs::Pose &targetPose) {
std::vector<geometry_msgs::Pose> single_traj{targetPose};
group.setStartStateToCurrentState();
group.setPlanningTime(10.0);
moveit_msgs::RobotTrajectory trajectory_msg;
double fraction = group.computeCartesianPath(single_traj, 0.01, 0.0, trajectory_msg, false);
robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), "panda_arm");
rt.setRobotTrajectoryMsg(*group.getCurrentState(), trajectory_msg);
trajectory_processing::IterativeParabolicTimeParameterization iptp;
bool success = iptp.computeTimeStamps(rt);
ROS_INFO("Computed time stamp %s", success ? "SUCCEDED" : "FAILED");
if(!success){
return false;
}
rt.getRobotTrajectoryMsg(trajectory_msg);
ROS_INFO("Planned cartesian plan (%.2f%% acheived)", fraction * 100.0);
plan.trajectory_ = trajectory_msg;
return true;
}
void TrajectoryUtil::manipulateVelocity(double velocity, moveit::planning_interface::MoveGroupInterface &group,
moveit::planning_interface::MoveGroupInterface::Plan &plan, std::string pathType)
{
if(pathType.compare(FLUID_PATH))
{
group.setMaxVelocityScalingFactor(velocity);
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");
for (auto &point : plan.trajectory_.joint_trajectory.points) {
point.time_from_start *= 1 / velocity;
for (double &v : point.velocities) {
v *= velocity;
}
if(pathType.compare(CARTESIAN_PATH))
{
for (int i = 0; i < plan.trajectory_.joint_trajectory.points.size(); i++) {
plan.trajectory_.joint_trajectory.points[i].time_from_start.operator*=((1 / velocity));
for (int j = 0; j < plan.trajectory_.joint_trajectory.points.at(i).velocities.size(); j++) {
if (plan.trajectory_.joint_trajectory.points.at(i).velocities.at(j) != 0.0) {
plan.trajectory_.joint_trajectory.points.at(i).velocities.at(j) *= velocity;
for (double &acceleration : point.accelerations) {
acceleration = sqrt(std::abs(acceleration));
}
}
for (int j = 0; j < plan.trajectory_.joint_trajectory.points.at(i).accelerations.size(); j++) {
plan.trajectory_.joint_trajectory.points.at(i).accelerations.at(j) =
sqrt(std::abs(plan.trajectory_.joint_trajectory.points.at(i).accelerations.at(j)));
}
}
}
}
src/util/TrajectoryUtil.h
+ 10
− 12
View file @ c511bf1f
......@@ -22,19 +22,17 @@ public:
static const std::string FLUID_PATH;
bool computePathToPose(moveit::planning_interface::MoveGroupInterface &group,
moveit::planning_interface::MoveGroupInterface::Plan &plan, geometry_msgs::Pose targetPose,
std::string pathType, double velocity);
moveit::planning_interface::MoveGroupInterface::Plan &plan,
geometry_msgs::Pose targetPose,
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);
bool initWaypoints(std::vector<geometry_msgs::Pose> &waypoints, ros::NodeHandle &node_handle,
geometry_msgs::Pose &initialPose);
private:
bool computeCartesianPath(moveit::planning_interface::MoveGroupInterface &group,
moveit::planning_interface::MoveGroupInterface::Plan &plan,
const geometry_msgs::Pose &targetPose);
void manipulateVelocity(double velocity, moveit::planning_interface::MoveGroupInterface &group,
moveit::planning_interface::MoveGroupInterface::Plan &plan, std::string pathType);
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