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)