def create_kdl_kin(base_link, end_link, urdf_filename=None, description_param="/robot_description"):
if urdf_filename is None:
robot = Robot.from_parameter_server(key=description_param)
else:
f = file(urdf_filename, 'r')
robot = Robot.from_xml_string(f.read())
f.close()
return KDLKinematics(robot, base_link, end_link)
def create_kinematic_chain_from_robot_description(
base_link="base_link",
end_link="gripper",
urdf_file=None,
robot_description="/robot_description"):
if urdf_file is None:
robot = Robot.from_parameter_server(key=robot_description)
else:
fileObj = file(urdf_file, 'r')
robot = Robot.from_xml_string(fileObj.read())
fileObj.close()
return KinematicChain(robot, base_link, end_link)
def __init__(self,
urdf_file_name,
base_link="base_link",
ee_name=_EE_NAME):
'''
Simple model of manipulator kinematics and controls
Assume following state and action vectors
urdf_file_name - model file to load
'''
# Load KDL tree
urdf_file = file(urdf_file_name, 'r')
self.robot = Robot.from_xml_string(urdf_file.read())
urdf_file.close()
self.tree = kdl_tree_from_urdf_model(self.robot)
task_space_ik_weights = np.diag([1.0, 1.0, 1.0, 0.0, 0.0,
0.0]).tolist()
#self.base_link = self.robot.get_root()
self.base_link = base_link
self.joint_chains = []
self.chain = KDLKinematics(self.robot, self.base_link, ee_name)
'''
def __init__(self, controller_namespace, mx_io):
rospy.Subscriber("/revel/eef_velocity",
Twist,
self.cart_vel_cb,
queue_size=1)
rospy.Subscriber("joint_states", JointState, self.js_cb, queue_size=1)
rospack = rospkg.RosPack()
path = rospack.get_path("svenzva_description")
full_path = path + "/robots/svenzva_arm.urdf"
f = file(full_path, 'r')
self.robot = Robot.from_xml_string(f.read())
f.close()
self.mx_io = mx_io
self.mNumJnts = 6
self.gear_ratios = [4, 7, 7, 3, 4, 1]
self.js = JointState()
self.min_limit = 20.0
self.max_limit = 100.0
self.last_twist = Twist()
self.last_cmd = []
self.last_cmd_zero = False
#Load Svenzva kinematic solver
self.jacobian_solver = SvenzvaKinematics()
self.ee_velocity = numpy.zeros(6)
self.cart_vel = Twist()
self.arm_speed_limit = rospy.get_param('arm_speed_limit', 20.0)
self.collision_check_enabled = rospy.get_param(
'collision_check_enabled', False)
if self.collision_check_enabled:
rospy.loginfo(
"MoveIt collision check ENABLED. Using environment information."
)
try:
rospy.wait_for_service('/check_state_validity')
self.state_validity_srv = rospy.ServiceProxy(
'/check_state_validity', GetStateValidity)
except rospy.ServiceException as exc:
rospy.loginfo(
"MoveIt not. Cartesian Velocity controller will not use collision checking."
)
rospy.logerr("Cartesian collision checking DISABLED")
self.collision_check_enabled = False
else:
rospy.loginfo(
"MoveIt collision check DISabled. Not using environment information."
)
self.group = None
self.loop()
def get_urdf_model(Robot):
def usage():
print("Tests for kdl_parser:\n")
print("kdl_parser <urdf file>")
print("\tLoad the URDF from file.")
print("kdl_parser")
print("\tLoad the URDF from the parameter server.")
sys.exit(1)
if len(sys.argv) > 2:
usage()
if len(sys.argv) == 2 and (sys.argv[1] == "-h" or sys.argv[1] == "--help"):
usage()
if (len(sys.argv) == 1):
robot = Robot.from_parameter_server()
else:
f = file(sys.argv[1], 'r')
robot = Robot.from_xml_string(f.read())
f.close()
return robot
def main():
import sys
def usage():
print("Tests for kdl_parser:\n")
print("kdl_parser <urdf file>")
print("\tLoad the URDF from file.")
print("kdl_parser")
print("\tLoad the URDF from the parameter server.")
sys.exit(1)
if len(sys.argv) > 2:
usage()
if len(sys.argv) == 2 and (sys.argv[1] == "-h" or sys.argv[1] == "--help"):
usage()
if (len(sys.argv) == 1):
robot = Robot.from_parameter_server()
else:
f = file(sys.argv[1], 'r')
robot = Robot.from_xml_string(f.read())
f.close()
tree = kdl_tree_from_urdf_model(robot)
num_non_fixed_joints = 0
for j in robot.joint_map:
if robot.joint_map[j].joint_type != 'fixed':
num_non_fixed_joints += 1
print("URDF non-fixed joints: %d;" % num_non_fixed_joints)
print("KDL joints: %d" % tree.getNrOfJoints())
print("URDF joints: %d; KDL segments: %d" %
(len(robot.joint_map), tree.getNrOfSegments()))
import random
base_link = robot.get_root()
end_link = robot.link_map.keys()[random.randint(0,
len(robot.link_map) - 1)]
chain = tree.getChain(base_link, end_link)
print("Root link: %s; Random end link: %s" % (base_link, end_link))
for i in range(chain.getNrOfSegments()):
print chain.getSegment(i).getName()
##
# Write in the list form
def write_to_list(self):
joint_names = self.urdf_model_.get_chain(self.base_link_,
self.end_link_,
links=False,
fixed=False)
list_fk = []
for it in joint_names:
joint = self.urdf_model_.joint_map[it]
trans = self.forward_kinematics(joint.child, joint.parent)
trans_wrt_origin = self.forward_kinematics(end_link=joint.child)
list_fk.append((joint.name, trans, trans_wrt_origin))
return list_fk
if __name__ == '__main__':
rospy.init_node('Forward_kinematics', anonymous=True)
create_kinematic_chain_from_robot_description()
if not rospy.is_shutdown():
robot = Robot.from_parameter_server()
kdl_chanin = KinematicChain(robot)
list_fk = kdl_chanin.write_to_list()
print "joint_0: ", list_fk[0]
else:
rospy.logerr("Try to connect ROS")
def __init__(self, hebi_group_name, hebi_mapping, hebi_gains, urdf_str, base_link, end_link):
rospy.loginfo("Creating {} instance".format(self.__class__.__name__))
self.openmeta_testbench_manifest_path = rospy.get_param('~openmeta/testbench_manifest_path', None)
if self.openmeta_testbench_manifest_path is not None:
self._testbench_manifest = load_json_file(self.openmeta_testbench_manifest_path)
# TestBench Parameters
self._params = {}
for tb_param in self._testbench_manifest['Parameters']:
self._params[tb_param['Name']] = tb_param['Value'] # WARNING: If you use these values - make sure to check the type
# TestBench Metrics
self._metrics = {}
for tb_metric in self._testbench_manifest['Metrics']: # FIXME: Hmm, this is starting to look a lot like OpenMDAO...
self._metrics[tb_metric['Name']] = tb_metric['Value']
if hebi_gains is None:
hebi_gains = {
'p': [float(self._params['p_gain'])]*3,
'i': [float(self._params['i_gain'])]*3,
'd': [float(self._params['d_gain'])]*3
}
hebi_families = []
hebi_names = []
for actuator in hebi_mapping:
family, name = actuator.split('/')
hebi_families.append(family)
hebi_names.append(name)
rospy.loginfo(" hebi_group_name: %s", hebi_group_name)
rospy.loginfo(" hebi_families: %s", hebi_families)
rospy.loginfo(" hebi_names: %s", hebi_names)
self.hebi_wrap = HebirosWrapper(hebi_group_name, hebi_families, hebi_names)
# Set PID Gains
cmd_msg = CommandMsg()
cmd_msg.name = hebi_mapping
cmd_msg.settings.name = hebi_mapping
cmd_msg.settings.control_strategy = [4, 4, 4]
cmd_msg.settings.position_gains.name = hebi_mapping
cmd_msg.settings.position_gains.kp = hebi_gains['p']
cmd_msg.settings.position_gains.ki = hebi_gains['i']
cmd_msg.settings.position_gains.kd = hebi_gains['d']
cmd_msg.settings.position_gains.i_clamp = [0.25, 0.25, 0.25] # TODO: Tune me.
self.hebi_wrap.send_command_with_acknowledgement(cmd_msg)
if base_link is None or end_link is None:
robot = Robot.from_xml_string(urdf_str)
if not base_link:
base_link = robot.get_root()
# WARNING: There may be multiple leaf nodes
if not end_link:
end_link = [x for x in robot.link_map.keys() if x not in robot.child_map.keys()][0]
# pykdl
self.kdl_fk = KDLKinematics(URDF.from_xml_string(urdf_str), base_link, end_link)
self._active_joints = self.kdl_fk.get_joint_names()
# joint data
self.position_fbk = [0.0]*self.hebi_wrap.hebi_count
self.velocity_fbk = [0.0]*self.hebi_wrap.hebi_count
self.effort_fbk = [0.0]*self.hebi_wrap.hebi_count
# joint state publisher
while not rospy.is_shutdown() and len(self.hebi_wrap.get_joint_positions()) < len(self.hebi_wrap.hebi_mapping):
rospy.sleep(0.1)
self._joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=1)
self.hebi_wrap.add_feedback_callback(self._joint_state_cb)
# Set up Waypoint/Trajectory objects
self.start_wp = WaypointMsg()
self.start_wp.names = self.hebi_wrap.hebi_mapping
self.end_wp = copy.deepcopy(self.start_wp)
self.goal = TrajectoryGoal()
self.goal.waypoints = [self.start_wp, self.end_wp]
self._hold_positions = [0.0]*3
self._hold = True
self.jointstate = JointState()
self.jointstate.name = self.hebi_wrap.hebi_mapping
rospy.sleep(1.0)
def main():
import sys
def usage():
print("Tests for kdl_parser:\n")
print("kdl_parser <urdf file>")
print("\tLoad the URDF from file.")
print("kdl_parser")
print("\tLoad the URDF from the parameter server.")
sys.exit(1)
if len(sys.argv) > 2:
usage()
if len(sys.argv) == 2 and (sys.argv[1] == "-h" or sys.argv[1] == "--help"):
usage()
if (len(sys.argv) == 1):
robot = Robot.from_parameter_server()
else:
f = file(sys.argv[1], 'r')
robot = Robot.from_xml_string(f.read())
f.close()
if True:
import random
base_link = robot.get_root()
end_link = robot.link_map.keys()[random.randint(
0,
len(robot.link_map) - 1)]
print "Root link: %s; Random end link: %s" % (base_link, end_link)
kdl_kin = KDLKinematics(robot, base_link, end_link)
q = kdl_kin.random_joint_angles()
print "Random angles:", q
pose = kdl_kin.forward(q)
print "FK:", pose
q_new = kdl_kin.inverse(pose)
print "IK (not necessarily the same):", q_new
if q_new is not None:
pose_new = kdl_kin.forward(q_new)
print "FK on IK:", pose_new
print "Error:", np.linalg.norm(pose_new * pose**-1 -
np.mat(np.eye(4)))
else:
print "IK failure"
J = kdl_kin.jacobian(q)
print "Jacobian:", J
M = kdl_kin.inertia(q)
print "Inertia matrix:", M
if False:
M_cart = kdl_kin.cart_inertia(q)
print "Cartesian inertia matrix:", M_cart
if True:
rospy.init_node("kdl_kinematics")
num_times = 20
while not rospy.is_shutdown() and num_times > 0:
base_link = robot.get_root()
end_link = robot.link_map.keys()[random.randint(
0,
len(robot.link_map) - 1)]
print "Root link: %s; Random end link: %s" % (base_link, end_link)
kdl_kin = KDLKinematics(robot, base_link, end_link)
q = kdl_kin.random_joint_angles()
pose = kdl_kin.forward(q)
q_guess = kdl_kin.random_joint_angles()
q_new = kdl_kin.inverse(pose, q_guess)
if q_new is None:
print "Bad IK, trying search..."
q_search = kdl_kin.inverse_search(pose)
pose_search = kdl_kin.forward(q_search)
print "Result error:", np.linalg.norm(pose_search * pose**-1 -
np.mat(np.eye(4)))
num_times -= 1
# Python imports
from PyKDL import *
# Python Libs
import numpy as np
# Ros Libs
import rospy
import math
from math import *
# Math Utils
from math_utils import *
f = file('', 'r')
robot = Robot.from_xml_string(
f.read(
"/home/mike/catkin_ws/src/csl_underwater_sim_gazebo/robot_description/lbv150_uvms.urdf"
))
f.close()
# Get Tree
tree = kdl_tree_from_urdf_model(robot)
#tree = treeFromUrdfModel(robot)
# Get Chain
arm_chain = tree.getChain("base_link", "arm_tool_link")
# Create A Chain For the Platform
rov_chain = Chain()
#From Global Frame to Platform Vehicle Center Frame (when * is third)
#Translation along X axis
