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__(self,
env,
use_qp_solver=False,
check_joint_limits=False,
use_mocap_ctrl=True,
**kwargs):
super(YumiBarAgent, self).__init__(env, **kwargs)
import PyKDL
from gym.utils.kdl_parser import kdl_tree_from_urdf_model
from gym.envs.yumi.yumi_env import YumiEnv
assert isinstance(env.unwrapped, YumiEnv)
assert env.unwrapped.has_two_arms
assert not env.unwrapped.block_gripper
self._raw_env = env.unwrapped # type: YumiEnv
self._sim = self._raw_env.sim
self._dt = env.unwrapped.dt
self._goal = None
self._phase = 0
self._phase_steps = 0
self._locked_l_to_r_tf = None
self._robot = YumiEnv.get_urdf_model()
self._kdl = PyKDL
self.use_mocap_ctrl = use_mocap_ctrl
self.use_qp_solver = use_qp_solver
self.check_joint_limits = check_joint_limits
if check_joint_limits and not use_qp_solver:
raise ValueError(
'Joint limits can only be checked with the QP solver!')
# make sure the simulator is ready before getting the transforms
self._sim.forward()
self._base_offset = self._sim.data.get_body_xpos(
'yumi_base_link').copy()
kdl_tree = kdl_tree_from_urdf_model(self._robot)
base_link = self._robot.get_root()
ee_arm_chain_r = kdl_tree.getChain(base_link, 'gripper_r_base')
ee_arm_chain_l = kdl_tree.getChain(base_link, 'gripper_l_base')
if self._raw_env.has_right_arm:
self._add_site_to_chain(ee_arm_chain_r, 'gripper_r_center')
if self._raw_env.has_left_arm:
self._add_site_to_chain(ee_arm_chain_l, 'gripper_l_center')
self._fkp_solver_r = PyKDL.ChainFkSolverPos_recursive(ee_arm_chain_r)
self._fkp_solver_l = PyKDL.ChainFkSolverPos_recursive(ee_arm_chain_l)
self._ikp_solver_r = PyKDL.ChainIkSolverPos_LMA(ee_arm_chain_r)
self._ikp_solver_l = PyKDL.ChainIkSolverPos_LMA(ee_arm_chain_l)
self._jac_solver_r = PyKDL.ChainJntToJacSolver(ee_arm_chain_r)
self._jac_solver_l = PyKDL.ChainJntToJacSolver(ee_arm_chain_l)
def __init__(self, urdf, world='world', tip='tip'):
# Try loading the URDF data into a KDL tree.
(ok, self.tree) = kdlp.treeFromString(urdf)
assert ok, "Unable to parse the URDF"
# Save the base and tip frame names.
self.baseframe = world
self.tipframe = tip
# Create the isolated chain from world to tip.
self.chain = self.tree.getChain(world, tip)
# Extract the number of joints.
self.N = self.chain.getNrOfJoints()
assert self.N > 0, "Found no joints in the chain"
# Create storage for the joint position, tip position, and
# Jacobian matrices (for the KDL library).
self.qkdl = kdl.JntArray(self.N)
self.Tkdl = kdl.Frame()
self.Jkdl = kdl.Jacobian(self.N)
# Also pre-allocate the memory for the numpy return variables.
# The content will be overwritten so the initial values are
# irrelevant.
self.p = np.zeros((3, 1))
self.R = np.identity(3)
self.J = np.zeros((6, self.N))
# Instantiate the solvers for tip position and Jacobian.
self.fkinsolver = kdl.ChainFkSolverPos_recursive(self.chain)
self.jacsolver = kdl.ChainJntToJacSolver(self.chain)
def __init__(self):
self.dh_params = [[-0.033, pi / 2, 0.145, pi, -1],
[0.155, 0, 0, pi / 2, -1],
[0.135, 0, 0, 0.0, -1],
[-0.002, pi / 2, 0, -pi / 2, -1],
[0, pi, -0.185, -pi, -1]]
self.joint_offset = [170*pi/180, 65*pi/180, -146*pi/180, 102.5*pi/180, 167.5*pi/180]
self.joint_limit_min = [-169*pi/180, -65*pi/180, -151*pi/180, -102.5*pi/180, -167.5*pi/180]
self.joint_limit_max = [169*pi/180, 90*pi/180, 146*pi/180, 102.5*pi/180, 167.5*pi/180]
# Setup the subscribers for the joint states
self.subscriber_joint_state_ = rospy.Subscriber('/joint_states', JointState, self.joint_state_callback,
queue_size=5)
# TF2 broadcaster
self.pose_broadcaster = tf2_ros.TransformBroadcaster()
# PyKDL
self.kine_chain = PyKDL.Chain()
self.setup_kdl_chain()
self.current_joint_position = PyKDL.JntArray(self.kine_chain.getNrOfJoints())
self.current_pose = PyKDL.Frame()
self.fk_solver = PyKDL.ChainFkSolverPos_recursive(self.kine_chain)
self.ik_solver = PyKDL.ChainIkSolverPos_LMA(self.kine_chain)
self.jac_calc = PyKDL.ChainJntToJacSolver(self.kine_chain)
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_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,
urdf_relative_path='../../assets/urdf/sawyer_arm_intera.urdf'):
'''
Uses inverse kinematics in order to go from end-effector cartesian velocity commands to joint velocity commands
:param urdf_relative_path: The path to the URDF file of the robot, relative to the current directory
'''
self.num_joints = 7
# Get the URDF
urdf_path = os.path.join(os.path.dirname(os.path.realpath(__file__)),
urdf_relative_path)
self.urdf_model = URDF.from_xml_string(open(urdf_path).read())
# Set the base link, and the tip link. I think that these are just strings, defined somewhere in the URDF. The name 'right_hand' is how the gripper is described on the real Sawyer, but it may have a different name in your own URDF.
self.base_link = self.urdf_model.get_root()
self.tip_link = "right_hand"
# # Create a KDL tree from the URDF model. This is an object defining the kinematics of the entire robot.
self.kdl_tree = treeFromUrdfModel(self.urdf_model)
# Create a KDL chain from the tree. This is one specific chain within the overall kinematics model.
self.arm_chain = self.kdl_tree[1].getChain(self.base_link,
self.tip_link)
# Create a solver which will be used to compute the forward kinematics
self.forward_kinematics_solver = PyKDL.ChainFkSolverPos_recursive(
self.arm_chain)
# Create a solver which will be used to compute the Jacobian
self.jacobian_solver = PyKDL.ChainJntToJacSolver(self.arm_chain)
#Velocity inverse kinematics solver
self.IK_v_solver = PyKDL.ChainIkSolverVel_pinv(self.arm_chain)
# Create a solver to retrieve the joint angles form the eef position
self.IK_solver = PyKDL.ChainIkSolverPos_NR(
self.arm_chain, self.forward_kinematics_solver, self.IK_v_solver)
def _setup(self):
# set up chebychev points
self.resolution = 100
self.duration = 0.8
# load in ros parameters
self.baselink = rospy.get_param("blue_hardware/baselink")
self.endlink = rospy.get_param("blue_hardware/endlink")
urdf = rospy.get_param("/robot_description")
flag, self.tree = kdl_parser.treeFromString(urdf)
# 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.joint_names = rospy.get_param("blue_hardware/joint_names")
self.joint_names = self.joint_names[:self.num_joints]
if self.debug:
rospy.loginfo(self.joint_names)
self.joints = kdl.JntArray(self.num_joints)
# todo make seperate in abstract class
self.ik = TRAC_IK(self.baselink,
self.endlink,
urdf,
0.005,
5e-5,
"Distance")
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__(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, 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, 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, hover_distance=0.15, verbose=True):
self._baxter = URDF.from_parameter_server(
key='robot_description'
) #Get Baxter URDF Model from ROS Parameter Server
self._kdl_tree = kdl_tree_from_urdf_model(
self._baxter) #Get kdl tree Baxter URDF Model
self._limb_name = limb # string
self._hover_distance = hover_distance # in meters
self._verbose = verbose # bool
self._limb = baxter_interface.Limb(limb) #Creates a Limb Object
self._gripper = baxter_interface.Gripper(
limb) #Creates a Gripper Object from the Limb Object
#self._kin = baxter_kinematics(limb)
#self._traj = Trajectory(self._limb_name)
self._base_link = self._baxter.get_root(
) #Gets root link of the Baxter URDF model
self._tip_link = limb + '_gripper' #Gets the gripper link of ther respective limb
self._arm_chain = self._kdl_tree.getChain(
self._base_link,
self._tip_link) #Creates chain from root link and gripper link
self._jac_kdl = PyKDL.ChainJntToJacSolver(
self._arm_chain
) #Creates a jacobian solver for the Baxter from the chain
self._joint_names = self._limb.joint_names(
) #List of joint names of the respective limb
self._trajectory = list() #List of Nominal Trajectory Waypoints
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
self._iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
rospy.wait_for_service(ns, 5.0)
# verify robot is enabled
print("Getting robot state... ")
self._rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
def __init__(self):
self.num_joints = 7
# Get the URDF
urdf_path = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'../assets/urdf/sawyer_intera.urdf')
self.urdf_model = URDF.from_xml_string(open(urdf_path).read())
# Set the base link, and the tip link.
self.base_link = self.urdf_model.get_root()
self.tip_link = "right_hand"
# # Create a KDL tree from the URDF model. This is an object defining the kinematics of the entire robot.
self.kdl_tree = treeFromUrdfModel(self.urdf_model)
# Create a KDL chain from the tree. This is one specific chain within the overall kinematics model.
self.arm_chain = self.kdl_tree[1].getChain(self.base_link,
self.tip_link)
# Create a solver which will be used to compute the forward kinematics
self.forward_kinematics_solver = PyKDL.ChainFkSolverPos_recursive(
self.arm_chain)
# Create a solver which will be used to compute the Jacobian
self.jacobian_solver = PyKDL.ChainJntToJacSolver(self.arm_chain)
#Velocity inverse kinematics solver
self.IK_v_solver = PyKDL.ChainIkSolverVel_pinv(self.arm_chain)
# Create a solver to retrieve the joint angles form the eef position
self.IK_solver = PyKDL.ChainIkSolverPos_NR(
self.arm_chain, self.forward_kinematics_solver, self.IK_v_solver)
def __init__(self):
self.num_joints = 7
# Get the URDF (you will need to write your own "load_urdf()" function)
urdf_path = os.path.join(assets_root,
'./kdl/panda_urdf/panda_arm_hand.urdf')
# urdf_path= os.path.join(robosuite.models.assets_root, "bullet_data/sawyer_description/urdf/sawyer_intera.urdf")
self.urdf_model = URDF.from_xml_string(
open(urdf_path).read().encode(
'ascii')) # encode to ascii solve the error
#self.kdl_tree = treeFromFile(urdf_path)
# Set the base link, and the tip link. I think that these are just strings, defined somewhere in the URDF. The name 'right_hand' is how the gripper is described on the real Sawyer, but it may have a different name in your own URDF.
self.base_link = self.urdf_model.get_root()
self.tip_link = "panda_hand" # hand of panda robot, see panda_arm_hand.urdf
# # Create a KDL tree from the URDF model. This is an object defining the kinematics of the entire robot.
self.kdl_tree = treeFromUrdfModel(self.urdf_model)
# Create a KDL chain from the tree. This is one specific chain within the overall kinematics model.
self.arm_chain = self.kdl_tree[1].getChain(self.base_link,
self.tip_link)
# Create a solver which will be used to compute the forward kinematics
self.forward_kinematics_solver = PyKDL.ChainFkSolverPos_recursive(
self.arm_chain)
# Create a solver which will be used to compute the Jacobian
self.jacobian_solver = PyKDL.ChainJntToJacSolver(self.arm_chain)
#Velocity inverse kinematics solver
self.IK_v_solver = PyKDL.ChainIkSolverVel_pinv(self.arm_chain)
# Create a solver to retrieve the joint angles form the eef position
self.IK_solver = PyKDL.ChainIkSolverPos_NR(
self.arm_chain, self.forward_kinematics_solver, self.IK_v_solver)
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._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, 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, urdf, base_link, end_link, kdl_tree=None):
if kdl_tree is None:
kdl_tree = kdl_tree_from_urdf_model(urdf)
self.tree = kdl_tree
self.urdf = urdf
base_link = base_link.split("/")[-1] # for dealing with tf convention
end_link = end_link.split("/")[-1] # for dealing with tf convention
self.chain = kdl_tree.getChain(base_link, end_link)
self.base_link = base_link
self.end_link = end_link
# record joint information in easy-to-use lists
self.joint_limits_lower = []
self.joint_limits_upper = []
self.joint_safety_lower = []
self.joint_safety_upper = []
self.joint_types = []
for jnt_name in self.get_joint_names():
jnt = urdf.joints[jnt_name]
if jnt.limits is not None:
self.joint_limits_lower.append(jnt.limits.lower)
self.joint_limits_upper.append(jnt.limits.upper)
else:
self.joint_limits_lower.append(None)
self.joint_limits_upper.append(None)
if jnt.safety is not None:
self.joint_safety_lower.append(jnt.safety.lower)
self.joint_safety_upper.append(jnt.safety.upper)
elif jnt.limits is not None:
self.joint_safety_lower.append(jnt.limits.lower)
self.joint_safety_upper.append(jnt.limits.upper)
else:
self.joint_safety_lower.append(None)
self.joint_safety_upper.append(None)
self.joint_types.append(jnt.joint_type)
def replace_none(x, v):
if x is None:
return v
return x
self.joint_limits_lower = np.array(
[replace_none(jl, -np.inf) for jl in self.joint_limits_lower])
self.joint_limits_upper = np.array(
[replace_none(jl, np.inf) for jl in self.joint_limits_upper])
self.joint_safety_lower = np.array(
[replace_none(jl, -np.inf) for jl in self.joint_safety_lower])
self.joint_safety_upper = np.array(
[replace_none(jl, np.inf) for jl in self.joint_safety_upper])
self.joint_types = np.array(self.joint_types)
self.num_joints = len(self.get_joint_names())
self._fk_kdl = kdl.ChainFkSolverPos_recursive(self.chain)
self._ik_v_kdl = kdl.ChainIkSolverVel_pinv(self.chain)
self._ik_p_kdl = kdl.ChainIkSolverPos_NR(self.chain, self._fk_kdl,
self._ik_v_kdl)
self._jac_kdl = kdl.ChainJntToJacSolver(self.chain)
self._dyn_kdl = kdl.ChainDynParam(self.chain, kdl.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):
# Subscriptions
self.objects = rospy.Subscriber('/found_objects', Object,
self.callback_obj)
self.tfBuffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tfBuffer)
self.gp_action = actionlib.SimpleActionClient('request_trajectory',
RequestTrajectoryAction)
self.grasping_poses = []
self.object_list = []
self.old_list = []
self.joint_limits_lower = []
self.joint_limits_upper = []
self.joint_names = []
self.gp_weights = np.zeros(3)
# Arm selection
self.left_arm = False
self.right_arm = False
self.frame_end = ''
self.desired_chain = 'left_chain'
self.manip_threshold = 0.05
self.distance_threshold = 1.15
# Gripper frames:
self.grip_left = 'left_gripper_tool_frame'
# self.grip_right = 'left_gripper_tool_frame'
self.grip_right = 'right_gripper_tool_frame'
self.frame_base = 'base_link'
# KDL chains:
self.right_chain = kdl.Chain()
self.left_chain = kdl.Chain()
self.ik_lambda = 0.35 # how much should singularities be avoided?
self.arms_chain() # generate chains for both arms
self.left_jnt_pos = kdl.JntArray(self.nJoints)
self.right_jnt_pos = kdl.JntArray(self.nJoints)
self.jac_solver_left = kdl.ChainJntToJacSolver(self.left_chain)
self.jac_solver_right = kdl.ChainJntToJacSolver(self.right_chain)
self.joint_list = rospy.Subscriber('/joint_states', JointState,
self.joint_callback)
rospy.sleep(0.01)
def __init__(self):
self._baxter = URDF.from_parameter_server(key='robot_description')
self._kdl_tree = kdl_tree_from_urdf_model(self._baxter)
self._base_link = self._baxter.get_root()
self._tip_link = 'right_hand'
self._arm_chain = self._kdl_tree.getChain(self._base_link,
self._tip_link)
self.jac_solver = PyKDL.ChainJntToJacSolver(self._arm_chain)
self.fk_solver = PyKDL.ChainFkSolverPos_recursive(self._arm_chain)
def __init__(self):
rospy.init_node('goal_selector', anonymous=True)
r = rospy.Rate(2)
self.objects = rospy.Subscriber('/found_objects', Object, self.callback_obj)
self.tfBuffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tfBuffer)
self.grasping_poses = []
self.object_list = []
self.old_list =[]
self.joint_limits_lower = []
self.joint_limits_upper = []
self.joint_names = []
self.gp_weights = np.zeros(6)
# Arm selection
self.left_arm = False
self.right_arm = False
self.frame_end = ''
self.desired_chain = 'left_chain'
self.manip_threshold = 0.05
self.distance_threshold = 1.15
# Gripper frames:
self.grip_left = 'left_gripper_tool_frame'
self.grip_right = 'right_gripper_tool_frame'
self.frame_base = 'base_footprint'
# KDL chains:
self.right_chain = kdl.Chain()
self.left_chain = kdl.Chain()
self.ik_lambda = 0.35 # how much should singularities be avoided?
self.arms_chain() # generate chains for both arms
self.left_jnt_pos = kdl.JntArray(self.nJoints)
self.right_jnt_pos = kdl.JntArray(self.nJoints)
self.jac_solver_left = kdl.ChainJntToJacSolver(self.left_chain)
self.jac_solver_right = kdl.ChainJntToJacSolver(self.right_chain)
self.joint_list = rospy.Subscriber('/joint_states', JointState, self.joint_callback)
rospy.sleep(0.5)
def __init__(self):
super(YoubotKDL, self).__init__()
# PyKDL
self.kine_chain = PyKDL.Chain()
self.setup_kdl_chain()
self.current_joint_position = PyKDL.JntArray(
self.kine_chain.getNrOfJoints())
self.current_pose = PyKDL.Frame()
self.fk_solver = PyKDL.ChainFkSolverPos_recursive(self.kine_chain)
self.ik_solver = PyKDL.ChainIkSolverPos_LMA(self.kine_chain)
self.jac_calc = PyKDL.ChainJntToJacSolver(self.kine_chain)
def get_jacobian(self):
# Obtain jacobian for the selected arm
self.jac_solver = kdl.ChainJntToJacSolver(self.chain)
jacobian = kdl.Jacobian(self.nJoints)
self.jac_solver.JntToJac(self.joint_values_kdl, jacobian)
jac_array = np.empty(0)
for row in range(jacobian.rows()):
for col in range(jacobian.columns()):
jac_array = np.hstack((jac_array, jacobian[row, col]))
jac_array = np.reshape(jac_array,
(jacobian.rows(), jacobian.columns()))
return jac_array
def prepare_kin_dyn(self):
flag, self.tree = kdl_parser.treeFromFile("data/jelly/jelly.urdf")
chain = self.tree.getChain(self.base_link, self.bl_link)
self.fk_bl = kdl.ChainFkSolverPos_recursive(chain)
self.jac_bl = kdl.ChainJntToJacSolver(chain)
chain = self.tree.getChain(self.base_link, self.br_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_right, front_left, 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
print(jk_list[0].JntToCart(joints, frame))
print(frame)
print(jk_list[1].JntToCart(joints, frame))
print(frame)
print(jk_list[2].JntToCart(joints, frame))
print(frame)
print(jk_list[3].JntToCart(joints, frame))
print(frame)
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())
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 __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())
