Skip to content

Commit

Permalink
running trajectory sims
Browse files Browse the repository at this point in the history
  • Loading branch information
Trexter committed May 26, 2017
1 parent 8b98ff6 commit 3fc3ae8
Show file tree
Hide file tree
Showing 8 changed files with 49 additions and 25 deletions.
2 changes: 2 additions & 0 deletions include/pauvsi_trajectory/Config.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,5 +38,7 @@

#define MAX_GEOM_REFINE_ITER 5

#define PUBLISH_PATH true


#endif /* PAUVSI_TRAJECTORY_INCLUDE_PAUVSI_TRAJECTORY_CONFIG_H_ */
5 changes: 4 additions & 1 deletion include/pauvsi_trajectory/Simulation.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@
#define PAUVSI_TRAJECTORY_INCLUDE_PAUVSI_TRAJECTORY_SIMULATION_H_

#define MOTOR_FORCE SIGMA 0.2
#define POSITION_SIGMA 0.01
#define POSITION_SIGMA 0.02
#define OMEGA_SIGMA 0.0005
#define VEL_SIGMA 0.01
#define QUAT_SIGMA 0.001
Expand All @@ -25,6 +25,9 @@

#define START_POS -9, -9, 0.5

#define MOTOR_ABS_MAX 25
#define MOTOR_ABS_MIN 0.1



