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