search

sikang / mpl_ros

A ROS wrapper for trajectory planning based on motion primitives
Apache License 2.0
558 stars 148 forks source link

Unable to use mpl_ros to generate trajectory for 2D #8

Open Kaustav6422 opened 5 years ago

Kaustav6422 commented 5 years ago

Hello Sikang, I seem to be facing the following issue while using mpl_ros for generating trajectory using OccMapPlanner. I have generated a occupancy gridmap using SLAM and fed it into the OccMapPlanner. The planner doesn't recognize the obstacles/unknown space and generates a trajectory through the obstacles as shown below.

selection_001

sikang commented 5 years ago

can you show an example of how you import the map into the planner? It seems like the obstacle is not represented correctly somehow...

Sent from my iPhone

On Aug 9, 2018, at 12:32 AM, Kaustav Mondal notifications@github.com wrote:

Hello Sikang, I seem to be facing the following issue while using mpl_ros for generating trajectory using OccMapPlanner. I have generated a occupancy gridmap using SLAM and fed it into the OccMapPlanner. The planner doesn't recognize the obstacles/unknown space and generates a trajectory through the obstacles as shown below.

— You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub, or mute the thread.

Kaustav6422 commented 5 years ago

I fixed the issue. The data in planning_ros_msgs::VoxelMap is int8[] which is like a std::vector/int. It works now. But I am curious why the lattices expand to the unknown cells. I have to change the value of the unknown cells to 100. 38821349_215692285794834_5394535282336858112_n

sikang commented 5 years ago

The unknown cell is treated as free by default in the map_util, but here the problem is actually caused by the other reason: the occupied cell is not inflated such that the path/traj that goes diagonally can potentially 'penetrate' thin walls. The attached figures illustrate this problem.

screenshot from 2018-08-10 05-20-39 screenshot from 2018-08-10 05-21-25

On Fri, Aug 10, 2018 at 2:27 AM, Kaustav Mondal notifications@github.com wrote:

I fixed the issue. The data in planning_ros_msgs::VoxelMap is int8[] which is like a std::vector. It works now. But I am curious why the lattices expand to the unknown cells. I have to change the value of the unknown cells to 100. [image: 38821349_215692285794834_5394535282336858112_n] https://user-images.githubusercontent.com/28791424/43942154-b7cb7744-9c2b-11e8-836c-9e6fa6b3e92c.png

— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/sikang/mpl_ros/issues/8#issuecomment-411988157, or mute the thread https://github.com/notifications/unsubscribe-auth/ABccl53yCjZ_oI25W7r4NejkjU6HrmCKks5uPSfHgaJpZM4V1Bv_ .

Kaustav6422 commented 5 years ago

I see the problem there. Thank you. I had another question related to the formulation of the optimization problem. When you include the constraint xdot = Ax + Bu in the optimization, you are not really considering the dynamics of the quadrotor. Why is that ? I see that you are using the differential flatness property of the quadrotor. But my question is still not answered. Why do we not include the dynamics of the quadrotor in the optimization problem ? I am obviously missing something.

sikang commented 5 years ago

Not sure what "dynamics" do you mean in the question, do you refer to the force and torque? The desired force and torque and full robot state can be expressed in terms of those flat outputs, thus people usually optimize the trajectory with flat outputs which is much easier to formulate and solve. In addition, it has been shown that the trajectory formed using flat outputs can be used for precise control for the real robot. Hence, we use the constraint for flat output instead of system's original non-linear state. The dynamics constraints like maximum thrust and torque are also replace by maximum velocity, acceleration and etc.

Kaustav6422 commented 5 years ago

I meant the nonlinear equations of the quadrotor. But I think what you are saying is that we use feedback linearization to convert a nonlinear system into a linearized system of chained integrators. Is this what you mean ?

sikang commented 5 years ago

You are right in a sense. But it is not `linearized', the flat outputs as [x, y, z] and their derivatives can be converted into the full system state which is non-linear. Instead of using dynamics constraints for the original 12 dimension including position and orientation, we choose to apply constraints for the flat output which is much easier to compute.

Kaustav6422 commented 5 years ago

You are right. The system isn't 'linearized' in the traditional sense.