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, 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, 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"
# 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)
# Instantiate the solvers for tip position and Jacobian.
self.fkinsolver = kdl.ChainFkSolverPos_recursive(self.chain)
self.jacsolver = kdl.ChainJntToJacSolver(self.chain)
def __init__(self, robotDescriptionString=None, arm_id='panda'):
if robotDescriptionString is None:
self.urdf_string = _rospy.get_param('robot_description')
self.arm_id = _rospy.get_param('arm_id')
else:
self.urdf_string = robotDescriptionString
self.arm_id = arm_id
self.baseLinkName = "{}_link0".format(self.arm_id)
self.eeLinkName = "{}_link7".format(self.arm_id)
isSuccessful, self.kdltree = _kdl_parser.treeFromString(
self.urdf_string)
if not isSuccessful:
raise RuntimeError("could not parse 'robot_description'")
self.ee_chain = self.kdltree.getChain(self.baseLinkName,
self.eeLinkName)
self.fk_ee = _kdl.ChainFkSolverPos_recursive(self.ee_chain)
self.jointposition = _kdl.JntArray(7)
self.jointvelocity = _kdl.JntArray(7)
self.eeFrame = _kdl.Frame()
# Calculate the jacobian expressed in the base frame of the chain, with reference point at the end effector of the *chain.
# http://docs.ros.org/hydro/api/orocos_kdl/html/classKDL_1_1ChainJntToJacSolver.html#details
# Sounds like base jacobian but will be used here for both
self.jac_ee = _kdl.ChainJntToJacSolver(self.ee_chain)
self.jacobian = _kdl.Jacobian(7)
#dynamics: (needs masses added to urdf!)
self.grav_vector = _kdl.Vector(0., 0., -9.81)
self.dynParam = _kdl.ChainDynParam(self.ee_chain, self.grav_vector)
self.inertiaMatrix_kdl = _kdl.JntSpaceInertiaMatrix(7)
self.inertiaMatrix = _np.eye((8))
self.gravity_torques_kdl = _kdl.JntArray(7)
self.gravity_torques = _np.zeros((8))
viscuousFriction = [1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5,
1.5] #TODO: load from URDF
self.viscuousFriction = _np.array(viscuousFriction)