def __init__(self, limb):
self._kuka = URDF.from_parameter_server(key='robot_description')
self._kdl_tree = kdl_tree_from_urdf_model(self._kuka)
self._base_link = self._kuka.get_root()
# self._tip_link = limb + '_gripper'
self._tip_link = 'tool0'
self._tip_frame = PyKDL.Frame()
self._arm_chain = self._kdl_tree.getChain(self._base_link,
self._tip_link)
# Kuka Interface Limb Instances
# self._limb_interface = baxter_interface.Limb(limb)
# self._joint_names = self._limb_interface.joint_names()
self._joint_names = [
'joint_a1', 'joint_a2', 'joint_a3', 'joint_a4', 'joint_a5',
'joint_a6'
]
self._num_jnts = len(self._joint_names)
# KDL Solvers
self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain)
self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._arm_chain)
self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain)
self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain,
self._fk_p_kdl,
self._ik_v_kdl)
self._jac_kdl = PyKDL.ChainJntToJacSolver(self._arm_chain)
self._dyn_kdl = PyKDL.ChainDynParam(self._arm_chain,
PyKDL.Vector.Zero())
def __init__(self, robot, ee_link=None):
rospack = rospkg.RosPack()
pykdl_dir = rospack.get_path('ur_pykdl')
TREE_PATH = pykdl_dir + '/urdf/' + robot + '.urdf'
self._ur = URDF.from_xml_file(TREE_PATH)
self._kdl_tree = kdl_tree_from_urdf_model(self._ur)
self._base_link = BASE_LINK
ee_link = EE_LINK if ee_link is None else ee_link
self._tip_link = ee_link
self._tip_frame = PyKDL.Frame()
self._arm_chain = self._kdl_tree.getChain(self._base_link,
self._tip_link)
# UR Interface Limb Instances
self._joint_names = JOINT_ORDER
self._num_jnts = len(self._joint_names)
# KDL Solvers
self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain)
self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._arm_chain)
self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain)
self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain,
self._fk_p_kdl,
self._ik_v_kdl)
self._jac_kdl = PyKDL.ChainJntToJacSolver(self._arm_chain)
self._dyn_kdl = PyKDL.ChainDynParam(self._arm_chain,
PyKDL.Vector.Zero())
def __init__(self, limb):
self._pr2 = URDF.from_parameter_server(key='robot_description')
self._kdl_tree = kdl_tree_from_urdf_model(self._pr2)
self._base_link = self._pr2.get_root()
# self._tip_link = limb + '_gripper'
self._tip_link = limb + '_wrist_roll_link'
self._tip_frame = PyKDL.Frame()
self._arm_chain = self._kdl_tree.getChain(self._base_link,
self._tip_link)
# Baxter Interface Limb Instances
# self._limb_interface = baxter_interface.Limb(limb)
# self._joint_names = self._limb_interface.joint_names()
self._joint_names = [
limb + '_shoulder_pan_joint', limb + '_shoulder_lift_joint',
limb + '_upper_arm_roll_joint', limb + '_elbow_flex_joint',
limb + '_forearm_roll_joint', limb + '_wrist_flex_joint',
limb + '_wrist_roll_joint'
]
self._num_jnts = len(self._joint_names)
# KDL Solvers
self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain)
self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._arm_chain)
self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain)
self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain,
self._fk_p_kdl,
self._ik_v_kdl)
self._jac_kdl = PyKDL.ChainJntToJacSolver(self._arm_chain)
self._dyn_kdl = PyKDL.ChainDynParam(self._arm_chain,
PyKDL.Vector.Zero())
def __init__(self, base_link=None, end_link=None):
self._robot = kdl_parser_py.urdf.urdf.URDF.from_parameter_server(
'robot_description')
(ok,
self._kdl_tree) = kdl_parser_py.urdf.treeFromUrdfModel(self._robot)
self._base_link = self._robot.get_root(
) if base_link is None else base_link
self._tip_link = end_link
self._tip_frame = PyKDL.Frame()
self._arm_chain = self._kdl_tree.getChain(self._base_link,
self._tip_link)
#self.joint_names = [joint.name for joint in self._robot.joints if joint.type!='fixed']
self.joint_names = [
self._arm_chain.getSegment(i).getJoint().getName()
for i in range(self._arm_chain.getNrOfSegments())
if self._arm_chain.getSegment(i).getJoint().getType() !=
PyKDL.Joint.None
]
self._num_jnts = len(self.joint_names)
# Store joint information for future use
self.get_joint_information()
# KDL Solvers
self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain)
self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._arm_chain)
#self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain)
#self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain, self._fk_p_kdl, self._ik_v_kdl)
self._jac_kdl = PyKDL.ChainJntToJacSolver(self._arm_chain)
self._dyn_kdl = PyKDL.ChainDynParam(self._arm_chain,
PyKDL.Vector.Zero())
def _init_real_consts(self):
"""
Initialize constants.
"""
self._home_position = self.cfgs.ARM.HOME_POSITION
robot_description = self.cfgs.ROBOT_DESCRIPTION
urdf_string = rospy.get_param(robot_description)
self._num_ik_solver = trac_ik.IK(self.cfgs.ARM.ROBOT_BASE_FRAME,
self.cfgs.ARM.ROBOT_EE_FRAME,
urdf_string=urdf_string)
_, urdf_tree = treeFromParam(robot_description)
base_frame = self.cfgs.ARM.ROBOT_BASE_FRAME
ee_frame = self.cfgs.ARM.ROBOT_EE_FRAME
self._urdf_chain = urdf_tree.getChain(base_frame,
ee_frame)
self.arm_jnt_names = self._get_kdl_joint_names()
self.arm_link_names = self._get_kdl_link_names()
self.arm_dof = len(self.arm_jnt_names)
self._jac_solver = kdl.ChainJntToJacSolver(self._urdf_chain)
self._fk_solver_pos = kdl.ChainFkSolverPos_recursive(self._urdf_chain)
self._fk_solver_vel = kdl.ChainFkSolverVel_recursive(self._urdf_chain)
self.ee_link = self.cfgs.ARM.ROBOT_EE_FRAME
def __init__(self, limb='right'):
self._sawyer = URDF.from_parameter_server(key='robot_description')
self._kdl_tree = kdl_tree_from_urdf_model(self._sawyer)
self._base_link = self._sawyer.get_root()
# RUDI: LETS GET RID OF INTERA
#self._tip_link = limb + '_hand'
self._tip_link = 'right_gripper_tip'
self._tip_frame = PyKDL.Frame()
self._arm_chain = self._kdl_tree.getChain(self._base_link,
self._tip_link)
# RUDI: LETS GET RID OF INTERA
# Sawyer Interface Limb Instances
# self._limb_interface = intera_interface.Limb(limb)
#self._joint_names = self._limb_interface.joint_names()
self._joint_names = ["right_j0", "right_j1", "right_j2", "right_j3", "right_j4", "right_j5", "right_j6"]
self._num_jnts = len(self._joint_names)
# KDL Solvers
self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain)
self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._arm_chain)
self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain)
self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain,
self._fk_p_kdl,
self._ik_v_kdl)
self._jac_kdl = PyKDL.ChainJntToJacSolver(self._arm_chain)
self._dyn_kdl = PyKDL.ChainDynParam(self._arm_chain,
PyKDL.Vector.Zero())
def __init__(self, limb):
self._sawyer = URDF.from_parameter_server(key='robot_description')
self._kdl_tree = kdl_tree_from_urdf_model(self._sawyer)
self._base_link = self._sawyer.get_root()
print "BASE LINK:", self._base_link
self._tip_link = limb + '_hand'
self._tip_frame = PyKDL.Frame()
self._arm_chain = self._kdl_tree.getChain(self._base_link,
self._tip_link)
# Sawyer Interface Limb Instances
self._limb_interface = intera_interface.Limb(limb)
self._joint_names = self._limb_interface.joint_names()
self._num_jnts = len(self._joint_names)
# KDL Solvers
self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain)
self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._arm_chain)
self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain)
self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain,
self._fk_p_kdl,
self._ik_v_kdl)
self._jac_kdl = PyKDL.ChainJntToJacSolver(self._arm_chain)
self._dyn_kdl = PyKDL.ChainDynParam(self._arm_chain,
PyKDL.Vector(0.0, 0.0, -9.81))
def __init__(self, urdf_param='robot_description'):
self._urdf = URDF.from_parameter_server(urdf_param)
(parse_ok, self._kdl_tree) = treeFromUrdfModel(self._urdf)
# Check @TODO Exception
if not parse_ok:
rospy.logerr(
'Error parsing URDF from parameter server ({0})'.format(
urdf_param))
else:
rospy.logdebug('Parsing URDF succesful')
self._base_link = self._urdf.get_root()
# @TODO Hardcoded
self._tip_link = 'link_6'
self._tip_frame = PyKDL.Frame()
self._arm_chain = self._kdl_tree.getChain(self._base_link,
self._tip_link)
# @TODO Hardcoded
self._joint_names = ['a1', 'a2', 'a3', 'a4', 'a5', 'a6']
self._num_joints = len(self._joint_names)
# KDL Solvers
self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain)
self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._arm_chain)
self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain)
self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain,
self._fk_p_kdl,
self._ik_v_kdl)
self._jac_kdl = PyKDL.ChainJntToJacSolver(self._arm_chain)
self._dyn_kdl = PyKDL.ChainDynParam(self._arm_chain,
PyKDL.Vector.Zero())
def __init__(self, limb, description=None):
if description is None:
self._franka = URDF.from_parameter_server(key='robot_description')
else:
self._franka = URDF.from_xml_file(description)
self._kdl_tree = kdl_tree_from_urdf_model(self._franka)
self._base_link = self._franka.get_root()
self._tip_link = limb.name + ('_hand'
if limb.has_gripper else '_link8')
self._tip_frame = PyKDL.Frame()
self._arm_chain = self._kdl_tree.getChain(self._base_link,
self._tip_link)
self._limb_interface = limb
self._joint_names = deepcopy(self._limb_interface.joint_names())
self._num_jnts = len(self._joint_names)
# KDL Solvers
self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain)
self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._arm_chain)
self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain)
self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain,
self._fk_p_kdl,
self._ik_v_kdl)
self._jac_kdl = PyKDL.ChainJntToJacSolver(self._arm_chain)
self._dyn_kdl = PyKDL.ChainDynParam(self._arm_chain,
PyKDL.Vector.Zero())
def __init__(self,
configs,
moveit_planner='ESTkConfigDefault',
analytical_ik=None,
use_moveit=True):
"""
Constructor for Arm parent class.
:param configs: configurations for arm
:param moveit_planner: moveit planner type
:param analytical_ik: customized analytical ik class
if you have one, None otherwise
:param use_moveit: use moveit or not, default is True
:type configs: YACS CfgNode
:type moveit_planner: string
:type analytical_ik: None or an Analytical ik class
:type use_moveit: bool
"""
self.configs = configs
self.moveit_planner = moveit_planner
self.moveit_group = None
self.scene = None
self.use_moveit = use_moveit
self.joint_state_lock = threading.RLock()
self.tf_listener = tf.TransformListener()
if self.use_moveit:
self._init_moveit()
robot_description = self.configs.ARM.ARM_ROBOT_DSP_PARAM_NAME
urdf_string = rospy.get_param(robot_description)
self.num_ik_solver = trac_ik.IK(configs.ARM.ARM_BASE_FRAME,
configs.ARM.EE_FRAME,
urdf_string=urdf_string)
if analytical_ik is not None:
self.ana_ik_solver = analytical_ik(configs.ARM.ARM_BASE_FRAME,
configs.ARM.EE_FRAME)
_, self.urdf_tree = treeFromParam(robot_description)
self.urdf_chain = self.urdf_tree.getChain(configs.ARM.ARM_BASE_FRAME,
configs.ARM.EE_FRAME)
self.arm_joint_names = self._get_kdl_joint_names()
self.arm_link_names = self._get_kdl_link_names()
self.arm_dof = len(self.arm_joint_names)
self.jac_solver = kdl.ChainJntToJacSolver(self.urdf_chain)
self.fk_solver_pos = kdl.ChainFkSolverPos_recursive(self.urdf_chain)
self.fk_solver_vel = kdl.ChainFkSolverVel_recursive(self.urdf_chain)
# Subscribers
self._joint_angles = dict()
self._joint_velocities = dict()
self._joint_efforts = dict()
rospy.Subscriber(configs.ARM.ROSTOPIC_JOINT_STATES, JointState,
self._callback_joint_states)
# Publishers
self.joint_pub = None
self._setup_joint_pub()
def __init__(self):
# Read parameters
self.frame_id = read_parameter('~frame_id', 'base_link')
self.tip_link = read_parameter('~tip_link', 'end_effector')
# Kinematics
self.urdf = URDF.from_parameter_server(key='robot_description')
self.kinematics = KDLKinematics(self.urdf, self.frame_id,
self.tip_link)
self.fk_vel_solver = PyKDL.ChainFkSolverVel_recursive(
self.kinematics.chain)
# Set-up publishers/subscribers
self.state_pub = rospy.Publisher('/grips/endpoint_state',
EndpointState)
rospy.Subscriber('/joint_states', JointState, self.joint_states_cb)
def initialize_kinematic_solvers(self):
robot_description = rospy.get_param('/yumi/velocity_control/robot_description', '/robot_description')
self._robot = URDF.from_parameter_server(key=robot_description)
self._kdl_tree = kdl_tree_from_urdf_model(self._robot)
# self._base_link = self._robot.get_root()
self._ee_link = 'gripper_' +self._arm_name + '_base' #name of the frame we want
self._ee_frame = PyKDL.Frame()
self._ee_arm_chain = self._kdl_tree.getChain(self._base_link, self._ee_link)
# KDL Solvers
self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._ee_arm_chain)
self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._ee_arm_chain)
self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._ee_arm_chain)
#self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain, self._fk_p_kdl, self._ik_v_kdl)
self._ik_p_kdl = PyKDL.ChainIkSolverPos_LMA(self._ee_arm_chain)
self._jac_kdl = PyKDL.ChainJntToJacSolver(self._ee_arm_chain)
self._dyn_kdl = PyKDL.ChainDynParam(self._ee_arm_chain, PyKDL.Vector.Zero())
class MoveGroupLeftArm(object):
def __init__(self):
#----------------control init begin-----------------------------------------------
super(MoveGroupLeftArm, self).__init__()
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('iiwa_move_to_ee_pose', anonymous=True)
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
group_name = "manipulator"
self.group = moveit_commander.MoveGroupCommander(group_name)
#group.set_max_velocity_scaling_factor(0.15)
self.group.set_max_acceleration_scaling_factor(0.1)
if self.group.set_planner_id(PLANNER_ID):
print "Using planner: %s" % PLANNER_ID
self.group.set_num_planning_attempts(100)
self.group.set_planning_time(3)
self.group.set_start_state_to_current_state()
self.display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20)
self.gripper_io_publisher = rospy.Publisher('command/GripperState', iiwa_msgs.msg.GripperState, queue_size=10)
self.upright_constraints = Constraints()
try:
rospy.wait_for_service('configuration/setSmartServoLimits', 1)
except:
print 'Service call timeout'
try:
self.set_speed_limits = rospy.ServiceProxy('configuration/setSmartServoLimits', SetSmartServoJointSpeedLimits)
response = self.set_speed_limits(0.3, 0.1, -1)
print 'Velocity limit set'
print response
except rospy.ServiceException, e:
print "service call failed: %s"%e
#----------------kinematics init begin---------------------------------------------
_iiwa_URDF = URDF.from_parameter_server(key='robot_description')
_iiwa_kdl_tree = kdl_tree_from_urdf_model(_iiwa_URDF)
_iiwa_base_link = _iiwa_URDF.get_root()
self.iiwa_chain = _iiwa_kdl_tree.getChain(_iiwa_base_link, 'tool_link_ee')
self.forward_kin_position_kdl = PyKDL.ChainFkSolverPos_recursive(self.iiwa_chain)
_forward_kin_velocity_kdl = PyKDL.ChainFkSolverVel_recursive(self.iiwa_chain)
self.inverse_kin_velocity_kdl = PyKDL.ChainIkSolverVel_pinv(self.iiwa_chain)
self.min_limits = PyKDL.JntArray(NUM_JOINTS)
self.max_limits = PyKDL.JntArray(NUM_JOINTS)
for idx, jnt in enumerate(MIN_JOINT_LIMITS_DEG):
self.min_limits[idx] = math.radians(jnt)
for idx, jnt in enumerate(MAX_JOINT_LIMITS_DEG):
self.max_limits[idx] = math.radians(jnt)
self.component_map = {}
def __init__(self,
limb,
ee_frame_name="panda_link8",
additional_segment_config=None,
description=None):
if description is None:
self._franka = URDF.from_parameter_server(key='robot_description')
else:
self._franka = URDF.from_xml_file(description)
self._kdl_tree = kdl_tree_from_urdf_model(self._franka)
if additional_segment_config is not None:
for c in additional_segment_config:
q = quaternion.from_rotation_matrix(c["origin_ori"]).tolist()
kdl_origin_frame = PyKDL.Frame(
PyKDL.Rotation.Quaternion(q.x, q.y, q.z, q.w),
PyKDL.Vector(*(c["origin_pos"].tolist())))
kdl_sgm = PyKDL.Segment(c["child_name"],
PyKDL.Joint(c["joint_name"]),
kdl_origin_frame,
PyKDL.RigidBodyInertia())
self._kdl_tree.addSegment(kdl_sgm, c["parent_name"])
self._base_link = self._franka.get_root()
self._tip_link = ee_frame_name
self._tip_frame = PyKDL.Frame()
self._arm_chain = self._kdl_tree.getChain(self._base_link,
self._tip_link)
self._limb_interface = limb
self._joint_names = deepcopy(self._limb_interface.joint_names())
self._num_jnts = len(self._joint_names)
# KDL Solvers
self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain)
self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._arm_chain)
self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain)
self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain,
self._fk_p_kdl,
self._ik_v_kdl)
self._jac_kdl = PyKDL.ChainJntToJacSolver(self._arm_chain)
self._dyn_kdl = PyKDL.ChainDynParam(self._arm_chain,
PyKDL.Vector.Zero())
def create_chain(self, ee_frame_name):
self._tip_link = ee_frame_name
self._arm_chain = self._kdl_tree.getChain(self._base_link,
self._tip_link)
self._joint_names = deepcopy(self._limb_interface.joint_names())
self._num_jnts = len(self._joint_names)
# KDL Solvers
self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain)
self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._arm_chain)
self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain)
self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain,
self._fk_p_kdl,
self._ik_v_kdl)
self._jac_kdl = PyKDL.ChainJntToJacSolver(self._arm_chain)
self._dyn_kdl = PyKDL.ChainDynParam(self._arm_chain,
PyKDL.Vector.Zero())
def _init_kinematics(self):
# Ros-Params
base_link = rospy.get_param("pyrobot/base_link")
gripper_link = rospy.get_param("pyrobot/gripper_link")
robot_description = rospy.get_param("pyrobot/robot_description")
urdf_string = rospy.get_param(robot_description)
self.num_ik_solver = trac_ik.IK(base_link,
gripper_link,
urdf_string=urdf_string)
_, self.urdf_tree = treeFromParam(robot_description)
self.urdf_chain = self.urdf_tree.getChain(base_link, gripper_link)
self.arm_joint_names = self._get_kdl_joint_names()
self.arm_link_names = self._get_kdl_link_names()
self.arm_dof = len(self.arm_joint_names)
self.jac_solver = kdl.ChainJntToJacSolver(self.urdf_chain)
self.fk_solver_pos = kdl.ChainFkSolverPos_recursive(self.urdf_chain)
self.fk_solver_vel = kdl.ChainFkSolverVel_recursive(self.urdf_chain)
def __init__(self):
limb = read_parameter('~limb', 'right')
if limb not in ['right', 'left']:
rospy.logerr('Unknown limb name [%s]' % limb)
return
# Read the controllers parameters
gains = read_parameter('~gains', dict())
self.kp = joint_list_to_kdl(gains['Kp'])
self.kd = joint_list_to_kdl(gains['Kd'])
self.publish_rate = read_parameter('~publish_rate', 500)
self.frame_id = read_parameter('~frame_id', 'base')
self.timeout = read_parameter('~timeout', 0.02) # seconds
# Set-up baxter interface
self.arm_interface = baxter_interface.Limb(limb)
# set_joint_torques must be commanded at a rate great than the timeout specified by set_command_timeout.
# If the timeout is exceeded before a new set_joint_velocities command is received, the controller will switch
# modes back to position mode for safety purposes.
self.arm_interface.set_command_timeout(self.timeout)
# Baxter kinematics
self.urdf = URDF.from_parameter_server(key='robot_description')
self.tip_link = '%s_gripper' % limb
self.kinematics = KDLKinematics(self.urdf, self.frame_id,
self.tip_link)
self.fk_vel_solver = PyKDL.ChainFkSolverVel_recursive(
self.kinematics.chain)
# Adjust the publish rate of baxter's state
joint_state_pub_rate = rospy.Publisher(
'/robot/joint_state_publish_rate', UInt16)
# Get the joint names and limits
self.joint_names = self.arm_interface.joint_names()
self.num_joints = len(self.joint_names)
self.lower_limits = np.zeros(self.num_joints)
self.upper_limits = np.zeros(self.num_joints)
# KDL joint may be in a different order than expected
kdl_lower_limits, kdl_upper_limits = self.kinematics.get_joint_limits()
for i, name in enumerate(self.kinematics.get_joint_names()):
if name in self.joint_names:
idx = self.joint_names.index(name)
self.lower_limits[idx] = kdl_lower_limits[i]
self.upper_limits[idx] = kdl_upper_limits[i]
self.middle_values = (self.upper_limits + self.lower_limits) / 2.0
# Move the arm to a neutral position
#~ self.arm_interface.move_to_neutral(timeout=10.0)
#~ neutral_pos = [-math.pi/4, 0.0, 0.0, 0.0, 0.0, math.pi/2, -math.pi/2]
neutral_pos = [1.0, -0.57, -1.2, 1.3, 0.86, 1.4, -0.776]
#~ neutral_pos = [pi/4, -pi/3, 0.0, pi/2, 0.0, pi/3, 0.0]
neutral_cmd = dict()
for i, name in enumerate(self.joint_names):
if name in ['left_s0', 'left_w2']:
neutral_cmd[name] = neutral_pos[i] * -1.0
else:
neutral_cmd[name] = neutral_pos[i]
self.arm_interface.move_to_joint_positions(neutral_cmd, timeout=10.0)
# Baxter faces
self.limb = limb
self.face = FaceImage()
self.face.set_image('happy')
# Set-up publishers/subscribers
self.suppress_grav_comp = rospy.Publisher(
'/robot/limb/%s/suppress_gravity_compensation' % limb, Empty)
joint_state_pub_rate.publish(int(self.publish_rate))
self.feedback_pub = rospy.Publisher('/takktile/force_feedback', Wrench)
rospy.Subscriber('/baxter/%s_ik_command' % limb, PoseStamped,
self.ik_command_cb)
self.gripper_closed = False
rospy.Subscriber('/takktile/calibrated', Touch, self.takktile_cb)
rospy.Subscriber('/robot/end_effector/%s_gripper/state' % limb,
EndEffectorState, self.end_effector_cb)
rospy.loginfo('Running Cartesian controller for the %s arm' % limb)
# Start torque controller timer
self.cart_command = None
self.torque_controller_timer = rospy.Timer(
rospy.Duration(1.0 / self.publish_rate), self.torque_controller_cb)
# Shutdown hookup to clean-up before killing the script
rospy.on_shutdown(self.shutdown)
def __init__(self,
name,
base,
ee_link,
namespace=None,
arm_name=None,
compute_fk_for_all=False):
# Joint states
self._joint_angles = dict()
self._joint_velocity = dict()
self._joint_effort = dict()
# Finger chain name
self._name = name
# Namespace
if None in [namespace, arm_name]:
ns = [
item for item in rospy.get_namespace().split('/')
if len(item) > 0
]
if len(ns) != 2:
rospy.ROSException(
'The controller must be called inside the namespace the manipulator namespace'
)
self._namespace = ns[0]
self._arm_name = ns[1]
else:
self._namespace = namespace
self._arm_name = arm_name
if self._namespace[0] != '/':
self._namespace = '/' + self._namespace
if self._namespace[-1] != '/':
self._namespace += '/'
# True if it has to compute the forward kinematics for all frames
self._compute_fk_for_all = compute_fk_for_all
# List of frames for each link
self._frames = dict()
# Load robot description (complete robot, including vehicle, if
# available)
self._robot_description = URDF.from_parameter_server(
key=self._namespace + 'robot_description')
# KDL tree of the whole structure
ok, self._kdl_tree = treeFromUrdfModel(self._robot_description)
# Base link
self._base_link = base
# Tip link
self._tip_link = ee_link
# Read the complete link name, with robot's namespace, if existent
for link in self._robot_description.links:
if self._arm_name not in link.name:
continue
linkname = link.name.split('/')[-1]
if self._base_link == linkname:
self._base_link = link.name
if self._tip_link == linkname:
self._tip_link = link.name
# Get arm chain
self._chain = self._kdl_tree.getChain(self._base_link, self._tip_link)
# Partial tree from base to each link
self._chain_part = dict()
for link in self._robot_description.links:
if link.name in self.link_names:
self._chain_part[link.name] = self._kdl_tree.getChain(
self._base_link, link.name)
# Get joint names from the chain
# Joint names
self._joint_names = list()
for idx in xrange(self._chain.getNrOfSegments()):
joint = self._chain.getSegment(idx).getJoint()
# Not considering fixed joints
if joint.getType() == 0:
name = joint.getName()
self._joint_names.append(name)
self._joint_angles[name] = 0.0
self._joint_velocity[name] = 0.0
self._joint_effort[name] = 0.0
# Jacobian solvers
self._jac_kdl = PyKDL.ChainJntToJacSolver(self._chain)
# Jacobian solver dictionary for partial trees
self._jac_kdl_part = dict()
for link in self._robot_description.links:
if link.name in self.link_names:
self._jac_kdl_part[link.name] = PyKDL.ChainJntToJacSolver(
self._chain_part[link.name])
# Forward position kinematics solvers
self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._chain)
# Forward velocity kinematics solvers
self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._chain)
# Inverse kinematic solvers
self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._chain)
self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._chain, self._fk_p_kdl,
self._ik_v_kdl, 100, 1e-6)
