def __init__(self, frame, verticalMovement=False, verbose=False):
self.frame = frame
self.verbose = verbose
self.tf = tf.TransformListener()
self.leftGripperAngle = 0
# Set all arm movement parameters
self.rightJointLimitsMax = np.radians(
[26.0, 68.0, 41.0, 0.01, 180.0, 0.01, 180.0])
self.rightJointLimitsMin = np.radians(
[-109.0, -24.0, -220.0, -132.0, -180.0, -120.0, -180.0])
self.leftJointLimitsMax = np.radians(
[109.0, 68.0, 220.0, 0.01, 270.0, 0.01, 180.0])
self.leftJointLimitsMin = np.radians(
[-26.0, -24.0, -41.0, -132.0, -270.0, -120.0, -180.0])
# self.initRightJointGuess = np.array([-0.236, 0.556, -0.091, -1.913, -1.371, -1.538, -3.372])
self.initRightJointGuess = np.array(
[0.13, 0.2, 0.63, -3.23, -2.0, -0.96, -0.1])
# self.initLeftJointGuess = np.array([0.203, 0.846, 1.102, -1.671, 5.592, -1.189, -3.640])
self.initLeftJointGuess = np.array(
[0.74, 0.03, 1.05, -0.32, -1.63, -1.28, 4.22])
if verticalMovement:
self.initRightJointGuess = np.array(
[-0.29, -0.35, -1.56, -1.76, -1.22, -1.52, 3.04])
self.initLeftJointGuess = np.array(
[0.20, 1.03, 1.22, -1.23, -0.51, -1.69, 8.75])
self.initJoints()
self.currentRightJointPositions = None
self.currentLeftJointPositions = None
# rospy.Subscriber('/r_arm_controller/state', JointTrajectoryControllerState, self.rightJointsCallback)
# rospy.Subscriber('/l_arm_controller/state', JointTrajectoryControllerState, self.leftJointsCallback)
self.rightArmClient = actionlib.SimpleActionClient(
'/r_arm_controller/joint_trajectory_action', JointTrajectoryAction)
self.rightArmClient.wait_for_server()
self.rightGripperClient = actionlib.SimpleActionClient(
'/r_gripper_controller/gripper_action', Pr2GripperCommandAction)
self.rightGripperClient.wait_for_server()
self.leftArmClient = actionlib.SimpleActionClient(
'/l_arm_controller/joint_trajectory_action', JointTrajectoryAction)
self.leftArmClient.wait_for_server()
self.leftGripperClient = actionlib.SimpleActionClient(
'/l_gripper_controller/gripper_action', Pr2GripperCommandAction)
self.leftGripperClient.wait_for_server()
self.pointHeadClient = actionlib.SimpleActionClient(
'/head_traj_controller/point_head_action', PointHeadAction)
self.pointHeadClient.wait_for_server()
# Initialize KDL for inverse kinematics
self.rightArmKdl = create_kdl_kin(self.frame, 'r_gripper_tool_frame')
self.rightArmKdl.joint_safety_lower = self.rightJointLimitsMin
self.rightArmKdl.joint_safety_upper = self.rightJointLimitsMax
self.leftArmKdl = create_kdl_kin(self.frame, 'l_gripper_tool_frame')
self.leftArmKdl.joint_safety_lower = self.leftJointLimitsMin
self.leftArmKdl.joint_safety_upper = self.leftJointLimitsMax
def initParams(self):
'''
Get parameters
'''
self.torso_frame = 'torso_lift_link'
self.main_arm = rospy.get_param('/hrl_manipulation_task/arm')
if self.main_arm == 'l': self.sub_arm = 'r'
else: self.sub_arm = 'l'
self.main_ee_frame = rospy.get_param('hrl_manipulation_task/main_ee_frame')
self.main_ee_pos_offset = rospy.get_param('hrl_manipulation_task/main_ee_pos_offset', None)
self.main_ee_orient_offset = rospy.get_param('hrl_manipulation_task/main_ee_orient_offset', None)
self.main_joint_names = rospy.get_param('/hrl_manipulation_task/main_joints')
self.sub_ee_frame = rospy.get_param('hrl_manipulation_task/sub_ee_frame')
self.sub_ee_pos_offset = rospy.get_param('hrl_manipulation_task/sub_ee_pos_offset', None)
self.sub_ee_orient_offset = rospy.get_param('hrl_manipulation_task/sub_ee_orient_offset', None)
self.sub_joint_names = rospy.get_param('/hrl_manipulation_task/sub_joints')
self.main_arm_kdl = create_kdl_kin(self.torso_frame, self.main_ee_frame)
self.sub_arm_kdl = create_kdl_kin(self.torso_frame, self.sub_ee_frame)
p = PyKDL.Vector(self.main_ee_pos_offset['x'], \
self.main_ee_pos_offset['y'], \
self.main_ee_pos_offset['z'])
M = PyKDL.Rotation.RPY(self.main_ee_orient_offset['rx'], self.main_ee_orient_offset['ry'], \
self.main_ee_orient_offset['rz'])
self.main_offset = PyKDL.Frame(M,p)
p = PyKDL.Vector(self.sub_ee_pos_offset['x'], \
self.sub_ee_pos_offset['y'], \
self.sub_ee_pos_offset['z'])
M = PyKDL.Rotation.RPY(self.sub_ee_orient_offset['rx'], self.sub_ee_orient_offset['ry'], \
self.sub_ee_orient_offset['rz'])
self.sub_offset = PyKDL.Frame(M,p)
def initPR2(self):
from pykdl_utils.kdl_kinematics import create_kdl_kin
rospy.loginfo("Trajectory generator for: PR2")
if not self.opt.arm:
rospy.logerr('Arm not specified for PR2')
sys.exit()
self.robot_kinematics = create_kdl_kin('torso_lift_link', self.opt.arm+'_gripper_tool_frame')
self.tf_listener = tf.TransformListener()
if self.opt.arm == None:
rospy.logerr('Need to specify --arm_to_use.\nExiting...')
sys.exit()
def initCrona(self):
from pykdl_utils.kdl_kinematics import create_kdl_kin
rospy.loginfo("Trajectory generator for: cRoNA")
if not self.opt.arm:
rospy.logerr('Arm not specified for cRoNA')
sys.exit()
#self.robot_kinematics = create_kdl_kin('torso_chest_link', self.opt.arm+'_hand_link')
self.robot_kinematics = create_kdl_kin('base_link', self.opt.arm+'_hand_link') # testing
self.tf_listener = tf.TransformListener()
if self.opt.arm == None:
rospy.logerr('Need to specify --arm_to_use.\nExiting...')
sys.exit()
def __init__(self, arm, tf_listener=None):
if arm != 'l' and arm != 'r':
raise Exception, 'Arm should only be "l" or "r"'
kinematics = create_kdl_kin('/torso_chest_link', arm + '_hand_link')
HRLArm.__init__(self, kinematics)
self.joint_names_list = kinematics.get_joint_names()
self.torso_position = None
self.arm_efforts = None
self.delta_jep = None
try:
self.kp = [rospy.get_param(arm+'_arm_controller/gains/'+nm+'/p') for nm in self.joint_names_list]
except:
print "kp is not on param server ... exiting"
assert(False)
# max_kp = np.max(self.kp)
#self.kp[-1] = 5. #This smells like a Hack.
#self.kp[-2] = 50.
#self.kp[-3] = 50.
try:
self.kd = [rospy.get_param(arm+'_arm_controller/gains/'+nm+'/d') for nm in self.joint_names_list]
except:
print "kd is not on param server ... exiting"
assert(False)
rospy.Subscriber('/joint_states', JointState, self.joint_states_cb)
# Set desired joint angle - either through a delta from the current position, or as an absolute value.
rospy.Subscriber ("/haptic_mpc/q_des", FloatArrayBare, self.set_ep_ros)
rospy.Subscriber ("/haptic_mpc/delta_q_des", FloatArrayBare, self.set_delta_ep_ros)
#rospy.Subscriber("/delta_jep_mpc_cvxgen", FloatArrayBare, self.set_ep_ros)
#self.marker_pub = rospy.Publisher(arm+'_arm/viz/markers', Marker)
#self.cep_marker_id = 1
try:
if tf_listener == None:
self.tf_lstnr = tf.TransformListener()
else:
self.tf_lstnr = tf_listener
except rospy.ServiceException, e:
rospy.loginfo("ServiceException caught while instantiating a TF listener. This seems to be normal.")
pass
def __init__(self, arm, tf_listener=None, base_link=None, end_link=None):
if not arm in ['l', 'r']:
raise Exception, 'Arm should be "l" or "r"'
if base_link is None:
rospy.logerr("[URDFArm] Must specify manipulator base link")
sys.exit(1)
else:
self.base_link = base_link
if end_link is None:
rospy.logerr("[URDFArm] Must specify manipulator end link")
sys.exit(1)
else:
self.end_link = end_link
kinematics = create_kdl_kin(self.base_link, self.end_link)
HRLArm.__init__(self, kinematics)
self.joint_names_list = kinematics.get_joint_names()
self.arm_efforts = None
self.delta_jep = None
try:
self.kp = [
rospy.get_param('/' + arm + '_arm_controller/gains/' + nm +
'/p') for nm in self.joint_names_list
]
except:
rospy.logerr("kp is not on param server. Exiting...")
sys.exit(1)
try:
self.kd = [
rospy.get_param('/' + arm + '_arm_controller/gains/' + nm +
'/d') for nm in self.joint_names_list
]
except:
rospy.logerr("kd is not on param server. Exiting...")
sys.exit(1)
rospy.Subscriber('/joint_states', JointState, self.joint_states_cb)
# Set desired joint angle - either through a delta from the current position, or as an absolute value.
rospy.Subscriber("haptic_mpc/q_des", FloatArrayBare,
self.set_ep_callback)
rospy.Subscriber("haptic_mpc/delta_q_des", FloatArrayBare,
self.set_delta_ep_callback)
#rospy.Subscriber("/delta_jep_mpc_cvxgen", FloatArrayBare, self.set_ep_callback)
self.marker_pub = rospy.Publisher('/' + arm + '_arm/viz/markers',
Marker)
self.cep_marker_id = 1
try:
if tf_listener == None:
self.tf_lstnr = tf.TransformListener()
else:
self.tf_lstnr = tf_listener
except rospy.ServiceException, e:
rospy.loginfo(
"ServiceException caught while instantiating a TF listener. This seems to be normal."
)
pass