Index: include/arm_kinematics/arm_kinematics.h
===================================================================
--- include/arm_kinematics/arm_kinematics.h	(revision 0)
+++ include/arm_kinematics/arm_kinematics.h	(revision 0)
@@ -0,0 +1,101 @@
+#ifndef ARM_KINEMATICS_H
+#define ARM_KINEMATICS_H
+
+#include <cstring>
+#include <ros/ros.h>
+#include <tf/transform_listener.h>
+#include <tf_conversions/tf_kdl.h>
+#include <tf/transform_datatypes.h>
+#include <kdl_parser/kdl_parser.hpp>
+#include <kdl/jntarray.hpp>
+#include <kdl/chainiksolverpos_nr_jl.hpp>
+#include <kdl/chainiksolvervel_pinv.hpp>
+#include <kdl/chainfksolverpos_recursive.hpp>
+#include <kinematics_msgs/GetPositionFK.h>
+#include <kinematics_msgs/GetPositionIK.h>
+#include <kinematics_msgs/GetKinematicSolverInfo.h>
+#include <kinematics_msgs/KinematicSolverInfo.h>
+#include <Eigen/Array>
+
+namespace arm_kinematics
+{
+class Kinematics {
+    public:
+        Kinematics();
+        bool init();
+        bool isActive();
+
+    protected:
+        ros::NodeHandle node_handle_, root_handle_;
+        std::string root_name, tip_name;
+        KDL::JntArray joint_min, joint_max;
+        KDL::Chain chain;
+        unsigned int num_joints;    
+        int maxIterations;
+
+        KDL::ChainFkSolverPos_recursive* fk_solver;
+        KDL::ChainIkSolverPos_NR_JL *ik_solver_pos;
+        KDL::ChainIkSolverVel_pinv* ik_solver_vel;
+
+        kinematics_msgs::KinematicSolverInfo ik_solver_info_, fk_solver_info_;
+        ros::ServiceServer ik_service,ik_solver_info_service;
+        ros::ServiceServer fk_service,fk_solver_info_service;
+
+        tf::TransformListener tf_listener;
+
+        bool active_;
+
+        kinematics_msgs::KinematicSolverInfo info;
+
+        bool loadModel(const std::string xml);
+        bool readJoints(urdf::Model &robot_model);
+        int getJointIndex(const std::string &name);
+        int getKDLSegmentIndex(const std::string &name);
+
+        /**
+         * @brief This is the basic IK service method that will compute and return an IK solution.
+         * @param A request message. See service definition for GetPositionIK for more information on this message.
+         * @param The response message. See service definition for GetPositionIK for more information on this message.
+         */
+        bool getPositionIK(kinematics_msgs::GetPositionIK::Request &request,
+                           kinematics_msgs::GetPositionIK::Response &response);
+
+        /**
+         * @brief This is the basic kinematics info service that will return information about the kinematics node.
+         * @param A request message. See service definition for GetKinematicSolverInfo for more information on this message.
+         * @param The response message. See service definition for GetKinematicSolverInfo for more information on this message.
+         */
+        bool getIKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request,
+                             kinematics_msgs::GetKinematicSolverInfo::Response &response);
+
+        /**
+         * @brief This is the basic kinematics info service that will return information about the kinematics node.
+         * @param A request message. See service definition for GetKinematicSolverInfo for more information on this message.
+         * @param The response message. See service definition for GetKinematicSolverInfo for more information on this message.
+         */
+        bool getFKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request,
+                             kinematics_msgs::GetKinematicSolverInfo::Response &response);
+
+        /**
+         * @brief This is the basic forward kinematics service that will return information about the kinematics node.
+         * @param A request message. See service definition for GetPositionFK for more information on this message.
+         * @param The response message. See service definition for GetPositionFK for more information on this message.
+         */
+        bool getPositionFK(kinematics_msgs::GetPositionFK::Request &request,
+                           kinematics_msgs::GetPositionFK::Response &response);
+
+        bool getCount(int &count, 
+                      const int &max_count, 
+                      const int &min_count);
+
+        Eigen::Matrix4f KDLToEigenMatrix(const KDL::Frame &p);
+        //consts
+        static const std::string IK_SERVICE;
+        static const std::string FK_SERVICE;
+        static const std::string IK_INFO_SERVICE;
+        static const std::string FK_INFO_SERVICE;
+
+};
+}
+
+#endif
Index: include/arm_kinematics_constraint_aware/arm_kinematics_constraint_aware.h
===================================================================
--- include/arm_kinematics_constraint_aware/arm_kinematics_constraint_aware.h	(revision 0)
+++ include/arm_kinematics_constraint_aware/arm_kinematics_constraint_aware.h	(revision 0)
@@ -0,0 +1,175 @@
+/*********************************************************************
+*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Sachin Chitta
+*********************************************************************/
+
+#ifndef ARM_IK_CONSTRAINT_AWARE_H
+#define ARM_IK_CONSTRAINT_AWARE_H
+
+#include <angles/angles.h>
+#include <arm_kinematics/arm_kinematics.h>
+
+//Pose command for the ik node
+#include <kinematics_msgs/GetConstraintAwarePositionIK.h>
+#include <kinematics_msgs/GetKinematicSolverInfo.h>
+#include <kinematics_msgs/GetPositionFK.h>
+
+#include <boost/shared_ptr.hpp>
+
+#include <planning_environment/monitors/planning_monitor.h>
+#include <planning_models/kinematic_model.h>
+
+#include <motion_planning_msgs/DisplayTrajectory.h>
+#include <motion_planning_msgs/LinkPadding.h>
+#include <motion_planning_msgs/ArmNavigationErrorCodes.h>
+
+#include <urdf/model.h>
+
+namespace arm_kinematics
+{
+class ArmIKConstraintAware : public arm_kinematics::Kinematics
+{
+public:
+
+  /** @class
+   *  @brief ROS/KDL based interface for the inverse kinematics of an arm
+   *  @author Sachin Chitta <sachinc@willowgarage.com>
+   *
+   *  This class provides a ROS/KDL interface to the inverse kinematics of an arm. 
+   *  It will compute a collision free solution to the inverse kinematics of an arm. 
+   *  The collision environment needs to be active for this method to work. This requires the presence of a node 
+   *  that is publishing collision maps. 
+   *  To use this node, you must have a roscore running with a robot description available from the ROS param server. 
+   */
+  ArmIKConstraintAware();
+
+  virtual ~ArmIKConstraintAware()
+	{
+    if (planning_monitor_)
+      delete planning_monitor_;
+    if (collision_models_)
+      delete collision_models_;
+	};
+
+  /**
+   * @brief This method searches for and returns the closest solution to the initial guess in the first set of solutions it finds. 
+   *
+   * @return < 0 if no solution is found
+   * @param q_in The initial guess for the inverse kinematics solution. The solver uses the joint value q_init(ik_->free_angle_) as 
+   * as an input to the inverse kinematics. ik_->free_angle_ can either be 0 or 2 corresponding to the shoulder pan or shoulder roll angle 
+   * @param p_in A KDL::Frame representation of the position of the end-effector for which the IK is being solved.
+   * @param q_out A std::vector of KDL::JntArray containing all found solutions.  
+   * @param timeout The amount of time (in seconds) to spend looking for a solution.
+   */  
+  int CartToJntSearch(const KDL::JntArray& q_in, 
+                      const KDL::Frame& p_in, 
+                      KDL::JntArray &q_out, 
+                      const double &timeout, 
+                      motion_planning_msgs::ArmNavigationErrorCodes& error_code);
+
+  /**
+   * @brief This method searches for and returns the first solution it finds that also satisifies both user defined callbacks.
+   *
+   * @return < 0 if no solution is found
+   * @param q_init The initial guess for the inverse kinematics solution. The solver uses the joint value q_init(pr2_ik_->free_angle_) as 
+   * @param p_in A KDL::Frame representation of the position of the end-effector for which the IK is being solved.
+   * @param q_out A std::vector of KDL::JntArray containing all found solutions.  
+   * @param desired_pose_callback A callback function to which the desired pose is passed in
+   * @param solution_callback A callback function to which IK solutions are passed in
+   */
+  int CartToJntSearch(const KDL::JntArray& q_in, 
+                      const KDL::Frame& p_in, 
+                      KDL::JntArray &q_out, 
+                      const double &timeout, 
+                      motion_planning_msgs::ArmNavigationErrorCodes &error_code,
+                      const boost::function<void(const KDL::JntArray&,const KDL::Frame&,motion_planning_msgs::ArmNavigationErrorCodes &)> &desired_pose_callback,
+                      const boost::function<void(const KDL::JntArray&,const KDL::Frame&,motion_planning_msgs::ArmNavigationErrorCodes &)> &solution_callback);
+
+  /**
+   * @brief This method searches for and returns the closest solution to the initial guess in the first set of solutions it finds. 
+   *
+   * @return < 0 if no solution is found
+   * @param q_in The initial guess for the inverse kinematics solution. The solver uses the joint value q_init(ik_->free_angle_) as 
+   * as an input to the inverse kinematics. ik_->free_angle_ can either be 0 or 2 corresponding to the shoulder pan or shoulder roll angle 
+   * @param p_in A KDL::Frame representation of the position of the end-effector for which the IK is being solved.
+   * @param q_out A std::vector of KDL::JntArray containing all found solutions.  
+   * @param timeout The amount of time (in seconds) to spend looking for a solution.
+   */
+  bool getConstraintAwarePositionIK(kinematics_msgs::GetConstraintAwarePositionIK::Request &request, 
+                                    kinematics_msgs::GetConstraintAwarePositionIK::Response &response);
+private:
+
+  ros::ServiceServer ik_collision_service_;
+  planning_environment::CollisionModels *collision_models_;
+  planning_environment::PlanningMonitor *planning_monitor_;
+  planning_models::KinematicState* kinematic_state_;
+  std::string group_;
+  bool use_collision_map_;
+  ros::Publisher vis_marker_publisher_;
+  ros::Publisher vis_marker_array_publisher_;
+  void contactFound(collision_space::EnvironmentModel::Contact &contact);
+  std::vector<std::string> default_collision_links_;
+  std::vector<std::string> end_effector_collision_links_;
+  std::vector<std::string> arm_links_;
+  void initialPoseCheck(const KDL::JntArray &jnt_array, 
+                        const KDL::Frame &p_in,
+                        motion_planning_msgs::ArmNavigationErrorCodes &error_code);
+  void collisionCheck(const KDL::JntArray &jnt_array, 
+                      const KDL::Frame &p_in,
+                      motion_planning_msgs::ArmNavigationErrorCodes &error_code);
+  void printStringVec(const std::string &prefix, const std::vector<std::string> &string_vector);
+  ros::Publisher display_trajectory_publisher_;
+  bool visualize_solution_;
+  kinematics_msgs::PositionIKRequest ik_request_;
+  motion_planning_msgs::OrderedCollisionOperations collision_operations_;
+  std::vector<motion_planning_msgs::LinkPadding> link_padding_;
+  std::vector<motion_planning_msgs::AllowedContactSpecification> allowed_contacts_;
+  motion_planning_msgs::Constraints constraints_;
+  bool setup_collision_environment_;
+  bool setupCollisionEnvironment(void);
+  void advertiseIK();
+
+  bool isReady(motion_planning_msgs::ArmNavigationErrorCodes &error_code);
+  void sendEndEffectorPose(const planning_models::KinematicState* state, bool valid);
+
+  //! A model of the robot to see which joints wrap around
+  urdf::Model robot_model_;
+  //! Flag that tells us if the robot model was initialized successfully
+  bool robot_model_initialized_;
+
+
+};
+}
+#endif
Index: manifest.xml
===================================================================
--- manifest.xml	(revision 36)
+++ manifest.xml	(working copy)
@@ -13,6 +13,10 @@
   <depend package="tf_conversions"/>
   <depend package="kdl_parser"/>
   <depend package="kinematics_msgs"/>
