# Steer to a pose for a differential drive robot

I am working on a diff-drive robot that needs to go and dock into a docking station to charge when the battery is low. The charging socket is on the back of the robot, like a vacuum cleaner, so this movement will have to be done in reverse, much like parking a car. I have a diagram that might help you understand the situation better:

From a ROS perspective, all related perception will be in the base_link frame, so that my robot will effectively be at $$(0, 0, 0)$$ at all times, and the docking station will be detected in this frame. Let's assume for now that the docking station is at $$(x_g, y_g, \theta_g)$$ as shown in the figure. In the ROS implementation, I'd be working with geometry_msgs/PoseStamped messages.

I need to be able to write geometry_msgs/Twist messages to the cmd_vel topic in order to make the robot move. Because this is a diff-drive robot, I know that I can only control linear.x and angular.z fields of the particular Twist message.

I would like to know if the control law expressed below is valid or not:

/**
* pose -> The desired pose (geometry_msgs/PoseStamped)
* command -> the resulting joint commands (geometry_msgs/Twist)
*/

double lin_vel_gain = 0.3;
double ang_vel_gain = 0.1;

/* Find the linear error */
double linear_error = std::hypot(pose.position.x, pose.position.y);

/* Find the angular error */
double angular_error = std::atan2(pose.position.y, pose.position.x);

command.linear.x = lin_vel_gain * linear_error;
command.angular.z = angular_vel_gain * angular_error;



I would also like to know how I can make this better, I was thinking of designing it according to a more classical path-planner, like this:

How would I go about obtaining these intermediate poses?

Any help, including literature and code samples are greatly appreciated.

Thanks!