def pub_head_registration(ud): cheek_pose_base_link = self.tf_list.transformPose( "/base_link", ud.cheek_pose) # find the center of the ellipse given a cheek click cheek_transformation = np.mat( tf_trans.euler_matrix(2.6 * np.pi / 6, 0, 0, 'szyx')) cheek_transformation[0:3, 3] = np.mat([-0.08, -0.04, 0]).T cheek_pose = PoseConverter.to_homo_mat(cheek_pose_base_link) #b_B_c[0:3,0:3] = np.eye(3) norm_xy = cheek_pose[0:2, 2] / np.linalg.norm(cheek_pose[0:2, 2]) head_rot = np.arctan2(norm_xy[1], norm_xy[0]) cheek_pose[0:3, 0:3] = tf_trans.euler_matrix(0, 0, head_rot, 'sxyz')[0:3, 0:3] self.cheek_pub.publish( PoseConverter.to_pose_stamped_msg("/base_link", cheek_pose)) ell_center = cheek_pose * cheek_transformation self.ell_center_pub.publish( PoseConverter.to_pose_stamped_msg("/base_link", ell_center)) # create an ellipsoid msg and command it ep = EllipsoidParams() ep.e_frame.transform = PoseConverter.to_tf_msg(ell_center) ep.height = 0.924 ep.E = 0.086 self.ep_pub.publish(ep) return 'succeeded'
def motion_model(self, f0, f1, stamp, use_kalman=False): if not use_kalman: # simple `repetition` model txn0, rxn0 = f0['pose'][L_POS], f0['pose'][A_POS] txn1, rxn1 = f1['pose'][L_POS], f1['pose'][A_POS] R0 = tx.euler_matrix(*rxn0) R1 = tx.euler_matrix(*rxn1) T0 = tx.compose_matrix(angles=rxn0, translate=txn0) T1 = tx.compose_matrix(angles=rxn1, translate=txn1) Tv = np.dot(T1, vm.inv(T0)) # Tv * T0 = T1 T2 = np.dot(Tv, T1) txn = tx.translation_from_matrix(T2) rxn = tx.euler_from_matrix(T2) x = f1['pose'].copy() P = f1['cov'].copy() x[0:3] = txn x[9:12] = rxn return x, P else: # dt MUST NOT BE None self.kf_.x = f0['pose'] self.kf_.P = f0['cov'] dt = (f1['stamp'] - f0['stamp']) self.kf_.predict(dt) txn, rxn = f1['pose'][L_POS], f1['pose'][A_POS] z = np.concatenate([txn, rxn]) self.kf_.update(z) dt = (stamp - f1['stamp']) self.kf_.predict(dt) return self.kf_.x.copy(), self.kf_.P.copy()
def __init__(self): super(PisaHand, self).__init__() self[ 'mesh_file'] = "package://ec_grasp_planner/data/pisa_hand_simple.dae" self['mesh_file_scale'] = 0.001 #self['surface_grasp']['object']['hand_transform'] = tra.concatenate_matrices(tra.translation_matrix([0.0, -0.05, 0.3]),tra.concatenate_matrices(tra.rotation_matrix(math.radians(90.), [0, 0, 1]),tra.rotation_matrix(math.radians(180.), [1, 0, 0]))) self['surface_grasp']['object'][ 'hand_transform'] = tra.concatenate_matrices( tra.translation_matrix([0.010, 0.007, 0.115])) #self['surface_grasp']['object']['pregrasp_pose'] = tra.concatenate_matrices(tra.translation_matrix([0, 0, 0.5]),tra.euler_matrix(0, math.pi / 2, 0)) #self['surface_grasp']['object']['pregrasp_pose'] = tra.concatenate_matrices(tra.translation_matrix([0.4, 0.0, 0.0]),tra.euler_matrix(0, math.pi / 2, 0)) self['surface_grasp']['object']['hand_pose'] = tra.translation_matrix( [0.010, 0.007, 0.115]) self['surface_grasp']['object']['downward_force'] = 7. self['surface_grasp']['object'][ 'pregrasp_transform'] = tra.concatenate_matrices( tra.translation_matrix([0.3, -0.03, 0.05]), tra.euler_matrix(-math.pi / 2, 0, -math.pi / 2), tra.euler_matrix(0, 0, -0.5)) self['surface_grasp']['object'][ 'post_grasp_transform'] = tra.concatenate_matrices( tra.translation_matrix([0, 0, 0.0]), tra.rotation_matrix(math.radians(-30.0), [0, 1, 0])) self['surface_grasp']['object']['hand_closing_duration'] = 5 self['surface_grasp']['object']['hand_closing_synergy'] = 5
def fwd_kin(self, joint_values): # Expects a 7 x 1 values for joint_values joint_values = np.insert( joint_values, len(joint_values), 0.0, axis=0) # added 0 at the end for the fixed joint T = np.eye(4) T_joint = np.zeros([self.numJoints + 1, 4, 4]) # +1 for the fixed joint at the end # As per Craigs convention, T = Rotx * Trans x * Rotz * Trans z; Franka follows this for i in range(self.numJoints + 1): Tx = tf_tran.translation_matrix((self.a[i], 0, 0)) # x translation Rx = tf_tran.euler_matrix(self.alpha[i], 0, 0, axes='sxyz') # x rotation aa = tf_tran.concatenate_matrices(Rx, Tx) Tz = tf_tran.translation_matrix((0, 0, self.d[i])) # z translation Rz = tf_tran.euler_matrix(0, 0, joint_values[i], axes='sxyz') # z rotation ab = tf_tran.concatenate_matrices(Rz, Tz) ac = tf_tran.concatenate_matrices(aa, ab) T = tf_tran.concatenate_matrices(T, ac) T_joint[ i, :, :] = T # gives the transformation of each joint wrt base CS return T, T_joint
def publish_transforms(): T_obj = euler_matrix(0.79, 0.0, 0.79).dot(translation_matrix((0, 1, 1))) T_robot = euler_matrix(0, 0.0, 1.5).dot(translation_matrix((0, -1, 0))) camera_offset = [0, 0.1, 0.1] T_obj_camera = np.linalg.inv(T_robot.dot( translation_matrix(camera_offset))).dot(T_obj) R_camera = lookAt(translation_from_matrix(T_obj_camera)) T_camera = translation_matrix(camera_offset).dot(R_camera) obj = geometry_msgs.msg.TransformStamped() obj.transform.rotation = Quaternion(*quaternion_from_matrix(T_obj)) obj.transform.translation = Vector3(*translation_from_matrix(T_obj)) obj.header.stamp = rospy.Time.now() obj.header.frame_id = "base_frame" obj.child_frame_id = "object_frame" br.sendTransform(obj) robot = geometry_msgs.msg.TransformStamped() robot.transform.rotation = Quaternion(*quaternion_from_matrix(T_robot)) robot.transform.translation = Vector3(*translation_from_matrix(T_robot)) robot.header.stamp = rospy.Time.now() robot.header.frame_id = "base_frame" robot.child_frame_id = "robot_frame" br.sendTransform(robot) camera = geometry_msgs.msg.TransformStamped() camera.transform.rotation = Quaternion(*quaternion_from_matrix(T_camera)) camera.transform.translation = Vector3(*translation_from_matrix(T_camera)) camera.header.stamp = rospy.Time.now() camera.header.frame_id = "robot_frame" camera.child_frame_id = "camera_frame" br.sendTransform(camera)
def sendTransform(out_pose): """发送TF(topic) Args: out_pose: ros输出格式数据 """ pub.publish(out_pose.fuse_tf) mat44_map_laser = tft.euler_matrix(0, 0, out_pose.fuse_tf.map_theta) mat44_map_laser[0][3] = out_pose.fuse_tf.map_x mat44_map_laser[1][3] = out_pose.fuse_tf.map_y mat44_laser_map = la.inv(mat44_map_laser) mat44_odom_laser = tft.euler_matrix(0, 0, out_pose.fuse_tf.odom_theta) mat44_odom_laser[0][3] = out_pose.fuse_tf.odom_x mat44_odom_laser[1][3] = out_pose.fuse_tf.odom_y mat44_map_odom = la.inv(np.dot(mat44_odom_laser, mat44_laser_map)) q = tft.quaternion_from_matrix(mat44_map_odom) br = tf2_ros.TransformBroadcaster() t = TransformStamped() t.header.stamp = rospy.Time.now() t.header.frame_id = "map" t.child_frame_id = "odom" t.transform.translation.x = mat44_map_odom[0][3] t.transform.translation.y = mat44_map_odom[1][3] t.transform.translation.z = 0.0 t.transform.rotation.x = q[0] t.transform.rotation.y = q[1] t.transform.rotation.z = q[2] t.transform.rotation.w = q[3] br.sendTransform(t)
def get_size_and_build_walls(self): # Get size of the ogrid ============================================== # Get some useful vectors between_vector = self.left_f - self.right_f mid_point = self.right_f + between_vector / 2 target_vector = self.target - mid_point self.mid_point = mid_point # For rotations of the `between_vector` and the enu x-axis b_theta = np.arctan2(between_vector[1], between_vector[0]) b_rot_mat = trns.euler_matrix(0, 0, b_theta)[:3, :3] # Make the endpoints rot_buffer = b_rot_mat.dot([20, 0, 0]) endpoint_left_f = self.left_f + rot_buffer endpoint_right_f = self.right_f - rot_buffer # Now lets build some wall points ====================================== if self.left_b is None: # For rotations of the `target_vector` and the enu x-axis t_theta = np.arctan2(target_vector[1], target_vector[0]) t_rot_mat = trns.euler_matrix(0, 0, t_theta)[:3, :3] rot_channel = t_rot_mat.dot([self.channel_length, 0, 0]) self.left_b = self.left_f + rot_channel self.right_b = self.right_f + rot_channel # Now draw contours from the boat to the start gate ==================== # Get vector from boat to mid_point mid_point_vector = self.boat_pos - self.mid_point b_theta = np.arctan2(mid_point_vector[1], mid_point_vector[0]) b_rot_mat = trns.euler_matrix(0, 0, b_theta)[:3, :3] rot_buffer = b_rot_mat.dot( [np.linalg.norm(mid_point_vector) * 1.5, 0, 0]) left_cone_point = self.left_f + rot_buffer right_cone_point = self.right_f + rot_buffer # Define bounds for the grid self.x_lower = min(left_cone_point[0], right_cone_point[0], endpoint_left_f[0], endpoint_right_f[0], self.target[0]) - self.edge_buffer self.x_upper = max(left_cone_point[0], right_cone_point[0], endpoint_left_f[0], endpoint_right_f[0], self.target[0]) + self.edge_buffer self.y_lower = min(left_cone_point[1], right_cone_point[1], endpoint_left_f[1], endpoint_right_f[1], self.target[1]) - self.edge_buffer self.y_upper = max(left_cone_point[1], right_cone_point[1], endpoint_left_f[1], endpoint_right_f[1], self.target[1]) + self.edge_buffer self.walls = [ self.left_b, self.left_f, left_cone_point, right_cone_point, self.right_f, self.right_b ] print self.walls
def matrix(self): if self.mode == 0: R = transformations.euler_matrix(self.phi, self.th, 0) else: R = transformations.euler_matrix(self.phi, 0, self.th) T = transformations.translation_matrix([self.x, self.y, self.z]) M = numpy.dot(T, R) return M
def __init__(self): self.subscriber = rospy.Subscriber("input_odom", Odometry, self.callback, queue_size=1) self.publisher = rospy.Publisher("output_odom", Odometry, queue_size=1) self.T_NED_V = TR.euler_matrix(pi, 0, 0, 'rxyz') self.T_XAV_UAV = TR.euler_matrix(pi, 0, 0, 'rxyz') self.T_XAV_V = TR.identity_matrix() # the output transform
def __init__(self, **kwargs): super(RBOHand2Kuka, self).__init__() #old parameters below - must be updated to new convention! self['surface_grasp']['initial_goal'] = np.array([ -0.05864322834179703, 0.4118988657714642, -0.05864200146127985, -1.6887810963180838, -0.11728653060066829, -0.8237944986945402, 0 ]) self['surface_grasp']['pregrasp_pose'] = tra.translation_matrix( [0, 0, -0.2]) self['surface_grasp']['hand_pose'] = tra.concatenate_matrices( tra.translation_matrix([0, 0, 0]), tra.rotation_matrix(math.radians(0.), [0, 0, 1])) self['surface_grasp']['downward_force'] = 7. self['surface_grasp']['valve_pattern'] = (np.array([[0., 4.1], [0., 0.1], [0., 5.], [0., 5.], [0., 2.], [0., 3.5]]), np.array([[1, 0]] * 6)) self['wall_grasp']['pregrasp_pose'] = tra.translation_matrix( [0.05, 0, -0.2]) self['wall_grasp']['table_force'] = 7.0 self['wall_grasp']['sliding_speed'] = 0.1 self['wall_grasp']['up_speed'] = 0.1 self['wall_grasp']['down_speed'] = 0.1 self['wall_grasp']['wall_force'] = 10.0 self['wall_grasp']['angle_of_attack'] = 1.0 #radians self['wall_grasp']['object_lift_time'] = 4.5 self['edge_grasp']['initial_goal'] = np.array([ -0.05864322834179703, 0.4118988657714642, -0.05864200146127985, -1.6887810963180838, -0.11728653060066829, -0.8237944986945402, 0 ]) self['edge_grasp']['pregrasp_pose'] = tra.translation_matrix( [0.2, 0, 0.4]) self['edge_grasp']['hand_object_pose'] = tra.concatenate_matrices( tra.translation_matrix([0, 0, 0.05]), tra.rotation_matrix(math.radians(10.), [1, 0, 0]), tra.euler_matrix(0, 0, -math.pi / 2.)) self['edge_grasp']['grasp_pose'] = tra.concatenate_matrices( tra.translation_matrix([0, -0.05, 0]), tra.rotation_matrix(math.radians(10.), [1, 0, 0]), tra.euler_matrix(0, 0, -math.pi / 2.)) self['edge_grasp']['postgrasp_pose'] = tra.translation_matrix( [0, 0, -0.1]) self['edge_grasp']['downward_force'] = 4.0 self['edge_grasp']['sliding_speed'] = 0.04 self['edge_grasp']['valve_pattern'] = (np.array([[0, 0], [0, 0], [1, 0], [1, 0], [1, 0], [1, 0]]), np.array([[0, 3.0]] * 6))
def set_tf_apriltag_camera(self, R, t): # detected tf: camera-apriltag, camera frame has wrong orientation tf_detected = np.zeros((4, 4), dtype=np.float64) tf_detected[0:3, 0:3] = R tf_detected[0:3, 3] = t[:, 0] tf_detected[3, 3] = 1 # correct camera frame and apriltag orientation to conform with specifications camera_rot = transformations.euler_matrix(np.pi/2, -np.pi/2, 0, axes="ryzx") # rotate C to C' apriltag_rot = transformations.euler_matrix(np.pi/2, -np.pi/2, 0, axes="rxzy") # rotate A' to A tf_camera_apriltag = camera_rot @ tf_detected @ apriltag_rot # Tcc' * Tc'a' * Ta'a = Tca self.tf_apriltag_camera = np.linalg.inv(tf_camera_apriltag) # Tac
def get_size_and_build_walls(self): # Get size of the ogrid ============================================== # Get some useful vectors between_vector = self.left_f - self.right_f mid_point = self.right_f + between_vector / 2 target_vector = self.target - mid_point self.mid_point = mid_point # For rotations of the `between_vector` and the enu x-axis b_theta = np.arctan2(between_vector[1], between_vector[0]) b_rot_mat = trns.euler_matrix(0, 0, b_theta)[:3, :3] # Make the endpoints rot_buffer = b_rot_mat.dot([20, 0, 0]) endpoint_left_f = self.left_f + rot_buffer endpoint_right_f = self.right_f - rot_buffer # Now lets build some wall points ====================================== if self.left_b is None: # For rotations of the `target_vector` and the enu x-axis t_theta = np.arctan2(target_vector[1], target_vector[0]) t_rot_mat = trns.euler_matrix(0, 0, t_theta)[:3, :3] rot_channel = t_rot_mat.dot([self.channel_length, 0, 0]) self.left_b = self.left_f + rot_channel self.right_b = self.right_f + rot_channel # Now draw contours from the boat to the start gate ==================== # Get vector from boat to mid_point mid_point_vector = self.boat_pos - self.mid_point b_theta = np.arctan2(mid_point_vector[1], mid_point_vector[0]) b_rot_mat = trns.euler_matrix(0, 0, b_theta)[:3, :3] rot_buffer = b_rot_mat.dot([np.linalg.norm(mid_point_vector) * 1.5, 0, 0]) left_cone_point = self.left_f + rot_buffer right_cone_point = self.right_f + rot_buffer # Define bounds for the grid self.x_lower = min(left_cone_point[0], right_cone_point[0], endpoint_left_f[0], endpoint_right_f[0], self.target[0]) - self.edge_buffer self.x_upper = max(left_cone_point[0], right_cone_point[0], endpoint_left_f[0], endpoint_right_f[0], self.target[0]) + self.edge_buffer self.y_lower = min(left_cone_point[1], right_cone_point[1], endpoint_left_f[1], endpoint_right_f[1], self.target[1]) - self.edge_buffer self.y_upper = max(left_cone_point[1], right_cone_point[1], endpoint_left_f[1], endpoint_right_f[1], self.target[1]) + self.edge_buffer self.walls = [self.left_b, self.left_f, left_cone_point, right_cone_point, self.right_f, self.right_b]
def __init__(self): self.subscriber = rospy.Subscriber("vicon", PoseStamped, self.callback, queue_size=1) self.reference_pub = rospy.Publisher("reference", Odometry, queue_size=1) self.ext_att_pub = rospy.Publisher("external_attitude", Quaternion, queue_size=1) self.T_V_NED = TR.euler_matrix(pi, 0, 0, 'rxyz') # self.T_XAV_V = TR.identity_matrix() # from subscriber callback self.T_UAV_XAV = TR.euler_matrix(pi, 0, 0, 'rxyz') self.T_UAV_NED = TR.identity_matrix() # the output transform
def pull_obj2robot(pose, angle, delta_spring, object): mesh, vertices = object.transform_object(pose, type="PoseStamped") pose2d, placement = get2dpose_object(pose, object) ripi = pose2d[0:2] #1. define nominal orientation of gripper for pullin T_gripper_nominal_world = tfm.euler_matrix(0, np.pi / 2, np.pi / 2, 'rxyz') #2. rotate gripper by desired angle of gripper(sampling different solutions) + orientation of object about z axis T_transform = tfm.euler_matrix(0, 0, angle + pose2d[2], 'rxyz') gripper_nominal_pose_world = pose_from_matrix(T_gripper_nominal_world, "PoseStamped", "yumi_body") pose_transform = pose_from_matrix(T_transform, "PoseStamped", "yumi_body") gripper_final_pose_world = transform_pose(gripper_nominal_pose_world, pose_transform) gripper_final_pose_world.pose.position.x = ripi[0] gripper_final_pose_world.pose.position.y = ripi[1] gripper_final_pose_world.pose.position.z = mesh.max_[2] - mesh.min_[ 2] + delta_spring #3. move gripper closer to edge (to get tactile imprint) gripper_poses = [gripper_final_pose_world, gripper_final_pose_world] poses_tip_world = deepcopy(gripper_poses) pose2d, stable_placement = get2dpose_object(pose, object) pose_proposals_base = proposals_base_from_object_pose(object, pose) pose_transform_frameB = unit_pose() if placement == 0: pose_transform_frameB.pose.position.x -= 0.09 / 2 elif placement == 2: pose_transform_frameB.pose.position.z -= 0.02 elif placement == 3: pose_transform_frameB.pose.position.z += 0.03 elif placement == 1: pose_transform_frameB.pose.position.x += 0.02 pose_transform_frameB.pose.position.z += 0.0 elif placement == 4: pose_transform_frameB.pose.position.z += 0.02 gripper_final_pose_world = transform_pose_intermediate_frame( gripper_final_pose_world, pose, pose_transform_frameB) #4 pull at corner # face_id, dict_faces = get_object_convex_face_id(gripper_poses, # object, # stable_placement, # pose_proposals_base, # arm="r") # corner_point_list = dict_faces['faces'][face_id] # gripper_final_pose_world.pose.position.x = corner_point_list[0][0] # gripper_final_pose_world.pose.position.y = corner_point_list[0][1] return gripper_final_pose_world
def test_move_out_of_collision(self): env = orpy.Environment() env.Load('data/pumablocks.env.xml') body = env.GetKinBody('lego0') Tinit = body.GetTransform() def move_until_collision(step, direction, timeout=1.0): start_time = time.time() Tcollision = body.GetTransform() while not env.CheckCollision(body): Tcollision[:3,3] += step*np.array(direction) body.SetTransform(Tcollision) if time.time()-start_time > timeout: return False return True # Move down into collision with the table body.SetTransform(Tinit) self.assertTrue( move_until_collision(0.005, [0, -1, 0]) ) result = criros.raveutils.move_out_of_collision(env, body, max_displacement=0.006) self.assertTrue(result) body.SetTransform(Tinit) self.assertTrue( move_until_collision(0.005, [0, -1, 0]) ) result = criros.raveutils.move_out_of_collision(env, body, max_displacement=0.001) self.assertFalse(result) # Show we can cope with tilted objects body.SetTransform(Tinit) move_until_collision(0.005, [0, -1, 0]) Toffset = tr.euler_matrix(np.deg2rad(10), 0, 0) Tbody = body.GetTransform() Tnew = np.dot(Tbody, Toffset) body.SetTransform(Tnew) result = criros.raveutils.move_out_of_collision(env, body, max_displacement=0.015) self.assertTrue(result)
def to_pose2d(pos): # pos = N?x3x4 transformation matrix # 2d rotation requires (-y) component # 2d translation requires (z, -x) component # == test == # -- opt 1 : einsum -- T_pre = tx.euler_matrix(-np.pi/2,0,-np.pi/2) #4x4 T_0i = tx.inverse_matrix(np.concatenate((pos[0], [[0,0,0,1]]), axis=0)) #4x4 T_cam = np.stack([np.concatenate((p, [[0,0,0,1]]), axis=0) for p in pos], axis=0) pos_n = np.einsum('ij,jk,nkl,lm->nim', T_pre, T_0i, T_cam, T_pre.T) # normalized position res = [] for p in pos_n: t = tx.translation_from_matrix(p) q = tx.euler_from_matrix(p) res.append([ t[0], t[1], q[-1] ]) res = np.stack(res, axis=0) return res # -------------------- # -- opt 2 : no einsum -- #T_pre = tx.euler_matrix(-np.pi/2,0,-np.pi/2) #4x4 #T_0i = tx.inverse_matrix(np.concatenate((pos[0], [[0,0,0,1]]), axis=0)) #4x4 #T_cam = np.stack([np.concatenate((p, [[0,0,0,1]]), axis=0) for p in pos], axis=0) #res = [] #for p in pos: # T_cam = np.concatenate((p, [[0,0,0,1]]), axis=0) # T_base = T_pre.dot(T_0i).dot(T_cam).dot(T_pre.T) # t = tx.translation_from_matrix(T_base) # q = tx.euler_from_matrix(T_base) # res.append([t[0], t[1], q[-1]]) #res = np.stack(res, axis=0) return res
def lookup_tag(tag_number): """ Returns the AR tag position in world coordinates Parameters ---------- tag_number : int AR tag number Returns ------- :obj:`autolab_core.RigidTransform` AR tag position in world coordinates """ to_frame = 'ar_marker_{}'.format(tag_number) tag_pos = None while not tag_pos: try: t = listener.getLatestCommonTime(from_frame, to_frame) # print(rospy.Time.now()) # t = rospy.Time.now() tag_pos, tag_rot = listener.lookupTransform(from_frame, to_frame, t) except: continue; # if not listener.frameExists(from_frame) or not listener.frameExists(to_frame): # print 'Frames not found' # print 'Did you place AR marker {} within view of the baxter left hand camera?'.format(tag_number) # exit(0) angles = tfs.euler_from_quaternion(tag_rot) # print(angles) trans = tfs.euler_matrix(*angles) print(RigidTransform(trans[:3, :3], tag_pos)) return RigidTransform(trans[:3, :3], tag_pos)
def Aruco_marker_detector(self): """ Use the build in python library to detect the aruco marker and its coordinates """ img = self.frame.copy() # Detect the markers in the image markerCorners, markerIds, rejectedCandidates = cv.aruco.detectMarkers(img, self.dictionary, parameters=self.parameters) # Get the transformation matrix to the marker if markerCorners: markerSize = 2.0 axisLength = 3.0 rvecs, tvecs, _ = cv.aruco.estimatePoseSingleMarkers(markerCorners, markerSize, self.K, self.distCoeffs) # Draw the axis on the marker frame_out = cv.aruco.drawAxis( img, self.K, self.distCoeffs, rvecs, tvecs, axisLength) rotMat = tr.euler_matrix(np.pi / 2.0, 0, 0) rotMat = rotMat[0:3, 0:3] tvecs = np.matmul(rotMat, tvecs[0][0].T) rotMat, _ = cv.Rodrigues(rvecs) eul = tr.euler_from_matrix(rotMat) yaw_angle = self.pos[3] - eul[2] # Publish the position of the marker marker_pos = PT() marker_pos.position.x = tvecs[0] marker_pos.position.y = - tvecs[2] marker_pos.position.z = tvecs[1] marker_pos.yaw = eul[2] * np.pi / 180.0 self.aruco_marker_pos_pub.publish(marker_pos) # Publish the image with the detected marker self.aruco_marker_img_pub.publish(self.bridge.cv2_to_imgmsg(frame_out, "bgr8"))
def d_spiral_point(self, point, radius, granularity=8, revolutions=1, direction='ccw', theta_offset=0, meters_per_rev=0): """ Sprials a point using discrete moves This produces a generator """ point = np.array(point) if direction == 'ccw': angle_incrment = 2 * np.pi / granularity else: angle_incrment = -2 * np.pi / granularity sprinkles = transformations.euler_matrix(0, 0, angle_incrment)[:3, :3] # Find first point to go to using boat rotation next_point = np.append(normalize(self.position[:2] - point[:2]), 0) # Doing this in 2d radius_increment = meters_per_rev / granularity for i in range(granularity * revolutions + 1): new = point + radius * next_point radius += radius_increment yield self.set_position(new).look_at(point).yaw_left(theta_offset) next_point = sprinkles.dot(next_point) yield self.set_position(new).look_at(point).yaw_left(theta_offset)
def prepare_trajectory(trajectory: np.ndarray, robot_pose: Pose, robot_model: rtb.DHRobot, model_pose: np.ndarray) -> np.ndarray: robot_orientation = Quaternion(robot_pose.orientation[0], robot_pose.orientation[1], robot_pose.orientation[2], robot_pose.orientation[3]) robot_position = Point(robot_pose.position[0], robot_pose.position[1], robot_pose.position[2]) robot_euler = T.euler_from_quaternion( (robot_orientation.x, robot_orientation.y, robot_orientation.z, robot_orientation.w)) robot_transformation = T.euler_matrix(robot_euler[0], robot_euler[1], robot_euler[2]) robot_transformation[0, 3] = robot_position.x robot_transformation[1, 3] = robot_position.y robot_transformation[2, 3] = robot_position.z robot_transformation = np.linalg.inv(robot_transformation) transformed_trajectory = [] for i in range(len(trajectory)): projected_point = robot_transformation.dot( np.concatenate((trajectory[i], 1), axis=None)) / 1000 projected_point = model_pose.dot(projected_point)[:-1] ik_result, fail, err = robot_model.ikine( sm.SE3(projected_point[0], projected_point[1], projected_point[2])) transformed_trajectory += [ik_result] return np.array(transformed_trajectory)
def eulerAnglesToRotationMatrix(theta): """Calculates rotation matrix given euler angles in 'xyz' sequence :param theta: list of 3 euler angles :return: 3x3 rotation matrix equivalent to the given euler angles """ return euler_matrix(theta[0], theta[1], theta[2], axes="sxyz")[:3, :3]
def generate_ep(self): cart_cur = self.cart_arm.get_end_effector_pose() if self.step_ind < self.num_steps: self.cart_cur_goal = self.trajectory[self.step_ind] self.step_ind += 1 else: self.cart_cur_goal = self.cart_goal self.cart_err = self.cart_arm.ep_error(cart_cur, self.cart_cur_goal) #self.cart_err[np.where(np.fabs(self.cart_err) < self.err_goal_thresh * 0.6)] = 0.0 cart_control = np.array([ pidc.update_state(cart_err_i) for pidc, cart_err_i in zip(self.controllers, self.cart_err) ]) cep_pos_cur, cep_rot_cur = self.cart_arm.get_ep() cep_pos_new = cep_pos_cur - cart_control[:3, 0] cart_rot_control = np.mat( tf_trans.euler_matrix(cart_control[3, 0], cart_control[4, 0], cart_control[5, 0]))[:3, :3] cep_rot_new = cep_rot_cur * cart_rot_control.T ep_new = (cep_pos_new, cep_rot_new) if self.debug: print "=" * 50 print "cart_cur", cart_cur print "-" * 50 print "cur goal", self.cart_cur_goal print "-" * 50 print "err", self.cart_err print "-" * 50 print "cart_control", cart_control return EPStopConditions.CONTINUE, ep_new
def plot_frame(self, frame_num=None, mark_num=None, xyz_rotation=(0, 0, 0), azimuth=60, elevation=30): """Plots the location of each marker in the specified frame """ #Get the frame if frame_num is not None: frame = self._get_frames()[:, :, frame_num] else: frame = self.read()[0][:, :, 0] # Apply any desired rotation rot_matrix = transformations.euler_matrix(*xyz_rotation)[0:3, 0:3] frame = rot_matrix.dot(frame.T).T xs = frame[:, 0] ys = frame[:, 1] zs = frame[:, 2] #Make the plot figure = plt.figure(figsize=(11, 10)) axes = figure.add_subplot(111, projection='3d') markers = ['r'] * self.get_num_points() if mark_num is not None: markers[mark_num] = 'b' axes.scatter(xs, ys, zs, c=markers, marker='o', s=60) axes.auto_scale_xyz([-0.35, 0.35], [-0.35, 0.35], [-0.35, 0.35]) axes.set_xlabel('X (m)') axes.set_ylabel('Y (m)') axes.set_zlabel('Z (m)')
def main(): rospy.init_node("arm_cart_vel_control") if True: ctrl_switcher = ControllerSwitcher() ctrl_switcher.carefree_switch( 'r', '%s_arm_controller', '$(find hrl_pr2_arms)/params/joint_traj_params_electric.yaml') rospy.sleep(0.5) ctrl_switcher.carefree_switch( 'r', '%s_joint_controller_low', '$(find hrl_pr2_arms)/params/joint_traj_params_electric_low.yaml') r_arm_js = create_pr2_arm('r', PR2ArmJointTrajectory, controller_name='%s_joint_controller_low') q = [ -0.34781704, 0.27341079, -1.75392154, -2.08626393, -3.43756314, -1.82146607, -1.85187734 ] r_arm_js.set_ep(q, 3) rospy.sleep(6) ctrl_switcher.carefree_switch( 'r', '%s_cart_low_rfh', '$(find kelsey_sandbox)/params/j_transpose_low_rfh.yaml') r_arm = create_pr2_arm('r', PR2ArmCartesianPostureBase) r_arm.set_posture() rospy.sleep(0.2) vel_ctrl = PR2ArmCartVelocityController(r_arm) #vel_frame = tf_trans.euler_matrix(0, -np.pi/2, 0)[:3,:3] #vel_ctrl.move_velocity(velocity_rot=vel_frame, velocity=0.005, is_translation=False) vel_frame = tf_trans.euler_matrix(0, 0, np.pi / 2)[:3, :3] vel_ctrl.move_velocity(velocity_rot=vel_frame, velocity=0.10, is_translation=False)
def circle_point(self, point, radius, granularity=8, theta_offset=0): ''' Circle a point whilst looking at it This produces a generator, so for use: circle = navigator.move.circle_point([1,2,0], 5) for p in circle: yield p.go() ''' point = np.array(point) angle_incrment = 2 * np.pi / granularity sprinkles = transformations.euler_matrix(0, 0, angle_incrment)[:3, :3] # Find first point to go to using boat rotation next_point = np.append(normalize(self.nav.pose[0][:2] - point[:2]), 0) # Doing this in 2d for i in range(granularity + 1): new = point + radius * next_point yield self.set_position(new).look_at(point).yaw_left( theta_offset ) # Remember this is a generator - not a twisted yield next_point = sprinkles.dot(next_point) yield self.set_position(new).look_at(point).yaw_left(theta_offset)
def test_frame3_rpy(self, x, y, z, roll, pitch, yaw): r2 = euler_matrix(roll, pitch, yaw) r2[0, 3] = x r2[1, 3] = y r2[2, 3] = z np.testing.assert_array_almost_equal(w.compile_and_execute(w.frame_rpy, [x, y, z, roll, pitch, yaw]), r2)
def place(self, x, y, z, alpha, beta, gamma, approach): # find pre-place pose T_grasp = euler_matrix(radians(alpha), radians(beta), radians(gamma), axes="sxyz") T_grasp[:3, 3] = np.array([x, y, z]) T_trans = np.identity(4) T_trans[2, 3] = -approach T_pre_grasp = np.dot(T_grasp, T_trans) alpha_pre, beta_pre, gamma_pre = euler_from_matrix(T_pre_grasp) alpha_pre, beta_pre, gamma_pre = (degrees(alpha_pre), degrees(beta_pre), degrees(gamma_pre)) x_pre, y_pre, z_pre = T_pre_grasp[:3, 3] # move to pre-place if (self.move(x_pre, y_pre, z_pre, alpha_pre, beta_pre, gamma_pre) and # move to place self.move(x, y, z, alpha, beta, gamma) and # open gripper self.open_gripper() and # move back to pre-grasp self.move(x_pre, y_pre, z_pre, alpha_pre, beta_pre, gamma_pre)): rospy.loginfo("Place finished") return True else: rospy.logerr("Place failed") self.clear_error() return False
def transform_frame(vec, orient, att, frame_id): vec = numpy.array(vec) if frame_id == 'aruco_map': # from 'aruco_map' to 'main_camera' rmat, j = Rodrigues(orient) vec.transpose() vec = rmat.dot(vec) vec.transpose() elif frame_id == 'main_camera': pass # from 'main_camera' to 'fcu' a = camera_angle vec[0], vec[1], vec[2] = vec[1] * math.cos(a) + vec[2] * math.sin( a), -vec[0], vec[2] * math.cos(a) - vec[1] * math.sin(a) # from 'fcu' to 'fcu_horiz' roll, pitch = att[0], att[1] rmat = euler_matrix(roll, pitch, 0, 'rxyz') rmat = rmat[0:3, 0:3] vec.transpose() vec = rmat.dot(vec) vec.transpose() return list(vec)
def _extract_twist_msg(twist_msg): pos = [twist_msg.linear.x, twist_msg.linear.y, twist_msg.linear.z] rot_euler = [twist_msg.angular.x, twist_msg.angular.y, twist_msg.angular.z] quat = tf_trans.quaternion_from_euler(*rot_euler, axes='sxyz') homo_mat = np.mat(tf_trans.euler_matrix(*rot_euler)) homo_mat[:3,3] = np.mat([pos]).T return homo_mat, quat, rot_euler
def calc_transformation_matrix(x, y, z, rotx, roty, rotz): # return transformation matrix based on translation and rotation components r_mat = transformations.euler_matrix(radians(rotz), radians(rotx), radians(roty), 'rzxy') t_mat = transformations.translation_matrix((x, y, z)) return numpy.dot(t_mat, r_mat)
def generate_ep(self): cart_cur = self.cart_arm.get_end_effector_pose() if self.step_ind < self.num_steps: self.cart_cur_goal = self.trajectory[self.step_ind] self.step_ind += 1 else: self.cart_cur_goal = self.cart_goal self.cart_err = self.cart_arm.ep_error(cart_cur, self.cart_cur_goal) #self.cart_err[np.where(np.fabs(self.cart_err) < self.err_goal_thresh * 0.6)] = 0.0 cart_control = np.array([pidc.update_state(cart_err_i) for pidc, cart_err_i in zip(self.controllers, self.cart_err)]) cep_pos_cur, cep_rot_cur = self.cart_arm.get_ep() cep_pos_new = cep_pos_cur - cart_control[:3,0] cart_rot_control = np.mat(tf_trans.euler_matrix(cart_control[3,0], cart_control[4,0], cart_control[5,0]))[:3,:3] cep_rot_new = cep_rot_cur * cart_rot_control.T ep_new = (cep_pos_new, cep_rot_new) if self.debug: print "="*50 print "cart_cur", cart_cur print "-"*50 print "cur goal", self.cart_cur_goal print "-"*50 print "err", self.cart_err print "-"*50 print "cart_control", cart_control return EPStopConditions.CONTINUE, ep_new
def publish_state(self, t): state_msg = TransformStamped() t_ned_imu = tft.translation_matrix(self.model.get_position()) R_ned_imu = tft.quaternion_matrix(self.model.get_orientation()) T_ned_imu = tft.concatenate_matrices(t_ned_imu, R_ned_imu) #rospy.loginfo("T_ned_imu = \n" + str(T_ned_imu)) if self.T_imu_vicon is None: # grab the static transform from imu to vicon frame from param server: frames = rospy.get_param("frames", None) ypr = frames['vicon_to_imu']['rotation'] q_vicon_imu = tft.quaternion_from_euler(*[radians(x) for x in ypr], axes='rzyx') # xyzw R_vicon_imu = tft.quaternion_matrix(q_vicon_imu) t_vicon_imu = tft.translation_matrix(frames['vicon_to_imu']['translation']) # rospy.loginfo(str(R_vicon_imu)) # rospy.loginfo(str(t_vicon_imu)) self.T_vicon_imu = tft.concatenate_matrices(t_vicon_imu, R_vicon_imu) self.T_imu_vicon = tft.inverse_matrix(self.T_vicon_imu) self.T_enu_ned = tft.euler_matrix(radians(90), 0, radians(180), 'rzyx') rospy.loginfo(str(self.T_enu_ned)) rospy.loginfo(str(self.T_imu_vicon)) #rospy.loginfo(str(T_vicon_imu)) # we have /ned -> /imu, need to output /enu -> /vicon: T_enu_vicon = tft.concatenate_matrices(self.T_enu_ned, T_ned_imu, self.T_imu_vicon ) state_msg.header.stamp = t state_msg.header.frame_id = "/enu" state_msg.child_frame_id = "/simflyer1/flyer_vicon" stt = state_msg.transform.translation stt.x, stt.y, stt.z = T_enu_vicon[:3,3] stro = state_msg.transform.rotation stro.x, stro.y, stro.z, stro.w = tft.quaternion_from_matrix(T_enu_vicon) self.pub_state.publish(state_msg)
def _makePreGraspPose(self, boxMat, axis): if axis==0: #x axis alpha = 0 gamma = 0 else: #y axis alpha = pi/2 gamma = -pi/2 ik_request = PositionIKRequest() ik_request.ik_link_name = self.toolLinkName with self._joint_state_lock: ik_request.ik_seed_state.joint_state = copy.deepcopy(self._joint_states) ik_request.pose_stamped = PoseStamped() ik_request.pose_stamped.header.stamp = rospy.Time.now() ik_request.pose_stamped.header.frame_id = self.frameID beta = pi/2 while beta < 3*pi/2: rotationMatrix = transformations.euler_matrix(alpha, beta, gamma, 'rzyz') distance = self.preGraspDistance + self.gripperFingerLength preGraspMat = transformations.translation_matrix([0,0,-distance]) fullMat = transformations.concatenate_matrices(boxMat, rotationMatrix, preGraspMat) p = transformations.translation_from_matrix(fullMat) q = transformations.quaternion_from_matrix(fullMat) print "trying {} radians".format(beta) try: self._ik_server(ik_request, Constraints(), rospy.Duration(5.0)) except rospy.service.ServiceException: beta += 0.1 else: if ik_resp.error_code.val > 0: return beta else: #print ik_resp.error_code.val beta += 0.01 rospy.logerr('No way to pick this up. All IK solutions failed.') return 7 * pi / 6
def test_frame3_rpy(self, x, y, z, roll, pitch, yaw): r1 = np.array(spw.frame_rpy(x, y, z, roll, pitch, yaw)).astype(float) r2 = euler_matrix(roll, pitch, yaw) r2[0, 3] = x r2[1, 3] = y r2[2, 3] = z self.assertTrue(np.isclose(r1, r2).all(), msg='\n{} != \n{}'.format(r1, r2))
def calculate_joints(self, position): self.joint_angle_matricies = [] for angle in position: self.joint_angle_matricies.append(transformations.euler_matrix(0, 0, angle)) T_c = [np.identity(4)] tf_msg = TFMessage() for index in xrange(len(self.urdf_transform_matricies)): urdf_transform_matrix = self.urdf_transform_matricies[index] joint_angle_matrix = self.joint_angle_matricies[index] transform_matrix = np.dot(urdf_transform_matrix, joint_angle_matrix) tf_stamp = TransformStamped() tf_stamp.child_frame_id = self.urdf_transforms[index].child_frame_id tf_stamp.header.frame_id = self.urdf_transforms[index].header.frame_id if index in (8, 10): #sets parent of fingers to link6 index = 6 T_c.append(np.dot(T_c[index], transform_matrix)) tf_stamp.transform = msgify(Transform, T_c[-1]) #tf_stamp.transform = msgify(Transform, transform_matrix) tf_msg.transforms.append(tf_stamp) #print transforms.header #print link_states.name[-1] #print link_states.pose[-1] #print '-----------------------------------------------' return tf_msg
def __init__(self, MPC1): self.lock = threading.Lock() self.MPC = MPC1 self.RATE = rospy.get_param('/rate', 50) self.dt = 0.0 self.time_start = 0.0 self.end = False self.pose_init = [0.0, 0.0, 0.0] self.flag = True "Desired values setup" # rotation matrix [4x4] from `world` frame to `body` self.bTw = tftr.euler_matrix(-np.pi, 0.0, 0.0, 'rxyz') # # in `world` frame # self.A = rospy.get_param('/A', 90.0) # [degrees] # self.pose_des = rospy.get_param('/pose_des', [0.5, 0.0, 2.0]) # # in 'body' frame # self.pose_des = self.transform_pose(self.pose_des) #print(self.pose_des.T) self.rot_z_des = 0.0 "ROS stuff" self.pub_cmd_vel = rospy.Publisher("/cmd_vel", Twist, queue_size=1, latch=False) self.sub_odom = rospy.Subscriber("/odom", Odometry, self.odometry_callback)
def calc_transformation(self, name, relative_to=None): calc_from_joint = False if relative_to: if relative_to in self.urdf.link_map: parent_link_name = relative_to elif relative_to in self.urdf.joint_map: parent_link_name = self.urdf.joint_map[name].parent calc_from_joint = True else: parent_link_name = self.urdf.get_root() calc_to_joint = False if name in self.urdf.link_map: child_link_name = name elif name in self.urdf.joint_map: child_link_name = self.urdf.joint_map[name].child calc_to_joint = True chains = self.urdf.get_chain(parent_link_name, child_link_name, joints=True, links=True) if calc_from_joint: chains = chains[1:] if calc_to_joint: chains = chains[:-1] poses = [] for name in chains: if name in self.urdf.joint_map: joint = self.urdf.joint_map[name] if joint.origin is not None: poses.append(joint.origin) elif name in self.urdf.link_map: link = self.urdf.link_map[name] if link.visual is not None and link.visual.origin is not None: poses.append(link.visual.origin) m = np.dot(T.translation_matrix((0,0,0)), T.euler_matrix(0,0,0)) for p in poses: n = np.dot(T.translation_matrix(tuple(p.xyz)), T.euler_matrix(*tuple(p.rpy))) m = np.dot(m, n) t = T.translation_from_matrix(m) q = T.quaternion_from_matrix(m) return tuple(t), (q[3], q[0], q[1], q[2])
def get_path(buoy_l, buoy_r): # Vector between the two buoys between_vector = buoy_r.position - buoy_l.position # Rotate that vector to point through the buoys r = trns.euler_matrix(0, 0, np.radians(90))[:3, :3] direction_vector = r.dot(between_vector) direction_vector /= np.linalg.norm(direction_vector) return between_vector, direction_vector
def fromROSPose2DModel(self, model): self.type = model.type self.pose = LocalPose() matrix = euler_matrix(0.0, 0.0, model.pose.theta) matrix[0][3] = model.pose.x matrix[1][3] = model.pose.y self.pose.pose = fromMatrix(matrix) self.geometry_type = 'POSE2D' geometry_string = 'POINT(%f %f %f)' % (model.pose.x, model.pose.y, 0.0) self.geometry = WKTElement(geometry_string)
def pose_to_matrix(self, ps): trans = np.matrix([-ps.pose.position.x, -ps.pose.position.y, ps.pose.position.z]).T r, p, y = euler_from_quaternion([ps.pose.orientation.x, ps.pose.orientation.y, ps.pose.orientation.z, ps.pose.orientation.w]) rot = np.matrix(euler_matrix(-r, -p, -y)[:3, :3]).T return rot, trans
def test_to_tf(self): tb = tb_angles(45,-5,24) self.assertTrue(tb.to_tf().flags.writeable) for yaw in self.yaw: for pitch in self.pitch: for roll in self.roll: tb = tb_angles(yaw,pitch,roll) m = tb.to_tf() m2 = tft.euler_matrix(yaw * pi / 180, pitch * pi / 180, roll * pi / 180, 'rzyx') self.assertTrue(np.allclose(m, m2),msg='%s and %s not close!' % (m,m2))
def MsgToPose(msg): #Parse the ROS message to a 4x4 pose format #Get translation and rotation (from Euler angles) pose = transformations.euler_matrix(msg.roll,msg.pitch,msg.yaw) pose[0,3] = msg.pt.x pose[1,3] = msg.pt.y pose[2,3] = msg.pt.z return pose
def get_rot_matrix(self, rev=False): frm, to = 'ardrone/odom', 'ardrone/ardrone_base_bottomcam' if rev: frm, to = to, frm self.tf.waitForTransform(frm, to, rospy.Time(0), rospy.Duration(3)) trans, rot = self.tf.lookupTransform(frm, to, rospy.Time(0)) x, y, z = euler_from_quaternion(rot) yaw = euler_from_quaternion(rot, axes='szxy')[0] return yaw, np.array(euler_matrix(-x, -y, z))
def pub_head_registration(ud): cheek_pose_base_link = self.tf_list.transformPose("/base_link", ud.cheek_pose) # find the center of the ellipse given a cheek click cheek_transformation = np.mat(tf_trans.euler_matrix(2.6 * np.pi/6, 0, 0, 'szyx')) cheek_transformation[0:3, 3] = np.mat([-0.08, -0.04, 0]).T cheek_pose = PoseConverter.to_homo_mat(cheek_pose_base_link) #b_B_c[0:3,0:3] = np.eye(3) norm_xy = cheek_pose[0:2, 2] / np.linalg.norm(cheek_pose[0:2, 2]) head_rot = np.arctan2(norm_xy[1], norm_xy[0]) cheek_pose[0:3,0:3] = tf_trans.euler_matrix(0, 0, head_rot, 'sxyz')[0:3,0:3] self.cheek_pub.publish(PoseConverter.to_pose_stamped_msg("/base_link", cheek_pose)) ell_center = cheek_pose * cheek_transformation self.ell_center_pub.publish(PoseConverter.to_pose_stamped_msg("/base_link", ell_center)) # create an ellipsoid msg and command it ep = EllipsoidParams() ep.e_frame.transform = PoseConverter.to_tf_msg(ell_center) ep.height = 0.924 ep.E = 0.086 self.ep_pub.publish(ep) return 'succeeded'
def place_generator(self): place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = 0.57 place_pose.pose.position.y = 0.16 place_pose.pose.position.z = 0.56 place_pose.pose.orientation.w = 1.0 P = transformations.quaternion_matrix( [ place_pose.pose.orientation.x, place_pose.pose.orientation.y, place_pose.pose.orientation.z, place_pose.pose.orientation.w, ] ) P[0, 3] = place_pose.pose.position.x P[1, 3] = place_pose.pose.position.y P[2, 3] = place_pose.pose.position.z places = [] yaw_angles = [0, 1, 57, -1, 57, 3, 14] x_vals = [0, 0.05, 0.1, 0.15] z_vals = [0.05, 0.1, 0.15] for y in yaw_angles: G = transformations.euler_matrix(0, 0, y) G[0, 3] = 0.0 # offset about x G[1, 3] = 0.0 # about y G[2, 3] = 0.0 # about z for z in z_vals: for x in x_vals: TM = np.dot(P, G) pl = deepcopy(place_pose) pl.pose.position.x = TM[0, 3] + x pl.pose.position.y = TM[1, 3] pl.pose.position.z = TM[2, 3] + z quat = transformations.quaternion_from_matrix(TM) pl.pose.orientation.x = quat[0] pl.pose.orientation.y = quat[1] pl.pose.orientation.z = quat[2] pl.pose.orientation.w = quat[3] pl.header.frame_id = REFERENCE_FRAME places.append(deepcopy(pl)) return places
def orient_division(self, grid, point, angle_offset_mult=1): """ Returns a position to go to on the other side of the line """ angle_offset = 1 * angle_offset_mult # rads point = np.array(point) r_pos = self.navigator.pose[0][:2] - point[:2] angle = np.arctan2(*r_pos[::-1]) - angle_offset center = np.array(grid.shape) / 2 line_pt2 = trns.euler_matrix(0, 0, angle)[:3, :3].dot(np.array([1E3, 0, 0])) cv2.line(grid, tuple(center), tuple(line_pt2[:2].astype(np.int32)), 255, 2) # Let's put ourselves on the other side of that there line return self.generate_target(angle, r_pos) + point[:2]
def _MsgToPose(msg): """ Parse the ROS message to a 4x4 pose format @param msg The ros message containing a pose @return A 4x4 transformation matrix containing the pose as read from the message """ # Get translation and rotation (from Euler angles) pose = transformations.euler_matrix(msg.roll * 0.0, msg.pitch * 0.0, msg.yaw * 0.0) pose[0, 3] = msg.pt.x pose[1, 3] = msg.pt.y pose[2, 3] = msg.pt.z return pose
def __init__(self): rospy.Subscriber("/mavros/local_position/pose", PoseStamped, self.localCb) rospy.Subscriber("/itech_ros/mavros_offboard/enu", PoseStamped, self.enuCb) self.localEnuPub = rospy.Publisher(util.topicName("mavros_local_pose_fusion", "local_enu"), PoseStamped, queue_size=10) self.offsetPub = rospy.Publisher(util.topicName("mavros_local_pose_fusion", "offset"), PointStamped, queue_size=10) self.offset = None self.localPose = PoseModel() self.localEnuPose = PoseModel() #tick posee are the poses at the time that we get positioning data. self.enuTickPose = PoseModel() self.localTickPose = PoseModel() self.r = euler_matrix(0, 0, math.pi/-2)
def pose_relative_rot(pose, r=0., p=0., y=0., degrees=True): """Return a pose rotated relative to a given pose.""" ps = deepcopy(pose) if degrees: r = math.radians(r) p = math.radians(p) y = math.radians(y) des_rot_mat = tft.euler_matrix(r,p,y) q_ps = [ps.pose.orientation.x, ps.pose.orientation.y, ps.pose.orientation.z, ps.pose.orientation.w] state_rot_mat = tft.quaternion_matrix(q_ps) final_rot_mat = np.dot(state_rot_mat, des_rot_mat) ps.pose.orientation = Quaternion( *tft.quaternion_from_matrix(final_rot_mat)) return ps
def post_grasp(self, new_pose, obj_idx, fl): ######### GRASP OBJECT/ REMOVE FROM SCENE ######. if fl == "true": self.close_gripper() self.aro = obj_idx else: self.open_gripper() self.aro = None rospy.sleep(2) ### POST GRASP RETREAT ### M1 = transformations.quaternion_matrix( [ new_pose.pose.orientation.x, new_pose.pose.orientation.y, new_pose.pose.orientation.z, new_pose.pose.orientation.w, ] ) M1[0, 3] = new_pose.pose.position.x M1[1, 3] = new_pose.pose.position.y M1[2, 3] = new_pose.pose.position.z M2 = transformations.euler_matrix(0, 0, 0) M2[0, 3] = 0.0 # offset about x M2[1, 3] = 0.0 # about y M2[2, 3] = 0.25 # about z T1 = np.dot(M2, M1) npo = deepcopy(new_pose) npo.pose.position.x = T1[0, 3] npo.pose.position.y = T1[1, 3] npo.pose.position.z = T1[2, 3] quat = transformations.quaternion_from_matrix(T1) npo.pose.orientation.x = quat[0] npo.pose.orientation.y = quat[1] npo.pose.orientation.z = quat[2] npo.pose.orientation.w = quat[3] npo.header.frame_id = REFERENCE_FRAME self.right_arm.plan(npo.pose) self.right_arm.go(wait=True)
def test_create_matrix(self): for yaw in self.yaw: for pitch in self.pitch: for roll in self.roll: m = tft.euler_matrix(yaw * pi / 180., pitch * pi / 180., roll * pi / 180., 'rzyx') tb = tb_angles(m) self.assertTbEqual(tb, yaw, pitch, roll) m2 = m[0:3,0:3] tb = tb_angles(m2) self.assertTbEqual(tb, yaw, pitch, roll) m3 = np.mat(m) tb = tb_angles(m3) self.assertTbEqual(tb, yaw, pitch, roll) m4 = m.tolist() tb = tb_angles(m4) self.assertTbEqual(tb, yaw, pitch, roll)
def test_to_mat(self): tb = tb_angles(45,-5,24) m1 = tb.matrix self.assertFalse(m1.flags.writeable) m1a = tb.matrix self.assertIs(m1, m1a) m2 = tb.to_matrix() self.assertTrue(m2.flags.writeable) self.assertIsNot(m1,m2) for yaw in self.yaw: for pitch in self.pitch: for roll in self.roll: tb = tb_angles(yaw,pitch,roll) m = tb.matrix self.assertAlmostEqual(np.linalg.det(m), 1) m2 = tft.euler_matrix(yaw * pi / 180, pitch * pi / 180, roll * pi / 180, 'rzyx')[0:3,0:3] self.assertTrue(np.allclose(m, m2),msg='%s and %s not close!' % (m,m2))
def publish_pointestimates(servoang): global X_points, Kc, MSG # MSG[9]=726 # servoang=0 t = np.mat("-1.6991; 51.3061; -83.5694") t[2, :] = t[2, :] + MSG[9] # print t R = euler_matrix(0.0022, 0.003 + ((servoang) / 180.0 * pi), -0.2652, "rzyx") R = R[0:3, 0:3].T # print R # print t t = R * t # print t T = np.bmat("R t") x = Kc * T * X_points x[0, :] = x[0, :] / x[2, :] x[1, :] = x[1, :] / x[2, :] x[2, :] = x[2, :] / x[2, :] # print x return x
def publish_pose(self, header, results, img): msg = PoseArray(header=header) for r in results: pose = r["face_origin"] pose_2d = r['face_2d_origin'] ori = r["pose"] mat = T.euler_matrix(-ori[0], -ori[1], -ori[2]) rotmat = mat[:3, :3] quat = T.quaternion_from_matrix(mat) quat = T.quaternion_multiply( [0.5, 0.5, -0.5, 0.5], quat) pose.orientation.x = quat[0] pose.orientation.y = quat[1] pose.orientation.z = quat[2] pose.orientation.w = quat[3] msg.poses.append(pose) if self.visualize: zvec = np.array([0, 0, 1], np.float32) yvec = np.array([0, 1, 0], np.float32) xvec = np.array([1, 0, 0], np.float32) zvec = _project_plane_yz(rotmat.dot(zvec)) * 50.0 yvec = _project_plane_yz(rotmat.dot(yvec)) * 50.0 xvec = _project_plane_yz(rotmat.dot(xvec)) * 50.0 face_2d_center = np.array([pose_2d.position.x, pose_2d.position.y]) img = _draw_line(img, face_2d_center, face_2d_center + zvec, (255, 0, 0), 3) img = _draw_line(img, face_2d_center, face_2d_center + yvec, (0, 255, 0), 3) img = _draw_line(img, face_2d_center, face_2d_center + xvec, (0, 0, 255), 3) self.pub_pose.publish(msg) if self.visualize: img_msg = self.cv_bridge.cv2_to_imgmsg(img, "bgr8") img_msg.header = header self.pub_debug_image.publish(img_msg)
def circle_point(self, point, radius, granularity=8, theta_offset=0): ''' Circle a point whilst looking at it This produces a generator, so for use: circle = navigator.move.circle_point([1,2,0], 5) for p in circle: yield p.go() ''' point = np.array(point) angle_incrment = 2 * np.pi / granularity sprinkles = transformations.euler_matrix(0, 0, angle_incrment)[:3, :3] # Find first point to go to using boat rotation next_point = np.append(normalize(self.nav.pose[0][:2] - point[:2]), 0) # Doing this in 2d for i in range(granularity + 1): new = point + radius * next_point yield self.set_position(new).look_at(point).yaw_left(theta_offset) # Remember this is a generator - not a twisted yield next_point = sprinkles.dot(next_point) yield self.set_position(new).look_at(point).yaw_left(theta_offset)
def search_IK(self, pose, q_guess, sigma=0.6, sample_size=10, search_penalties=[10, 10, 8, 5, 2, 2, 1]): q_samples = [] q_diffs = [] for i in range(sample_size): if i != 0: rot_vals = np.random.normal(0, sigma, 3) else: rot_vals = [0]*3 rot_vals = np.clip(rot_vals, -np.pi, np.pi) rand_rot = tf_trans.euler_matrix(*rot_vals) new_pose = pose.copy() new_pose[:3,:3] = rand_rot[:3,:3] * new_pose[:3,:3] cur_time = rospy.Time.now().to_sec() q_sample = self.IK(new_pose, q_guess) if q_sample is None: continue q_samples.append(q_sample) q_diffs.append(np.sum(np.array(search_penalties) * self.angle_difference(q_guess, q_sample))) if len(q_samples) == 0: return None return q_samples[np.argmin(q_diffs)]
def main(): rospy.init_node("arm_cart_vel_control") if True: ctrl_switcher = ControllerSwitcher() ctrl_switcher.carefree_switch('r', '%s_arm_controller', '$(find hrl_pr2_arms)/params/joint_traj_params_electric.yaml') rospy.sleep(0.5) ctrl_switcher.carefree_switch('r', '%s_joint_controller_low', '$(find hrl_pr2_arms)/params/joint_traj_params_electric_low.yaml') r_arm_js = create_pr2_arm('r', PR2ArmJointTrajectory, controller_name='%s_joint_controller_low') q = [-0.34781704, 0.27341079, -1.75392154, -2.08626393, -3.43756314, -1.82146607, -1.85187734] r_arm_js.set_ep(q, 3) rospy.sleep(6) ctrl_switcher.carefree_switch('r', '%s_cart_low_rfh', '$(find kelsey_sandbox)/params/j_transpose_low_rfh.yaml') r_arm = create_pr2_arm('r', PR2ArmCartesianPostureBase) r_arm.set_posture() rospy.sleep(0.2) vel_ctrl = PR2ArmCartVelocityController(r_arm) #vel_frame = tf_trans.euler_matrix(0, -np.pi/2, 0)[:3,:3] #vel_ctrl.move_velocity(velocity_rot=vel_frame, velocity=0.005, is_translation=False) vel_frame = tf_trans.euler_matrix(0, 0, np.pi/2)[:3,:3] vel_ctrl.move_velocity(velocity_rot=vel_frame, velocity=0.10, is_translation=False)
def update(self, dt): # The following model is completely arbitrary and should not be taken to be representative of # real vehicle performance! # But, it should be good enough to test out control modes etc. yaw_cmd, pitch_cmd, roll_cmd, alt_cmd, motor_enable = self.u cur_yaw, cur_pitch, cur_roll = tft.euler_from_quaternion(self.get_orientation(), 'rzyx') # Feedback control: attitude angles track inputs exactly; altitude is input to # PID controller for thrust if motor_enable: thrust = self.thrust_cmd # accelerations due to pitch/roll in body frame: pitch_clipped, pwas_clipped = clip(pitch_cmd, -50, 50) roll_clipped, rwas_clipped = clip(roll_cmd, -50, 50) yaw_cmd = yaw_cmd else: thrust = 0.0 yaw_cmd = degrees(cur_yaw) pitch_clipped = degrees(cur_pitch) roll_clipped = degrees(cur_roll) pwas_clipped = rwas_clipped = False # Propagate dynamics # Velocity accel_thrust = thrust*THRUST_SCALING/MASS if pwas_clipped or rwas_clipped: rospy.logwarn('Pitch and/or roll clipped! Pre-clip values %f %f' % (pitch_cmd, roll_cmd)) accel_body = np.array([-sin(radians(pitch_clipped))*accel_thrust, sin(radians(roll_clipped))*accel_thrust, 0.0]) # vertical accel will be added in inertial frame # rotate body frame accelerations into inertial frame, and add drag and vertical forces: Ryaw = tft.euler_matrix(cur_yaw, 0, 0, 'rzyx') accel_inertial_lateral = np.dot(Ryaw[:3,:3], accel_body) cur_velocity = self.get_velocity() accel_inertial = accel_inertial_lateral + \ np.array([- LINEAR_DRAG*cur_velocity[0] - QUADRATIC_DRAG*np.sign(cur_velocity[0])*(cur_velocity[0]**2), - LINEAR_DRAG*cur_velocity[1] - QUADRATIC_DRAG*np.sign(cur_velocity[1])*(cur_velocity[1]**2), GRAVITY - accel_thrust*cos(radians(pitch_clipped))*cos(radians(roll_clipped))]) velocity = cur_velocity + accel_inertial*dt # Position position = self.get_position() + velocity*dt position[2] = min(0, position[2]) # can't go lower than the ground # Angles if position[2] == 0.0 and velocity[2] > 0: # On the ground.. velocity = np.zeros(3) orientation = tft.quaternion_from_euler(radians(yaw_cmd), 0, 0, 'rzyx') else: # assume perfect yaw rate, and pitch and roll tracking: orientation = tft.quaternion_from_euler(radians(yaw_cmd), radians(pitch_clipped), radians(roll_clipped), 'rzyx') # Angular rate # from J. Diebel, "Representing attitude: Euler angles, unit quaternions, and rotation vectors," 2006 q_cur = np.array((orientation[3],) + tuple(orientation[:3])) # convert to wxyz q_prev = np.array((self.prev_orientation[3],) + tuple(self.prev_orientation[:3])) # convert to wxyz if dt > EPS: q_dot = (q_cur - q_prev)/dt else: q_dot = np.zeros(4) Wprime = np.array([[-q_cur[1], q_cur[0], q_cur[3], -q_cur[2]], [-q_cur[2], -q_cur[3], q_cur[0], q_cur[1]], [-q_cur[3], q_cur[2], -q_cur[1], q_cur[0]]]) ang_vel_body = 2*np.dot(Wprime,q_dot) # Assemble new state vector self.x = np.concatenate([position, velocity, q_cur, ang_vel_body]) # Save for future finite differences self.prev_orientation = orientation[:] # be sure to copy not reference...
def homo_mat_from_2d(x, y, rot): mat2d = np.mat(tf_trans.euler_matrix(0, 0, rot)) mat2d[0,3] = x mat2d[1,3] = y return mat2d