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 gravity_compensation_publisher(data):
try:
old
except NameError:
old = 0
if old <> data.position[1]:
print 123
pub = rospy.Publisher('/panda_arm_controller/command',
Float64MultiArray,
queue_size=10)
data_to_send = Float64MultiArray()
robot = URDF.from_parameter_server()
tree = kdl_tree_from_urdf_model(robot)
chain = tree.getChain("panda_link0", "panda_link7")
grav_vector = kdl.Vector(0, 0, -9.81) # relative to kdl chain base link
dyn_kdl = kdl.ChainDynParam(chain, grav_vector)
jt_positions = kdl.JntArray(7)
jt_positions[0] = data.position[2]
jt_positions[1] = data.position[3]
jt_positions[2] = data.position[4]
jt_positions[3] = data.position[5]
jt_positions[4] = data.position[6]
jt_positions[5] = data.position[7]
jt_positions[6] = data.position[8]
old = data.position[1]
grav_matrix = kdl.JntArray(7)
dyn_kdl.JntToGravity(jt_positions, grav_matrix)
data_to_send.data = [grav_matrix[i] for i in range(grav_matrix.rows())]
pub.publish(data_to_send)
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):
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='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, 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._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, 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, 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):
ns = [
item for item in rospy.get_namespace().split('/') if len(item) > 0
]
if len(ns) != 2:
rospy.ROSException(
'The controller must be called in the manipulator namespace')
self._namespace = ns[0]
self._arm_name = ns[1]
if self._namespace[0] != '/':
self._namespace = '/' + self._namespace
if self._namespace[-1] != '/':
self._namespace += '/'
# The arm interface loads the parameters from the URDF file and
# parameter server and initializes the KDL tree
self._arm_interface = ArmInterface()
self._base_link = self._arm_interface.base_link
self._tip_link = self._arm_interface.tip_link
self._tip_frame = PyKDL.Frame()
# KDL Solvers
self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_interface.chain)
self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(
self._arm_interface.chain, self._arm_interface._fk_p_kdl,
self._ik_v_kdl, 100, 1e-6)
self._dyn_kdl = PyKDL.ChainDynParam(self._arm_interface.chain,
PyKDL.Vector.Zero())
# Add a callback function to calculate the end effector's state by each
# update of the joint state
self._arm_interface.add_callback('joint_states',
self.on_update_endeffector_state)
# Publish the current manipulability index at each update of the joint
# states
# self._arm_interface.add_callback('joint_states',
# self.publish_man_index)
# Publish the end effector state
self._endeffector_state_pub = rospy.Publisher('endpoint_state',
EndPointState,
queue_size=1)
# Publish the manipulability index
self._man_index_pub = rospy.Publisher('man_index',
Float64,
queue_size=1)
self._services = dict()
# Provide the service to calculate the inverse kinematics using the KDL solver
self._services['ik'] = rospy.Service('ik_solver', SolveIK,
self.solve_ik)
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 __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__(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)
def kdl2np(asd):
x = []
for i in range(n_joints):
x.append(asd[i])
return np.asarray(x)
n_itr = 1000
for i in range(n_itr):
for j in range(n_joints):
q[j] = (q_max[j] - q_min[j]) * np.random.rand() - (q_max[j] -
q_min[j]) / 2
qdot[j] = (q_max[j] - q_min[j]) * np.random.rand() - (q_max[j] -
q_min[j]) / 2
q_kdl[j] = q[j]
qdot_kdl[j] = qdot[j]
kdl.ChainDynParam(kuka_chain, grav).JntToCoriolis(q_kdl, qdot_kdl, C_kdl)
C_u2c = C_sym(q, qdot)
for tau_idx in range(n_joints):
error[tau_idx] += np.absolute(
(kdl2np(C_kdl)[tau_idx] - u2c2np(C_u2c)[tau_idx]))
print "Errors in coriolis forces with", n_itr, "iterations and comparing against KDL:\n", error
sum_error = 0
for err in range(n_joints):
sum_error += error[err]
print "Sum of errors:\n", sum_error
def __init__(self, freq_control=100, margin_workspace=0.05):
# Load robot
urdf_model = URDF.from_parameter_server()
fetch = kdl_tree_from_urdf_model(urdf_model)
fetch_arm = fetch.getChain(BASE_LINK, END_LINK)
self.dof = fetch_arm.getNrOfJoints()
self.kdl_pos = PyKDL.ChainFkSolverPos_recursive(fetch_arm)
self.kdl_jac = PyKDL.ChainJntToJacSolver(fetch_arm)
self.kdl_dyn = PyKDL.ChainDynParam(fetch_arm,
PyKDL.Vector(0, 0, -9.81))
self.kdl_q = PyKDL.JntArray(self.dof)
self.kdl_A = PyKDL.JntSpaceInertiaMatrix(self.dof)
self.kdl_J = PyKDL.Jacobian(self.dof)
self.kdl_x = PyKDL.Frame()
# self.kdl_G = PyKDL.JntArray(self.dof)
# self.G = np.zeros((self.dof,))
# Initialize robot values
self.lock = threading.Lock()
self.thread_q = np.zeros((self.dof, ))
self.thread_dq = np.zeros((self.dof, ))
self.thread_tau = np.zeros((self.dof, ))
self.q = np.zeros((self.dof, ))
self.dq = np.zeros((self.dof, ))
self.tau = np.zeros((self.dof, ))
self.x = np.zeros((3, ))
self.quat = np.array([0., 0., 0., 1.])
self.lim_norm_x = self.compute_workspace() - margin_workspace
self.A = np.zeros((self.dof, self.dof))
self.A_inv = np.zeros((self.dof, self.dof))
self.J = np.zeros((6, self.dof))
self.q_init = np.array([
0.,
-np.pi / 4.,
0.,
np.pi / 4,
0.,
np.pi / 2,
0.,
]) # Face down
self.q_tuck = np.array([1.32, 1.40, -0.2, 1.72, 0.0, 1.66,
0.0]) # Storage position
self.q_stow = np.array([1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0])
self.q_intermediate_stow = np.array(
[0.7, -0.3, 0.0, -0.3, 0.0, -0.57, 0.0])
self.x_des = np.array([0.8, 0., 0.35]) # q_init
self.x_init = np.array([0.8, 0., 0.35]) # q_init
# self.quat_des = np.array([-0.707, 0., 0.707, 0.]) # Face up
self.quat_des = np.array([0., 0.707, 0., 0.707]) # Face down
self.state = "INITIALIZE"
print("Switching to state: " + self.state)
self.t_start = time.time()
self.freq_control = freq_control
# Initialize pub and sub
self.pub = rospy.Publisher("arm_controller/joint_torque/command",
Float64MultiArray,
queue_size=1)
sub = rospy.Subscriber(
"joint_states", JointState,
lambda joint_states: self.read_joint_sensors(joint_states))
# Initialize ROS
rospy.init_node("joint_space_controller")
def list2np(asd):
return np.asarray(asd)
n_itr = 1000
for i in range(n_itr):
for j in range(n_joints):
q[j] = (q_max[j] - q_min[j]) * np.random.rand() - (q_max[j] -
q_min[j]) / 2
q_kdl[j] = q[j]
q_np[j] = q[j]
rbdl.CompositeRigidBodyAlgorithm(gantry_rbdl, q_np, M_rbdl)
kdl.ChainDynParam(gantry_chain, gravity_kdl).JntToMass(q_kdl, M_kdl)
M_pb = pb.calculateMassMatrix(gantry_pb, q)
M_u2c = M_sym(q)
for row_idx in range(n_joints):
for col_idx in range(n_joints):
error_kdl_u2c[row_idx][col_idx] += np.absolute(
M_kdl[row_idx, col_idx] - u2c2np(M_u2c[row_idx, col_idx]))
error_rbdl_u2c[row_idx][col_idx] += np.absolute(
(M_rbdl[row_idx, col_idx]) - u2c2np(M_u2c[row_idx, col_idx]))
error_pb_u2c[row_idx][col_idx] += np.absolute(
list2np(M_pb[row_idx][col_idx]) -
u2c2np(M_u2c[row_idx, col_idx]))
error_kdl_rbdl[row_idx][col_idx] += np.absolute(
(M_rbdl[row_idx, col_idx]) - list2np(M_kdl[row_idx, col_idx]))
error_pb_kdl[row_idx][col_idx] += np.absolute(
def u2c2np(asd):
return cs.Function("temp", [], [asd])()["o0"].toarray()
def kdl2np(asd):
return np.asarray(asd)
n_itr = 1000
for i in range(n_itr):
for j in range(n_joints):
q[j] = (q_max[j] - q_min[j]) * np.random.rand() - (q_max[j] -
q_min[j]) / 2
q_kdl[j] = q[j]
kdl.ChainDynParam(kuka_chain, grav).JntToMass(q_kdl, M_kdl)
M_u2c = M_sym(q)
for row_idx in range(n_joints):
for col_idx in range(n_joints):
error[row_idx][col_idx] += np.absolute(
(kdl2np(M_kdl[row_idx, col_idx])) -
u2c2np(M_u2c[row_idx, col_idx]))
print "Errors in inertia matrix with", n_itr, "iterations and comparing against KDL:\n", error
sum_error = 0
for row in range(n_joints):
for col in range(n_joints):
sum_error += error[row][col]
print "Sum of errors:\n", sum_error
n_itr = 1000
for i in range(n_itr):
for j in range(n_joints):
q[j] = (q_max[j] - q_min[j])*np.random.rand()-(q_max[j] - q_min[j])/2
q_kdl[j] = q[j]
q_np[j] = q[j]
qdot[j] = (q_max[j] - q_min[j])*np.random.rand()-(q_max[j] - q_min[j])/2
qdot_kdl[j] = qdot[j]
qdot_np[j] = qdot[j]
zeros_pb[j] = 0.
rbdl.NonlinearEffects(gantry_rbdl, q_np, qdot_np, C_rbdl)
kdl.ChainDynParam(gantry_kdl, gravity_kdl).JntToGravity(q_kdl, g_kdl)
kdl.ChainDynParam(gantry_kdl, gravity_kdl).JntToCoriolis(q_kdl, qdot_kdl, C_kdl)
pb.setGravity(0, 0, 0)
C_pb = pb.calculateInverseDynamics(gantry_pb, q, qdot, zeros_pb)
pb.setGravity(0, 0, -9.81)
g_pb = pb.calculateInverseDynamics(gantry_pb, q, zeros_pb, zeros_pb)
g_u2c = g_sym(q)
C_u2c = C_sym(q, qdot)
for tau_idx in range(n_joints):
error_kdl_rbdl[tau_idx] += np.absolute(((list2np(g_kdl[tau_idx]) + list2np(C_kdl[tau_idx])) - C_rbdl[tau_idx]))
error_kdl_u2c[tau_idx] += np.absolute((list2np(C_kdl[tau_idx]) - u2c2np(C_u2c[tau_idx])))
error_rbdl_u2c[tau_idx] += np.absolute(((u2c2np(g_u2c[tau_idx]) + u2c2np(C_u2c)[tau_idx]) - C_rbdl[tau_idx]))
error_pb_u2c[tau_idx] += np.absolute((u2c2np(C_u2c[tau_idx]) - list2np(C_pb[tau_idx])))
for i in range(n_itr):
for j in range(n_joints):
q[j] = (q_max[j] - q_min[j]) * np.random.rand() - (q_max[j] -
q_min[j]) / 2
q_kdl[j] = q[j]
q_np[j] = q[j]
qdot[j] = (q_max[j] - q_min[j]) * np.random.rand() - (q_max[j] -
q_min[j]) / 2
qdot_kdl[j] = qdot[j]
qdot_np[j] = qdot[j]
zeros_pb[j] = 0.
rbdl.NonlinearEffects(snake_rbdl, q_np, qdot_np, C_rbdl)
kdl.ChainDynParam(snake_chain, gravity_kdl).JntToGravity(q_kdl, g_kdl)
kdl.ChainDynParam(snake_chain,
gravity_kdl).JntToCoriolis(q_kdl, qdot_kdl, C_kdl)
pb.setGravity(0, 0, 0)
C_pb = pb.calculateInverseDynamics(snake_pb, q, qdot, zeros_pb)
pb.setGravity(0, 0, -9.81)
g_pb = pb.calculateInverseDynamics(snake_pb, q, zeros_pb, zeros_pb)
g_u2c = g_sym(q)
C_u2c = C_sym(q, qdot)
for tau_idx in range(n_joints):
error_kdl_rbdl[tau_idx] += np.absolute(
((list2np(g_kdl[tau_idx]) + list2np(C_kdl[tau_idx])) -
C_rbdl[tau_idx]))
error_kdl_u2c[tau_idx] += np.absolute(
def list2np(asd):
return np.asarray(asd)
n_itr = 1000
for i in range(n_itr):
for j in range(n_joints):
q[j] = (q_max[j] - q_min[j]) * np.random.rand() - (q_max[j] -
q_min[j]) / 2
q_kdl[j] = q[j]
q_np[j] = q[j]
zeros_pb[j] = 0.
rbdl.InverseDynamics(snake_rbdl, q_np, zeros_rbdl, zeros_rbdl, g_rbdl)
kdl.ChainDynParam(snake_chain, gravity_kdl).JntToGravity(q_kdl, g_kdl)
g_pb = pb.calculateInverseDynamics(snake_pb, q, zeros_pb, zeros_pb)
g_u2c = g_sym(q)
for tau_idx in range(n_joints):
error_kdl_rbdl[tau_idx] += np.absolute(
(list2np(g_kdl[tau_idx]) - g_rbdl[tau_idx]))
error_kdl_u2c[tau_idx] += np.absolute(
(list2np(g_kdl[tau_idx]) - u2c2np(g_u2c[tau_idx])))
error_rbdl_u2c[tau_idx] += np.absolute(
(u2c2np(g_u2c[tau_idx]) - g_rbdl[tau_idx]))
error_pb_u2c[tau_idx] += np.absolute(
(u2c2np(g_u2c[tau_idx]) - list2np(g_pb[tau_idx])))
error_pb_kdl[tau_idx] += np.absolute(
(list2np(g_kdl[tau_idx]) - list2np(g_pb[tau_idx])))
error_pb_rbdl[tau_idx] += np.absolute(g_rbdl[tau_idx] -
def u2c2np(asd):
return cs.Function("temp",[],[asd])()["o0"].toarray()
def list2np(asd):
return np.asarray(asd)
n_itr = 1000
for i in range(n_itr):
for j in range(n_joints):
q[j] = (q_max[j] - q_min[j])*np.random.rand()-(q_max[j] - q_min[j])/2
q_kdl[j] = q[j]
q_np[j] = q[j]
rbdl.CompositeRigidBodyAlgorithm(snake_rbdl, q_np, M_rbdl)
kdl.ChainDynParam(snake_chain, gravity_kdl).JntToMass(q_kdl, M_kdl)
M_pb = pb.calculateMassMatrix(snake_pb, q)
M_u2c = M_sym(q)
for row_idx in range(n_joints):
for col_idx in range(n_joints):
error_kdl_u2c[row_idx][col_idx] += np.absolute(M_kdl[row_idx,col_idx] - u2c2np(M_u2c[row_idx, col_idx]))
error_rbdl_u2c[row_idx][col_idx] += np.absolute((M_rbdl[row_idx,col_idx]) - u2c2np(M_u2c[row_idx, col_idx]))
error_pb_u2c[row_idx][col_idx] += np.absolute(list2np(M_pb[row_idx][col_idx]) - u2c2np(M_u2c[row_idx, col_idx]))
error_kdl_rbdl[row_idx][col_idx] += np.absolute((M_rbdl[row_idx,col_idx]) - list2np(M_kdl[row_idx, col_idx]))
error_pb_kdl[row_idx][col_idx] += np.absolute(list2np(M_pb[row_idx][col_idx]) - list2np(M_kdl[row_idx, col_idx]))
error_pb_rbdl[row_idx][col_idx] += np.absolute((M_rbdl[row_idx,col_idx]) - list2np(M_pb[row_idx][col_idx]))
sum_error_kdl_rbdl = 0
def kdl2np(asd):
x = []
for i in range(n_joints):
x.append(asd[i])
return np.asarray(x)
n_itr = 1000
for i in range(n_itr):
for j in range(n_joints):
q[j] = (q_max[j] - q_min[j]) * np.random.rand() - (q_max[j] -
q_min[j]) / 2
q_kdl[j] = q[j]
kdl.ChainDynParam(kuka_chain, gravity_kdl).JntToGravity(q_kdl, G_kdl)
G_u2c = G_sym(q)
#print G_u2c
#print G_kdl
for tau_idx in range(n_joints):
error[tau_idx] += np.absolute(
(kdl2np(G_kdl)[tau_idx] - u2c2np(G_u2c)[tau_idx]))
print "Errors in gravity forces with", n_itr, "iterations and comparing against KDL:\n", error
sum_error = 0
for err in range(n_joints):
sum_error += error[err]
print "Sum of errors:\n", sum_error
def main():
rospy.init_node('main_test', anonymous=True)
rospy.Subscriber("/iiwa/joint_states", JointState, iiwaStateCallback)
# Initialize publishers for send computation torques.
for i in range(0,7):
torque_controller_pub.append(rospy.Publisher("/iiwa/joint" + str(i+1) + "_torque_controller/command", Float64, queue_size=10))
rate = rospy.Rate(int(1.0/dt))
# Get parameters for kinematic from setup.yaml file
base_link = rospy.get_param("/master/base_link_name")
tool_link = rospy.get_param("/master/tool_link_name")
urdf = rospy.get_param("iiwa_urdf_model")
rospy.loginfo("tip_name: %s" % tool_link)
rospy.loginfo("root_name: %s" % base_link)
data = json.load(open("/workspace/base_ws/src/profi2021_metrics_graber/data/master_tests/track_1_traj.json", "r"))
# Generate kinematic model for orocos_kdl
(ok, tree) = kdl_parser_py.urdf.treeFromString(urdf)
chain = tree.getChain(base_link, tool_link)
L = np.transpose(np.array([[1, 1, 1, 0.01, 0.01, 0.01]]))
iksolverpos = kdl.ChainIkSolverPos_LMA(chain, L)
# Generate dynamic model for orocos_kdl
grav = kdl.Vector(0, 0, -9.82)
dyn_model = kdl.ChainDynParam(chain, grav)
startControllers()
torq_msg = Float64()
counter = 0
u = kdl.JntArray(7)
q_dest = kdl.JntArray(7)
frame_dest = kdl.Frame()
# Setting up initial point
frame_dest.p[0] = data[0]["x"]
frame_dest.p[1] = data[0]["y"]
frame_dest.p[2] = 0.3
# frame_dest.p[0] = 0.3
# frame_dest.p[1] = 0.0
# frame_dest.p[2] = 0.05
frame_dest.M = kdl.Rotation.RotY(3.14)
ret = iksolverpos.CartToJnt(q, frame_dest, q_dest)
while not rospy.is_shutdown():
frame_dest.p[0] = data[int(counter/10)]["x"]
frame_dest.p[1] = data[int(counter/10)]["y"]; # Start controllers
if counter>89*10:
counter=0
ret = iksolverpos.CartToJnt(q, frame_dest, q_dest)
rospy.loginfo("IK solution: %f, %f, %f, %f, %f, %f, %f", q_dest[0], q_dest[1], q_dest[2], q_dest[3], q_dest[4], q_dest[5], q_dest[6])
dyn_model.JntToGravity(q, grav_torques)
rospy.loginfo("Estimation torque on joins: %f, %f, %f, %f, %f, %f, %f\n" % (grav_torques[0], grav_torques[1],grav_torques[2], grav_torques[3], grav_torques[4], grav_torques[5], grav_torques[6]))
for i in range(0,7):
u[i] = KP[i]*(q_dest[i]-q[i]) - KD[i]*dq[i] + grav_torques[i]
torq_msg.data = u[i]
torque_controller_pub[i].publish(torq_msg)
counter+=1
rate.sleep()
