def __init__(self): #/vel_based_pos_traj_controller/ self.client = actionlib.SimpleActionClient( 'icl_phri_ur5/follow_joint_trajectory', FollowJointTrajectoryAction) self.goal = FollowJointTrajectoryGoal() self.goal.trajectory = JointTrajectory() self.goal.trajectory.joint_names = JOINT_NAMES print("Waiting for server...") self.client.wait_for_server() print("Connected to server") joint_states = rospy.wait_for_message("joint_states", JointState) print(joint_states) joint_states = list(deepcopy(joint_states).position) del joint_states[-1] self.joints_pos_start = np.array(joint_states) print("Init done") self.listener = tf.TransformListener() self.Transformer = tf.TransformerROS() joint_weights = [12, 5, 4, 3, 2, 1] self.ik = InverseKinematicsUR5() self.ik.setJointWeights(joint_weights) self.ik.setJointLimits(-pi, pi) # self.sub = rospy.Subscriber('/target_position', Int32MultiArray, self.pickplace_cb) # pickplace_sub = message_filters.Subscriber('pick', Int32) # tag_sub = message_filters.Subscriber('tag', Int32) # ts = message_filters.ApproximateTimeSynchronizer([pickplace_sub, tag_sub], 10, 0.1, allow_headerless=True) # ts.registerCallback(self.pickplace_cb) self.sub_pickplace = rospy.Subscriber('/CHOICE', String, self.pickplace_cb)
def __init__(self, simulation=False, max_dist_from_table=0.35, *args, **kwargs): self.simulation = simulation self.ur_script_pub = rospy.Publisher('/ur_driver/URScript', String, queue_size=10, *args, **kwargs) self.closed_form_IK_solver = InverseKinematicsUR5() self.closed_form_IK_solver.setEERotationOffsetROS() # self.closed_form_IK_solver.setJointWeights(self.joint_weights) self.closed_form_IK_solver.setJointLimits(-np.pi, np.pi) super(CostarUR5Driver, self).__init__(steps_per_meter=10, base_steps=5, dof=6, closed_form_IK_solver=self.closed_form_IK_solver) self.client = client = actionlib.SimpleActionClient( 'follow_joint_trajectory', FollowJointTrajectoryAction)
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 __init__(self): #/vel_based_pos_traj_controller/ self.client = actionlib.SimpleActionClient('icl_phri_ur5/follow_joint_trajectory', FollowJointTrajectoryAction) self.goal = FollowJointTrajectoryGoal() self.goal.trajectory = JointTrajectory() self.goal.trajectory.joint_names = JOINT_NAMES print ("Waiting for server...") self.client.wait_for_server() print ("Connected to server") joint_states = rospy.wait_for_message("joint_states", JointState) print(joint_states) joint_states = list(deepcopy(joint_states).position) del joint_states[-1] self.joints_pos_start = np.array(joint_states) print ("Init done") self.listener = tf.TransformListener() self.Transformer = tf.TransformerROS() joint_weights = [12,5,4,3,2,1] self.ik = InverseKinematicsUR5() self.ik.setJointWeights(joint_weights) self.ik.setJointLimits(-pi, pi) self.sub = rospy.Subscriber('/target_position', Int32MultiArray, self.pickplace_cb) self.sub_cancel = rospy.Subscriber('/voice_command', Int32, self.cancel_cb) self.gripper_ac = RobotiqActionClient('icl_phri_gripper/gripper_controller') self.gripper_ac.wait_for_server() self.gripper_ac.initiate() self.gripper_ac.send_goal(0.10) self.gripper_ac.wait_for_result() self.cancel = False
def __init__(self): self.tf_listener = tf.TransformListener() self.Transformer = tf.TransformerROS() self.ee_link = '/ee_link' self.base_link = '/base_link' self.ik = InverseKinematicsUR5() self.ik.setEERotationOffsetROS() self.ik.setJointWeights([6, 5, 4, 3, 2, 1]) self.ik.setJointLimits(-pi, pi)
def __init__(self): #/vel_based_pos_traj_controller/ # icl_phri_ur5 self.client = actionlib.SimpleActionClient('arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction) self.goal = FollowJointTrajectoryGoal() self.goal.trajectory = JointTrajectory() self.goal.trajectory.joint_names = JOINT_NAMES print ("Waiting for server...") self.client.wait_for_server() print ("Connected to server") joint_states = rospy.wait_for_message("joint_states", JointState) self.joints_pos_start = np.array(joint_states.position) print ("Init done") # self.listener = tf.TransformListener() # self.Transformer = tf.TransformerROS() joint_weights = [6,5,4,3,2,1] self.ik = InverseKinematicsUR5() self.ik.setJointWeights(joint_weights)
def __init__(self, ip_address, simulation=False, world="/world", listener=None, traj_step_t=0.1, max_acc=1, max_vel=1, max_goal_diff=0.02, goal_rotation_weight=0.01, max_q_diff=1e-6): self.simulation = simulation self.ur_script_pub = rospy.Publisher('/ur_driver/URScript', String, queue_size=10) base_link = "base_link" end_link = "ee_link" planning_group = "manipulator" self.closed_form_IK_solver = InverseKinematicsUR5() # self.closed_form_ur5_ik.enableDebugMode() self.joint_weights = np.array([6.0, 5.0, 4.0, 2.5, 1.5, 1.2]) self.closed_form_IK_solver.setEERotationOffsetROS() self.closed_form_IK_solver.setJointWeights(self.joint_weights) self.closed_form_IK_solver.setJointLimits(-np.pi, np.pi) super(CostarUR5Driver, self).__init__(base_link, end_link, planning_group, steps_per_meter=10, base_steps=10, dof=6, closed_form_IK_solver=self.closed_form_IK_solver) self.client = client = actionlib.SimpleActionClient( 'follow_joint_trajectory', FollowJointTrajectoryAction)
def __init__(self): #/vel_based_pos_traj_controller/ self.client = actionlib.SimpleActionClient( 'arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction) self.goal = FollowJointTrajectoryGoal() self.goal.trajectory = JointTrajectory() self.goal.trajectory.joint_names = JOINT_NAMES print("Waiting for server...") self.client.wait_for_server() print("Connected to server") joint_states = rospy.wait_for_message("joint_states", JointState) self.joints_pos_start = np.array(joint_states.position) print("Init done") self.listener = tf.TransformListener() self.Transformer = tf.TransformerROS() joint_weights = [10, 5, 4, 3, 2, 1] self.ik = InverseKinematicsUR5() self.ik.setJointWeights(joint_weights) self.ik.setJointLimits(-pi, pi) self.sub = rospy.Subscriber('/target_position', Int32MultiArray, self.pickplace_cb)
def __init__(self): #/vel_based_pos_traj_controller/ #==========================init UR5================================== self.client = actionlib.SimpleActionClient( 'icl_phri_ur5/follow_joint_trajectory', FollowJointTrajectoryAction) self.goal = FollowJointTrajectoryGoal() self.goal.trajectory = JointTrajectory() self.goal.trajectory.joint_names = JOINT_NAMES print("Waiting for server...") self.client.wait_for_server() print("Connected to server") joint_states = rospy.wait_for_message("joint_states", JointState) print(joint_states) joint_states = list(deepcopy(joint_states).position) del joint_states[-1] self.joints_pos_start = np.array(joint_states) print("Init done") #==========================init tf================================== self.listener = tf.TransformListener() self.Transformer = tf.TransformerROS() self.broadcaster = tf.TransformBroadcaster() self.base_link = '/base_link' self.ee_link = '/ee_link' #==========================init IK================================== joint_weights = [12, 5, 4, 3, 2, 1] self.ik = InverseKinematicsUR5() self.ik.setJointWeights(joint_weights) self.ik.setJointLimits(-pi, pi) #==========================init subscriber================================== self.sub_pickplace = rospy.Subscriber('/CHOICE', String, self.pickplace_cb) #==========================init Gripper================================== rospy.loginfo('Waiting for gripper service.') self.gripper_sc = GripperServiceClient('/gripper_control') self.gripper_sc.open() rospy.loginfo('finish init')
def init_ur5(self, topic): self.script_pub = rospy.Publisher("/icl_phri_ur5/ur_driver/URScript", String, queue_size=1) self.client = actionlib.SimpleActionClient( topic, FollowJointTrajectoryAction) self.goal = FollowJointTrajectoryGoal() self.goal.trajectory = JointTrajectory() self.goal.trajectory.joint_names = self.JOINT_NAMES print("Waiting for server...") self.client.wait_for_server() print("Connected to server") joint_states = rospy.wait_for_message("joint_states", JointState) print(joint_states) joint_states = list(deepcopy(joint_states).position) del joint_states[-1] self.joints_pos_start = np.array(joint_states) joint_weights = [12, 5, 4, 3, 2, 1] self.ik = InverseKinematicsUR5() self.ik.setJointWeights(joint_weights) self.ik.setJointLimits(-math.pi, math.pi) trans, rot = self.lookup_pos() initial_pos = [-0.651, -0.100, 0.410] # self.send_script('movel([0, 0, 0.3, 0, 0, 0], a=0.1, v=0.1)') if np.linalg.norm(np.asarray(initial_pos) - trans) > 0.02: try: trans[2] = initial_pos[2] self.move(self.define_grasp(trans)) self.move_joints(self.INIT_JOINTS, self.SPEED * 2) except: rospy.logerr('init failed, fall back') self.move_joints(self.INIT_JOINTS, self.SPEED * 2) raise # self.sub = rospy.Subscriber('/target_position', Int32MultiArray, self.pickplace_cb) print("Init done")
from inverseKinematicsUR5 import InverseKinematicsUR5, transformRobotParameter import numpy as np from math import pi theta0 = [0.6, 0.2, 0.5, 1.2, 0.1, -0.1] theta = [0.7, 0.3, 0.2, 1.0, 0.5, 0.1] gd = transformRobotParameter(theta) print gd joint_weights = [6, 5, 4, 3, 2, 1] ik = InverseKinematicsUR5() ik.setJointWeights(joint_weights) ik.setJointLimits(-pi, pi) # print ik.solveIK(gd) print ik.findClosestIK(gd, theta0)