Woops, here it is. Sorry about that.
Cheers,
Ugo
On Mon, Feb 21, 2011 at 6:13 PM, Sachin Chitta <
sachinc@willowgarage.com>wrote:
> Can you attach the patch here and I will take a look.
>
> Thanks,
> Sachin
>
> On Mon, Feb 21, 2011 at 10:08 AM, Ugo Cupcic <ugo@shadowrobot.com> wrote:
>
>> Hi,
>>
>> As Kaijen and Gil suggested, I started working on adding collision
>> detection to the generic arm_kinematics pkg. (for the time being I just did
>> some reorganization, copy paste from pr2_ik_constraint_aware, changes to get
>> it to compile)
>>
>> If someones can help me get it working, it would be very nice. In case you
>> want to start right away, I attached a patch.
>>
>> It's not working (yet), but at least it compiles ;)
>>
>> Cheers,
>>
>> Ugo
>>
>> --
>> Ugo Cupcic | Shadow Robot Company | ugo@shadowrobot.com
>> Software Engineer | 251 Liverpool Road |
>> need a Hand? | London N1 1LX | +44 20 7700 2487
>> http://www.shadowrobot.com/hand/ @shadowrobot
>>
>>
>> _______________________________________________
>> ros-users mailing list
>> ros-users@code.ros.org
>> https://code.ros.org/mailman/listinfo/ros-users
>>
>>
>
>
> --
> Sachin Chitta
> Research Scientist
> Willow Garage
>
--
Ugo Cupcic | Shadow Robot Company |
ugo@shadowrobot.com
Software Engineer | 251 Liverpool Road |
need a Hand? | London N1 1LX | +44 20 7700 2487
http://www.shadowrobot.com/hand/ @shadowrobot
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