mpc_node.cpp

Go to the documentation of this file.

#include <ros/ros.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <tf/tfMessage.h>
#include <tf/transform_broadcaster.h>

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/QuaternionStamped.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

#include <pr2_controllers_msgs/Pr2GripperCommandAction.h>
#include <actionlib/client/simple_action_client.h>

#include <motld/TrackedObjects.h>

#include <mpc/types.h>
#include <mpc/mpc.h>
#include <mpc/finite_differences.h>
#include <reactive_grasping_pr2/pr2_arm_ik.h>

#ifndef SQR
#define SQR(X) ((X)*(X))
#endif

using namespace MPC;

typedef actionlib::SimpleActionClient<pr2_controllers_msgs::Pr2GripperCommandAction> GripperClient;

class ArmMPC {

private:
        bool initialized_;
        VectorT center_;
        tf::TransformBroadcaster *tf_broadcaster_;
        tf::TransformListener  *tf_transformer_;

        ros::Subscriber obj_sub_;
        
        ros::Publisher gripper_pose_pub_;
        ros::Publisher marker_pub_;
        int curr_t_;

        float eps_;
        float max_mov_dist_;

        bool on_way_to_goal_;
        bool last_goal_set_;
        bool last_pos_set_;
        VectorT last_goal_;
        VectorT last_pos_;
        pr2_arm_ik::Obstacles *obs_;

        int n, m, N;
        bool restart_ddp_;
        DDPParams params;
        Trajectory curr_traj_;
        MatrixT u0_;
        geometry_msgs::PoseStamped gripper_pose_;
        GripperClient *close_gripper_;

        ros::NodeHandle nh_;
        ros::NodeHandle private_nh_;

        // names of parameters to read
        std::string root_frame_;
        std::string gripper_frame_;
        std::string object_topic_;
        std::string gripper_action_topic_;
        std::string gripper_close_topic_;
        std::string marker_topic_;

public:
        ArmMPC(ros::NodeHandle &nh) : nh_(nh), private_nh_(ros::NodeHandle("~")), initialized_(false) {
                // read parameters
                private_nh_.param("root_frame", root_frame_, std::string("/torso_lift_link"));
                private_nh_.param("gripper_frame", gripper_frame_, std::string("/r_gripper_tool_frame"));
                private_nh_.param("object_topic", object_topic_, std::string("/objects"));
                private_nh_.param("gripper_action_topic", gripper_action_topic_, std::string("/r_cart/command_pose"));
                private_nh_.param("gripper_close_topic", gripper_close_topic_, std::string("/r_gripper_controller/gripper_action"));
                private_nh_.param("marker_topic", marker_topic_, std::string("MPC_markers"));
                     
                // setup tf transformer and broadcaster
                tf_broadcaster_ = new tf::TransformBroadcaster();
                tf_transformer_ = new tf::TransformListener();

                // setup mpc callback
                obj_sub_ = nh_.subscribe(object_topic_, 1, &ArmMPC::mpc_callback, this);

                // setup pose client
                gripper_pose_pub_ = nh_.advertise<geometry_msgs::PoseStamped>(gripper_action_topic_, 1);
                // setup gripper client
                close_gripper_ = new GripperClient(gripper_close_topic_, true);
                while(!close_gripper_->waitForServer(ros::Duration(5.0))){
                        ROS_INFO("Waiting for the gripper action server to come up");
                }                     

                // setup visualization
                marker_pub_ = private_nh_.advertise<visualization_msgs::MarkerArray>(marker_topic_, 1); 

                // setup DDP and dynamics
                n = 3;
                m = 3;
                N = 20;
                u0_.setZero(m, N);
                VectorT internal_goal(VectorT::Zero(n));
                // setup some constants
                eps_ = 0.02;
                max_mov_dist_ = 0.05;
                // plan full number of steps in the first callback
                restart_ddp_ = true;


                // use finite differences
                /*
                  euclidean::setGoal(internal_goal);
                  finite_differences::setBaseFunctions(&euclidean::dynamics, &euclidean::cost);
                  bool init_success = DDP_init(&finite_differences::dynamics, &finite_differences::dynamicsD, &finite_differences::cost, &finite_differences::costD);
                */

                // real euclidean robot dynamics
                pr2_arm_ik::setGoal(internal_goal);
                bool init_success = DDP_init(&pr2_arm_ik::dynamics, &pr2_arm_ik::dynamicsD, &pr2_arm_ik::cost, &pr2_arm_ik::costD);
                //*/
                // clear obstacles
                obs_ = &pr2_arm_ik::getObstacles();
                obs_->clear();

                // init temporaries
                center_.resize(3);
                last_goal_.resize(n);
                last_pos_.resize(3);
                on_way_to_goal_ = false;
                last_goal_set_ = false;
                last_pos_set_ = false;
                curr_t_ = 1;
                
                // mark as initialized
                initialized_ = init_success;
                ROS_INFO("Done initializing");
        }

        ~ArmMPC() {
                delete obj_sub_;
                delete close_gripper_;
        }

