def active_tilt(point, axis, angle, velocity, active_distance=0.125): '''Tilt primitive motion of robot. Parameters: point (list): 3-D coordinate of point in rotation axis axis (list): 3-D vector of rotation axis (right-hand rule) angle (double): angle of tilting velocity (double): robot velocity between 0 and 1 Returns: ''' # Normalize axis vector axis = axis / np.linalg.norm(axis) # Pose variables. The parameters can be seen from "$ rosmsg show Pose" pose_target = group.get_current_pose().pose pos_initial = [ pose_target.position.x, pose_target.position.y, pose_target.position.z ] ori_initial = [ pose_target.orientation.x, pose_target.orientation.y, pose_target.orientation.z, pose_target.orientation.w ] # Tilt center point. Closest point from tcp to axis line center = np.add(point, np.dot(np.subtract(pos_initial, point), axis) * axis) # Closest distance from tcp to axis line radius = np.linalg.norm(np.subtract(center, pos_initial)) # Pair of orthogonal vectors in tilt plane v1 = -np.subtract( np.add(center, np.dot(np.subtract(pos_initial, center), axis) * axis), pos_initial) v1 = v1 / np.linalg.norm(v1) v2 = np.cross(axis, v1) # Interpolate orientation poses via quaternion slerp q = helper.axis_angle2quaternion(axis, angle) ori_target = tf.transformations.quaternion_multiply(q, ori_initial) ori_waypoints = helper.slerp( ori_initial, ori_target, np.arange(1.0 / angle, 1.0 + 1.0 / angle, 1.0 / angle)) waypoints = [] for t in range(1, angle + 1): circle = np.add( center, radius * (math.cos(math.radians(t))) * v1 + radius * (math.sin(math.radians(t))) * v2) pose_target.position.x = circle[0] pose_target.position.y = circle[1] + active_distance * t / (angle + 1) pose_target.position.z = circle[2] pose_target.orientation.x = ori_waypoints[t - 1][0] pose_target.orientation.y = ori_waypoints[t - 1][1] pose_target.orientation.z = ori_waypoints[t - 1][2] pose_target.orientation.w = ori_waypoints[t - 1][3] waypoints.append(copy.deepcopy(pose_target)) (plan, fraction) = group.compute_cartesian_path( waypoints, 0.01, 0) # waypoints, resolution=1cm, jump_threshold) retimed_plan = group.retime_trajectory( robot.get_current_state(), plan, velocity) # Retime trajectory with scaled velocity group.execute(retimed_plan)
def regrasp(axis, angle, velocity): with open("/home/john/catkin_ws/src/shallow_depth_insertion/config/sdi_config.yaml", 'r') as stream: try: config = yaml.safe_load(stream) except yaml.YAMLError as exc: print(exc) pose_target = group.get_current_pose().pose pos_initial = [pose_target.position.x, pose_target.position.y, pose_target.position.z] ori_initial = [pose_target.orientation.x, pose_target.orientation.y, pose_target.orientation.z, pose_target.orientation.w] T_we = tf.TransformListener().fromTranslationRotation(pos_initial, ori_initial) tcp2fingertip = config['tcp2fingertip'] contact_A_e = [tcp2fingertip, -config['object_thickness']/2, 0, 1] #TODO: depends on axis direction contact_A_w = np.matmul(T_we, contact_A_e) visualization.visualizer(contact_A_w[:3], "s", 0.01, 1) #DEBUG # Interpolate orientation poses via quaternion slerp q = helper.axis_angle2quaternion(axis, angle) ori_target = tf.transformations.quaternion_multiply(q, ori_initial) ori_waypoints = helper.slerp(ori_initial, ori_target, np.arange(1.0/angle , 1.0+1.0/angle, 1.0/angle)) theta_0 = config['theta_0'] waypoints = [] action_name = rospy.get_param('~action_name', 'command_robotiq_action') robotiq_client = actionlib.SimpleActionClient(action_name, CommandRobotiqGripperAction) for psi in range(1, angle+1): # Calculate width a = config['delta_0'] * math.cos(math.radians(psi)) b = config['delta_0'] * math.sin(math.radians(psi)) c = config['object_thickness'] * math.cos(math.radians(psi)) d = config['object_thickness'] * math.sin(math.radians(psi)) opposite = a - d width = b + c # Calculate position if theta_0 + psi <= 90: hori = math.fabs(tcp2fingertip*math.cos(math.radians(theta_0 + psi))) + math.fabs((width/2.0)*math.sin(math.radians(theta_0+psi))) verti = math.fabs(tcp2fingertip*math.sin(math.radians(theta_0 + psi))) - math.fabs((width/2.0)*math.cos(math.radians(theta_0+psi))) else: hori = -math.fabs(tcp2fingertip*math.sin(math.radians(theta_0 + psi-90))) + math.fabs((width/2.0)*math.cos(math.radians(theta_0+psi-90))) verti = math.fabs(tcp2fingertip*math.cos(math.radians(theta_0 + psi-90))) + math.fabs((width/2.0)*math.sin(math.radians(theta_0+psi-90))) if axis[0] > 0: pose_target.position.y = contact_A_w[1] + hori pose_target.position.z = contact_A_w[2] + verti #print "CASE 1" elif axis[0] < 0: pose_target.position.y = contact_A_w[1] - hori pose_target.position.z = contact_A_w[2] + verti #print "CASE 2" elif axis[1] > 0: pose_target.position.x = contact_A_w[0] - hori pose_target.position.z = contact_A_w[2] + verti #print "CASE 3" elif axis[1] < 0: pose_target.position.x = contact_A_w[0] + hori pose_target.position.z = contact_A_w[2] + verti #print "CASE 4" pose_target.orientation.x = ori_waypoints[psi-1][0] pose_target.orientation.y = ori_waypoints[psi-1][1] pose_target.orientation.z = ori_waypoints[psi-1][2] pose_target.orientation.w = ori_waypoints[psi-1][3] waypoints.append(copy.deepcopy(pose_target)) (plan, fraction) = group.compute_cartesian_path(waypoints, 0.01, 0) retimed_plan = group.retime_trajectory(robot.get_current_state(), plan, velocity) group.execute(retimed_plan, wait=False) opening_at_zero = config['max_opening']-2*config['finger_thickness'] psi = 0 while psi < angle: pose = group.get_current_pose().pose q_current = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w] psi = 2*math.degrees(math.acos(np.dot(q_current, ori_initial))) if psi > 100: psi = -(psi-360) a = config['delta_0'] * math.cos(math.radians(psi)) b = config['delta_0'] * math.sin(math.radians(psi)) c = config['object_thickness'] * math.cos(math.radians(psi)) d = config['object_thickness'] * math.sin(math.radians(psi)) opposite = a - d width = b + c #pos = int((opening_at_zero - width)/config['opening_per_count']) Robotiq.goto(robotiq_client, pos=width, speed=config['gripper_speed'], force=config['gripper_force'], block=False) psi = round(psi, 2)
def regrasp(robot, axis, angle, psi_init=0, acc=0.1, vel=0.1, wait=True): with open( "/home/john/Desktop/dexterous_ungrasping_urx/config/du_config.yaml", 'r') as stream: try: config = yaml.safe_load(stream) except yaml.YAMLError as exc: print(exc) action_name = rospy.get_param('~action_name', 'command_robotiq_action') robotiq_client = actionlib.SimpleActionClient(action_name, CommandRobotiqGripperAction) tcp2fingertip = config['tcp2fingertip'] init_pose = robot.get_pose() contact_A_e = m3d.Vector( config['object_thickness'] / 2, 0, tcp2fingertip ) #TODO: depends on axis direction and gripper orientation. Refer to gripper axis for now and adjust accordingly. contact_A_w = init_pose * contact_A_e # Interpolate orientation poses via quaternion slerp q_initial = helper.matrix2quaternion(init_pose.orient) q = helper.axis_angle2quaternion(axis, angle) q_target = tf.transformations.quaternion_multiply(q, q_initial) q_waypoints = helper.slerp( q_initial, q_target, np.arange(1.0 / angle, 1.0 + 1.0 / angle, 1.0 / angle)) theta_0 = config['theta_0'] waypoints = [] pose_via = init_pose.copy() for psi in range(psi_init + 1, angle + 1): # Calculate width a = config['delta_0'] * math.cos(math.radians(psi)) b = config['delta_0'] * math.sin(math.radians(psi)) c = config['object_thickness'] * math.cos(math.radians(psi)) d = config['object_thickness'] * math.sin(math.radians(psi)) opposite = a - d width = b + c # Calculate position if theta_0 + psi <= 90: hori = math.fabs(tcp2fingertip * math.cos( math.radians(theta_0 + psi))) + math.fabs( (width / 2.0) * math.sin(math.radians(theta_0 + psi))) verti = math.fabs(tcp2fingertip * math.sin( math.radians(theta_0 + psi))) - math.fabs( (width / 2.0) * math.cos(math.radians(theta_0 + psi))) else: hori = -math.fabs(tcp2fingertip * math.sin( math.radians(theta_0 + psi - 90))) + math.fabs( (width / 2.0) * math.cos(math.radians(theta_0 + psi - 90))) verti = math.fabs(tcp2fingertip * math.cos( math.radians(theta_0 + psi - 90))) + math.fabs( (width / 2.0) * math.sin(math.radians(theta_0 + psi - 90))) if axis[0] > 0: pose_via.pos[1] = contact_A_w[1] + hori pose_via.pos[2] = contact_A_w[2] + verti #print "CASE 1" elif axis[0] < 0: pose_via.pos[1] = contact_A_w[1] - hori pose_via.pos[2] = contact_A_w[2] + verti #print "CASE 2" elif axis[1] > 0: pose_via.pos[0] = contact_A_w[0] - hori pose_via.pos[2] = contact_A_w[2] + verti #print "CASE 3" elif axis[1] < 0: pose_via.pos[0] = contact_A_w[0] + hori pose_via.pos[2] = contact_A_w[2] + verti #print "CASE 4" pose_via.orient = tf.transformations.quaternion_matrix( q_waypoints[psi - psi_init - 1])[:3, :3] waypoints.append(copy.deepcopy(pose_via)) #print init_pose #print waypoints robot.movexs(command='movel', pose_list=waypoints, acc=acc, vel=vel, radius=0.005, wait=False) psi = 0 while psi < angle: pose_current = robot.get_pose() q_current = helper.matrix2quaternion(pose_current.orient) psi = 2 * math.degrees(math.acos(np.dot(q_current, q_initial))) if psi > 100: psi = -(psi - 360) a = config['delta_0'] * math.cos(math.radians(psi)) b = config['delta_0'] * math.sin(math.radians(psi)) c = config['object_thickness'] * math.cos(math.radians(psi)) d = config['object_thickness'] * math.sin(math.radians(psi)) opposite = a - d width = b + c + 0.004 Robotiq.goto(robotiq_client, pos=width, speed=config['gripper_speed'], force=config['gripper_force'], block=False) psi = round(psi, 2) rospy.sleep(0.5) return width
def regrasp(axis, angle, velocity): with open( "../config/Go_stone_du_config.yaml", 'r' ) as stream: # This saves the relative config between the obj and the gripper try: config = yaml.safe_load(stream) except yaml.YAMLError as exc: print(exc) pose_target = group.get_current_pose().pose # tcp is at wrist pos_initial = [ pose_target.position.x, pose_target.position.y, pose_target.position.z ] ori_initial = [ pose_target.orientation.x, pose_target.orientation.y, pose_target.orientation.z, pose_target.orientation.w ] T_we = tf.TransformListener().fromTranslationRotation( pos_initial, ori_initial) tcp2fingertip = config['tcp2fingertip'] contact_A_e = [tcp2fingertip, config['object_thickness'] / 2 + 0.00, 0, 1] #TODO: depends on axis direction contact_A_w = np.matmul(T_we, contact_A_e) visualization.visualizer(contact_A_w[:3], "s", 0.01, 1) #DEBUG # Interpolate orientation poses via quaternion slerp q = helper.axis_angle2quaternion(axis, angle) ori_target = tf.transformations.quaternion_multiply( q, ori_initial) # target orientation of the gripper ori_waypoints = helper.slerp( ori_initial, ori_target, np.arange(1.0 / angle, 1.0 + 1.0 / angle, 1.0 / angle)) theta_0 = config['theta_0'] waypoints = [] action_name = rospy.get_param('~action_name', 'command_robotiq_action') robotiq_client = actionlib.SimpleActionClient( action_name, CommandRobotiqGripperAction ) # this is for new robotiq gripper control package for psi in range(1, angle + 1): # psi is the angle to regrasp # Calculate width a = config['delta_0'] * math.cos(math.radians(psi)) b = config['delta_0'] * math.sin(math.radians(psi)) c = config['object_thickness'] * math.cos(math.radians(psi)) d = config['object_thickness'] * math.sin(math.radians(psi)) opposite = a - d width = b + c # Calculate position if theta_0 + psi <= 90: # hori (verti) is the horizontal (vertical) distance between ee_link and contact_A, in base_link frame hori = math.fabs(tcp2fingertip * math.cos( math.radians(theta_0 + psi))) + math.fabs( (width / 2.0) * math.sin(math.radians(theta_0 + psi))) verti = math.fabs(tcp2fingertip * math.sin( math.radians(theta_0 + psi))) - math.fabs( (width / 2.0) * math.cos(math.radians(theta_0 + psi))) else: hori = -math.fabs(tcp2fingertip * math.sin( math.radians(theta_0 + psi - 90))) + math.fabs( (width / 2.0) * math.cos(math.radians(theta_0 + psi - 90))) verti = math.fabs(tcp2fingertip * math.cos( math.radians(theta_0 + psi - 90))) + math.fabs( (width / 2.0) * math.sin(math.radians(theta_0 + psi - 90))) if axis[0] > 0: # axis is always orthorgonal to base_link frame [1 0 0] or [-1 0 0] or [0 1 0] or [0 -1 0] pose_target.position.y = contact_A_w[ 1] + hori # ee_link position in base_link frame pose_target.position.z = contact_A_w[2] + verti #print "CASE 1" elif axis[0] < 0: pose_target.position.y = contact_A_w[1] - hori - ( 0.085 / (25 * angle / 37)) * psi / angle pose_target.position.z = contact_A_w[2] + verti + ( 0.1 / (25 * angle / 37)) * psi / angle #print "CASE 2" elif axis[1] > 0: pose_target.position.x = contact_A_w[0] - hori pose_target.position.z = contact_A_w[2] + verti #print "CASE 3" elif axis[1] < 0: pose_target.position.x = contact_A_w[0] + hori pose_target.position.z = contact_A_w[2] + verti print "CASE 4" pose_target.orientation.x = ori_waypoints[psi - 1][ 0] # ee_link orientation in base_link frame pose_target.orientation.y = ori_waypoints[psi - 1][1] pose_target.orientation.z = ori_waypoints[psi - 1][2] pose_target.orientation.w = ori_waypoints[psi - 1][3] waypoints.append(copy.deepcopy(pose_target)) (plan, fraction) = group.compute_cartesian_path(waypoints, 0.01, 0) print "Executing regrasp motion." retimed_plan = group.retime_trajectory(robot.get_current_state(), plan, velocity) group.execute(retimed_plan, wait=False) opening_at_zero = config['max_opening'] - 2 * config['finger_thickness'] psi = 0 while psi < angle: # while the manipulator is moving, also need to compute the gripper width simultaneously pose = group.get_current_pose().pose q_current = [ pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w ] psi = 2 * math.degrees( math.acos(np.dot(q_current, ori_initial)) ) #first get psi, and then compute a b c d. During this time, the manipulator keeps if psi > 100: psi = -(psi - 360) a = config['delta_0'] * math.cos(math.radians(psi)) b = config['delta_0'] * math.sin(math.radians(psi)) c = config['object_thickness'] * math.cos(math.radians(psi)) d = config['object_thickness'] * math.sin(math.radians(psi)) opposite = a - d width = b + c Robotiq.goto( robotiq_client, pos=width + 0.004, speed=config['gripper_speed'], force=config['gripper_force'], block=False ) #0.006 for coin; 0.000 for book; 0.005 for poker; 0.007 for flat surface; 0.0065 for curved surface psi = round(psi, 2) rospy.sleep(0.5) print width + 0.003 return width + 0.003