def initSimCody(self):
import cody_arm_darpa_m3 as cody_arm
# Load the skin list from the param server
self.robot_path = '/simcody'
self.skin_topic_list = rospy.get_param(self.base_path +
self.robot_path + '/skin_list')
self.torso_frame = rospy.get_param(self.base_path + self.robot_path +
'/torso_frame')
self.inertial_frame = rospy.get_param(self.base_path +
self.robot_path +
'/inertial_frame')
rospy.loginfo(
"RobotHapticState: Initialising Sim haptic state publisher" +
"with the following skin topics: \n%s" % str(self.skin_topic_list))
self.skin_client = sc.TaxelArrayClient(self.skin_topic_list,
self.torso_frame,
self.tf_listener)
self.skin_client.setTrimThreshold(self.trim_threshold)
# TODO: Add config switching here.
rospy.loginfo("RobotHapticState: Initialising robot interface")
if not self.opt.arm:
rospy.logerr("RobotHapticState: No arm specified for Sim Cody")
sys.exit()
self.robot = cody_arm.CodyArmClient(self.opt.arm)
def initCrona(self):
import urdf_arm_darpa_m3 as urdf_arm
self.robot_path = "/crona"
#self.skin_topic_list = rospy.get_param(self.base_path +
# self.robot_path +
# '/skin_list/' + self.opt.sensor)
self.skin_topic_list = None
self.torso_frame = rospy.get_param(self.base_path + self.robot_path +
'/torso_frame')
self.end_effector_frame = rospy.get_param(self.base_path +
self.robot_path +
'/end_effector_frame')
self.inertial_frame = rospy.get_param(self.base_path +
self.robot_path +
'/inertial_frame')
self.skin_client = sc.TaxelArrayClient([], self.torso_frame,
self.tf_listener)
rospy.loginfo(
"RobotHapticState: Initialising CRONA haptic state publisher with the following skin topics: \n%s"
% str(self.skin_topic_list))
rospy.loginfo("RobotHapticState: Initialising robot interface")
if not self.opt.arm:
rospy.logerr("RobotHapticState: No arm specified for CRONA")
sys.exit()
self.robot = urdf_arm.URDFArm(self.opt.arm,
self.tf_listener,
base_link=self.torso_frame,
end_link=self.end_effector_frame)
self.skins = []
self.Jc = []
# Push joint angles to the param server.
if self.opt.arm in ['l', 'r']:
arm_path = '/left'
if self.opt.arm == 'r':
arm_path = '/right'
self.joint_limits_max = rospy.get_param(self.base_path +
self.robot_path +
'/joint_limits' + arm_path +
'/max')
self.joint_limits_min = rospy.get_param(self.base_path +
self.robot_path +
'/joint_limits' + arm_path +
'/min')
# Push the arm specific param to the location the controller looks.
self.setControllerJointLimits(self.joint_limits_max,
self.joint_limits_min)
def initCody(self, num_of_joints=7):
import hrl_cody_arms.cody_arm_client
# Load the skin list from the param server
self.robot_path = '/cody'
self.skin_topic_list = rospy.get_param(self.base_path +
self.robot_path +
'/skin_list/' + self.opt.sensor)
self.torso_frame = rospy.get_param(self.base_path + self.robot_path +
'/torso_frame')
self.inertial_frame = rospy.get_param(self.base_path +
self.robot_path +
'/inertial_frame')
rospy.loginfo(
"RobotHapticState: Initialising Cody haptic state publisher" +
" with the following skin topics: \n%s" %
str(self.skin_topic_list))
self.skin_client = sc.TaxelArrayClient(self.skin_topic_list,
self.torso_frame,
self.tf_listener)
self.skin_client.setTrimThreshold(self.trim_threshold)
rospy.loginfo("RobotHapticState: Initialising robot interface")
if not self.opt.arm:
rospy.logerr("RobotHapticState: No arm specified for Cody")
sys.exit()
if num_of_joints == 7:
self.robot = hrl_cody_arms.cody_arm_client.CodyArmClient_7DOF(
self.opt.arm)
elif num_of_joints == 5:
self.robot = hrl_cody_arms.cody_arm_client.CodyArmClient_5DOF(
self.opt.arm)
# Push joint angles to the param server.
if self.opt.arm in ['l', 'r']:
arm_path = '/left'
if self.opt.arm == 'r':
arm_path = '/right'
self.joint_limits_max = rospy.get_param(self.base_path +
self.robot_path +
'/joint_limits' + arm_path +
'/max')
self.joint_limits_min = rospy.get_param(self.base_path +
self.robot_path +
'/joint_limits' + arm_path +
'/min')
# Push the arm specific param to the location the controller looks.
self.setControllerJointLimits(self.joint_limits_max,
self.joint_limits_min)
def initPR2(self):
# Robot kinematic classes and skin clients. These are specific to each robot
import urdf_arm_darpa_m3 as urdf_arm
# Load parameters from ROS Param server
self.robot_path = "/pr2"
self.skin_topic_list = rospy.get_param(self.base_path +
self.robot_path +
'/skin_list/' + self.opt.sensor)
self.torso_frame = rospy.get_param(self.base_path + self.robot_path +
'/torso_frame')
self.inertial_frame = rospy.get_param(self.base_path +
self.robot_path +
'/inertial_frame')
self.end_effector_frame = rospy.get_param(self.base_path +
self.robot_path +
'/end_effector_frame')
rospy.loginfo(
"RobotHapticState: Initialising PR2 haptic state publisher" +
"with the following skin topics: \n%s" % str(self.skin_topic_list))
self.skin_client = sc.TaxelArrayClient(self.skin_topic_list,
self.torso_frame,
self.tf_listener)
self.skin_client.setTrimThreshold(self.trim_threshold)
rospy.loginfo("RobotHapticState: Initialising robot interface")
if not self.opt.arm:
rospy.logerr("RobotHapticState: No arm specified for PR2")
sys.exit()
# Create the robot object. Provides interfaces to the robot arm and various kinematic functions.
self.robot = urdf_arm.URDFArm(self.opt.arm, self.tf_listener,
self.torso_frame,
self.end_effector_frame)
# Push joint angles to the param server.
if self.opt.arm in ['l', 'r']:
arm_path = '/left'
if self.opt.arm == 'r':
arm_path = '/right'
self.joint_limits_max = rospy.get_param(self.base_path +
self.robot_path +
'/joint_limits' + arm_path +
'/max')
self.joint_limits_min = rospy.get_param(self.base_path +
self.robot_path +
'/joint_limits' + arm_path +
'/min')
# Push the arm specific param to the location the controller looks.
self.setControllerJointLimits(self.joint_limits_max,
self.joint_limits_min)
def initSim(self, robot_type='sim3'):
import gen_sim_arms as sim_robot
# Load the skin list from the param server
if robot_type == 'sim3':
import hrl_common_code_darpa_m3.robot_config.three_link_planar_capsule as sim_robot_config
self.robot_path = '/sim3'
elif robot_type == 'sim3_nolim':
import hrl_common_code_darpa_m3.robot_config.three_link_planar_capsule_nolim as sim_robot_config
self.robot_path = '/sim3'
elif robot_type == 'sim_equal_links_1':
import hrl_common_code_darpa_m3.robot_config.multi_link_one_planar as sim_robot_config
self.robot_path = '/sim_equal_links_1'
self.skin_topic_list = rospy.get_param(self.base_path +
self.robot_path +
'/skin_list/' + self.opt.sensor)
self.torso_frame = rospy.get_param(self.base_path + self.robot_path +
'/torso_frame')
self.inertial_frame = rospy.get_param(self.base_path +
self.robot_path +
'/inertial_frame')
rospy.loginfo(
"RobotHapticState: Initialising Sim haptic state publisher" +
"with the following skin topics: \n%s" % str(self.skin_topic_list))
self.skin_client = sc.TaxelArrayClient(self.skin_topic_list,
self.torso_frame,
self.tf_listener)
self.skin_client.setTrimThreshold(self.trim_threshold)
# TODO: Add config switching here.
rospy.loginfo("RobotHapticState: Initialising Sim robot interface")
sim_config = sim_robot_config
self.robot = sim_robot.ODESimArm(sim_config)
self.joint_limits_max = rospy.get_param(self.base_path +
self.robot_path +
'/joint_limits/max')
self.joint_limits_min = rospy.get_param(self.base_path +
self.robot_path +
'/joint_limits/min')
# Push the arm specific param to the location the controller looks.
self.setControllerJointLimits(self.joint_limits_max,
self.joint_limits_min)
def initDarpa(self):
# Robot kinematic classes and skin clients. These are specific to each robot
import hrl_darpa_arm.darpa_arm_robot_client
#import pr2_arm_darpa_m3_deprecated as pr2_arm # DEPRECATED
# Load parameters from ROS Param server
self.robot_path = "/darpa"
self.skin_topic_list = rospy.get_param(self.base_path +
self.robot_path +
'/skin_list/' + self.opt.sensor)
self.torso_frame = rospy.get_param(self.base_path + self.robot_path +
'/torso_frame')
self.inertial_frame = rospy.get_param(self.base_path +
self.robot_path +
'/inertial_frame')
rospy.loginfo(
"RobotHapticState: Initialising Darpa haptic state publisher" +
"with the following skin topics: \n%s" % str(self.skin_topic_list))
self.skin_client = sc.TaxelArrayClient(self.skin_topic_list,
self.torso_frame,
self.tf_listener)
self.skin_client.setTrimThreshold(self.trim_threshold)
rospy.loginfo("RobotHapticState: Initialising robot interface")
# Create the robot object. Provides interfaces to the robot arm and various kinematic functions.
self.robot = hrl_darpa_arm.darpa_arm_robot_client.SingleLinkRobotClient(
)
self.joint_limits_max = rospy.get_param(self.base_path +
self.robot_path +
'/joint_limits' + '/max')
self.joint_limits_min = rospy.get_param(self.base_path +
self.robot_path +
'/joint_limits' + '/min')
# Push the arm specific param to the location the controller looks.
self.setControllerJointLimits(self.joint_limits_max,
self.joint_limits_min)