        void open_gripper(bool wait=false) {
                pr2_controllers_msgs::Pr2GripperCommandGoal open;
                open.command.position = 0.08;
                open.command.max_effort = -1.0;
    
                close_gripper_->sendGoal(open);
                if (wait) {
                        close_gripper_->waitForResult();
                        if(close_gripper_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
                                ROS_INFO("The gripper opened!");
                        else
                                ROS_INFO("The gripper failed to open.");
                }
        } 
        
        void close_gripper(bool wait=false) {
                pr2_controllers_msgs::Pr2GripperCommandGoal close;
                close.command.position = 0.0;
                close.command.max_effort = 20.0;
    
                close_gripper_->sendGoal(close);
                if (wait) {
                        close_gripper_->waitForResult();
                        if(close_gripper_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
                                ROS_INFO("The gripper closed!");
                        else
                                ROS_INFO("The gripper failed to close.");
                }
        } 

        void mpc_callback(const motld::TrackedObjectsConstPtr &objects) {
                ROS_INFO("mpc_callback called!");
                if (not initialized_) {
                        ROS_WARN("mpc not initialized");
                        return;
                }
                if (objects->name.size() < 1) {
                        ROS_WARN("no object poses received");
                        return;
                }
                if (objects->name.size() != objects->pose.poses.size()) {
                        ROS_WARN("object poses and object names array sizes do not match");
                        return;
                }
                
                if (on_way_to_goal_ && curr_t_ +1 >= curr_traj_.x.cols()) {
                        // TODO: there was a sleep here in the python implementation to prevent weird near goal behaviour 
                        // when the last action has not been completely executed
                }

                ros::Time goal_stamp = objects->pose.header.stamp;
                ros::Time time_stamp_now = ros::Time::now();
                // we always assume that the first object is the goal
                VectorT goal(3);
                goal(0) = objects->pose.poses[0].position.x;
                goal(1) = objects->pose.poses[0].position.y;
                goal(2) = objects->pose.poses[0].position.z;

                if (tf_transformer_->waitForTransform(objects->pose.header.frame_id, gripper_frame_, goal_stamp, ros::Duration(1.)) == false) {
                        ROS_ERROR("Did not find transform between %s and %s", objects->pose.header.frame_id.c_str(), gripper_frame_.c_str());
                        return;
                }
                ROS_INFO("Got transform!");

                tf::StampedTransform trans;
                tf_transformer_->lookupTransform(objects->pose.header.frame_id, gripper_frame_, goal_stamp, trans);

                // set new center
                center_(0) = trans.getOrigin().x();
                center_(1) = trans.getOrigin().y();
                center_(2) = trans.getOrigin().z();

                // internal coordinates
                VectorT x0(15);
                x0(0) = center_(0) - goal(0);
                x0(1) = center_(1) - goal(1);
                x0(2) = center_(2) - goal(2);

                // update parts
                tf::Vector3 tmp_vec = btVector3(-0.08, -0.1, 0.);

                tmp_vec = trans(tmp_vec);
                x0(3) = tmp_vec.getX() - goal(0);
                x0(4) = tmp_vec.getY() - goal(1);
                x0(5) = tmp_vec.getZ() - goal(2);

                tmp_vec.setX(-0.08);
                tmp_vec.setY(0.1);
                tmp_vec.setZ(0.);
                tmp_vec = trans(tmp_vec);
                x0(6) = tmp_vec.getX() - goal(0);
                x0(7) = tmp_vec.getY() - goal(1);
                x0(8) = tmp_vec.getZ() - goal(2);

                tmp_vec.setX(-0.1);
                tmp_vec.setY(0.);
                tmp_vec.setZ(0.);
                tmp_vec = trans(tmp_vec);
                x0(9) = tmp_vec.getX() - goal(0);
                x0(10) = tmp_vec.getY() - goal(1);
                x0(11) = tmp_vec.getZ() - goal(2);

                tmp_vec.setX(-0.2);
                tmp_vec.setY(0.);
                tmp_vec.setZ(0.);
                tmp_vec = trans(tmp_vec);
                x0(12) = tmp_vec.getX() - goal(0);
                x0(13) = tmp_vec.getY() - goal(1);
                x0(14) = tmp_vec.getZ() - goal(2);

                if (objects->name.size() > 1) {
                        // assume all other objects are obstacles
                        obs_->resize(objects->name.size() - 1);
                        (*obs_)[0].resize(3);
                        (*obs_)[0](0) = objects->pose.poses[1].position.x - goal(0);
                        (*obs_)[0](1) = objects->pose.poses[1].position.y - goal(1);
                        (*obs_)[0](2) = objects->pose.poses[1].position.z - goal(2);
                }

                float goal_diff = x0.head(3).norm();

                if (goal_diff < eps_ + 0.01) {
                        ROS_INFO("already at goal / goal reached");
                        close_gripper();
                        return;
                }
                // TODO: insert obstacles
                float goal_change = 100.;
                float pos_change = 100.;
                if (last_goal_set_) {
                        goal_change = (last_goal_ - goal).norm();
                }
                if (last_pos_set_) {
                        pos_change = (center_ - last_pos_).norm();
                }
                ROS_INFO("pchange: %f gchange: %f max_dist: %f", pos_change, goal_change, max_mov_dist_);
                if (pos_change > max_mov_dist_ || goal_change > max_mov_dist_) {
                        // reset policy, since we are too far from our last computed one
                        //actions_.setZero(actions_.rows(), actions_.cols());
                        on_way_to_goal_ = false;
                        restart_ddp_ = true;
                }

                /* check if we have a policy that gets us to the goal
                   if so and the goal did not change significantly execute it */
                int x_len = curr_traj_.x.cols();
                // TODO: check if we want to enable the on_way_to_goal_ behavior or not!
                if (false && on_way_to_goal_ && goal_change < eps_ && curr_t_ < x_len - 1) {
                        curr_t_++;
                } else {
                        // run ddp
                        if (restart_ddp_) {
                                ROS_INFO("restarting ddp");
                                u0_.setZero();
                                DDP(x0, u0_, params, 100, curr_traj_);
                                restart_ddp_ = false;
                        } else {
                                u0_ = curr_traj_.u;
                                DDP(x0, u0_, params, 10, curr_traj_);
                        }
                        curr_t_ = 1;
                }
                
                // fill gripper message
                // skip too small position changes (smaller than controller sensitivity)
                double dist = (curr_traj_.x.col(curr_t_).head(3) - x0.head(3)).norm();
                while (curr_t_ + 1 < curr_traj_.x.cols() && dist < 0.01) {
                    ++curr_t_;
                    dist = (curr_traj_.x.col(curr_t_).head(3) - x0.head(3)).norm();
                }
                ROS_INFO_STREAM("chosen dist = " << dist);
                const VectorT &next = curr_traj_.x.col(curr_t_);

                gripper_pose_.header.frame_id = objects->pose.header.frame_id;
                gripper_pose_.header.stamp = time_stamp_now;
                gripper_pose_.pose.position.x = next(0) + goal(0);
                gripper_pose_.pose.position.y = next(1) + goal(1);
                gripper_pose_.pose.position.z = next(2) + goal(2);

                // for now assume that we want to come in with a fixed
                // rotation on the z axis
                geometry_msgs::QuaternionStamped quat;
                quat.header.frame_id = root_frame_;
                quat.header.stamp = goal_stamp;
                // create fixed quaternion
                quat.quaternion = tf::createQuaternionMsgFromRollPitchYaw(0., 0., M_PI/2.);
                // transform into goal frame
                // TODO: make sure transformation is present
                tf_transformer_->transformQuaternion(objects->pose.header.frame_id, quat, quat);
                gripper_pose_.pose.orientation = quat.quaternion;
                
                // remember last goal
                last_goal_ = goal;
                last_goal_set_ = true;

                // remember last pos
                last_pos_ = center_;
                last_pos_set_ = true;

                // check if we found a goal fulfilling trajectory
                if (curr_traj_.x.col(curr_traj_.x.cols()-1).norm() < eps_) {
                        on_way_to_goal_ = true;
                } else {
                        on_way_to_goal_ = false;
                }
                
                // send action
                gripper_pose_pub_.publish(gripper_pose_);
                open_gripper();

                // TODO: send out marker array
                if (marker_pub_.getNumSubscribers() > 0) {
                        ROS_INFO("mpc sending markers");
                        visualization_msgs::MarkerArray markers;
                        int id = 0;
                        for (int p = 0; p < curr_traj_.x.cols(); ++p) {
                                visualization_msgs::Marker marker;
                                marker.header.frame_id = objects->pose.header.frame_id;
                                marker.header.stamp = time_stamp_now;
                                marker.type = marker.SPHERE;
                                marker.action = marker.ADD;
                                marker.id = id;
                                marker.scale.x = 0.02;
                                marker.scale.y = 0.02;
                                marker.scale.z = 0.02;
                                marker.color.a = 1.0;
                                marker.color.r = 1.0;
                                marker.color.g = 1.0;
                                marker.color.b = 0.0;
                                marker.pose.orientation.w = 1.0;
                                marker.pose.position.x = curr_traj_.x(0, p) + goal(0);
                                marker.pose.position.y = curr_traj_.x(1, p) + goal(1);
                                marker.pose.position.z = curr_traj_.x(2, p) + goal(2);
                                markers.markers.push_back(marker);
                                ++id;
                                // add parts
                                marker.color.g = 0.0;
                                for (int i = 3; i + 3 <= curr_traj_.x.rows(); i+=3) {
                                        marker.id = id;
                                        marker.pose.position.x = curr_traj_.x(i, p) + goal(0);
                                        marker.pose.position.y = curr_traj_.x(i+1, p) + goal(1);
                                        marker.pose.position.z = curr_traj_.x(i+2, p) + goal(2);
                                        markers.markers.push_back(marker);
                                        ++id;
                                }
                        }
                        marker_pub_.publish(markers);
                }
        }
};

int main(int argc, char *argv[]) {
        ros::init(argc, argv, "mpc_node");
        ros::NodeHandle n;
        ArmMPC mpc_client(n);
        while (ros::ok()) {
                ros::spinOnce();
        }
}