def __init__(self):
robot_description = '/robot_description'
urdf_string = rospy.get_param(robot_description)
self.num_ik_solver_r = trac_ik.IK('yumi_body',
'yumi_tip_r',
urdf_string=urdf_string)
self.num_ik_solver_l = trac_ik.IK('yumi_body',
'yumi_tip_l',
urdf_string=urdf_string)
self.pos_tol = 0.001 # 0.001 working well with pulling
self.ori_tol = 0.01 # 0.01 working well with pulling
def __init__(self):
"""
Constructor, sets up the internal robot description from
the ROS parameter server and the numerical IK solver for
each arm, given a particular base frame and EE frame
"""
robot_description = '/robot_description'
urdf_string = rospy.get_param(robot_description)
self.num_ik_solver_r = trac_ik.IK('yumi_body',
'yumi_tip_r',
urdf_string=urdf_string)
self.num_ik_solver_l = trac_ik.IK('yumi_body',
'yumi_tip_l',
urdf_string=urdf_string)
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,
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_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, yumi_pb, cfg, exec_thread=True, sim_step_repeat=10):
"""
Class constructor. Sets up internal motion planning interface
for each arm, forward and inverse kinematics solvers, and background
threads for updating the position of the robot.
Args:
yumi_pb (airobot Robot): Instance of PyBullet simulated robot, from
airobot library
cfg (YACS CfgNode): Configuration parameters
exec_thread (bool, optional): Whether or not to start the
background joint position control thread. Defaults to True.
sim_step_repeat (int, optional): Number of simulation steps
to take each time the desired joint position value is
updated. Defaults to 10
"""
self.cfg = cfg
self.yumi_pb = yumi_pb
self.sim_step_repeat = sim_step_repeat
self.joint_lock = threading.RLock()
self._both_pos = self.yumi_pb.arm.get_jpos()
self._single_pos = {}
self._single_pos['right'] = \
self.yumi_pb.arm.arms['right'].get_jpos()
self._single_pos['left'] = \
self.yumi_pb.arm.arms['left'].get_jpos()
self.moveit_robot = moveit_commander.RobotCommander()
self.moveit_scene = moveit_commander.PlanningSceneInterface()
self.moveit_planner = 'RRTConnectkConfigDefault'
self.robot_description = '/robot_description'
self.urdf_string = rospy.get_param(self.robot_description)
self.mp_left = GroupPlanner('left_arm',
self.moveit_robot,
self.moveit_planner,
self.moveit_scene,
max_attempts=3,
planning_time=5.0,
goal_tol=0.5,
eef_delta=0.01,
jump_thresh=10)
self.mp_right = GroupPlanner('right_arm',
self.moveit_robot,
self.moveit_planner,
self.moveit_scene,
max_attempts=3,
planning_time=5.0,
goal_tol=0.5,
eef_delta=0.01,
jump_thresh=10)
self.fk_solver_r = KDLKinematics(URDF.from_parameter_server(),
"yumi_body", "yumi_tip_r")
self.fk_solver_l = KDLKinematics(URDF.from_parameter_server(),
"yumi_body", "yumi_tip_l")
self.num_ik_solver_r = trac_ik.IK('yumi_body',
'yumi_tip_r',
urdf_string=self.urdf_string)
self.num_ik_solver_l = trac_ik.IK('yumi_body',
'yumi_tip_l',
urdf_string=self.urdf_string)
self.ik_pos_tol = 0.001 # 0.001 working well with pulling
self.ik_ori_tol = 0.01 # 0.01 working well with pulling
self.execute_thread = threading.Thread(target=self._execute_both)
self.execute_thread.daemon = True
if exec_thread:
self.execute_thread.start()
self.step_sim_mode = False
else:
self.step_sim_mode = True
