diff --git a/sawyer_gazebo/include/sawyer_gazebo/arm_kinematics_interface.h b/sawyer_gazebo/include/sawyer_gazebo/arm_kinematics_interface.h index 36b1a7b..98d2840 100644 --- a/sawyer_gazebo/include/sawyer_gazebo/arm_kinematics_interface.h +++ b/sawyer_gazebo/include/sawyer_gazebo/arm_kinematics_interface.h @@ -54,7 +54,7 @@ bool init(ros::NodeHandle& nh, std::string side); struct Kinematics { - KDL::Chain chain; + std::unique_ptr chain; std::vector joint_names; std::unique_ptr fk_pos_solver; std::unique_ptr fk_vel_solver; diff --git a/sawyer_gazebo/src/arm_kinematics_interface.cpp b/sawyer_gazebo/src/arm_kinematics_interface.cpp index c18b439..ba32c14 100644 --- a/sawyer_gazebo/src/arm_kinematics_interface.cpp +++ b/sawyer_gazebo/src/arm_kinematics_interface.cpp @@ -29,517 +29,517 @@ namespace sawyer_gazebo { -bool ArmKinematicsInterface::init(ros::NodeHandle& nh, std::string side) -{ - side_ = side; - if (!parseParams(nh)) + bool ArmKinematicsInterface::init(ros::NodeHandle &nh, std::string side) { - return false; - } + side_ = side; + if (!parseParams(nh)) + { + return false; + } - // Limits must be retrieved before any Kinematics object is created - joint_limits_ = retrieveJointLimits(); + // Limits must be retrieved before any Kinematics object is created + joint_limits_ = retrieveJointLimits(); - // Init Solvers to default endpoint - if (!createKinematicChain(tip_name_)) - { - return false; - } + // Init Solvers to default endpoint + if (!createKinematicChain(tip_name_)) + { + return false; + } // Init gravity solvers, if not the same frame as the regular tip - if (tip_name_ != gravity_tip_name_ && !createKinematicChain(gravity_tip_name_)) - { - return false; + if (tip_name_ != gravity_tip_name_ && !createKinematicChain(gravity_tip_name_)) + { + return false; + } + // Init Solvers to default hand camera + if (!createKinematicChain(camera_name_)) + { + return false; + } + gravity_torques_seq_ = 0; + endpoint_state_seq_ = 0; + gravity_torques_pub_ = nh.advertise( + "limb/" + side_ + "/gravity_compensation_torques", 1); + endpoint_state_pub_ = nh.advertise( + "limb/" + side_ + "/endpoint_state", 1); + tip_state_pub_ = nh.advertise( + "limb/" + side_ + "/tip_states", 1); + joint_state_sub_ = nh.subscribe("joint_states", 1, + &ArmKinematicsInterface::jointStateCallback, this); + joint_command_sub_ = nh.subscribe("limb/" + side_ + "/joint_command", 1, + &ArmKinematicsInterface::jointCommandCallback, this); + fk_service_ = nh.advertiseService( + "/ExternalTools/" + side_ + "/PositionKinematicsNode/FKService", + &ArmKinematicsInterface::servicePositionFK, this); + ik_service_ = nh.advertiseService( + "/ExternalTools/" + side_ + "/PositionKinematicsNode/IKService", + &ArmKinematicsInterface::servicePositionIK, this); + // Update at 100Hz + update_timer_ = nh.createTimer(100, &ArmKinematicsInterface::update, this); + joint_limits_pub_ = nh.advertise( + "joint_limits", 1, true); + joint_limits_pub_.publish(joint_limits_); + return true; } - // Init Solvers to default hand camera - if (!createKinematicChain(camera_name_)) + + intera_core_msgs::JointLimits ArmKinematicsInterface::retrieveJointLimits() { - return false; + auto joint_limits = intera_core_msgs::JointLimits(); + // Cycle through all tree joints, + for (const auto &kv : tree_.getSegments()) + { + auto jnt = kv.second.segment.getJoint(); + if (jnt.getTypeName() == "None" || jnt.getTypeName() == "Unknown") + continue; + auto joint_name = kv.second.segment.getJoint().getName(); + auto joint_limits_ptr = robot_model_.getJoint(joint_name)->limits; + // Save off any joint that has limits + if (joint_limits_ptr) + { + joint_limits.joint_names.push_back(joint_name); + joint_limits.position_lower.push_back(joint_limits_ptr->lower); + joint_limits.position_upper.push_back(joint_limits_ptr->upper); + joint_limits.velocity.push_back(joint_limits_ptr->velocity); + joint_limits.effort.push_back(joint_limits_ptr->effort); + // Acceleration grabbed from previously retrieved Parameter Server + if (acceleration_map_.find(joint_name) != acceleration_map_.end()) + { + joint_limits.accel.push_back(acceleration_map_[joint_name]); + } + else + { + ROS_INFO_NAMED("kinematics", "Unable to find Acceleration values for joint %s...", + joint_name.c_str()); + joint_limits.accel.push_back(8.0); + } + } + } + return joint_limits; } - gravity_torques_seq_ = 0; - endpoint_state_seq_ = 0; - gravity_torques_pub_ = nh.advertise( - "limb/"+side_+"/gravity_compensation_torques", 1); - endpoint_state_pub_ = nh.advertise( - "limb/"+side_+"/endpoint_state", 1); - tip_state_pub_ = nh.advertise( - "limb/"+side_+"/tip_states", 1); - joint_state_sub_ = nh.subscribe("joint_states", 1, - &ArmKinematicsInterface::jointStateCallback, this); - joint_command_sub_ = nh.subscribe("limb/"+side_+"/joint_command", 1, - &ArmKinematicsInterface::jointCommandCallback, this); - fk_service_ = nh.advertiseService( - "/ExternalTools/"+side_+"/PositionKinematicsNode/FKService", - &ArmKinematicsInterface::servicePositionFK, this); - ik_service_ = nh.advertiseService( - "/ExternalTools/"+side_+"/PositionKinematicsNode/IKService", - &ArmKinematicsInterface::servicePositionIK, this); - // Update at 100Hz - update_timer_ = nh.createTimer(100, &ArmKinematicsInterface::update, this); - joint_limits_pub_ = nh.advertise( - "joint_limits", 1, true); - joint_limits_pub_.publish(joint_limits_); - return true; -} -intera_core_msgs::JointLimits ArmKinematicsInterface::retrieveJointLimits() -{ - auto joint_limits = intera_core_msgs::JointLimits(); - // Cycle through all tree joints, - for (const auto& kv : tree_.getSegments()) + bool ArmKinematicsInterface::createKinematicChain(std::string tip_name) { - auto jnt = kv.second.segment.getJoint(); - if (jnt.getTypeName() == "None" || jnt.getTypeName() == "Unknown") - continue; - auto joint_name = kv.second.segment.getJoint().getName(); - auto joint_limits_ptr = robot_model_.getJoint(joint_name)->limits; - // Save off any joint that has limits - if (joint_limits_ptr) + if (kinematic_chain_map_.find(tip_name) != kinematic_chain_map_.end()) + { + ROS_WARN_NAMED("kinematics", "Kinematic chain from %s to %s already exists!", + root_name_.c_str(), tip_name.c_str()); + return false; + } + Kinematics kin; + kin.chain.reset(new KDL::Chain); + if (!tree_.getChain(root_name_, tip_name, *kin.chain)) + { + ROS_ERROR_NAMED("kinematics", "Couldn't find chain %s to %s", + root_name_.c_str(), tip_name.c_str()); + return false; + } + // Save off Joint Names + for (size_t seg_idx = 0; seg_idx < kin.chain->getNrOfSegments(); seg_idx++) { - joint_limits.joint_names.push_back(joint_name); - joint_limits.position_lower.push_back(joint_limits_ptr->lower); - joint_limits.position_upper.push_back(joint_limits_ptr->upper); - joint_limits.velocity.push_back(joint_limits_ptr->velocity); - joint_limits.effort.push_back(joint_limits_ptr->effort); - // Acceleration grabbed from previously retrieved Parameter Server - if (acceleration_map_.find(joint_name) != acceleration_map_.end()) + const auto &jnt = kin.chain->getSegment(seg_idx).getJoint(); + if (jnt.getTypeName() == "None" || jnt.getTypeName() == "Unknown") + continue; + kin.joint_names.push_back(kin.chain->getSegment(seg_idx).getJoint().getName()); + } + // Construct Solvers + kin.gravity_solver = std::make_unique(*kin.chain, KDL::Vector(0.0, 0.0, -9.8)); + kin.fk_pos_solver = std::make_unique(*kin.chain); + kin.fk_vel_solver = std::make_unique(*kin.chain); + auto num_jnts = kin.joint_names.size(); + KDL::JntArray q_min(num_jnts); + KDL::JntArray q_max(num_jnts); + KDL::JntArray v_max(num_jnts); + KDL::JntArray a_max(num_jnts); + for (size_t i = 0; i < num_jnts; i++) + { + auto joint_limit_idx = std::distance(joint_limits_.joint_names.begin(), + std::find(joint_limits_.joint_names.begin(), + joint_limits_.joint_names.end(), + kin.joint_names[i])); + if (joint_limit_idx < joint_limits_.joint_names.size()) { - joint_limits.accel.push_back(acceleration_map_[joint_name]); + q_min(i) = joint_limits_.position_lower[joint_limit_idx]; + q_max(i) = joint_limits_.position_upper[joint_limit_idx]; + v_max(i) = joint_limits_.velocity[joint_limit_idx]; + a_max(i) = joint_limits_.accel[joint_limit_idx]; } else { - ROS_INFO_NAMED("kinematics", "Unable to find Acceleration values for joint %s...", - joint_name.c_str()); - joint_limits.accel.push_back(8.0); + ROS_WARN_NAMED("kinematics", "Unable to find Joint Limits for joint %s", + kin.joint_names[i].c_str()); } } + kin.ik_solver = std::make_unique(*kin.chain, q_min, q_max, + v_max, a_max, kin.joint_names); + kin.ik_solver->setVelocitySolveType(sns_ik::SNS_FastOptimal); + kinematic_chain_map_.insert(std::make_pair(tip_name, std::move(kin))); + return true; } - return joint_limits; -} - -bool ArmKinematicsInterface::createKinematicChain(std::string tip_name) -{ - if(kinematic_chain_map_.find(tip_name) != kinematic_chain_map_.end()) - { - ROS_WARN_NAMED("kinematics", "Kinematic chain from %s to %s already exists!", - root_name_.c_str(), tip_name.c_str()); - return false; - } - Kinematics kin; - if (!tree_.getChain(root_name_, tip_name, kin.chain)) - { - ROS_ERROR_NAMED("kinematics", "Couldn't find chain %s to %s", - root_name_.c_str(), tip_name.c_str()); - return false; - } - // Save off Joint Names - for (size_t seg_idx = 0; seg_idx < kin.chain.getNrOfSegments(); seg_idx++) + bool ArmKinematicsInterface::parseParams(const ros::NodeHandle &nh) { - const auto& jnt = kin.chain.getSegment(seg_idx).getJoint(); - if (jnt.getTypeName() == "None" || jnt.getTypeName() == "Unknown") - continue; - kin.joint_names.push_back(kin.chain.getSegment(seg_idx).getJoint().getName()); - } - // Construct Solvers - kin.gravity_solver = std::make_unique(kin.chain, KDL::Vector(0.0, 0.0, -9.8)); - kin.fk_pos_solver = std::make_unique(kin.chain); - kin.fk_vel_solver = std::make_unique(kin.chain); - auto num_jnts = kin.joint_names.size(); - KDL::JntArray q_min(num_jnts); - KDL::JntArray q_max(num_jnts); - KDL::JntArray v_max(num_jnts); - KDL::JntArray a_max(num_jnts); - for (size_t i = 0; i < num_jnts; i++) - { - auto joint_limit_idx = std::distance(joint_limits_.joint_names.begin(), - std::find(joint_limits_.joint_names.begin(), - joint_limits_.joint_names.end(), - kin.joint_names[i])); - if (joint_limit_idx < joint_limits_.joint_names.size()) + std::string urdf_xml; + ROS_DEBUG_NAMED("kinematics", "Reading xml file from parameter server"); + if (!nh.getParam("/robot_description", urdf_xml)) + { + ROS_FATAL_NAMED("kinematics", + "Could not load the xml from parameter server: %s", urdf_xml.c_str()); + return false; + } + if (!nh.getParam("limb/" + side_ + "/root_name", root_name_)) + { + ROS_FATAL_NAMED("kinematics", + "No root name for Kinematic Chain found on parameter server"); + return false; + } + if (!nh.getParam("limb/" + side_ + "/tip_name", tip_name_)) + { + ROS_FATAL_NAMED("kinematics", + "No tip name for Kinematic Chain found on parameter server"); + return false; + } + if (!nh.getParam("limb/" + side_ + "/gravity_tip_name", gravity_tip_name_)) + { + ROS_FATAL_NAMED("kinematics", + "No tip name for Kinematic Chain found on parameter server"); + return false; + } + if (!nh.getParam("limb/" + side_ + "/camera_name", camera_name_)) + { + ROS_FATAL_NAMED("kinematics", + "No hand camera name found on parameter server"); + return false; + } + robot_model_.initString(urdf_xml); + if (!kdl_parser::treeFromUrdfModel(robot_model_, tree_)) { - q_min(i) = joint_limits_.position_lower[joint_limit_idx]; - q_max(i) = joint_limits_.position_upper[joint_limit_idx]; - v_max(i) = joint_limits_.velocity[joint_limit_idx]; - a_max(i) = joint_limits_.accel[joint_limit_idx]; + ROS_FATAL_NAMED("kinematics", + "Failed to extract kdl tree from xml robot description."); + return false; } - else + if (!nh.getParam("/robot_config/joint_config/joint_acceleration_limit", acceleration_map_)) { - ROS_WARN_NAMED("kinematics", "Unable to find Joint Limits for joint %s", - kin.joint_names[i].c_str()); + ROS_FATAL_NAMED("kinematics", + "Failed to find joint_acceleration_limit on the param server."); + return false; } + return true; } - kin.ik_solver = std::make_unique(kin.chain, q_min, q_max, - v_max, a_max, kin.joint_names); - kin.ik_solver->setVelocitySolveType(sns_ik::SNS_FastOptimal); - kinematic_chain_map_.insert(std::make_pair(tip_name, std::move(kin))); - return true; -} -bool ArmKinematicsInterface::parseParams(const ros::NodeHandle& nh) -{ - std::string urdf_xml; - ROS_DEBUG_NAMED("kinematics", "Reading xml file from parameter server"); - if (!nh.getParam("/robot_description", urdf_xml)) - { - ROS_FATAL_NAMED("kinematics", - "Could not load the xml from parameter server: %s", urdf_xml.c_str()); - return false; - } - if (!nh.getParam("limb/"+side_+"/root_name", root_name_)) - { - ROS_FATAL_NAMED("kinematics", - "No root name for Kinematic Chain found on parameter server"); - return false; - } - if (!nh.getParam("limb/"+side_+"/tip_name", tip_name_)) - { - ROS_FATAL_NAMED("kinematics", - "No tip name for Kinematic Chain found on parameter server"); - return false; - } - if (!nh.getParam("limb/"+side_+"/gravity_tip_name", gravity_tip_name_)) + void ArmKinematicsInterface::update(const ros::TimerEvent &e) { - ROS_FATAL_NAMED("kinematics", - "No tip name for Kinematic Chain found on parameter server"); - return false; + publishEndpointState(); + publishGravityTorques(); } - if (!nh.getParam("limb/"+side_+"/camera_name", camera_name_)) + + void ArmKinematicsInterface::jointStateCallback(const sensor_msgs::JointStateConstPtr &msg) { - ROS_FATAL_NAMED("kinematics", - "No hand camera name found on parameter server"); - return false; + auto joint_state = std::make_shared(*msg); + joint_state_buffer_.set(joint_state); } - robot_model_.initString(urdf_xml); - if (!kdl_parser::treeFromUrdfModel(robot_model_, tree_)) - { - ROS_FATAL_NAMED("kinematics", - "Failed to extract kdl tree from xml robot description."); - return false; - } - if (!nh.getParam("/robot_config/joint_config/joint_acceleration_limit", acceleration_map_)) + + void ArmKinematicsInterface::jointCommandCallback(const intera_core_msgs::JointCommandConstPtr &msg) { - ROS_FATAL_NAMED("kinematics", - "Failed to find joint_acceleration_limit on the param server."); - return false; + auto joint_command = std::make_shared(*msg); + joint_command_buffer_.set(joint_command); } - return true; -} - -void ArmKinematicsInterface::update(const ros::TimerEvent& e) -{ - publishEndpointState(); - publishGravityTorques(); -} - -void ArmKinematicsInterface::jointStateCallback(const sensor_msgs::JointStateConstPtr& msg) -{ - auto joint_state = std::make_shared(*msg); - joint_state_buffer_.set(joint_state); -} -void ArmKinematicsInterface::jointCommandCallback(const intera_core_msgs::JointCommandConstPtr& msg) -{ - auto joint_command = std::make_shared(*msg); - joint_command_buffer_.set(joint_command); -} - -void ArmKinematicsInterface::publishGravityTorques() -{ - std::shared_ptr joint_state; - joint_state_buffer_.get(joint_state); - if (joint_state) + void ArmKinematicsInterface::publishGravityTorques() { - intera_core_msgs::SEAJointState gravity_torques; - auto num_jnts = kinematic_chain_map_[tip_name_].chain.getNrOfJoints(); - KDL::JntArray jnt_pos(num_jnts), jnt_vel(num_jnts), jnt_eff(num_jnts), jnt_accelerations(num_jnts); - KDL::JntArray jnt_gravity_model(num_jnts), jnt_gravity_only(num_jnts), jnt_zero(num_jnts); - jointStateToKDL(*joint_state, kinematic_chain_map_[tip_name_], jnt_pos, jnt_vel, jnt_eff); - - std::shared_ptr joint_command; - joint_command_buffer_.get(joint_command); - if (joint_command) + std::shared_ptr joint_state; + joint_state_buffer_.get(joint_state); + if (joint_state) { - jointCommandToGravityMsg(kinematic_chain_map_[tip_name_].joint_names, *joint_command, gravity_torques); - for (auto i = 0; i < joint_command->acceleration.size(); i++) - jnt_accelerations(i) = joint_command->acceleration[i]; - } + intera_core_msgs::SEAJointState gravity_torques; + auto num_jnts = kinematic_chain_map_[tip_name_].chain->getNrOfJoints(); + KDL::JntArray jnt_pos(num_jnts), jnt_vel(num_jnts), jnt_eff(num_jnts), jnt_accelerations(num_jnts); + KDL::JntArray jnt_gravity_model(num_jnts), jnt_gravity_only(num_jnts), jnt_zero(num_jnts); + jointStateToKDL(*joint_state, kinematic_chain_map_[tip_name_], jnt_pos, jnt_vel, jnt_eff); - gravity_torques.name = kinematic_chain_map_[tip_name_].joint_names; - gravity_torques.actual_position.resize(num_jnts); - gravity_torques.actual_velocity.resize(num_jnts); - gravity_torques.actual_effort.resize(num_jnts); - gravity_torques.gravity_model_effort.resize(num_jnts); - gravity_torques.gravity_only.resize(num_jnts); - gravity_torques.interaction_torque.resize(num_jnts); - gravity_torques.hysteresis_model_effort.resize(num_jnts); - gravity_torques.crosstalk_model_effort.resize(num_jnts); + std::shared_ptr joint_command; + joint_command_buffer_.get(joint_command); + if (joint_command) + { + jointCommandToGravityMsg(kinematic_chain_map_[tip_name_].joint_names, *joint_command, gravity_torques); + for (auto i = 0; i < joint_command->acceleration.size(); i++) + jnt_accelerations(i) = joint_command->acceleration[i]; + } - computeGravity(kinematic_chain_map_[tip_name_], jnt_pos, jnt_vel, jnt_accelerations, jnt_gravity_model); - computeGravity(kinematic_chain_map_[tip_name_], jnt_pos, jnt_zero, jnt_zero, jnt_gravity_only); - auto clamp_limit = [](double i, double limit) { return std::max(std::min(limit, i), -limit); }; - for (size_t jnt_idx = 0; jnt_idx < num_jnts; jnt_idx++) - { - gravity_torques.actual_position[jnt_idx] = jnt_pos(jnt_idx); - gravity_torques.actual_velocity[jnt_idx] = jnt_vel(jnt_idx); - gravity_torques.actual_effort[jnt_idx] = jnt_eff(jnt_idx); - auto torque_limit = robot_model_.getJoint(gravity_torques.name[jnt_idx])->limits->effort; - gravity_torques.gravity_model_effort[jnt_idx] = clamp_limit(jnt_gravity_model(jnt_idx), torque_limit); - gravity_torques.gravity_only[jnt_idx] = clamp_limit(jnt_gravity_only(jnt_idx), torque_limit); + gravity_torques.name = kinematic_chain_map_[tip_name_].joint_names; + gravity_torques.actual_position.resize(num_jnts); + gravity_torques.actual_velocity.resize(num_jnts); + gravity_torques.actual_effort.resize(num_jnts); + gravity_torques.gravity_model_effort.resize(num_jnts); + gravity_torques.gravity_only.resize(num_jnts); + gravity_torques.interaction_torque.resize(num_jnts); + gravity_torques.hysteresis_model_effort.resize(num_jnts); + gravity_torques.crosstalk_model_effort.resize(num_jnts); + + computeGravity(kinematic_chain_map_[tip_name_], jnt_pos, jnt_vel, jnt_accelerations, jnt_gravity_model); + computeGravity(kinematic_chain_map_[tip_name_], jnt_pos, jnt_zero, jnt_zero, jnt_gravity_only); + auto clamp_limit = [](double i, double limit) + { return std::max(std::min(limit, i), -limit); }; + for (size_t jnt_idx = 0; jnt_idx < num_jnts; jnt_idx++) + { + gravity_torques.actual_position[jnt_idx] = jnt_pos(jnt_idx); + gravity_torques.actual_velocity[jnt_idx] = jnt_vel(jnt_idx); + gravity_torques.actual_effort[jnt_idx] = jnt_eff(jnt_idx); + auto torque_limit = robot_model_.getJoint(gravity_torques.name[jnt_idx])->limits->effort; + gravity_torques.gravity_model_effort[jnt_idx] = clamp_limit(jnt_gravity_model(jnt_idx), torque_limit); + gravity_torques.gravity_only[jnt_idx] = clamp_limit(jnt_gravity_only(jnt_idx), torque_limit); + } + gravity_torques.header.frame_id = root_name_; + gravity_torques_seq_++; + gravity_torques.header.seq = gravity_torques_seq_; + gravity_torques.header.stamp = ros::Time::now(); + gravity_torques_pub_.publish(gravity_torques); } - gravity_torques.header.frame_id = root_name_; - gravity_torques_seq_++; - gravity_torques.header.seq = gravity_torques_seq_; - gravity_torques.header.stamp = ros::Time::now(); - gravity_torques_pub_.publish(gravity_torques); } -} -void ArmKinematicsInterface::jointCommandToGravityMsg(const std::vector& joint_names, - const intera_core_msgs::JointCommand& command_msg, - intera_core_msgs::SEAJointState& gravity_msg) -{ - auto num_jnts = joint_names.size(); - gravity_msg.commanded_position.resize(num_jnts); - gravity_msg.commanded_velocity.resize(num_jnts); - gravity_msg.commanded_acceleration.resize(num_jnts); - gravity_msg.commanded_effort.resize(num_jnts); - bool use_position = (command_msg.position.size() == command_msg.names.size() && - (command_msg.mode == command_msg.POSITION_MODE || - command_msg.mode == command_msg.TRAJECTORY_MODE)); - bool use_velocity = (command_msg.velocity.size() == command_msg.names.size() && - (command_msg.mode == command_msg.VELOCITY_MODE || - command_msg.mode == command_msg.TRAJECTORY_MODE)); - bool use_torque = (command_msg.effort.size() == command_msg.names.size() && - command_msg.mode == command_msg.TORQUE_MODE); - bool use_acceleration = (command_msg.acceleration.size() == command_msg.names.size() && - command_msg.mode == command_msg.TRAJECTORY_MODE); - for (auto i = 0; i < num_jnts; i++) + void ArmKinematicsInterface::jointCommandToGravityMsg(const std::vector &joint_names, + const intera_core_msgs::JointCommand &command_msg, + intera_core_msgs::SEAJointState &gravity_msg) { - for (auto j = 0; j < command_msg.names.size(); j++) + auto num_jnts = joint_names.size(); + gravity_msg.commanded_position.resize(num_jnts); + gravity_msg.commanded_velocity.resize(num_jnts); + gravity_msg.commanded_acceleration.resize(num_jnts); + gravity_msg.commanded_effort.resize(num_jnts); + bool use_position = (command_msg.position.size() == command_msg.names.size() && + (command_msg.mode == command_msg.POSITION_MODE || + command_msg.mode == command_msg.TRAJECTORY_MODE)); + bool use_velocity = (command_msg.velocity.size() == command_msg.names.size() && + (command_msg.mode == command_msg.VELOCITY_MODE || + command_msg.mode == command_msg.TRAJECTORY_MODE)); + bool use_torque = (command_msg.effort.size() == command_msg.names.size() && + command_msg.mode == command_msg.TORQUE_MODE); + bool use_acceleration = (command_msg.acceleration.size() == command_msg.names.size() && + command_msg.mode == command_msg.TRAJECTORY_MODE); + for (auto i = 0; i < num_jnts; i++) { - if (command_msg.names[j] == joint_names[i]) + for (auto j = 0; j < command_msg.names.size(); j++) { - if (use_position) + if (command_msg.names[j] == joint_names[i]) + { + if (use_position) gravity_msg.commanded_position[i] = command_msg.position[j]; - if (use_velocity) - gravity_msg.commanded_velocity[i] = command_msg.velocity[j]; - if (use_torque) + if (use_velocity) + gravity_msg.commanded_velocity[i] = command_msg.velocity[j]; + if (use_torque) gravity_msg.commanded_effort[i] = command_msg.effort[j]; - if (use_acceleration) + if (use_acceleration) gravity_msg.commanded_acceleration[i] = command_msg.acceleration[j]; + } } } } -} -void ArmKinematicsInterface::jointStateToKDL(const sensor_msgs::JointState& joint_state, const Kinematics& kin, - KDL::JntArray& jnt_pos, KDL::JntArray& jnt_vel, KDL::JntArray& jnt_eff) -{ - auto num_jnts = kin.joint_names.size(); - auto num_msg = joint_state.name.size(); - // Check to see if there are any values before allocating space - if (!joint_state.position.empty()) - { - jnt_pos.resize(num_jnts); - KDL::SetToZero(jnt_pos); - } - if (!joint_state.velocity.empty()) - { - jnt_vel.resize(num_jnts); - KDL::SetToZero(jnt_vel); - } - if (!joint_state.effort.empty()) - { - jnt_eff.resize(num_jnts); - KDL::SetToZero(jnt_eff); - } - for(size_t jnt_idx = 0; jnt_idx < num_jnts; jnt_idx++) + void ArmKinematicsInterface::jointStateToKDL(const sensor_msgs::JointState &joint_state, const Kinematics &kin, + KDL::JntArray &jnt_pos, KDL::JntArray &jnt_vel, KDL::JntArray &jnt_eff) { - for (size_t msg_idx = 0; msg_idx < num_msg; msg_idx++) + auto num_jnts = kin.joint_names.size(); + auto num_msg = joint_state.name.size(); + // Check to see if there are any values before allocating space + if (!joint_state.position.empty()) { - if (joint_state.name[msg_idx] == kin.joint_names[jnt_idx]) + jnt_pos.resize(num_jnts); + KDL::SetToZero(jnt_pos); + } + if (!joint_state.velocity.empty()) + { + jnt_vel.resize(num_jnts); + KDL::SetToZero(jnt_vel); + } + if (!joint_state.effort.empty()) + { + jnt_eff.resize(num_jnts); + KDL::SetToZero(jnt_eff); + } + for (size_t jnt_idx = 0; jnt_idx < num_jnts; jnt_idx++) + { + for (size_t msg_idx = 0; msg_idx < num_msg; msg_idx++) { - if (msg_idx < joint_state.position.size()) - jnt_pos(jnt_idx) = joint_state.position[msg_idx]; - if (msg_idx < joint_state.velocity.size()) - jnt_vel(jnt_idx) = joint_state.velocity[msg_idx]; - if (msg_idx < joint_state.effort.size()) - jnt_eff(jnt_idx) = joint_state.effort[msg_idx]; - break; + if (joint_state.name[msg_idx] == kin.joint_names[jnt_idx]) + { + if (msg_idx < joint_state.position.size()) + jnt_pos(jnt_idx) = joint_state.position[msg_idx]; + if (msg_idx < joint_state.velocity.size()) + jnt_vel(jnt_idx) = joint_state.velocity[msg_idx]; + if (msg_idx < joint_state.effort.size()) + jnt_eff(jnt_idx) = joint_state.effort[msg_idx]; + break; + } } } } -} -bool ArmKinematicsInterface::servicePositionIK(intera_core_msgs::SolvePositionIK::Request& req, - intera_core_msgs::SolvePositionIK::Response& res) -{ - auto req_size = req.pose_stamp.size(); - res.joints.resize(req_size, sensor_msgs::JointState()); - res.result_type.resize(req_size, res.IK_FAILED); - for (size_t i = 0; i < req_size; i++) + bool ArmKinematicsInterface::servicePositionIK(intera_core_msgs::SolvePositionIK::Request &req, + intera_core_msgs::SolvePositionIK::Response &res) { - res.joints[i].header.stamp = ros::Time::now(); - // Try to find the kinematic chain, if not, create it - auto kinematic_chain_it = kinematic_chain_map_.find(req.tip_names[i]); - if (!req.tip_names.size() || kinematic_chain_it == kinematic_chain_map_.end()) + auto req_size = req.pose_stamp.size(); + res.joints.resize(req_size, sensor_msgs::JointState()); + res.result_type.resize(req_size, res.IK_FAILED); + for (size_t i = 0; i < req_size; i++) { - if (!createKinematicChain(req.tip_names[i])) + res.joints[i].header.stamp = ros::Time::now(); + // Try to find the kinematic chain, if not, create it + auto kinematic_chain_it = kinematic_chain_map_.find(req.tip_names[i]); + if (!req.tip_names.size() || kinematic_chain_it == kinematic_chain_map_.end()) { - // If chain is not found and cannot be created, leave isValid false and move on - res.result_type[i] = res.IK_ENDPOINT_DOES_NOT_EXIST; - continue; + if (!createKinematicChain(req.tip_names[i])) + { + // If chain is not found and cannot be created, leave isValid false and move on + res.result_type[i] = res.IK_ENDPOINT_DOES_NOT_EXIST; + continue; + } + else + { + // Update the iterator now that the chain is created + kinematic_chain_it = kinematic_chain_map_.find(req.tip_names[i]); + } } - else + auto num_jnts = kinematic_chain_it->second.chain->getNrOfJoints(); + KDL::JntArray jnt_seed(num_jnts); + KDL::JntArray jnt_result(num_jnts); + // Nullspace + KDL::JntArray jnt_nullspace_bias(0); // Size Zero tells SNS not to use the nullspace + if (req.use_nullspace_goal.size() > i && req.use_nullspace_goal[i]) { - // Update the iterator now that the chain is created - kinematic_chain_it = kinematic_chain_map_.find(req.tip_names[i]); + jointStatePositionToKDL(req.nullspace_goal[i], + kinematic_chain_it->second, jnt_nullspace_bias); + if (req.nullspace_gain.size() > i) + kinematic_chain_it->second.ik_solver->setNullspaceGain(req.nullspace_gain[i]); } - } - auto num_jnts = kinematic_chain_it->second.chain.getNrOfJoints(); - KDL::JntArray jnt_seed(num_jnts); - KDL::JntArray jnt_result(num_jnts); - // Nullspace - KDL::JntArray jnt_nullspace_bias(0); // Size Zero tells SNS not to use the nullspace - if (req.use_nullspace_goal.size() > i && req.use_nullspace_goal[i]){ - jointStatePositionToKDL(req.nullspace_goal[i], - kinematic_chain_it->second, jnt_nullspace_bias); - if(req.nullspace_gain.size() > i) - kinematic_chain_it->second.ik_solver->setNullspaceGain(req.nullspace_gain[i]); - } - if (req.seed_mode == req.SEED_USER || req.seed_mode == req.SEED_AUTO) - { - if (req.seed_angles.size() > i && - req.seed_angles[i].name.size() && req.seed_angles[i].position.size() && - req.seed_angles[i].name.size() == req.seed_angles[i].position.size()) + if (req.seed_mode == req.SEED_USER || req.seed_mode == req.SEED_AUTO) { - - jointStatePositionToKDL(req.seed_angles[i], - kinematic_chain_it->second, jnt_seed); - if (computePositionIK(kinematic_chain_it->second, - req.pose_stamp[i].pose, jnt_nullspace_bias, jnt_seed, jnt_result)) + if (req.seed_angles.size() > i && + req.seed_angles[i].name.size() && req.seed_angles[i].position.size() && + req.seed_angles[i].name.size() == req.seed_angles[i].position.size()) { - res.result_type[i] = req.SEED_USER; - } - /* if (jointsInCollision(kinematic_chain_map_[req.tip_names[i]], jnt_result)) + + jointStatePositionToKDL(req.seed_angles[i], + kinematic_chain_it->second, jnt_seed); + if (computePositionIK(kinematic_chain_it->second, + req.pose_stamp[i].pose, jnt_nullspace_bias, jnt_seed, jnt_result)) + { + res.result_type[i] = req.SEED_USER; + } + /* if (jointsInCollision(kinematic_chain_map_[req.tip_names[i]], jnt_result)) { res.result_type[i] = res.IK_IN_COLLISION; } TODO(imcmahon) Utilize FCL for collision checking */ + } } - } - if (req.seed_mode == req.SEED_CURRENT || - (req.seed_mode == req.SEED_AUTO && - (res.result_type[i] == res.IK_FAILED || res.result_type[i] == res.IK_IN_COLLISION) - ) - ) - { + if (req.seed_mode == req.SEED_CURRENT || + (req.seed_mode == req.SEED_AUTO && + (res.result_type[i] == res.IK_FAILED || res.result_type[i] == res.IK_IN_COLLISION))) + { std::shared_ptr joint_state; joint_state_buffer_.get(joint_state); if (joint_state.get()) { jointStatePositionToKDL(*joint_state.get(), - kinematic_chain_it->second, jnt_seed); + kinematic_chain_it->second, jnt_seed); if (computePositionIK(kinematic_chain_it->second, - req.pose_stamp[i].pose, jnt_nullspace_bias, jnt_seed, jnt_result)) + req.pose_stamp[i].pose, jnt_nullspace_bias, jnt_seed, jnt_result)) { res.result_type[i] = req.SEED_CURRENT; } - /* if (jointsInCollision(kinematic_chain_map_[req.tip_names[i]], jnt_result)) + /* if (jointsInCollision(kinematic_chain_map_[req.tip_names[i]], jnt_result)) { res.result_type[i] = res.IK_IN_COLLISION; } TODO(imcmahon) Utilize FCL for collision checking */ - } - } - // Reset Nullspace Gain - kinematic_chain_it->second.ik_solver->setNullspaceGain(1.0); - // Result - res.joints[i].name = kinematic_chain_it->second.joint_names; - res.joints[i].position.resize(num_jnts); - for(size_t j = 0; j < num_jnts; j++){ - res.joints[i].position[j] = jnt_result(j); + } + } + // Reset Nullspace Gain + kinematic_chain_it->second.ik_solver->setNullspaceGain(1.0); + // Result + res.joints[i].name = kinematic_chain_it->second.joint_names; + res.joints[i].position.resize(num_jnts); + for (size_t j = 0; j < num_jnts; j++) + { + res.joints[i].position[j] = jnt_result(j); + } } + return true; } - return true; -} -bool ArmKinematicsInterface::servicePositionFK(intera_core_msgs::SolvePositionFK::Request& req, - intera_core_msgs::SolvePositionFK::Response& res) -{ - auto req_size = req.configuration.size(); - res.inCollision.resize(req_size, false); - res.isValid.resize(req_size, false); - res.pose_stamp.resize(req_size, geometry_msgs::PoseStamped()); - for (size_t i = 0; i < req_size; i++) + bool ArmKinematicsInterface::servicePositionFK(intera_core_msgs::SolvePositionFK::Request &req, + intera_core_msgs::SolvePositionFK::Response &res) { - res.pose_stamp[i].header.stamp = ros::Time::now(); - // Try to find the kinematic chain, if not, create it - if (!req.tip_names.size() || - (kinematic_chain_map_.find(req.tip_names[i]) == kinematic_chain_map_.end() && - !createKinematicChain(req.tip_names[i]))) + auto req_size = req.configuration.size(); + res.inCollision.resize(req_size, false); + res.isValid.resize(req_size, false); + res.pose_stamp.resize(req_size, geometry_msgs::PoseStamped()); + for (size_t i = 0; i < req_size; i++) { + res.pose_stamp[i].header.stamp = ros::Time::now(); + // Try to find the kinematic chain, if not, create it + if (!req.tip_names.size() || + (kinematic_chain_map_.find(req.tip_names[i]) == kinematic_chain_map_.end() && + !createKinematicChain(req.tip_names[i]))) + { // If chain is not found and cannot be created, leave isValid false and move on continue; + } + KDL::JntArray jnt_pos; + jointStatePositionToKDL(req.configuration[i], kinematic_chain_map_[req.tip_names[i]], jnt_pos); + if (computePositionFK(kinematic_chain_map_[req.tip_names[i]], jnt_pos, res.pose_stamp[i].pose)) + { + res.isValid[i] = true; + } } - KDL::JntArray jnt_pos; - jointStatePositionToKDL(req.configuration[i], kinematic_chain_map_[req.tip_names[i]], jnt_pos); - if (computePositionFK(kinematic_chain_map_[req.tip_names[i]], jnt_pos, res.pose_stamp[i].pose)) - { - res.isValid[i] = true; - } + return true; } - return true; -} -bool ArmKinematicsInterface::computeGravity(const Kinematics& kin, - const KDL::JntArray& jnt_pos, - const KDL::JntArray& jnt_vel, - const KDL::JntArray& jnt_accel, - KDL::JntArray& jnt_torques) -{ - std::vector f_ext(kin.chain.getNrOfSegments(), KDL::Wrench::Zero()); - jnt_torques.resize(kin.chain.getNrOfJoints()); - return !(kin.gravity_solver->CartToJnt(jnt_pos, jnt_vel, jnt_accel, f_ext, jnt_torques) < 0); -} - -bool ArmKinematicsInterface::computePositionFK(const Kinematics& kin, - const KDL::JntArray& jnt_pos, - geometry_msgs::Pose& result) -{ - KDL::Frame p_out; - if (kin.fk_pos_solver->JntToCart(jnt_pos, p_out, kin.chain.getNrOfSegments()) < 0) + bool ArmKinematicsInterface::computeGravity(const Kinematics &kin, + const KDL::JntArray &jnt_pos, + const KDL::JntArray &jnt_vel, + const KDL::JntArray &jnt_accel, + KDL::JntArray &jnt_torques) { - return false; + std::vector f_ext(kin.chain->getNrOfSegments(), KDL::Wrench::Zero()); + jnt_torques.resize(kin.chain->getNrOfJoints()); + return !(kin.gravity_solver->CartToJnt(jnt_pos, jnt_vel, jnt_accel, f_ext, jnt_torques) < 0); } - tf::poseKDLToMsg(p_out, result); - return true; -} + bool ArmKinematicsInterface::computePositionFK(const Kinematics &kin, + const KDL::JntArray &jnt_pos, + geometry_msgs::Pose &result) + { + KDL::Frame p_out; + if (kin.fk_pos_solver->JntToCart(jnt_pos, p_out, kin.chain->getNrOfSegments()) < 0) + { + return false; + } + tf::poseKDLToMsg(p_out, result); + return true; + } -bool ArmKinematicsInterface::computePositionIK(const Kinematics& kin, - const geometry_msgs::Pose& cart_pose, - const KDL::JntArray& jnt_nullspace_bias, - const KDL::JntArray& jnt_seed, KDL::JntArray& result) -{ - KDL::Frame pose_kdl; - tf::poseMsgToKDL(cart_pose, pose_kdl); - return !(kin.ik_solver->CartToJnt(jnt_seed, pose_kdl, jnt_nullspace_bias, result) < 0); -} + bool ArmKinematicsInterface::computePositionIK(const Kinematics &kin, + const geometry_msgs::Pose &cart_pose, + const KDL::JntArray &jnt_nullspace_bias, + const KDL::JntArray &jnt_seed, KDL::JntArray &result) + { + KDL::Frame pose_kdl; + tf::poseMsgToKDL(cart_pose, pose_kdl); + return !(kin.ik_solver->CartToJnt(jnt_seed, pose_kdl, jnt_nullspace_bias, result) < 0); + } -bool ArmKinematicsInterface::computeVelocityFK(const Kinematics& kin, - const KDL::JntArrayVel& jnt_vel, - geometry_msgs::Twist& result) -{ - KDL::FrameVel v_out; - if (kin.fk_vel_solver->JntToCart(jnt_vel, v_out, kin.chain.getNrOfSegments()) < 0) + bool ArmKinematicsInterface::computeVelocityFK(const Kinematics &kin, + const KDL::JntArrayVel &jnt_vel, + geometry_msgs::Twist &result) { - return false; + KDL::FrameVel v_out; + if (kin.fk_vel_solver->JntToCart(jnt_vel, v_out, kin.chain->getNrOfSegments()) < 0) + { + return false; + } + tf::twistKDLToMsg(v_out.GetTwist(), result); + return true; } - tf::twistKDLToMsg(v_out.GetTwist(), result); - return true; -} -/* TODO(imcmahon): once ChainFDSolverTau is upstreamed + /* TODO(imcmahon): once ChainFDSolverTau is upstreamed bool ArmKinematicsInterface::computeEffortFK(const Kinematics& kin, const KDL::JntArray& jnt_pos, const KDL::JntArray& jnt_eff, @@ -554,46 +554,46 @@ bool ArmKinematicsInterface::computeEffortFK(const Kinematics& kin, return true; } */ -void ArmKinematicsInterface::publishEndpointState() -{ - std::shared_ptr joint_state; - joint_state_buffer_.get(joint_state); - if (joint_state.get()) + void ArmKinematicsInterface::publishEndpointState() { - intera_core_msgs::EndpointStates endpoint_states; - for(const auto& chain : kinematic_chain_map_) + std::shared_ptr joint_state; + joint_state_buffer_.get(joint_state); + if (joint_state.get()) { - intera_core_msgs::EndpointState endpoint_state; - endpoint_state.valid = true; - KDL::JntArray jnt_pos, jnt_vel, jnt_eff; - jointStateToKDL(*joint_state.get(), chain.second, jnt_pos, jnt_vel, jnt_eff); - if (!computePositionFK(chain.second, jnt_pos, endpoint_state.pose)) - { - endpoint_state.valid &= false; - } - KDL::JntArrayVel jnt_array_vel(jnt_pos, jnt_vel); - if (!computeVelocityFK(chain.second, jnt_array_vel, endpoint_state.twist)) + intera_core_msgs::EndpointStates endpoint_states; + for (const auto &chain : kinematic_chain_map_) { - endpoint_state.valid &= false; - } - /* TODO(imcmahon) once ChainFDSolverTau is upstreamed + intera_core_msgs::EndpointState endpoint_state; + endpoint_state.valid = true; + KDL::JntArray jnt_pos, jnt_vel, jnt_eff; + jointStateToKDL(*joint_state.get(), chain.second, jnt_pos, jnt_vel, jnt_eff); + if (!computePositionFK(chain.second, jnt_pos, endpoint_state.pose)) + { + endpoint_state.valid &= false; + } + KDL::JntArrayVel jnt_array_vel(jnt_pos, jnt_vel); + if (!computeVelocityFK(chain.second, jnt_array_vel, endpoint_state.twist)) + { + endpoint_state.valid &= false; + } + /* TODO(imcmahon) once ChainFDSolverTau is upstreamed if(!computeEffortFK(kinematic_chain_map_[tip_name_], jnt_pos, jnt_eff, endpoint_state.wrench)){ endpoint_state.valid &= false; } */ - endpoint_states.names.push_back(chain.first); - endpoint_states.states.push_back(endpoint_state); - if(chain.first == tip_name_) - { - endpoint_state.header.frame_id = root_name_; - endpoint_state.header.stamp = ros::Time::now(); - endpoint_state_pub_.publish(endpoint_state); + endpoint_states.names.push_back(chain.first); + endpoint_states.states.push_back(endpoint_state); + if (chain.first == tip_name_) + { + endpoint_state.header.frame_id = root_name_; + endpoint_state.header.stamp = ros::Time::now(); + endpoint_state_pub_.publish(endpoint_state); + } } + endpoint_states.header.frame_id = root_name_; + endpoint_states.header.stamp = ros::Time::now(); + tip_state_pub_.publish(endpoint_states); } - endpoint_states.header.frame_id = root_name_; - endpoint_states.header.stamp = ros::Time::now(); - tip_state_pub_.publish(endpoint_states); } -} -} // namespace sawyer_gazebo +} // namespace sawyer_gazebo diff --git a/sawyer_gazebo/src/head_interface.cpp b/sawyer_gazebo/src/head_interface.cpp index d39fbc1..98bf13f 100644 --- a/sawyer_gazebo/src/head_interface.cpp +++ b/sawyer_gazebo/src/head_interface.cpp @@ -68,7 +68,7 @@ bool HeadInterface::startHead(const ros::NodeHandle& nh, boost::shared_ptr(); try { - cv_ptr->image = cv::imread(img_path, CV_LOAD_IMAGE_UNCHANGED); + cv_ptr->image = cv::imread(img_path, cv::IMREAD_UNCHANGED); if (cv_ptr->image.data) { cv_ptr->encoding = sensor_msgs::image_encodings::BGR8; diff --git a/sawyer_sim_controllers/CMakeLists.txt b/sawyer_sim_controllers/CMakeLists.txt index 9370dbb..63c20cd 100644 --- a/sawyer_sim_controllers/CMakeLists.txt +++ b/sawyer_sim_controllers/CMakeLists.txt @@ -44,6 +44,7 @@ add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} + yaml-cpp ) install(TARGETS ${PROJECT_NAME} diff --git a/sawyer_sim_examples/package.xml b/sawyer_sim_examples/package.xml index ec128bf..9439f4c 100644 --- a/sawyer_sim_examples/package.xml +++ b/sawyer_sim_examples/package.xml @@ -31,6 +31,7 @@ sawyer_gazebo gazebo_ros gazebo_msgs + tf diff --git a/sawyer_sim_examples/scripts/ik_pick_and_place_demo.py b/sawyer_sim_examples/scripts/ik_pick_and_place_demo.py index ec2b593..de9f938 100755 --- a/sawyer_sim_examples/scripts/ik_pick_and_place_demo.py +++ b/sawyer_sim_examples/scripts/ik_pick_and_place_demo.py @@ -35,7 +35,7 @@ Point, Quaternion, ) - +from tf.transformations import quaternion_slerp import intera_interface class PickAndPlace(object): @@ -54,6 +54,7 @@ def __init__(self, limb="right", hover_distance = 0.15, tip_name="right_gripper_ def move_to_start(self, start_angles=None): print("Moving the {0} arm to start pose...".format(self._limb_name)) + print("Moving to start angles: ", start_angles) if not start_angles: start_angles = dict(zip(self._joint_names, [0]*7)) self._guarded_move_to_joint_position(start_angles) @@ -79,14 +80,16 @@ def _approach(self, pose): approach = copy.deepcopy(pose) # approach with a pose the hover-distance above the requested pose approach.position.z = approach.position.z + self._hover_distance + print("approach pose: ", approach) joint_angles = self._limb.ik_request(approach, self._tip_name) + print("Approach joint_angles: ", joint_angles) self._limb.set_joint_position_speed(0.001) self._guarded_move_to_joint_position(joint_angles) self._limb.set_joint_position_speed(0.1) def _retract(self): # retrieve current pose from endpoint - current_pose = self._limb.endpoint_pose() + current_pose = copy.deepcopy(self._limb.endpoint_pose()) ik_pose = Pose() ik_pose.position.x = current_pose['position'].x ik_pose.position.y = current_pose['position'].y @@ -101,25 +104,31 @@ def _servo_to_pose(self, pose, time=4.0, steps=400.0): ''' An *incredibly simple* linearly-interpolated Cartesian move ''' r = rospy.Rate(1/(time/steps)) # Defaults to 100Hz command rate current_pose = self._limb.endpoint_pose() - ik_delta = Pose() - ik_delta.position.x = (current_pose['position'].x - pose.position.x) / steps - ik_delta.position.y = (current_pose['position'].y - pose.position.y) / steps - ik_delta.position.z = (current_pose['position'].z - pose.position.z) / steps - ik_delta.orientation.x = (current_pose['orientation'].x - pose.orientation.x) / steps - ik_delta.orientation.y = (current_pose['orientation'].y - pose.orientation.y) / steps - ik_delta.orientation.z = (current_pose['orientation'].z - pose.orientation.z) / steps - ik_delta.orientation.w = (current_pose['orientation'].w - pose.orientation.w) / steps + ik_delta = Point() + ik_delta.x = (current_pose['position'].x - pose.position.x) / steps + ik_delta.y = (current_pose['position'].y - pose.position.y) / steps + ik_delta.z = (current_pose['position'].z - pose.position.z) / steps + q_current = [current_pose['orientation'].x, + current_pose['orientation'].y, + current_pose['orientation'].z, + current_pose['orientation'].w] + q_pose = [pose.orientation.x, + pose.orientation.y, + pose.orientation.z, + pose.orientation.w] for d in range(int(steps), -1, -1): if rospy.is_shutdown(): return ik_step = Pose() - ik_step.position.x = d*ik_delta.position.x + pose.position.x - ik_step.position.y = d*ik_delta.position.y + pose.position.y - ik_step.position.z = d*ik_delta.position.z + pose.position.z - ik_step.orientation.x = d*ik_delta.orientation.x + pose.orientation.x - ik_step.orientation.y = d*ik_delta.orientation.y + pose.orientation.y - ik_step.orientation.z = d*ik_delta.orientation.z + pose.orientation.z - ik_step.orientation.w = d*ik_delta.orientation.w + pose.orientation.w + ik_step.position.x = d*ik_delta.x + pose.position.x + ik_step.position.y = d*ik_delta.y + pose.position.y + ik_step.position.z = d*ik_delta.z + pose.position.z + # Perform a proper quaternion interpolation + q_slerp = quaternion_slerp(q_current, q_pose, d/steps) + ik_step.orientation.x = q_slerp[0] + ik_step.orientation.y = q_slerp[1] + ik_step.orientation.z = q_slerp[2] + ik_step.orientation.w = q_slerp[3] joint_angles = self._limb.ik_request(ik_step, self._tip_name) if joint_angles: self._limb.set_joint_positions(joint_angles) @@ -129,6 +138,7 @@ def _servo_to_pose(self, pose, time=4.0, steps=400.0): rospy.sleep(1.0) def pick(self, pose): + print("pick") if rospy.is_shutdown(): return # open the gripper @@ -145,6 +155,7 @@ def pick(self, pose): self._retract() def place(self, pose): + print("place") if rospy.is_shutdown(): return # servo above pose @@ -178,7 +189,7 @@ def load_gazebo_models(table_pose=Pose(position=Point(x=0.75, y=0.0, z=0.0)), spawn_sdf = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel) resp_sdf = spawn_sdf("cafe_table", table_xml, "/", table_pose, table_reference_frame) - except rospy.ServiceException, e: + except rospy.ServiceException as e: rospy.logerr("Spawn SDF service call failed: {0}".format(e)) # Spawn Block URDF rospy.wait_for_service('/gazebo/spawn_urdf_model') @@ -186,7 +197,7 @@ def load_gazebo_models(table_pose=Pose(position=Point(x=0.75, y=0.0, z=0.0)), spawn_urdf = rospy.ServiceProxy('/gazebo/spawn_urdf_model', SpawnModel) resp_urdf = spawn_urdf("block", block_xml, "/", block_pose, block_reference_frame) - except rospy.ServiceException, e: + except rospy.ServiceException as e: rospy.logerr("Spawn URDF service call failed: {0}".format(e)) def delete_gazebo_models(): @@ -198,7 +209,7 @@ def delete_gazebo_models(): delete_model = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel) resp_delete = delete_model("cafe_table") resp_delete = delete_model("block") - except rospy.ServiceException, e: + except rospy.ServiceException as e: print("Delete Model service call failed: {0}".format(e)) def main(): diff --git a/sawyer_simulator.rosinstall b/sawyer_simulator.rosinstall index 93d95a3..3fe898e 100644 --- a/sawyer_simulator.rosinstall +++ b/sawyer_simulator.rosinstall @@ -1,16 +1,24 @@ +- git: + local-name: sawyer_simulator + uri: https://github.com/tunglm2203/sawyer_simulator.git + version: release-5.2.0 +- git: + local-name: sawyer_robot + uri: https://github.com/tunglm2203/sawyer_robot.git + version: master - git: local-name: intera_sdk - uri: https://github.com/RethinkRobotics/intera_sdk.git + uri: https://github.com/tunglm2203/intera_sdk.git version: release-5.2.0 - git: local-name: intera_common - uri: https://github.com/RethinkRobotics/intera_common.git + uri: https://github.com/tunglm2203/intera_common.git version: release-5.2.0 - git: - local-name: sawyer_robot - uri: https://github.com/RethinkRobotics/sawyer_robot.git - version: release-5.2.0 + local-name: sns_ik + uri: https://github.com/tunglm2203/sns_ik.git + version: melodic-devel - git: - local-name: sawyer_simulator - uri: https://github.com/RethinkRobotics/sawyer_simulator.git + local-name: sawyer_moveit + uri: https://github.com/tunglm2203/sawyer_moveit.git version: release-5.2.0