def prepare_kin_dyn(self):
robot_description = rospy.get_param("/robot_description")
flag, self.tree = kdl_parser.treeFromParam(robot_description)
chain = self.tree.getChain(self.base_link, self.rl_link)
self.fk_bl = kdl.ChainFkSolverPos_recursive(chain)
self.jac_bl = kdl.ChainJntToJacSolver(chain)
chain = self.tree.getChain(self.base_link, self.rr_link)
self.fk_br = kdl.ChainFkSolverPos_recursive(chain)
self.jac_br = kdl.ChainJntToJacSolver(chain)
chain = self.tree.getChain(self.base_link, self.fl_link)
self.fk_fl = kdl.ChainFkSolverPos_recursive(chain)
self.jac_fl = kdl.ChainJntToJacSolver(chain)
chain = self.tree.getChain(self.base_link, self.fr_link)
self.fk_fr = kdl.ChainFkSolverPos_recursive(chain)
self.jac_fr = kdl.ChainJntToJacSolver(chain)
# convention front_left, front_front, back_right, back_left
self.jac_list = [self.jac_fl, self.jac_fr, self.jac_bl, self.jac_br]
self.fk_list = [self.fk_fl, self.fk_fr, self.fk_bl, self.fk_br]
joints = kdl.JntArray(3)
joints[0] = 0
joints[1] = 0
joints[2] = 0
frame = kdl.Frame()
jk_list = self.fk_list
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__(self):
self.base_link = 'base_link'
self.ee_link = 'ee_link'
flag, self.tree = kdl_parser.treeFromParam('/robot_description')
self.chain = self.tree.getChain(self.base_link, self.ee_link)
self.num_joints = self.tree.getNrOfJoints()
self.vel_ik_solver = kdl.ChainIkSolverVel_pinv(self.chain)
self.pos_fk_solver = kdl.ChainFkSolverPos_recursive(self.chain)
self.pos_ik_solver = kdl.ChainIkSolverPos_LMA(self.chain)
self.cam_model = image_geometry.PinholeCameraModel()
self.joint_state = kdl.JntArrayVel(self.num_joints)
self.joint_names = [
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
rospy.init_node('ur_eef_vel_controller')
rospy.Subscriber('/joint_states', JointState, self.arm_joint_state_cb)
rospy.Subscriber('/rdda_interface/joint_states', JointState,
self.finger_joint_state_cb)
self.joint_vel_cmd_pub = rospy.Publisher(
'/joint_group_vel_controller/command',
Float64MultiArray,
queue_size=10)
self.joint_pos_cmd_pub = rospy.Publisher(
'/scaled_pos_traj_controller/command',
JointTrajectory,
queue_size=10)
self.speed_scaling_pub = rospy.Publisher('/speed_scaling_factor',
Float64,
queue_size=10)
self.switch_controller_cli = rospy.ServiceProxy(
'/controller_manager/switch_controller', SwitchController)
self.joint_pos_cli = actionlib.SimpleActionClient(
'/scaled_pos_joint_traj_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
cam_info = rospy.wait_for_message('/camera/depth/camera_info',
CameraInfo,
timeout=10)
self.cam_model.fromCameraInfo(cam_info)
self.bridge = CvBridge() # allows conversion from depth image to array
self.tf_listener = tf.TransformListener()
self.image_offset = 100
self.pos_ctrler_running = False
rospy.sleep(0.5)
def _setup(self):
# load in ros parameters
self.baselink = rospy.get_param("sawyer/baselink")
self.endlink = rospy.get_param("sawyer/endlink")
flag, self.tree = kdl_parser.treeFromParam("/robot_description")
self.joint_names = rospy.get_param("named_poses/right/joint_names")
print self.joint_names
# build kinematic chain and fk and jacobian solvers
chain_ee = self.tree.getChain(self.baselink, self.endlink)
self.fk_ee = kdl.ChainFkSolverPos_recursive(chain_ee)
self.jac_ee = kdl.ChainJntToJacSolver(chain_ee)
# building robot joint state
self.num_joints = chain_ee.getNrOfJoints()
self.joints = kdl.JntArray(self.num_joints)
def urdf_load(urdfString,
startJoint,
endJoint,
full_joint_list,
fixed_ee_joint=None,
Debug=False):
'''
Takes in a urdf file and parses that into different object representations of the robot arm
:param urdfString: (string) file path to the urdf file. example: < 'mico.urdf' >
:param startJoint: (string) name of the starting joint in the chain
:param endJoint: (string) name of the final joint in the chain
NOTE: this is the final ROTATING joint in the chain, for a fixed joint on the end effector, add this as the fixed joint!
:param fixed_ee_joint (string) name of the fixed joint after end joint in the chain. This is read in just to get a final
displacement on the chain, i.e. usually just to add in an extra displacement offset for the end effector
:return: returns the robot parsed object from urdf_parser, Mike's "arm" version of the robot arm, as well as the kdl tree
'''
if urdfString == '':
urdf_robot = URDF.from_parameter_server()
(ok, kdl_tree) = pyurdf.treeFromParam('/robot_description')
else:
urdf_robot = URDF.from_xml_file(urdfString)
(ok, kdl_tree) = pyurdf.treeFromFile(urdfString)
if not (startJoint == '' or endJoint == ''):
chain = kdl_tree.getChain(startJoint, endJoint)
if full_joint_list == ():
arm, arm_c = convertToArm(urdf_robot,
startJoint,
endJoint,
fixed_ee_joint,
Debug=Debug)
else:
arm, arm_c = convertToArmJointList(urdf_robot,
full_joint_list,
fixed_ee_joint,
Debug=Debug)
if Debug:
o = open('out', 'w')
o.write(str(urdf_robot))
return urdf_robot, arm, arm_c, kdl_tree
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 _get_chain(self):
success, tree = treeFromParam('/robot_description')
if not success:
return
chain = tree.getChain(self.root, self.tip)
return chain
rospy.logerr("Missing base_link")
exit()
if not rospy.has_param("~eef"):
rospy.logerr("Missing eef")
exit()
if not rospy.has_param("~max_trials"):
rospy.logerr("Missing max_trials")
exit()
base_link = rospy.get_param("~base_link")
eef = rospy.get_param("~eef")
max_trials = rospy.get_param("~max_trials")
tree = urdf.treeFromParam("/robot_description")
if not tree[0]:
rospy.logerr("Missing /robot_description")
exit()
chain = tree[1].getChain(base_link, eef)
jac_solver = kdl.ChainJntToJacSolver(chain)
q = kdl.JntArray(chain.getNrOfJoints())
J = kdl.Jacobian(chain.getNrOfJoints())
rospy.loginfo("Initialized test_kdl_py")
init_time = rospy.Time.now()
for i in range(max_trials):
for j in range(chain.getNrOfJoints()):
q[j] = random.uniform(-math.pi, math.pi)