def execute_cb(self,goal):
self.moving_to_new_x_pos = False
self.moving_to_new_y_pos = False
self.stop_base_movement = False
self.max_virtual_x_vel = 0.05
self.max_virtual_y_vel = 0.05
self.commanded_virtual_x_pos = 0.0
self.commanded_virtual_y_pos = 0.0
self.commanded_virtual_x_vel = 0.0
self.commanded_virtual_y_vel = 0.0
self.virtual_x_cmd_time_sec = 0.0
self.virtual_y_cmd_time_sec = 0.0
self.virtual_x_running_time_sec = 0.0
self.virtual_y_running_time_sec = 0.0
youbot_urdf = URDF.from_parameter_server()
self.kin_with_virtual = KDLKinematics(youbot_urdf, "virtual", "gripper_palm_link")
self.kin_grasp = KDLKinematics(youbot_urdf, "base_link", "gripper_palm_link")
# Create a timer that will be used to monitor the velocity of the virtual
# joints when we need them to be positioning themselves.
self.vel_monitor_timer = rospy.Timer(rospy.Duration(0.1), self.vel_monitor_timer_callback)
self.arm_joint_pub = rospy.Publisher("arm_1/arm_controller/position_command",JointPositions,queue_size=10)
self.gripper_pub = rospy.Publisher("arm_1/gripper_controller/position_command", JointPositions, queue_size=10)
self.vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
# Give the publishers time to get setup before trying to do any actual work.
rospy.sleep(2)
# Initialize at the candle position.
self.publish_arm_joint_positions(armJointPosCandle)
rospy.sleep(2.0)
#self.publish_arm_joint_positions(armJointPos1)
#rospy.sleep(3.0)
#self.publish_arm_joint_positions(armJointPos2)
#rospy.sleep(10.0)
#self.publish_arm_joint_positions(armJointPosCandle)
#rospy.sleep(2.0)
# Go through the routine of picking up the block.
self.grasp_routine()
self._result.successOrNot=1
self._as.set_succeeded(self._result)
def jointCartesianPos_server():
rospy.init_node('jointCartesianPos_server')
global right_kin
robot = URDF.from_parameter_server(key='robot_description')
base_link = 'base'
right_end_link = 'right_gripper'
right_kin = KDLKinematics(robot, base_link, right_end_link)
global left_kin
left_end_link = 'left_gripper'
left_kin = KDLKinematics(robot, base_link, right_end_link)
s = rospy.Service('getCartesianJointPos', getCartesianJointPos,
handle_jointCartesianPos)
rospy.spin()
def __init__(self, config):
base_link = config['base_link']
end_link = config['end_link']
if 'robot_description_param' in config:
self.robot = URDF.from_parameter_server(
config['robot_description_param'])
else:
self.robot = URDF.from_parameter_server()
self.config = config
self.base_link = base_link
# set up kinematics stuff
self.tree = kdl_tree_from_urdf_model(self.robot)
self.chain = self.tree.getChain(base_link, end_link)
self.kdl_kin = KDLKinematics(self.robot, base_link, end_link)
self.base_link = base_link
self.end_link = end_link
self.skill_instances = {}
self.skill_features = {}
self.skill_models = {}
self.parent_skills = {}
self.pubs = {}
def __init__(self):
rospy.loginfo("Creating IK Controller class")
# setup flags and useful vars
self.running_flag = False
self.step_scale = rospy.get_param("~scale", STEP_SCALE)
self.freq = rospy.get_param("~freq", FREQ)
self.arm = rospy.get_param("~arm", ARM)
self.q = np.zeros(7)
with cl.suppress_stdout_stderr():
self.urdf = URDF.from_parameter_server()
self.kin = KDLKinematics(self.urdf, "base", "%s_hand"%self.arm)
self.goal = np.array(self.kin.forward(self.q))
self.mutex = threading.Lock()
self.joint_vel_limit = rospy.get_param("~joint_vel_limit", JOINT_VEL_LIMIT)
self.q_sim = np.zeros(7)
self.damping = rospy.get_param("~damping", DAMPING)
self.joint_names = self.kin.get_joint_names()
self.qdot_dict = {}
self.limb_interface = intera_interface.Limb()
self.ep = EndpointState()
# create all subscribers, publishers, and timers
self.int_timer = rospy.Timer(rospy.Duration(1/float(self.freq)), self.ik_step_timercb)
self.actual_js_sub = rospy.Subscriber("robot/joint_states", JointState, self.actual_js_cb)
self.endpoint_update_cb = rospy.Subscriber("robot/limb/right/endpoint_state", EndpointState, self.endpoint_upd_cb)
def __init__(self, max_reach=1.1, group_name="panda_arm"):
# Moveit init
moveit_commander.roscpp_initialize(sys.argv)
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.move_group = moveit_commander.MoveGroupCommander(group_name)
self.move_group.set_num_planning_attempts(10)
self.move_group.set_planning_time(2)
self.max_vel = 1.0
self.max_acc = 0.1
self.move_group.set_max_velocity_scaling_factor(
self.max_vel) # Allow 10 % of the set maximum joint velocities
self.move_group.set_max_acceleration_scaling_factor(self.max_acc)
# URDF and kinematics init
self.urdf_robot = URDF.from_parameter_server()
self.kdl_kin = KDLKinematics(self.urdf_robot,
self.urdf_robot.links[1].name,
self.urdf_robot.links[9].name)
self.max_reach = max_reach # Maximum distance from 0,0,0 the robot might be able to reach
# Start working
self.go_home()
def __init__(self,robot,base_link,end_link,group,
move_group_ns="move_group",
planning_scene_topic="planning_scene",
robot_ns="",
verbose=False,
kdl_kin=None,
closed_form_IK_solver = None,
joint_names=[]):
self.robot = robot
self.tree = kdl_tree_from_urdf_model(self.robot)
self.chain = self.tree.getChain(base_link, end_link)
if kdl_kin is None:
self.kdl_kin = KDLKinematics(self.robot, base_link, end_link)
else:
self.kdl_kin = kdl_kin
self.base_link = base_link
self.joint_names = joint_names
self.end_link = end_link
self.group = group
self.robot_ns = robot_ns
self.client = actionlib.SimpleActionClient(move_group_ns, MoveGroupAction)
self.verbose = verbose
self.closed_form_IK_solver = closed_form_IK_solver
self.closed_form_ur5_ik = InverseKinematicsUR5()
# self.closed_form_ur5_ik.enableDebugMode()
self.closed_form_ur5_ik.setEERotationOffsetROS()
self.closed_form_ur5_ik.setJointWeights([6,5,4,3,2,1])
self.closed_form_ur5_ik.setJointLimits(-np.pi, np.pi)
def get_jacabian_from_joint(self, urdfname, jointq, flag):
#robot = URDF.from_xml_file("/data/ros/ur_ws/src/universal_robot/ur_description/urdf/ur5.urdf")
robot = URDF.from_xml_file(urdfname)
tree = kdl_tree_from_urdf_model(robot)
# print tree.getNrOfSegments()
chain = tree.getChain("base_link", "ee_link")
# print chain.getNrOfJoints()
# forwawrd kinematics
kdl_kin = KDLKinematics(robot, "base_link", "ee_link")
q = jointq
#q = [0, 0, 1, 0, 1, 0]
pose = kdl_kin.forward(
q) # forward kinematics (returns homogeneous 4x4 matrix)
# print pose
#print list(pose)
q0 = Kinematic()
if flag == 1:
q_ik = q0.best_sol_for_other_py([1.] * 6, 0, q0.Forward(q))
else:
q_ik = kdl_kin.inverse(pose) # inverse kinematics
# print "----------iverse-------------------\n", q_ik
if q_ik is not None:
pose_sol = kdl_kin.forward(q_ik) # should equal pose
print "------------------forward ------------------\n", pose_sol
J = kdl_kin.jacobian(q)
#print 'J:', J
return J, pose
def __init__(self):
# Read the controllers parameters
gains = read_parameter('~gains', dict())
self.kp = joint_list_to_kdl(gains['Kp'])
self.kd = joint_list_to_kdl(gains['Kd'])
self.publish_rate = read_parameter('~publish_rate', 500)
self.frame_id = read_parameter('~frame_id', 'base_link')
self.tip_link = read_parameter('~tip_link', 'end_effector')
# Kinematics
self.urdf = URDF.from_parameter_server(key='robot_description')
self.kinematics = KDLKinematics(self.urdf, self.frame_id,
self.tip_link)
# Get the joint names and limits
self.joint_names = self.kinematics.get_joint_names()
self.num_joints = len(self.joint_names)
# Set-up publishers/subscribers
self.torque_pub = dict()
for i, name in enumerate(self.joint_names):
self.torque_pub[name] = rospy.Publisher('/%s/command' % (name),
Float64)
rospy.Subscriber('/joint_states', JointState, self.joint_states_cb)
rospy.Subscriber('/grips/endpoint_state', EndpointState,
self.endpoint_state_cb)
rospy.Subscriber('/grips/ik_command', PoseStamped, self.ik_command_cb)
rospy.loginfo('Running Cartesian controller for Grips')
# Start torque controller timer
self.cart_command = None
self.endpoint_state = None
self.joint_states = None
self.torque_controller_timer = rospy.Timer(
rospy.Duration(1.0 / self.publish_rate), self.torque_controller_cb)
# Shutdown hookup to clean-up before killing the script
rospy.on_shutdown(self.shutdown)
def main(urdf_file, bag_file, out_file):
robot = URDF.from_xml_file(urdf_file)
kdl_kin = KDLKinematics(robot, BASE_LINK, END_LINK)
joint_names = kdl_kin.get_joint_names()
link_names = kdl_kin.get_link_names()
csv_headers = ['time'] + [
link + axis
for link, axis in itertools.product(link_names, AXIS_SUFFIX)
]
with open(out_file, 'wb') as f:
writer = csv.DictWriter(f,
fieldnames=csv_headers,
quoting=csv.QUOTE_MINIMAL)
writer.writeheader()
bag = rosbag.Bag(bag_file)
for topic, msg, t in bag.read_messages(topics=['/joint_states']):
joint_vals = {
name: val
for name, val in zip(msg.name, msg.position)
}
q = [joint_vals[n] for n in joint_names]
vals = {'time': t.secs + 1e-9 * t.nsecs}
for i, link in enumerate(link_names):
pos = kdl_kin._do_kdl_fk(q, i)[0:3, 3].ravel().tolist()[0]
vals.update({
link + axis: pos[val]
for val, axis in enumerate(AXIS_SUFFIX)
})
writer.writerow(vals)
def __init__(self):
"""Read robot describtion"""
self.robot_ = URDF.from_parameter_server()
self.kdl_kin_ = KDLKinematics(self.robot_, 'base_link', 'end_effector')
self.cur_pose_ = None
self.cur_jnt_ = None
# Creates the SimpleActionClient, passing the type of the action
self.client_ = actionlib.SimpleActionClient(
'/mitsubishi_arm/mitsubishi_trajectory_controller/follow_joint_trajectory',
control_msgs.msg.FollowJointTrajectoryAction)
self.client_.wait_for_server()
self.goal_ = control_msgs.msg.FollowJointTrajectoryGoal()
#time_from_start=rospy.Duration(10)
self.goal_.trajectory.joint_names = [
'j1', 'j2', 'j3', 'j4', 'j5', 'j6'
]
#To be reached 1 second after starting along the trajectory
trajectory_point = trajectory_msgs.msg.JointTrajectoryPoint()
trajectory_point.positions = [0] * 6
trajectory_point.time_from_start = rospy.Duration(0.1)
self.goal_.trajectory.points.append(trajectory_point)
rospy.Subscriber('joint_states', sensor_msgs.msg.JointState,
self.fwd_kinematics_cb)
rospy.Subscriber('command', geometry_msgs.msg.Pose,
self.inv_kinematics_cb)
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, robot):
self.reaching_bound_pos = 0.03
self.reaching_bound_rot = 0.3
self.reaching_target = [
[0.5, 0.05, 0.8, -1.0, 0.0, 0.0, 6.123233995736766e-17],
[0.5, 0, 0.9, -1.0, 0.0, 0.0, 6.123233995736766e-17],
[0.45, 0.15, 0.9, -1.0, 0.0, 0.0, 6.123233995736766e-17],
[
0.4, 0.1, 0.9, -0.9659258262890683, 1.5848e-17, -0.2588,
5.91e-17
],
[
0.4, 0.1, 0.9, -0.8660254037844387, 3.0616e-17, -0.4999,
5.30e-17
],
[
0.45, 0.15, 0.9, -0.7500000000000001, -0.24999999999999986,
-0.4330127018922193, -0.4330127018922192
]
]
self.num_target = len(self.reaching_target)
self.CubeState = LinkState()
self.CubeState.link_name = "jaco_on_table::Cube3"
self.CubeState.reference_frame = "jaco_on_table::robot_base"
cube_topic = "/gazebo/set_link_state"
self.pub_cube = rospy.Publisher(cube_topic,
gazebo_msgs.msg.LinkState,
queue_size=1)
self.kdl_kin = KDLKinematics(robot, "robot_base",
"jaco_fingers_base_link")
self.current_target = 0
self.end_game = False
self.time_taken = [0] * self.num_target
self.current_time = time.time()
def __init__(self):
rospy.loginfo("Creating VelocityController class")
# Create KDL model
with cl.suppress_stdout_stderr(): # Eliminates urdf tag warnings
self.robot = URDF.from_parameter_server()
self.kin = KDLKinematics(self.robot, "base", "right_hand")
self.names = self.kin.get_joint_names()
# Limb interface
self.arm = intera_interface.Limb("right")
self.hand = intera_interface.gripper.Gripper('right')
# Grab M0 and Blist from saywer_MR_description.py
self.M0 = s.M #Zero config of right_hand
self.Blist = s.Blist #6x7 screw axes mx of right arm
# Shared variables
self.mutex = threading.Lock()
self.time_limit = rospy.get_param("~time_limit", TIME_LIMIT)
self.damping = rospy.get_param("~damping", DAMPING)
self.joint_vel_limit = rospy.get_param("~joint_vel_limit", JOINT_VEL_LIMIT)
self.q = np.zeros(7) # Joint angles
self.qdot = np.zeros(7) # Joint velocities
self.T_goal = np.array(self.kin.forward(self.q)) # Ref se3
# Subscriber
self.ref_pose_sub = rospy.Subscriber('ref_pose', Pose, self.ref_pose_cb)
self.hand.calibrate()
self.r = rospy.Rate(100)
while not rospy.is_shutdown():
self.calc_joint_vel()
self.r.sleep()
def __init__(self, rate=100):
robot = URDF.from_xml_file(
"/home/pedge/catkin_ws/src/baxter_force_control/urdf/box.urdf")
self.kdl_kin = KDLKinematics(robot, 'base_link', 'human')
self.sub = None
self.timer = None
self.vel_control = rospy.get_param('~vel_control', True)
rospy.set_param("~vel_control", self.vel_control)
self.rate = rate
self.current = copy(self.start_pos)
self.pub = rospy.Publisher('robot/joint_states',
JointState,
queue_size=100)
self.srv = rospy.Service('~switch', Empty, self.switch)
self.left_endpoint_pub = rospy.Publisher(
'/robot/limb/left/endpoint_state', EndpointState, queue_size=100)
self.right_endpoint_pub = rospy.Publisher(
'/robot/limb/right/endpoint_state', EndpointState, queue_size=100)
self.box_sub = rospy.Subscriber('box/human_grip', PoseStamped,
self.update_box_joints)
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
self.endpoint_sub = rospy.Subscriber('tf', TFMessage,
self.publish_endpoints)
if self.vel_control:
self.vel_init()
else:
self.pos_init()
rospy.loginfo("Initialization complete.")
def __init__(self, urdf, base_link='base_link', endpoint='human', initial_alpha=None):
self.robot = URDF.from_xml_file(urdf)
self.kdl_kin = KDLKinematics(self.robot, base_link, endpoint)
num_joints = self.kdl_kin.num_joints
super(BoxOptimizer, self).__init__(num_joints, self.kdl_kin.jacobian, self.kdl_kin.jacobian, 'joint_states',
np.zeros(num_joints), np.random.random(3),initial_alpha,
self.kdl_kin.random_joint_angles)
def __init__(self):
super(TomDataset, self).__init__("TOM")
# hold bag file names
self.trash_bags = []
self.box_bags = []
# hold demos
self.box = []
self.trash = []
self.trash_poses = []
self.box_poses = []
self.move_poses = []
self.lift_poses = []
self.test_poses = []
self.pickup_poses = []
self.trash_data = []
self.box_data = []
self.move_data = []
self.lift_data = []
self.test_data = []
self.pickup_data = []
self.robot = URDF.from_parameter_server()
self.base_link = "torso_link"
self.end_link = "r_gripper_base_link"
# set up kinematics stuff
self.tree = kdl_tree_from_urdf_model(self.robot)
self.chain = self.tree.getChain(self.base_link, self.end_link)
self.kdl_kin = KDLKinematics(self.robot, self.base_link, self.end_link)
def get_jacabian_from_joint(self, urdfname, jointq):
robot = URDF.from_xml_file(urdfname)
tree = kdl_tree_from_urdf_model(robot)
# print tree.getNrOfSegments()
chain = tree.getChain("base_link", "ee_link")
# print chain.getNrOfJoints()
# forwawrd kinematics
kdl_kin = KDLKinematics(robot, "base_link", "ee_link")
q = jointq
#q = [0, 0, 1, 0, 1, 0]
pose = kdl_kin.forward(
q) # forward kinematics (returns homogeneous 4x4 matrix)
# # print pose
# #print list(pose)
# q0=Kinematic(q)
# if flag==1:
# q_ik=q0.best_sol_for_other_py( [1.] * 6, 0, q0.Forward())
# else:
# q_ik = kdl_kin.inverse(pose) # inverse kinematics
# # print "----------iverse-------------------\n", q_ik
#
# if q_ik is not None:
# pose_sol = kdl_kin.forward(q_ik) # should equal pose
# print "------------------forward ------------------\n",pose_sol
J = kdl_kin.jacobian(q)
#print 'J:', J
return J, pose
def __init__(self,
robot,
base_link,
end_link,
group,
move_group_ns="move_group",
planning_scene_topic="planning_scene",
robot_ns="",
verbose=False,
kdl_kin=None,
closed_form_IK_solver=None,
joint_names=[]):
self.robot = robot
self.tree = kdl_tree_from_urdf_model(self.robot)
self.chain = self.tree.getChain(base_link, end_link)
if kdl_kin is None:
self.kdl_kin = KDLKinematics(self.robot, base_link, end_link)
else:
self.kdl_kin = kdl_kin
self.base_link = base_link
self.joint_names = joint_names
self.end_link = end_link
self.group = group
self.robot_ns = robot_ns
self.client = actionlib.SimpleActionClient(move_group_ns,
MoveGroupAction)
self.verbose = verbose
self.closed_form_IK_solver = closed_form_IK_solver
def __init__(self,
robot,
base_link,
end_link,
group,
move_group_ns="move_group",
planning_scene_topic="planning_scene",
robot_ns="",
verbose=False,
kdl_kin=None,
closed_form_IK_solver=None,
joint_names=[]):
self.robot = robot
self.tree = kdl_tree_from_urdf_model(self.robot)
self.chain = self.tree.getChain(base_link, end_link)
if kdl_kin is None:
self.kdl_kin = KDLKinematics(self.robot, base_link, end_link)
else:
self.kdl_kin = kdl_kin
self.base_link = base_link
self.joint_names = joint_names
self.end_link = end_link
self.group = group
self.robot_ns = robot_ns
self.client = actionlib.SimpleActionClient(move_group_ns,
MoveGroupAction)
self.acceleration_magnification = 1
rospy.wait_for_service('compute_cartesian_path')
self.cartesian_path_plan = rospy.ServiceProxy('compute_cartesian_path',
GetCartesianPath)
self.verbose = verbose
self.closed_form_IK_solver = closed_form_IK_solver
def configure(self):
if self.configured:
return False
rospy.init_node("arm_ik_controller", argv=sys.argv, anonymous=False)
self.tf_listener = tf.TransformListener()
#Parse the URDF tree
self.robot = URDF.from_parameter_server()
#self.tree = kdl_tree_from_urdf_model(self.robot)
#self.chain = self.tree.getChain(self.tf_base_link_name, self.tf_end_link_name)
self.kdl_kin = KDLKinematics(self.robot, self.tf_base_link_name,
self.tf_end_link_name)
#Variables holding the joint information
self.arm_joint_names = self.kdl_kin.get_joint_names()
rospy.loginfo("The joint names controlled will be:")
rospy.loginfo("%s" % self.arm_joint_names)
#Mini Vector field
#self.minivf.configure(self.tf_base_link_name, self.tf_end_link_name)
#Data structure containing the goals for the cubes
self.arm_setpoints = dict()
self.arm_jointdata = dict()
for name in self.arm_joint_names:
self.arm_setpoints[name] = {'stiff': 15.0, 'eq_point': 0.0}
self.arm_jointdata[name] = {'pos': 0.0}
self.cart_goal = None
self.fresh_cart_goal = False
self.interpolator.configure()
#Start the subscriber
rospy.Subscriber(self.ros_transform_topic_name,
geometry_msgs.msg.TransformStamped,
self.cb_command,
queue_size=1)
#Another subscriber for the stiffness data
rospy.Subscriber(self.ros_stiff_topic_name, CubeStiffArray,
self.cb_stiff_command)
#Subscriber to find the current joint positions
rospy.Subscriber(self.ros_cube_joint_states_topic_name, JointState,
self.cb_joint_states)
#Prepare our publisher
#self.cubes_pub = rospy.Publisher("/iai_qb_cube_driver/command", CubeCmdArray, queue_size=3)
self.cubes_pub = rospy.Publisher(self.ros_cube_control_out_topic_name,
CubeCmdArray,
queue_size=3)
#Set up a timer for the function talking to the cubes
rospy.Timer(rospy.Duration(0.05), self.cb_cube_controller)
return True
def get_data_from_kdl(self):
robot = URDF.from_xml_file(self.urdfname)
tree = kdl_tree_from_urdf_model(robot)
chain = tree.getChain("base_link", "ee_link")
# print chain.getNrOfJoints()
# forwawrd kinematics
kdl_kin = KDLKinematics(robot, "base_link", "ee_link")
return kdl_kin
def __init__(self):
self.robot = self.init_robot(
"/home/zy/catkin_ws/src/aubo_robot/aubo_description/urdf/aubo_i5.urdf"
)
self.kdl_kin = KDLKinematics(self.robot, "base_link", "wrist3_Link")
self.tree = kdl_tree_from_urdf_model(self.robot)
self.chain = self.tree.getChain("base_link", "wrist3_Link")
self.q = [0, 0, 0, 0, 0, 0]
def get_jacabian_from_joint(self, urdfname, jointq, flag):
robot = URDF.from_xml_file(urdfname)
tree = kdl_tree_from_urdf_model(robot)
chain = tree.getChain("base_link", "ee_link")
kdl_kin = KDLKinematics(robot, "base_link", "ee_link")
J = kdl_kin.jacobian(jointq)
pose = kdl_kin.forward(jointq)
return J, pose
def __init__(self):
self.robot = URDF.from_parameter_server()
self.kdl_kin = KDLKinematics(self.robot, "base_link", "end1")
self.end_pos_e = np.array([0, 0, 0, 1])
self.end_pos_e = self.end_pos_e.reshape(self.end_pos_e.shape[0], 1)
self.ik_solver = IK("base_link", "end1")
self.seed_state = [0.0] * self.ik_solver.number_of_joints
def fwd_kin(self):
self.robot = URDF.from_parameter_server()
self.kin = KDLKinematics(self.robot, BASE, EE_FRAME)
g = self.kin.forward(self.q)
p = np.array(g[0:3,-1]).ravel()
self.pose.pose.position = Point(*p.ravel())
self.pose.pose.orientation = Quaternion(*tr.quaternion_from_matrix(g))
return
def __init__(self, urdf_str):
self.urdf_tree = URDF.from_xml_string(urdf_str)
base_link = "link0"
nlinks = len(self.urdf_tree.link_map.keys())
end_link = "link{}".format(nlinks - 1)
print("Link chain goes from {} to {}".format(base_link, end_link))
self.kdl_kin = KDLKinematics(self.urdf_tree, base_link, end_link)
self.ik_solver = IK(base_link, end_link, urdf_string=urdf_str)
def __init__(self):
# rospy.init_node('grasp_kdl')
self.robot = URDF.from_parameter_server()
base_link = 'world'
end_link = 'palm_link'
self.kdl_kin = KDLKinematics(self.robot, base_link, end_link)
self.ik_solver = IK(base_link, end_link)
self.seed_state = [0.0] * self.ik_solver.number_of_joints
def __init__(self, hebiros_wrapper, urdf_str, base_link, end_link):
"""SMACH State
:type hebiros_wrapper: HebirosWrapper
:param hebiros_wrapper: HebirosWrapper instance for Leg HEBI group
:type urdf_str: str
:param urdf_str: Serialized URDF str
:type base_link: str
:param base_link: Leg base link name in URDF
:type end_link: str
:param end_link: Leg end link name in URDF
"""
State.__init__(self,
outcomes=['ik_failed', 'success'],
input_keys=[
'prev_joint_pos', 'target_end_link_point',
'execution_time'
],
output_keys=['prev_joint_pos', 'active_joints'])
self.hebi_wrap = hebiros_wrapper
self.urdf_str = urdf_str
self.base_link = base_link
self.end_link = end_link
self.active = False
# hardware interface
self._hold_leg_position = True
self._hold_joint_angles = []
self.hebi_wrap.add_feedback_callback(self._hold_leg_pos_cb)
# pykdl
self.kdl_fk = KDLKinematics(URDF.from_xml_string(urdf_str), base_link,
end_link)
self._active_joints = self.kdl_fk.get_joint_names()
# trac-ik
self.trac_ik = IK(base_link,
end_link,
urdf_string=urdf_str,
timeout=0.01,
epsilon=1e-4,
solve_type="Distance")
self.ik_pos_xyz_bounds = [0.01, 0.01, 0.01]
self.ik_pos_wxyz_bounds = [31416.0, 31416.0, 31416.0
] # NOTE: This implements position-only IK
# 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)
def __init__(self, sim=True):
f = file('/home/hirolab/catkin_ws/src/vspgrid/urdf/widowx2.urdf', 'r')
robot = URDF.from_xml_string(f.read()) # parsed URDF
# robot = URDF.from_parameter_server()
self.kin = KDLKinematics(robot, 'base', 'mconn')
# Selection of end-effector parameters for WidowX
# x, y, z, yaw, pitch
self.cart_from_matrix = lambda T: np.array([
T[0, 3], T[1, 3], T[2, 3],
math.atan2(T[1, 0], T[0, 0]), -math.asin(T[2, 0])
])
self.home = np.array([0.05, 0, 0.25, 0, 0]) # home end-effector pose
self.q = np.array([0, 0, 0, 0, 0]) # joint angles
self.dt = 0.05 # algorithm time step
self.sim = sim # true if virtual robot
def publish(eventStop):
# for arbotix
pub1 = rospy.Publisher("q1/command", Float64, queue_size=5)
pub2 = rospy.Publisher("q2/command", Float64, queue_size=5)
pub3 = rospy.Publisher("q3/command", Float64, queue_size=5)
pub4 = rospy.Publisher("q4/command", Float64, queue_size=5)
pub5 = rospy.Publisher("q5/command", Float64, queue_size=5)
# for visualization
jointPub = rospy.Publisher("/joint_states",
JointState,
queue_size=5)
jmsg = JointState()
jmsg.name = ['q1', 'q2', 'q3', 'q4', 'q5']
while not rospy.is_shutdown() and not eventStop.is_set():
jmsg.header.stamp = rospy.Time.now()
jmsg.position = self.q
if self.sim:
jointPub.publish(jmsg)
pub1.publish(self.q[0])
pub2.publish(self.q[1])
pub3.publish(self.q[2])
pub4.publish(self.q[3])
pub5.publish(self.q[4])
eventStop.wait(self.dt)
rospy.init_node("robot")
eventStop = threading.Event()
threadJPub = threading.Thread(target=publish, args=(eventStop, ))
threadJPub.daemon = True
threadJPub.start() # Update joint angles in a background process
def kdl_computation(self):
# forward kinematics (returns homogeneous 4x4 matrix)
kdl_kin = KDLKinematics(self.robot, "base_link", "wrist3_Link")
# self.aubo_q=np.asarray(self.aubo_q)
pose = kdl_kin.forward(self.aubo_q)
# print pose
J = kdl_kin.jacobian(self.aubo_q)
# print 'J:', J
return pose, J