#endif /* PAUVSI_TRAJECTORY_INCLUDE_PAUVSI_TRAJECTORY_SIMULATION_H_ */
8 changes: 4 additions & 4 deletions include/pauvsi_trajectory/TrajectoryGenerator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -354,21 +354,21 @@ bool TrajectoryGenerator::testSegmentForGeometricFeasibility(TrajectorySegment s

for(auto e : geoConstraints)
{
if(e.type == e.PLANE_MIN)
if(e.type == e.Z_PLANE_MIN)
{
if(z < e.z_min)
{
failureTime = t;
ROS_DEBUG_STREAM("-failed at " << t);
ROS_DEBUG_STREAM("-failed at " << t << " z min: " << e.z_min);
return false;
}
}
else if(e.type == e.PLANE_MAX)
else if(e.type == e.Z_PLANE_MAX)
{
if(z > e.z_max)
{
failureTime = t;
ROS_DEBUG_STREAM("+failed at " << t);
ROS_DEBUG_STREAM("+failed at " << t << " z min: " << e.z_max);
return false;
}
}
Expand Down
12 changes: 6 additions & 6 deletions include/pauvsi_trajectory/Types.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,8 +54,8 @@ struct Point{

struct GeometricConstraint{
enum Type {
PLANE_MIN,
PLANE_MAX
Z_PLANE_MIN,
Z_PLANE_MAX
};

Type type;
Expand All @@ -71,11 +71,11 @@ struct GeometricConstraint{
type = _type;
switch(type)
{
case PLANE_MIN:
val = z_min;
case Z_PLANE_MIN:
z_min = val;
break;
case PLANE_MAX:
val = z_max;
case Z_PLANE_MAX:
z_max = val;
break;
}
}
Expand Down
36 changes: 28 additions & 8 deletions src/trajectory_server.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,33 +43,43 @@ bool generateTrajectory(pauvsi_trajectory::trajectoryGeneration::Request &req, p
TrajectoryGenerator tg;
DynamicTrajectoryConstraints dc;

ROS_DEBUG_STREAM("request: " << req);

dc.start.pos = Point(req.startPosition.x, req.startPosition.y, req.startPosition.z);
dc.start.vel = Point(req.startVelocity.x, req.startVelocity.y, req.startVelocity.z);
dc.start.accel = Point(req.startAcceleration.x, req.startAcceleration.y, req.startAcceleration.z);
dc.start.jerk = Point(req.startJerk.x, req.startJerk.y, req.startJerk.z);
dc.start.snap = Point(req.startSnap.x, req.startSnap.y, req.startSnap.z);

dc.start.geoConstraint.push_back(GeometricConstraint(GeometricConstraint::PLANE_MAX, req.startMaxZ));
dc.start.geoConstraint.push_back(GeometricConstraint(GeometricConstraint::PLANE_MIN, req.startMinZ));
dc.start.geoConstraint.push_back(GeometricConstraint(GeometricConstraint::Z_PLANE_MIN, req.startMinZ));
ROS_DEBUG_STREAM("start const 1 " << dc.start.geoConstraint.back().z_min);
dc.start.geoConstraint.push_back(GeometricConstraint(GeometricConstraint::Z_PLANE_MAX, req.startMaxZ));
ROS_DEBUG_STREAM("start const 1 " << dc.start.geoConstraint.back().z_max);

dc.end.pos = Point(req.goalPosition.x, req.goalPosition.y, req.goalPosition.z);
dc.end.vel = Point(req.goalVelocity.x, req.goalVelocity.y, req.goalVelocity.z);
dc.end.accel = Point(req.goalAcceleration.x, req.goalAcceleration.y, req.goalAcceleration.z);
dc.end.jerk = Point(req.goalJerk.x, req.goalJerk.y, req.goalJerk.z);
dc.end.snap = Point(req.goalSnap.x, req.goalSnap.y, req.goalSnap.z);

for(int i = 0; i < (int)req.middle_size; i++)
for(int i = 0; i < (int)req.middle.size(); i++)
{
dc.middle.push_back(BasicWaypointConstraint(Point(req.middle[i].x, req.middle[i].y, req.middle[i].z), 0));
dc.middle[i].geoConstraint.push_back(GeometricConstraint(GeometricConstraint(GeometricConstraint::PLANE_MIN, req.middleGeometricConstraints[(2*i + 0)])));
dc.middle[i].geoConstraint.push_back(GeometricConstraint(GeometricConstraint(GeometricConstraint::PLANE_MAX, req.middleGeometricConstraints[(2*i + 1)])));
dc.middle[i].geoConstraint.push_back(GeometricConstraint(GeometricConstraint(GeometricConstraint::Z_PLANE_MIN, req.middleGeometricConstraints[(2*i + 0)])));
ROS_DEBUG_STREAM("middle " << i << " min: " << dc.middle.at(i).geoConstraint.back().z_min);
dc.middle[i].geoConstraint.push_back(GeometricConstraint(GeometricConstraint(GeometricConstraint::Z_PLANE_MAX, req.middleGeometricConstraints[(2*i + 1)])));
ROS_DEBUG_STREAM(" max " << dc.middle.at(i).geoConstraint.back().z_max);
}

//compute trajectory
TrajectorySegment seg = tg.computeHighOrderMinimumTimeTrajectory(dc, phys);

res.tf = seg.tf;

res.trajectory.layout.dim.push_back(std_msgs::MultiArrayDimension());
res.trajectory.layout.dim.push_back(std_msgs::MultiArrayDimension());
res.trajectory.layout.dim.push_back(std_msgs::MultiArrayDimension());

res.trajectory.layout.data_offset = 0;
res.trajectory.layout.dim[0].label = "number of polynomials";
res.trajectory.layout.dim[0].stride = 3 * seg.x.size();
Expand All @@ -85,15 +95,25 @@ bool generateTrajectory(pauvsi_trajectory::trajectoryGeneration::Request &req, p

for(int i = 0; i < (int)seg.x.size(); i++)
{
res.trajectory.data.at(i) = seg.x[i];
//res.trajectory.data.at(i) = seg.x[i];
res.trajectory.data.push_back(seg.x[i]);
}
for(int i = 0; i < (int)seg.y.size(); i++)
{
res.trajectory.data.at(seg.y.size()*1 + i) = seg.y[i];
//res.trajectory.data.at(seg.y.size()*1 + i) = seg.y[i];
res.trajectory.data.push_back(seg.y[i]);
}
for(int i = 0; i < (int)seg.z.size(); i++)
{
res.trajectory.data.at(seg.z.size()*2 + i) = seg.z[i];
//res.trajectory.data.at(seg.z.size()*2 + i) = seg.z[i];
res.trajectory.data.push_back(seg.z[i]);
}

if(PUBLISH_PATH)
{
nav_msgs::Path path = tg.generateTrajectorySegmentPath(seg);
path_pub.publish(path);
ROS_DEBUG_STREAM("published path");
}

return true;
Expand Down
4 changes: 2 additions & 2 deletions src/virtual-flight-controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,8 +67,8 @@ int main(int argc, char **argv)
J << J_MATRIX;
torque_transition << TORQUE_TRANSITION;
mass = MASS;
min_motor_force = MOTOR_FORCE_MIN;
max_motor_force = MOTOR_FORCE_MAX;
min_motor_force = MOTOR_ABS_MIN;
max_motor_force = MOTOR_ABS_MAX;

rng.seed(RANDOM_SEED);

Expand Down
1 change: 0 additions & 1 deletion srv/trajectoryGeneration.srv
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,6 @@ float64 startMaxZ
# x2, y2, z2;
# ...
geometry_msgs/Vector3[] middle
int32 middle_size

# minZ1, maxZ1;
# minZ2, maxZ2;
Expand Down
6 changes: 3 additions & 3 deletions test/unit_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -169,11 +169,11 @@ int main(int argc, char **argv)


GeometricConstraint floor, ceil, obs;
floor.type = GeometricConstraint::PLANE_MIN;
floor.type = GeometricConstraint::Z_PLANE_MIN;
floor.z_min = 0;
obs.type = GeometricConstraint::PLANE_MIN;
obs.type = GeometricConstraint::Z_PLANE_MIN;
obs.z_min = 2.25;
ceil.type = GeometricConstraint::PLANE_MAX;
ceil.type = GeometricConstraint::Z_PLANE_MAX;
ceil.z_max = 3;

dc.start.t = 0;
Expand Down

0 comments on commit 3fc3ae8

Please sign in to comment.