+  <depend package="planning_environment"/>
+  <depend package="pr2_arm_kinematics"/>
+  <depend package="collision_space"/>
+  <depend package="planning_models"/>
 
 </package>
 
Index: arm_kinematics.cpp
===================================================================
--- arm_kinematics.cpp	(revision 36)
+++ arm_kinematics.cpp	(working copy)
@@ -1,362 +0,0 @@
-// bsd license blah blah
-#include <cstring>
-#include <ros/ros.h>
-#include <tf/transform_listener.h>
-#include <tf_conversions/tf_kdl.h>
-#include <tf/transform_datatypes.h>
-#include <kdl_parser/kdl_parser.hpp>
-#include <kdl/jntarray.hpp>
-#include <kdl/chainiksolverpos_nr_jl.hpp>
-#include <kdl/chainiksolvervel_pinv.hpp>
-#include <kdl/chainfksolverpos_recursive.hpp>
-#include <kinematics_msgs/GetPositionFK.h>
-#include <kinematics_msgs/GetPositionIK.h>
-#include <kinematics_msgs/GetKinematicSolverInfo.h>
-#include <kinematics_msgs/KinematicSolverInfo.h>
-using std::string;
-
-static const std::string IK_SERVICE = "get_ik";
-static const std::string FK_SERVICE = "get_fk";
-static const std::string IK_INFO_SERVICE = "get_ik_solver_info";
-static const std::string FK_INFO_SERVICE = "get_fk_solver_info";
-
-class Kinematics {
-    public:
-        Kinematics();
-        bool init();
-
-    private:
-        ros::NodeHandle nh, nh_private;
-        std::string root_name, tip_name;
-        KDL::JntArray joint_min, joint_max;
-        KDL::Chain chain;
-        unsigned int num_joints;
-
-        KDL::ChainFkSolverPos_recursive* fk_solver;
-        KDL::ChainIkSolverPos_NR_JL *ik_solver_pos;
-        KDL::ChainIkSolverVel_pinv* ik_solver_vel;
-
-        ros::ServiceServer ik_service,ik_solver_info_service;
-        ros::ServiceServer fk_service,fk_solver_info_service;
-
-        tf::TransformListener tf_listener;
-
-        kinematics_msgs::KinematicSolverInfo info;
-
-        bool loadModel(const std::string xml);
-        bool readJoints(urdf::Model &robot_model);
-        int getJointIndex(const std::string &name);
-        int getKDLSegmentIndex(const std::string &name);
-
-        /**
-         * @brief This is the basic IK service method that will compute and return an IK solution.
-         * @param A request message. See service definition for GetPositionIK for more information on this message.
-         * @param The response message. See service definition for GetPositionIK for more information on this message.
-         */
-        bool getPositionIK(kinematics_msgs::GetPositionIK::Request &request,
-                           kinematics_msgs::GetPositionIK::Response &response);
-
-        /**
-         * @brief This is the basic kinematics info service that will return information about the kinematics node.
-         * @param A request message. See service definition for GetKinematicSolverInfo for more information on this message.
-         * @param The response message. See service definition for GetKinematicSolverInfo for more information on this message.
-         */
-        bool getIKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request,
-                             kinematics_msgs::GetKinematicSolverInfo::Response &response);
-
-        /**
-         * @brief This is the basic kinematics info service that will return information about the kinematics node.
-         * @param A request message. See service definition for GetKinematicSolverInfo for more information on this message.
-         * @param The response message. See service definition for GetKinematicSolverInfo for more information on this message.
-         */
-        bool getFKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request,
-                             kinematics_msgs::GetKinematicSolverInfo::Response &response);
-
-        /**
-         * @brief This is the basic forward kinematics service that will return information about the kinematics node.
-         * @param A request message. See service definition for GetPositionFK for more information on this message.
-         * @param The response message. See service definition for GetPositionFK for more information on this message.
-         */
-        bool getPositionFK(kinematics_msgs::GetPositionFK::Request &request,
-                           kinematics_msgs::GetPositionFK::Response &response);
-};
-
-
-
-Kinematics::Kinematics(): nh_private ("~") {
-}
-
-bool Kinematics::init() {
-    // Get URDF XML
-    std::string urdf_xml, full_urdf_xml;
-    nh.param("urdf_xml",urdf_xml,std::string("robot_description"));
-    nh.searchParam(urdf_xml,full_urdf_xml);
-    ROS_DEBUG("Reading xml file from parameter server");
-    std::string result;
-    if (!nh.getParam(full_urdf_xml, result)) {
-        ROS_FATAL("Could not load the xml from parameter server: %s", urdf_xml.c_str());
-        return false;
-    }
-
-    // Get Root and Tip From Parameter Service
-    if (!nh_private.getParam("root_name", root_name)) {
-        ROS_FATAL("GenericIK: No root name found on parameter server");
-        return false;
-    }
-    if (!nh_private.getParam("tip_name", tip_name)) {
-        ROS_FATAL("GenericIK: No tip name found on parameter server");
-        return false;
-    }
-
-    // Load and Read Models
-    if (!loadModel(result)) {
-        ROS_FATAL("Could not load models!");
-        return false;
-    }
-
-    // Get Solver Parameters
-    int maxIterations;
-    double epsilon;
-
-    nh_private.param("maxIterations", maxIterations, 1000);
-    nh_private.param("epsilon", epsilon, 1e-2);
-
-    // Build Solvers
-    fk_solver = new KDL::ChainFkSolverPos_recursive(chain);
-    ik_solver_vel = new KDL::ChainIkSolverVel_pinv(chain);
-    ik_solver_pos = new KDL::ChainIkSolverPos_NR_JL(chain, joint_min, joint_max,
-            *fk_solver, *ik_solver_vel, maxIterations, epsilon);
-
-    ROS_INFO("Advertising services");
-    fk_service = nh_private.advertiseService(FK_SERVICE,&Kinematics::getPositionFK,this);
-    ik_service = nh_private.advertiseService(IK_SERVICE,&Kinematics::getPositionIK,this);
-    ik_solver_info_service = nh_private.advertiseService(IK_INFO_SERVICE,&Kinematics::getIKSolverInfo,this);
-    fk_solver_info_service = nh_private.advertiseService(FK_INFO_SERVICE,&Kinematics::getFKSolverInfo,this);
-
-    return true;
-}
-
-bool Kinematics::loadModel(const std::string xml) {
-    urdf::Model robot_model;
-    KDL::Tree tree;
-
-    if (!robot_model.initString(xml)) {
-        ROS_FATAL("Could not initialize robot model");
-        return -1;
-    }
-    if (!kdl_parser::treeFromString(xml, tree)) {
-        ROS_ERROR("Could not initialize tree object");
-        return false;
-    }
-    if (!tree.getChain(root_name, tip_name, chain)) {
-        ROS_ERROR("Could not initialize chain object");
-        return false;
-    }
-
-    if (!readJoints(robot_model)) {
-        ROS_FATAL("Could not read information about the joints");
-        return false;
-    }
-
-    return true;
-}
-
-bool Kinematics::readJoints(urdf::Model &robot_model) {
-    num_joints = 0;
-    // get joint maxs and mins
-    boost::shared_ptr<const urdf::Link> link = robot_model.getLink(tip_name);
-    boost::shared_ptr<const urdf::Joint> joint;
-
-    while (link && link->name != root_name) {
-        joint = robot_model.getJoint(link->parent_joint->name);
-        if (!joint) {
-            ROS_ERROR("Could not find joint: %s",link->parent_joint->name.c_str());
-            return false;
-        }
-        if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
-            ROS_INFO( "adding joint: [%s]", joint->name.c_str() );
-            num_joints++;
-        }
-        link = robot_model.getLink(link->getParent()->name);
-    }
-
-    joint_min.resize(num_joints);
-    joint_max.resize(num_joints);
-    info.joint_names.resize(num_joints);
-    info.limits.resize(num_joints);
-
-    link = robot_model.getLink(tip_name);
-    unsigned int i = 0;
-    while (link && i < num_joints) {
-        joint = robot_model.getJoint(link->parent_joint->name);
-        if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
-            ROS_INFO( "getting bounds for joint: [%s]", joint->name.c_str() );
-
-            float lower, upper;
-            int hasLimits;
-            if ( joint->type != urdf::Joint::CONTINUOUS ) {
-                lower = joint->limits->lower;
-                upper = joint->limits->upper;
-                hasLimits = 1;
-            } else {
-                lower = -M_PI;
-                upper = M_PI;
-                hasLimits = 0;
-            }
-            int index = num_joints - i -1;
-
-            joint_min.data[index] = lower;
-            joint_max.data[index] = upper;
-            info.joint_names[index] = joint->name;
-            info.limits[index].joint_name = joint->name;
-            info.limits[index].has_position_limits = hasLimits;
-            info.limits[index].min_position = lower;
-            info.limits[index].max_position = upper;
-            i++;
-        }
-        link = robot_model.getLink(link->getParent()->name);
-    }
-    return true;
-}
-
-
-int Kinematics::getJointIndex(const std::string &name) {
-    for (unsigned int i=0; i < info.joint_names.size(); i++) {
-        if (info.joint_names[i] == name)
-            return i;
-    }
-    return -1;
-}
-
-int Kinematics::getKDLSegmentIndex(const std::string &name) {
-    int i=0; 
-    while (i < (int)chain.getNrOfSegments()) {
-        if (chain.getSegment(i).getName() == name) {
-            return i+1;
-        }
-        i++;
-    }
-    return -1;
-}
-
-bool Kinematics::getPositionIK(kinematics_msgs::GetPositionIK::Request &request,
-                               kinematics_msgs::GetPositionIK::Response &response) {
-
-    geometry_msgs::PoseStamped pose_msg_in = request.ik_request.pose_stamped;
-    tf::Stamped<tf::Pose> transform;
-    tf::Stamped<tf::Pose> transform_root;
-    tf::poseStampedMsgToTF( pose_msg_in, transform );
-
-    //Do the IK
-    KDL::JntArray jnt_pos_in;
-    KDL::JntArray jnt_pos_out;
-    jnt_pos_in.resize(num_joints);
-    for (unsigned int i=0; i < num_joints; i++) {
-        int tmp_index = getJointIndex(request.ik_request.ik_seed_state.joint_state.name[i]);
-        if (tmp_index >=0) {
-            jnt_pos_in(tmp_index) = request.ik_request.ik_seed_state.joint_state.position[i];
-        } else {
-            ROS_ERROR("i: %d, No joint index for %s",i,request.ik_request.ik_seed_state.joint_state.name[i].c_str());
-        }
-    }
-
-    //Convert F to our root_frame
-    try {
-        tf_listener.transformPose(root_name, transform, transform_root);
-    } catch (...) {
-        ROS_ERROR("Could not transform IK pose to frame: %s", root_name.c_str());
-        response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
-       return false;
-    }
-
-    KDL::Frame F_dest;
-    tf::TransformTFToKDL(transform_root, F_dest);
-
-    int ik_valid = ik_solver_pos->CartToJnt(jnt_pos_in, F_dest, jnt_pos_out);
-
-    if (ik_valid >= 0) {
-        response.solution.joint_state.name = info.joint_names;
-        response.solution.joint_state.position.resize(num_joints);
-        for (unsigned int i=0; i < num_joints; i++) {
-            response.solution.joint_state.position[i] = jnt_pos_out(i);
-            ROS_DEBUG("IK Solution: %s %d: %f",response.solution.joint_state.name[i].c_str(),i,jnt_pos_out(i));
-        }
-        response.error_code.val = response.error_code.SUCCESS;
-        return true;
-    } else {
-        ROS_DEBUG("An IK solution could not be found");
-        response.error_code.val = response.error_code.NO_IK_SOLUTION;
-        return true;
-    }
-}
-
-bool Kinematics::getIKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request,
-                                 kinematics_msgs::GetKinematicSolverInfo::Response &response) {
-    response.kinematic_solver_info = info;
-    return true;
-}
-
-bool Kinematics::getFKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request,
-                                 kinematics_msgs::GetKinematicSolverInfo::Response &response) {
-    response.kinematic_solver_info = info;
-    return true;
-}
-
-bool Kinematics::getPositionFK(kinematics_msgs::GetPositionFK::Request &request,
-                               kinematics_msgs::GetPositionFK::Response &response) {
-    KDL::Frame p_out;
-    KDL::JntArray jnt_pos_in;
-    geometry_msgs::PoseStamped pose;
-    tf::Stamped<tf::Pose> tf_pose;
-
-    jnt_pos_in.resize(num_joints);
-    for (unsigned int i=0; i < num_joints; i++) {
-        int tmp_index = getJointIndex(request.robot_state.joint_state.name[i]);
-        if (tmp_index >=0)
-            jnt_pos_in(tmp_index) = request.robot_state.joint_state.position[i];
-    }
-
-    response.pose_stamped.resize(request.fk_link_names.size());
-    response.fk_link_names.resize(request.fk_link_names.size());
-
-    bool valid = true;
-    for (unsigned int i=0; i < request.fk_link_names.size(); i++) {
-        int segmentIndex = getKDLSegmentIndex(request.fk_link_names[i]);
-        ROS_DEBUG("End effector index: %d",segmentIndex);
-        ROS_DEBUG("Chain indices: %d",chain.getNrOfSegments());
-        if (fk_solver->JntToCart(jnt_pos_in,p_out,segmentIndex) >=0) {
-            tf_pose.frame_id_ = root_name;
-            tf_pose.stamp_ = ros::Time();
-            tf::PoseKDLToTF(p_out,tf_pose);
-            try {
-                tf_listener.transformPose(request.header.frame_id,tf_pose,tf_pose);
-            } catch (...) {
-                ROS_ERROR("Could not transform FK pose to frame: %s",request.header.frame_id.c_str());
-                response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
-                return false;
-            }
-            tf::poseStampedTFToMsg(tf_pose,pose);
-            response.pose_stamped[i] = pose;
-            response.fk_link_names[i] = request.fk_link_names[i];
-            response.error_code.val = response.error_code.SUCCESS;
-        } else {
-            ROS_ERROR("Could not compute FK for %s",request.fk_link_names[i].c_str());
-            response.error_code.val = response.error_code.NO_FK_SOLUTION;
-            valid = false;
-        }
-    }
-    return true;
-}
-
-int main(int argc, char **argv) {
-    ros::init(argc, argv, "arm_kinematics");
-    Kinematics k;
-    if (k.init()<0) {
-        ROS_ERROR("Could not initialize kinematics node");
-        return -1;
-    }
-
-    ros::spin();
-    return 0;
-}
-
Index: src/main.cpp
===================================================================
--- src/main.cpp	(revision 0)
+++ src/main.cpp	(revision 0)
@@ -0,0 +1,62 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*
+ * Author: Sachin Chitta
+ */
+
+#include <ros/ros.h>
+#include <arm_kinematics_constraint_aware/arm_kinematics_constraint_aware.h>
+#include <boost/thread.hpp>
+
+void spinThread()
+{
+  ros::spin();
+}
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "arm_kinematics_constraint_aware");
+  arm_kinematics::ArmIKConstraintAware arm_kinematics_constraint_aware;
+  if(arm_kinematics_constraint_aware.isActive())
+  {
+    ROS_INFO("Initialized arm_kinematics_constraint_aware.");
+    boost::thread spin_thread(&spinThread);
+    //    ros::spin();
+    while(ros::ok())
+    {
+      ros::Duration(0.01).sleep();
+    }
+  }
+  else
+  {
+    ROS_ERROR("Could not initialize collision environment, kinematics node will not start.");
+  }
+  return(0);
+}
Index: src/arm_kinematics.cpp
===================================================================
--- src/arm_kinematics.cpp	(revision 0)
+++ src/arm_kinematics.cpp	(revision 0)
@@ -0,0 +1,358 @@
+// bsd license blah blah
+#include "arm_kinematics/arm_kinematics.h"
+
+using std::string;
+
+namespace arm_kinematics 
+{
+const std::string Kinematics::IK_SERVICE = "get_ik";
+const std::string Kinematics::FK_SERVICE = "get_fk";
+const std::string Kinematics::IK_INFO_SERVICE = "get_ik_solver_info";
+const std::string Kinematics::FK_INFO_SERVICE = "get_fk_solver_info";
+
+Kinematics::Kinematics(): node_handle_ ("~") {
+}
+
+bool Kinematics::init() {
+    // Get URDF XML
+    std::string urdf_xml, full_urdf_xml;
+    root_handle_.param("urdf_xml",urdf_xml,std::string("robot_description"));
+    root_handle_.searchParam(urdf_xml,full_urdf_xml);
+    ROS_DEBUG("Reading xml file from parameter server");
+    std::string result;
+
+    active_ = false;
+
+    if (!root_handle_.getParam(full_urdf_xml, result)) {
+        ROS_FATAL("Could not load the xml from parameter server: %s", urdf_xml.c_str());
+        return false;
+    }
+
+    // Get Root and Tip From Parameter Service
+    if (!node_handle_.getParam("root_name", root_name)) {
+        ROS_FATAL("GenericIK: No root name found on parameter server");
+        return false;
+    }
+    if (!node_handle_.getParam("tip_name", tip_name)) {
+        ROS_FATAL("GenericIK: No tip name found on parameter server");
+        return false;
+    }
+
+    // Load and Read Models
+    if (!loadModel(result)) {
+        ROS_FATAL("Could not load models!");
+        return false;
+    }
+
+    // Get Solver Parameters
+    double epsilon;
+
+    node_handle_.param("maxIterations", maxIterations, 1000);
+    node_handle_.param("epsilon", epsilon, 1e-2);
+
+    // Build Solvers
+    fk_solver = new KDL::ChainFkSolverPos_recursive(chain);
+    ik_solver_vel = new KDL::ChainIkSolverVel_pinv(chain);
+    ik_solver_pos = new KDL::ChainIkSolverPos_NR_JL(chain, joint_min, joint_max,
+            *fk_solver, *ik_solver_vel, maxIterations, epsilon);
+
+    ROS_INFO("Advertising services");
+    fk_service = node_handle_.advertiseService(FK_SERVICE,&Kinematics::getPositionFK,this);
+    ik_service = node_handle_.advertiseService(IK_SERVICE,&Kinematics::getPositionIK,this);
+    ik_solver_info_service = node_handle_.advertiseService(IK_INFO_SERVICE,&Kinematics::getIKSolverInfo,this);
+    fk_solver_info_service = node_handle_.advertiseService(FK_INFO_SERVICE,&Kinematics::getFKSolverInfo,this);
+
+    active_ = true;
+    return true;
+}
+
+bool Kinematics::loadModel(const std::string xml) {
+    urdf::Model robot_model;
+    KDL::Tree tree;
+
+    if (!robot_model.initString(xml)) {
+        ROS_FATAL("Could not initialize robot model");
+        return -1;
+    }
+    if (!kdl_parser::treeFromString(xml, tree)) {
+        ROS_ERROR("Could not initialize tree object");
+        return false;
+    }
+    if (!tree.getChain(root_name, tip_name, chain)) {
+        ROS_ERROR("Could not initialize chain object");
+        return false;
+    }
+
+    if (!readJoints(robot_model)) {
+        ROS_FATAL("Could not read information about the joints");
+        return false;
+    }
+
+    return true;
+}
+
+bool Kinematics::readJoints(urdf::Model &robot_model) {
+    num_joints = 0;
+    // get joint maxs and mins
+    boost::shared_ptr<const urdf::Link> link = robot_model.getLink(tip_name);
+    boost::shared_ptr<const urdf::Joint> joint;
+
+    while (link && link->name != root_name) {
+        joint = robot_model.getJoint(link->parent_joint->name);
+        if (!joint) {
+            ROS_ERROR("Could not find joint: %s",link->parent_joint->name.c_str());
+            return false;
+        }
+        if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
+            ROS_INFO( "adding joint: [%s]", joint->name.c_str() );
+            num_joints++;
+        }
+        link = robot_model.getLink(link->getParent()->name);
+    }
+
+    joint_min.resize(num_joints);
+    joint_max.resize(num_joints);
+    info.joint_names.resize(num_joints);
+    info.link_names.resize(num_joints);
+    info.limits.resize(num_joints);
+
+    link = robot_model.getLink(tip_name);
+    unsigned int i = 0;
+    while (link && i < num_joints) {
+        joint = robot_model.getJoint(link->parent_joint->name);
+        if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
+            ROS_INFO( "getting bounds for joint: [%s]", joint->name.c_str() );
+
+            float lower, upper;
+            int hasLimits;
+            if ( joint->type != urdf::Joint::CONTINUOUS ) {
+                lower = joint->limits->lower;
+                upper = joint->limits->upper;
+                hasLimits = 1;
+            } else {
+                lower = -M_PI;
+                upper = M_PI;
+                hasLimits = 0;
+            }
+            int index = num_joints - i -1;
+
+            joint_min.data[index] = lower;
+            joint_max.data[index] = upper;
+            info.joint_names[index] = joint->name;
+            info.link_names[index] = link->name;
+            info.limits[index].joint_name = joint->name;
+            info.limits[index].has_position_limits = hasLimits;
+            info.limits[index].min_position = lower;
+            info.limits[index].max_position = upper;
+            i++;
+        }
+        link = robot_model.getLink(link->getParent()->name);
+    }
+    return true;
+}
+
+
+int Kinematics::getJointIndex(const std::string &name) {
+    for (unsigned int i=0; i < info.joint_names.size(); i++) {
+        if (info.joint_names[i] == name)
+            return i;
+    }
+    return -1;
+}
+
+int Kinematics::getKDLSegmentIndex(const std::string &name) {
+    int i=0; 
+    while (i < (int)chain.getNrOfSegments()) {
+        if (chain.getSegment(i).getName() == name) {
+            return i+1;
+        }
+        i++;
+    }
+    return -1;
+}
+
+bool Kinematics::getPositionIK(kinematics_msgs::GetPositionIK::Request &request,
+                               kinematics_msgs::GetPositionIK::Response &response) {
+
+    geometry_msgs::PoseStamped pose_msg_in = request.ik_request.pose_stamped;
+    tf::Stamped<tf::Pose> transform;
+    tf::Stamped<tf::Pose> transform_root;
+    tf::poseStampedMsgToTF( pose_msg_in, transform );
+
+    //Do the IK
+    KDL::JntArray jnt_pos_in;
+    KDL::JntArray jnt_pos_out;
+    jnt_pos_in.resize(num_joints);
+    for (unsigned int i=0; i < num_joints; i++) {
+        int tmp_index = getJointIndex(request.ik_request.ik_seed_state.joint_state.name[i]);
+        if (tmp_index >=0) {
+            jnt_pos_in(tmp_index) = request.ik_request.ik_seed_state.joint_state.position[i];
+        } else {
+            ROS_ERROR("i: %d, No joint index for %s",i,request.ik_request.ik_seed_state.joint_state.name[i].c_str());
+        }
+    }
+
+    //Convert F to our root_frame
+    try {
+        tf_listener.transformPose(root_name, transform, transform_root);
+    } catch (...) {
+        ROS_ERROR("Could not transform IK pose to frame: %s", root_name.c_str());
+        response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
+       return false;
+    }
+
+    KDL::Frame F_dest;
+    tf::TransformTFToKDL(transform_root, F_dest);
+
+    int ik_valid = ik_solver_pos->CartToJnt(jnt_pos_in, F_dest, jnt_pos_out);
+
+    if (ik_valid >= 0) {
+        response.solution.joint_state.name = info.joint_names;
+        response.solution.joint_state.position.resize(num_joints);
+        for (unsigned int i=0; i < num_joints; i++) {
+            response.solution.joint_state.position[i] = jnt_pos_out(i);
+            ROS_DEBUG("IK Solution: %s %d: %f",response.solution.joint_state.name[i].c_str(),i,jnt_pos_out(i));
+        }
+        response.error_code.val = response.error_code.SUCCESS;
+        return true;
+    } else {
+        ROS_DEBUG("An IK solution could not be found");
+        response.error_code.val = response.error_code.NO_IK_SOLUTION;
+        return true;
+    }
+}
+
+bool Kinematics::getIKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request,
+                                 kinematics_msgs::GetKinematicSolverInfo::Response &response) {
+    response.kinematic_solver_info = info;
+    return true;
+}
+
+bool Kinematics::getFKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request,
+                                 kinematics_msgs::GetKinematicSolverInfo::Response &response) {
+    response.kinematic_solver_info = info;
+    return true;
+}
+
+bool Kinematics::getPositionFK(kinematics_msgs::GetPositionFK::Request &request,
+                               kinematics_msgs::GetPositionFK::Response &response) {
+    KDL::Frame p_out;
+    KDL::JntArray jnt_pos_in;
+    geometry_msgs::PoseStamped pose;
+    tf::Stamped<tf::Pose> tf_pose;
+
+    jnt_pos_in.resize(num_joints);
+    for (unsigned int i=0; i < num_joints; i++) {
+        int tmp_index = getJointIndex(request.robot_state.joint_state.name[i]);
+        if (tmp_index >=0)
+            jnt_pos_in(tmp_index) = request.robot_state.joint_state.position[i];
+    }
+
+    response.pose_stamped.resize(request.fk_link_names.size());
+    response.fk_link_names.resize(request.fk_link_names.size());
+
+    bool valid = true;
+    for (unsigned int i=0; i < request.fk_link_names.size(); i++) {
+        int segmentIndex = getKDLSegmentIndex(request.fk_link_names[i]);
+        ROS_DEBUG("End effector index: %d",segmentIndex);
+        ROS_DEBUG("Chain indices: %d",chain.getNrOfSegments());
+        if (fk_solver->JntToCart(jnt_pos_in,p_out,segmentIndex) >=0) {
+            tf_pose.frame_id_ = root_name;
+            tf_pose.stamp_ = ros::Time();
+            tf::PoseKDLToTF(p_out,tf_pose);
+            try {
+                tf_listener.transformPose(request.header.frame_id,tf_pose,tf_pose);
+            } catch (...) {
+                ROS_ERROR("Could not transform FK pose to frame: %s",request.header.frame_id.c_str());
+                response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
+                return false;
+            }
+            tf::poseStampedTFToMsg(tf_pose,pose);
+            response.pose_stamped[i] = pose;
+            response.fk_link_names[i] = request.fk_link_names[i];
+            response.error_code.val = response.error_code.SUCCESS;
+        } else {
+            ROS_ERROR("Could not compute FK for %s",request.fk_link_names[i].c_str());
+            response.error_code.val = response.error_code.NO_FK_SOLUTION;
+            valid = false;
+        }
+    }
+    return true;
+}
+
+
+bool Kinematics::getCount(int &count, 
+                          const int &max_count, 
+                          const int &min_count)
+{
+  if(count > 0)
+  {
+    if(-count >= min_count)
+    {   
+      count = -count;
+      return true;
+    }
+    else if(count+1 <= max_count)
+    {
+      count = count+1;
+      return true;
+    }
+    else
+    {
+      return false;
+    }
+  }
+  else
+  {
+    if(1-count <= max_count)
+    {
+      count = 1-count;
+      return true;
+    }
+    else if(count-1 >= min_count)
+    {
+      count = count -1;
+      return true;
+    }
+    else
+      return false;
+  }
+}
+
+bool Kinematics::isActive()
+{
+  if(active_)
+    return true;
+  return false;
+}
+
+Eigen::Matrix4f Kinematics::KDLToEigenMatrix(const KDL::Frame &p)
+{
+  Eigen::Matrix4f b = Eigen::Matrix4f::Identity();
+  for(int i=0; i < 3; i++)
+  {
+    for(int j=0; j<3; j++)
+    {
+      b(i,j) = p.M(i,j);
+    }
+    b(i,3) = p.p(i);
+  }
+  return b;
+}
+
+
+
+}//end namespace
+
+int main(int argc, char **argv) {
+    ros::init(argc, argv, "arm_kinematics");
+    arm_kinematics::Kinematics k;
+    if (k.init()<0) {
+        ROS_ERROR("Could not initialize kinematics node");
+        return -1;
+    }
+
+    ros::spin();
+    return 0;
+}
+
Index: src/arm_kinematics_constraint_aware.cpp
===================================================================
--- src/arm_kinematics_constraint_aware.cpp	(revision 0)
+++ src/arm_kinematics_constraint_aware.cpp	(revision 0)
@@ -0,0 +1,672 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*
+ * Author: Sachin Chitta
+ */
+
+#include <arm_kinematics_constraint_aware/arm_kinematics_constraint_aware.h>
+#include <pr2_arm_kinematics/pr2_arm_kinematics_utils.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <kdl_parser/kdl_parser.hpp>
+#include <tf_conversions/tf_kdl.h>
+#include "ros/ros.h"
+#include <algorithm>
+#include <numeric>
+
+#include <sensor_msgs/JointState.h>
+#include <kinematics_msgs/utils.h>
+#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
+
+
+using namespace KDL;
+using namespace tf;
+using namespace std;
+using namespace ros;
+
+namespace arm_kinematics {
+
+static const std::string IK_WITH_COLLISION_SERVICE = "get_constraint_aware_ik";
+static const double IK_DEFAULT_TIMEOUT = 10.0;
+
+ArmIKConstraintAware::ArmIKConstraintAware(): Kinematics(),setup_collision_environment_(false)
+{
+  init();
+
+  node_handle_.param<bool>("visualize_solution",visualize_solution_,true);
+  ROS_DEBUG("Advertising services");
+
+  if(!isActive())
+  {
+    ROS_ERROR("Could not load arm_kinematics_constraint_aware server");
+  }
+  else
+  {
+    ROS_INFO("Loading arm_kinematics_constraint_aware server");
+  }
+  advertiseIK();
+
+  if(setupCollisionEnvironment())
+    ROS_INFO("Collision environment setup.");
+  else
+    ROS_ERROR("Could not initialize collision environment");
+}
+
+void ArmIKConstraintAware::advertiseIK()
+{
+  ik_collision_service_ = node_handle_.advertiseService(IK_WITH_COLLISION_SERVICE,&ArmIKConstraintAware::getConstraintAwarePositionIK,this);
+  display_trajectory_publisher_ = root_handle_.advertise<motion_planning_msgs::DisplayTrajectory>("ik_solution_display", 1);
+}
+
+bool ArmIKConstraintAware::isReady(motion_planning_msgs::ArmNavigationErrorCodes &error_code)
+{
+ if(!active_)
+  {
+    ROS_ERROR("IK service is not ready");
+    return false;
+  }
+  if(!setup_collision_environment_)
+  {
+    ROS_INFO("Waiting for collision environment setup.");
+    if(!setupCollisionEnvironment())
+    {
+      ROS_INFO("Could not initialize collision environment");
+      error_code.val = error_code.COLLISION_CHECKING_UNAVAILABLE;
+      return false;
+    }    
+    else
+    {
+      setup_collision_environment_ = true;
+    }
+  }
+  error_code.val = error_code.SUCCESS;
+  return true;
+}
+
+
+int ArmIKConstraintAware::CartToJntSearch(const KDL::JntArray& q_in, 
+                                          const KDL::Frame& p_in, 
+                                          KDL::JntArray &q_out, 
+                                          const double &timeout, 
+                                          motion_planning_msgs::ArmNavigationErrorCodes& error_code)
+{
+  if(!isReady(error_code))
+    return -1;
+
+  ROS_ERROR("Timeout set to: %f", timeout);
+
+  bool ik_valid = CartToJntSearch(q_in,
+                                  p_in,
+                                  q_out, 
+                                  timeout,
+                                  error_code,
+                                  boost::bind(&ArmIKConstraintAware::initialPoseCheck, this, _1, _2, _3),
+                                  boost::bind(&ArmIKConstraintAware::collisionCheck, this, _1, _2, _3)) >= 0;
+
+  return ik_valid;
+
+}
+
+
+
+int ArmIKConstraintAware::CartToJntSearch(const KDL::JntArray& q_in, 
+                                          const KDL::Frame& p_in, 
+                                          KDL::JntArray &q_out, 
+                                          const double &timeout, 
+                                          motion_planning_msgs::ArmNavigationErrorCodes &error_code,
+                                          const boost::function<void(const KDL::JntArray&,const KDL::Frame&,motion_planning_msgs::ArmNavigationErrorCodes &)> &desired_pose_callback,
+                                          const boost::function<void(const KDL::JntArray&,const KDL::Frame&,motion_planning_msgs::ArmNavigationErrorCodes &)> &solution_callback)
+{
+  Eigen::Matrix4f b = KDLToEigenMatrix(p_in);
+  KDL::JntArray q_init = q_in;
+  //replaced free_angle_ by num_joints. Is that bad?
+  //double initial_guess = q_init(num_joints);
+
+  ros::Time start_time = ros::Time::now();
+  double loop_time = 0;
+  int count = 0;
+
+  int num_positive_increments = (int)(maxIterations);// (int)((pr2_arm_ik_.solver_info_.limits[free_angle_].max_position-initial_guess)/search_discretization_angle_);
+  int num_negative_increments = (int)(maxIterations); //(int)((initial_guess-pr2_arm_ik_.solver_info_.limits[free_angle_].min_position)/search_discretization_angle_);
+  //ROS_DEBUG("%f %f %f %d %d \n\n",initial_guess,pr2_arm_ik_.solver_info_.limits[free_angle_].max_position,pr2_arm_ik_.solver_info_.limits[free_angle_].min_position,num_positive_increments,num_negative_increments);
+
+  if(!desired_pose_callback.empty())
+    desired_pose_callback(q_init,p_in,error_code);
+  if(error_code.val != error_code.SUCCESS)
+  {
+    return -1;
+  }
+  bool callback_check = true;
+  if(solution_callback.empty())
+    callback_check = false;
+
+  while(loop_time < timeout)
+  {
+    if(ik_solver_pos->CartToJnt(q_in, p_in, q_out) > 0)//CartToJnt(q_init,p_in,q_out) > 0)
+    {
+      if(callback_check)
+      {
+        solution_callback(q_out,p_in,error_code);
+        if(error_code.val == error_code.SUCCESS)
+        {
+          return 1;
+        }
+      }
+      else
+      {
+        error_code.val = error_code.SUCCESS;
+        return 1;
+      }
+    }
+    if(!getCount(count,num_positive_increments,-num_negative_increments))
+    {
+      error_code.val = error_code.NO_IK_SOLUTION;
+      return -1;
+    }
+    //q_init(free_angle_) = initial_guess + search_discretization_angle_ * count;
+    //ROS_DEBUG("Redundancy search, index:%d, free angle value: %f",count,q_init(free_angle_));
+    loop_time = (ros::Time::now()-start_time).toSec();
+  }
+  if(loop_time >= timeout)
+  {
+    ROS_DEBUG("IK Timed out in %f seconds",timeout);
+    error_code.val = error_code.TIMED_OUT;
+  }
+  else
+  {
+    ROS_DEBUG("No IK solution was found");
+    error_code.val = error_code.NO_IK_SOLUTION;
+  }
+  return -1;
+}
+
+
+
+
+bool ArmIKConstraintAware::getConstraintAwarePositionIK(kinematics_msgs::GetConstraintAwarePositionIK::Request &request_in,
+                                                        kinematics_msgs::GetConstraintAwarePositionIK::Response &response)
+{
+
+  if(!isReady(response.error_code))
+    return true;
+
+  /*if(!pr2_arm_kinematics::checkConstraintAwareIKService(request_in,response,ik_solver_info_))
+  {
+    ROS_ERROR("IK service request is malformed");
+    return true;
+    }*/
+
+  ros::Time start_time = ros::Time::now();
+  ROS_INFO("Received IK request is in the frame: %s",request_in.ik_request.pose_stamped.header.frame_id.c_str());
+
+  ik_request_ = request_in.ik_request;
+  collision_operations_ = request_in.ordered_collision_operations;
+  link_padding_ = request_in.link_padding;
+  allowed_contacts_ = request_in.allowed_contacts;
+  constraints_ = request_in.constraints;
+
+  geometry_msgs::PoseStamped pose_msg_in = ik_request_.pose_stamped;
+  geometry_msgs::PoseStamped pose_msg_out;
+
+  tf::Stamped<tf::Pose> transform;
+  tf::Stamped<tf::Pose> transform_root;
+
+  tf::poseStampedMsgToTF( pose_msg_in, transform );
+
+  //Convert F to our root_frame
+  try {
+          tf_listener.transformPose(root_name, transform, transform_root);
+  } catch (...) {
+          ROS_ERROR("Could not transform IK pose to frame: %s", root_name.c_str());
+          response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
+          return false;
+  }
+
+  motion_planning_msgs::Constraints emp;
+
+  //setting up planning monitor
+  planning_monitor_->prepareForValidityChecks(ik_solver_info_.joint_names,
+                                              collision_operations_,
+                                              allowed_contacts_,
+                                              emp,
+                                              constraints_,
+                                              link_padding_,
+                                              response.error_code);  
+
+  //can't transform constraints
+  if(response.error_code.val == response.error_code.FRAME_TRANSFORM_FAILURE) {
+    return true;
+  }
+  
+  kinematic_state_ = new planning_models::KinematicState(planning_monitor_->getKinematicModel());
+
+  planning_monitor_->setRobotStateAndComputeTransforms(ik_request_.robot_state, *kinematic_state_);
+
+  ik_request_.pose_stamped = pose_msg_out;
+  ROS_INFO("Transformed IK request is in the frame: %s",ik_request_.pose_stamped.header.frame_id.c_str());
+
+  KDL::JntArray jnt_pos_out;
+  KDL::JntArray jnt_pos_in;
+  KDL::Frame p_in;
+
+
+  tf::TransformTFToKDL(transform_root, p_in);
+
+  //tf::PoseMsgToKDL(pose_msg_out.pose,p_in);
+  jnt_pos_in.resize(num_joints);
+  jnt_pos_out.resize(num_joints);
+  for(unsigned int i=0; i < request_in.ik_request.ik_seed_state.joint_state.name.size(); i++)
+  {
+    int tmp_index = pr2_arm_kinematics::getJointIndex(request_in.ik_request.ik_seed_state.joint_state.name[i],ik_solver_info_);
+    if(tmp_index != -1) {
+      ROS_ERROR_STREAM("Setting name " << request_in.ik_request.ik_seed_state.joint_state.name[i]
+                      << " index " << tmp_index 
+                      << " to " << request_in.ik_request.ik_seed_state.joint_state.position[i]);
+      jnt_pos_in(tmp_index) = request_in.ik_request.ik_seed_state.joint_state.position[i];
+    }
+  }
+
+  ros::Time ik_solver_time = ros::Time::now();
+  bool ik_valid = CartToJntSearch(jnt_pos_in,
+                                  p_in,
+                                  jnt_pos_out, 
+                                  request_in.timeout.toSec(),
+                                  response.error_code);
+  ROS_INFO("IK solver time: %f",(ros::Time::now()-ik_solver_time).toSec());
+
+  planning_monitor_->revertToDefaultState();
+  if(ik_valid)
+  {
+    response.solution.joint_state.name = ik_solver_info_.joint_names;
+    response.solution.joint_state.position.resize(num_joints);
+    for(unsigned int i=0; i < num_joints; i++)
+    {
+      response.solution.joint_state.position[i] = jnt_pos_out(i);
+      ROS_DEBUG("IK Solution: %s %d: %f",response.solution.joint_state.name[i].c_str(),i,jnt_pos_out(i));
+    }
+    if(visualize_solution_)
+    {
+      motion_planning_msgs::DisplayTrajectory display_trajectory;
+      display_trajectory.trajectory.joint_trajectory.points.resize(1);
+      display_trajectory.trajectory.joint_trajectory.points[0].positions = response.solution.joint_state.position;
+      display_trajectory.trajectory.joint_trajectory.joint_names = response.solution.joint_state.name;
+      planning_monitor_->convertKinematicStateToRobotState(*kinematic_state_,display_trajectory.robot_state);
+      display_trajectory_publisher_.publish(display_trajectory);
+    }
+    ROS_INFO("IK service time: %f",(ros::Time::now()-start_time).toSec());
+    response.error_code.val = response.error_code.SUCCESS;
+    delete kinematic_state_;
+    return true;
+  }
+  else
+  {
+    ROS_ERROR("An IK solution could not be found");
+    if(response.error_code.val != response.error_code.IK_LINK_IN_COLLISION) {
+      sendEndEffectorPose(kinematic_state_,true);
+    }
+    delete kinematic_state_;
+    return true;
+  }
+}
+
+void ArmIKConstraintAware::collisionCheck(const KDL::JntArray &jnt_array, 
+                                          const KDL::Frame &p_in,
+                                          motion_planning_msgs::ArmNavigationErrorCodes &error_code)
+{
+  std::map<std::string, double> joint_values;
+  for(unsigned int i=0; i < ik_solver_info_.joint_names.size(); i++)
+  {
+    joint_values[ik_solver_info_.joint_names[i]] = jnt_array(i);
+  }
+  kinematic_state_->setKinematicState(joint_values);
+  planning_monitor_->getEnvironmentModel()->updateRobotModel(kinematic_state_);
+
+  bool check = (!planning_monitor_->getEnvironmentModel()->isCollision() &&  !planning_monitor_->getEnvironmentModel()->isSelfCollision());
+  if(!check) {
+    planning_monitor_->broadcastCollisions();
+    error_code.val = error_code.KINEMATICS_STATE_IN_COLLISION;
+  }
+  else {
+    error_code.val = error_code.SUCCESS;
+  }
+  
+  if(!planning_monitor_->checkGoalConstraints(kinematic_state_, false)) {
+    error_code.val = error_code.INVALID_GOAL_POSITION_CONSTRAINTS;
+    ROS_INFO("Constraints violated at current state");
+  }
+}
+
+void ArmIKConstraintAware::initialPoseCheck(const KDL::JntArray &jnt_array, 
+                                             const KDL::Frame &p_in,
+                                             motion_planning_msgs::ArmNavigationErrorCodes &error_code)
+{
+  std::string kinematic_frame_id = root_name;
+  std::string planning_frame_id = planning_monitor_->getWorldFrameId();
+  geometry_msgs::PoseStamped pose_stamped;
+  tf::PoseKDLToMsg(p_in,pose_stamped.pose);
+  pose_stamped.header.stamp = ros::Time::now();
+  pose_stamped.header.frame_id = kinematic_frame_id;
+  if (!tf_listener.canTransform(planning_frame_id,
+                        pose_stamped.header.frame_id,
+                        pose_stamped.header.stamp))
+  {
+    std::string err;
+    ros::Time tmp;
+    if(tf_listener.getLatestCommonTime(pose_stamped.header.frame_id,planning_frame_id,tmp,&err) != tf::NO_ERROR)
+    {
+      ROS_ERROR("Cannot transform from '%s' to '%s'. TF said: %s",
+                pose_stamped.header.frame_id.c_str(),planning_frame_id.c_str(), err.c_str());
+    }
+    else
+      pose_stamped.header.stamp = tmp;
+  }
+    
+  try
+  {
+    tf_listener.transformPose(planning_frame_id,pose_stamped,pose_stamped);
+  }
+  catch(tf::TransformException& ex)
+  {
+    ROS_ERROR("Cannot transform from '%s' to '%s'. Tf said: %s",
+              pose_stamped.header.frame_id.c_str(),planning_frame_id.c_str(), ex.what());
+    error_code.val = error_code.FRAME_TRANSFORM_FAILURE;
+    return;
+  }
+  
+  //need to disable collisions correctly, and re-enable at the end
+  std::vector<std::vector<bool> > orig_allowed;
+  std::map<std::string, unsigned int> vecIndices;
+  planning_monitor_->getEnvironmentModel()->getCurrentAllowedCollisionMatrix(orig_allowed, vecIndices);
+
+  std::vector<std::vector<bool> > new_allowed = orig_allowed;
+  for(unsigned int i = 0; i < arm_links_.size(); i++) {
+    unsigned int vind = vecIndices[arm_links_[i]];
+    for(unsigned int j = 0; j < new_allowed.size(); j++) {
+      new_allowed[vind][j] = true;
+      new_allowed[j][vind] = true;
+    }
+  }
+  //planning_monitor_->printAllowedCollisionMatrix(orig_allowed, vecIndices);
+  //planning_monitor_->printAllowedCollisionMatrix(new_allowed, vecIndices);
+  planning_monitor_->getEnvironmentModel()->setAllowedCollisionMatrix(new_allowed, vecIndices);
+
+  btTransform transform;
+  tf::poseMsgToTF(pose_stamped.pose,transform);
+  if(!kinematic_state_->hasLinkState(ik_request_.ik_link_name)) {
+    ROS_ERROR("Could not find end effector root_link %s", ik_request_.ik_link_name.c_str());
+    error_code.val = error_code.INVALID_LINK_NAME;
+    return;
+  }
+  kinematic_state_->updateKinematicStateWithLinkAt(ik_request_.ik_link_name, transform);
+  planning_monitor_->getEnvironmentModel()->updateRobotModel(kinematic_state_);
+
+  bool check = ( !planning_monitor_->getEnvironmentModel()->isCollision() &&  
+            !planning_monitor_->getEnvironmentModel()->isSelfCollision() );
+  if(!check) {
+    error_code.val = error_code.IK_LINK_IN_COLLISION;
+    planning_monitor_->broadcastCollisions();
+    sendEndEffectorPose(kinematic_state_, false);
+  }
+  else
+    error_code.val = error_code.SUCCESS;
+    
+  planning_monitor_->getEnvironmentModel()->setAllowedCollisionMatrix(orig_allowed, vecIndices);
+
+   ROS_DEBUG("Initial pose check done with result %s",check ? "not_in_collision" : "in_collision" );
+}
+
+void ArmIKConstraintAware::sendEndEffectorPose(const planning_models::KinematicState* state, bool valid) {
+  if(!robot_model_initialized_) return;
+  visualization_msgs::MarkerArray hand_array;
+  unsigned int id = 0;
+  for(unsigned int i = 0; i < end_effector_collision_links_.size(); i++) {
+    boost::shared_ptr<const urdf::Link> urdf_link = robot_model_.getLink(end_effector_collision_links_[i]);
+    if(urdf_link == NULL) {
+      ROS_DEBUG_STREAM("No entry in urdf for link " << end_effector_collision_links_[i]);
+      continue;
+    }
+    if(!urdf_link->collision) {
+      continue;
+    }
+    const urdf::Geometry *geom = urdf_link->collision->geometry.get();
+    if(!geom) {
+      ROS_DEBUG_STREAM("No collision geometry for link " << end_effector_collision_links_[i]);
+      continue;
+    }
+    const urdf::Mesh *mesh = dynamic_cast<const urdf::Mesh*>(geom);
+    if(mesh) {
+      if (!mesh->filename.empty()) {
+        planning_models::KinematicState::LinkState* ls = state->getLinkState(end_effector_collision_links_[i]);
+        visualization_msgs::Marker mark;
+        mark.header.frame_id = planning_monitor_->getWorldFrameId();
+        mark.header.stamp = ros::Time::now();
+        mark.id = id++;
+        if(!valid) {
+          mark.ns = "initial_pose_collision";
+        } else {
+          mark.ns = "initial_pose_ok";
+        }
+        mark.type = mark.MESH_RESOURCE;
+        mark.scale.x = 1.0;
+        mark.scale.y = 1.0;
+        mark.scale.z = 1.0;
+        if(!valid) {
+          mark.color.r = 1.0;
+        } else {
+          mark.color.g = 1.0;
+        }
+        mark.color.a = .8;
+        mark.pose.position.x = ls->getGlobalCollisionBodyTransform().getOrigin().x();
+        mark.pose.position.y = ls->getGlobalCollisionBodyTransform().getOrigin().y();
+        mark.pose.position.z = ls->getGlobalCollisionBodyTransform().getOrigin().z();
+        mark.pose.orientation.x = ls->getGlobalCollisionBodyTransform().getRotation().x();
+        mark.pose.orientation.y = ls->getGlobalCollisionBodyTransform().getRotation().y();
+        mark.pose.orientation.z = ls->getGlobalCollisionBodyTransform().getRotation().z();
+        mark.pose.orientation.w = ls->getGlobalCollisionBodyTransform().getRotation().w();
+        mark.mesh_resource = mesh->filename;
+        hand_array.markers.push_back(mark);
+      }
+    }
+  }
+  vis_marker_array_publisher_.publish(hand_array);
+}
+
+void ArmIKConstraintAware::printStringVec(const std::string &prefix, const std::vector<std::string> &string_vector)
+{
+  ROS_DEBUG("%s",prefix.c_str());
+  for(unsigned int i=0; i < string_vector.size(); i++)
+  {
+    ROS_DEBUG("%s",string_vector[i].c_str());
+  }
+}
+
+bool ArmIKConstraintAware::setupCollisionEnvironment()
+{
+  node_handle_.param<std::string>("group", group_, std::string());
+  node_handle_.param<bool>("use_collision_map", use_collision_map_, true);
+  if (group_.empty())
+  {
+    ROS_ERROR("No 'group' parameter specified. Without the name of the group of joints to monitor, action cannot start");
+    return false;
+  }
+
+  std::string urdf_xml,full_urdf_xml;
+  root_handle_.param("urdf_xml", urdf_xml, std::string("robot_description"));
+  if(!root_handle_.getParam(urdf_xml,full_urdf_xml))
+  {
+    ROS_ERROR("Could not load the xml from parameter server: %s\n", urdf_xml.c_str());
+    robot_model_initialized_ = false;
+  }
+  else
+  {
+    robot_model_.initString(full_urdf_xml);
+    robot_model_initialized_ = true;
+  }
+
+  // monitor robot
+  collision_models_ = new planning_environment::CollisionModels("robot_description");
+
+  if(!collision_models_)
+  {
+    ROS_INFO("Could not initialize collision models");
+    return false;
+  }
+  planning_monitor_ = new planning_environment::PlanningMonitor(collision_models_, &tf_listener);
+
+  if(!planning_monitor_)
+  {
+    ROS_ERROR("Could not initialize planning monitor");
+    return false;
+  }
+  else
+  {
+    ROS_INFO("Initialized planning monitor");
+  }
+
+  planning_monitor_->setUseCollisionMap(use_collision_map_);
+  planning_monitor_->startEnvironmentMonitor();
+  if(!collision_models_->loadedModels())
+  {
+    ROS_ERROR("Could not load models");
+    return false;
+  }
+  if (!collision_models_->getKinematicModel()->hasModelGroup(group_))
+  {
+    ROS_ERROR("Group '%s' is not known", group_.c_str());
+    return false;
+  }
+  else
+    ROS_INFO("Configuring action for '%s'", group_.c_str());
+
+  const std::vector<const planning_models::KinematicModel::JointModel*>& p_joints = collision_models_->getKinematicModel()->getModelGroup(group_)->getJointModels();
+  for(unsigned int i=0; i < p_joints.size(); i++)
+  {
+    default_collision_links_.push_back(p_joints[i]->getChildLinkModel()->getName()); 
+  }
+
+  //getting arm link names
+  arm_links_ = collision_models_->getPlanningGroupLinks().at(group_);
+
+  if (planning_monitor_->getExpectedJointStateUpdateInterval() > 1e-3)
+    planning_monitor_->waitForState();
+  if (planning_monitor_->getExpectedMapUpdateInterval() > 1e-3 && use_collision_map_)
+    planning_monitor_->waitForMap();
+  //planning_monitor_->setCollisionCheckOnlyLinks(default_collision_links_,true);
+
+  end_effector_collision_links_.clear();
+  
+  const planning_models::KinematicModel::LinkModel* end_effector_link = planning_monitor_->getKinematicModel()->getLinkModel(tip_name);
+  std::vector<const planning_models::KinematicModel::LinkModel*> tempLinks;
+  planning_monitor_->getKinematicModel()->getChildLinkModels(end_effector_link, tempLinks);
+  for (unsigned int i = 0 ; i < tempLinks.size() ; ++i)
+    end_effector_collision_links_.push_back(tempLinks[i]->getName());
+  for(unsigned int i=0; i < end_effector_collision_links_.size(); i++)
+  {
+    default_collision_links_.push_back(end_effector_collision_links_[i]);
+  }
+
+  printStringVec("Default collision links",default_collision_links_);
+  printStringVec("End effector links",end_effector_collision_links_);
+
+  ROS_DEBUG("Root link name is: %s",root_name.c_str());
+
+  planning_monitor_->setOnCollisionContactCallback(boost::bind(&ArmIKConstraintAware::contactFound, this, _1));
+  vis_marker_publisher_ = root_handle_.advertise<visualization_msgs::Marker>("kinematics_collisions", 128);
+  vis_marker_array_publisher_ = root_handle_.advertise<visualization_msgs::MarkerArray>("kinematics_collisions_array", 128);
+
+  setup_collision_environment_ = true;
+  return true;
+}
+
+/** \brief The ccost and display arguments should be bound by the caller. This is a callback function that gets called by the planning
+ * environment when a collision is found */
+void ArmIKConstraintAware::contactFound(collision_space::EnvironmentModel::Contact &contact)
+{
+
+  static int count = 0;
+  
+  std::string ns_name;
+  if(contact.link1 != NULL) {
+    //ROS_INFO_STREAM("Link 1 is " << contact.link2->name);
+    if(contact.link1_attached_body_index == 0) {
+      ns_name += contact.link1->getName()+"+";
+    } else {
+      if(contact.link1->getAttachedBodyModels().size() < contact.link1_attached_body_index) {
+        ROS_ERROR("Link doesn't have attached body with indicated index");
+      } else {
+        ns_name += contact.link1->getAttachedBodyModels()[contact.link1_attached_body_index-1]->getName()+"+";
+      }
+    }
+  } 
+  
+  if(contact.link2 != NULL) {
+    //ROS_INFO_STREAM("Link 2 is " << contact.link2->name);
+    if(contact.link2_attached_body_index == 0) {
+      ns_name += contact.link2->getName();
+    } else {
+      if(contact.link2->getAttachedBodyModels().size() < contact.link2_attached_body_index) {
+        ROS_ERROR("Link doesn't have attached body with indicated index");
+      } else {
+        ns_name += contact.link2->getAttachedBodyModels()[contact.link2_attached_body_index-1]->getName();
+      }
+    }
+  } 
+  
+  if(!contact.object_name.empty()) {
+    //ROS_INFO_STREAM("Object is " << contact.object_name);
+    ns_name += contact.object_name;
+  }
+  
+  visualization_msgs::Marker mk;
+  mk.header.stamp = planning_monitor_->lastPoseUpdate();
+  mk.header.frame_id = planning_monitor_->getWorldFrameId();
+  mk.ns = ns_name;
+  mk.id = count++;
+  mk.type = visualization_msgs::Marker::SPHERE;
+  mk.action = visualization_msgs::Marker::ADD;
+  mk.pose.position.x = contact.pos.x();
+  mk.pose.position.y = contact.pos.y();
+  mk.pose.position.z = contact.pos.z();
+  mk.pose.orientation.w = 1.0;
+  
+  mk.scale.x = mk.scale.y = mk.scale.z = 0.01;
+  
+  mk.color.a = 0.6;
+  mk.color.r = 1.0;
+  mk.color.g = 0.04;
+  mk.color.b = 0.04;
+  
+  //mk.lifetime = ros::Duration(30.0);
+  
+  vis_marker_publisher_.publish(mk);
+}
+
+} // namespace
+
Index: CMakeLists.txt
===================================================================
--- CMakeLists.txt	(revision 36)
+++ CMakeLists.txt	(working copy)
@@ -16,4 +16,8 @@
 #set the default path for built libraries to the "lib" directory
 set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
 
-rosbuild_add_executable(arm_kinematics arm_kinematics.cpp)
+rosbuild_add_executable(arm_kinematics src/arm_kinematics.cpp)
+
+rosbuild_add_library(arm_kinematics_constraint_aware_lib src/arm_kinematics_constraint_aware.cpp src/arm_kinematics.cpp)
+rosbuild_add_executable(arm_kinematics_constraint_aware src/main.cpp)
+target_link_libraries(arm_kinematics_constraint_aware arm_kinematics_constraint_aware_lib)
\ No newline at end of file
