def __init__(self): rospy.init_node(NODE_NAME) sys.path.append( '~/Documents/RobotSemantics/semseg_ws/src/image_segnet/object_class_pcl/src' ) for name, default in PARAM_LIST: setattr(self, name, rospy.get_param(PARAM_PREFIX + name, default)) rospy.Subscriber(self.in_pcl, PointCloud2, self.pcl_cb) rospy.Subscriber(self.in_viz + '/' + self.camera_used + '/camera_info', CameraInfo, self.ci_cb) rospy.Subscriber( self.in_viz + '/' + self.camera_used + '/image/compressed', CompressedImage, self.image_cb) rospy.Subscriber(self.in_semseg + '/class_matrix', numpy_msg(Floats), self.clmat_cb) self.pcl_pub = rospy.Publisher(self.out_pcl, PointCloud2, queue_size=20) self.clmats = {} self.cis = {} self.imgsC = {} self.timestamps = [] self.counterTS = 0 self.counter = 0 #SHOULD BE CHANGED TO SOMETHING ADAPTIVE - NOW manually SETTED self.camera0PM = [ 638.81494, 0, 625.98561, 0, 0, 585.79797, 748.57858, 0, 0, 0, 1, 0 ] self.tf_listener = tf.TransformListener() self.adjustment = np.dot( tf_help.rotation_matrix(math.pi / 2, (0, 1, 0)), tf_help.rotation_matrix(math.pi, (0, 0, 1)))
def compute_contact_coordinates(self, disk_center_position): rot_psi = tfms.rotation_matrix(np.radians(self._ginsberg_euler.x), [0, 0, 1]) init_rot = tfms.rotation_matrix(math.pi / 2, [0, 0, 1]) rot_theta = tfms.rotation_matrix(np.radians(self._ginsberg_euler.y), [0, 1, 0]) rot_phi = tfms.rotation_matrix(np.radians(self._ginsberg_euler.z), [0, 0, 1]) rot_StoQ = np.matmul(np.matmul(rot_psi, init_rot), rot_theta) rot_StoB = np.matmul(rot_StoQ, rot_phi) ground_contact_vector = np.matmul( rot_StoQ, np.array([[self._object_radius], [0], [0], [0]])) contact_position_x = disk_center_position.x + ground_contact_vector[0] contact_position_y = disk_center_position.y + ground_contact_vector[1] contact_position = Vector3() contact_position.x = contact_position_x contact_position.y = contact_position_y contact_position.z = 0 self._ginsberg_contact_position_pub.publish(contact_position)
def test_euler_XYZ_from_rotation_matrix(self): """ r in 'rxyz' is 'r'otating frames indicating rotations are applied consecutively with respect to current frames' axes, "relative-rotation". Order of rotations are first X, second Y and third Z. Rotation matrix composition rule for relative rotations: Rx * Ry * Rz. s in 'sxyz' is 's'tationary frames indicating rotations are applied with respect to the coordinate frame of the INITIAL frame, "fixed-axis rotation". Rotation matrix composition rule for fixed-axis rotations: Rz * Ry * Rx. """ Rx_90 = tr.rotation_matrix( math.pi / 4, [1, 0, 0]) # first, rotate 90 degrees around x axis Ry_90 = tr.rotation_matrix( math.pi / 5, [0, 1, 0]) # second, 45 degrees around y axis of the current frame Rz_90 = tr.rotation_matrix( math.pi / 6, [0, 0, 1]) # third, 30 degrees around z axis of the current frame R1 = np.matmul(Rz_90, Ry_90) R = np.matmul(R1, Rx_90) euler_angles = tr.euler_from_matrix(R, 'sxyz') euler_angles_expected = [math.pi / 4, math.pi / 5, math.pi / 6] np.testing.assert_almost_equal(euler_angles, euler_angles_expected)
def gp_sampling(self, p, R): h = 100. # Depth of the field of view b = h / np.cos(self.mbes_angle / 2.) n = 80 # Number of sampling points # Triangle vertices Rxmin = rotation_matrix(-self.mbes_angle / 2., (1, 0, 0))[0:3, 0:3] Rxmax = rotation_matrix(self.mbes_angle / 2., (1, 0, 0))[0:3, 0:3] Rxmin = np.matmul(R, Rxmin) Rxmax = np.matmul(R, Rxmax) p2 = p + Rxmax[:, 2] / np.linalg.norm(Rxmax[:, 2]) * b p3 = p + Rxmin[:, 2] / np.linalg.norm(Rxmin[:, 2]) * b # Sampling step i_range = np.linalg.norm(p3 - p2) / n # Across ping direction direc = ((p3 - p2) / np.linalg.norm(p3 - p2)) # start = time.time() p2s = np.full((n, len(p2)), p2) direcs = np.full((n, len(direc)), direc) i_ranges = np.full((1, n), i_range) * (np.asarray(range(0, n))) sampling_points = p2s + i_ranges.reshape(n, 1) * direcs # Sample GP here mu, sigma = self.gp.sample(np.asarray(sampling_points)[:, 0:2]) mu_array = np.array([mu]) # Concatenate sampling points x,y with sampled z mbes_gp = np.concatenate( (np.asarray(sampling_points)[:, 0:2], mu_array.T), axis=1) # print(mbes_gp) return mbes_gp
def compute_contact_coordinates(self): """Trace of ground contact as the object rolls without slipping""" rot_psi = tfms.rotation_matrix(self._body_euler.x, [0,0,1]) init_rot = tfms.rotation_matrix(math.pi/2, [0,0,1]) rot_theta = tfms.rotation_matrix(self._body_euler.y, [0,1,0]) rot_phi = tfms.rotation_matrix(self._body_euler.z, [0,0,1]) rot_StoQ = np.matmul(np.matmul(rot_psi, init_rot),rot_theta) # this frame # does not roll with the object. So suitable to determine ground contact point rot_StoB = np.matmul(rot_StoQ, rot_phi) # from world to body frame. no use here. ground_contact_vector = np.matmul(rot_StoQ,np.array([[OBJECT_RADIUS],[0],[0],[0]])) contact_position_x = self._disk_center_position.x + ground_contact_vector[0] contact_position_y = self._disk_center_position.y + ground_contact_vector[1] self._ground_contact = Vector3() self._ground_contact.x = contact_position_x self._ground_contact.y = contact_position_y self._ground_contact.z = 0 #publish self._pub_kinematics._ground_contact_publisher.publish(self._ground_contact)
def flipOrbToNDT(orbPose): qOrb = [orbPose.qx, orbPose.qy, orbPose.qz, orbPose.qw] orbFlip = trafo.concatenate_matrices( trafo.quaternion_matrix(qOrb), trafo.rotation_matrix(np.pi / 2, (1, 0, 0)), trafo.rotation_matrix(np.pi / 2, (0, 0, 1))) return trafo.quaternion_from_matrix(orbFlip)
def flipOrbToNDT (orbPose): qOrb = [orbPose.qx, orbPose.qy, orbPose.qz, orbPose.qw] orbFlip = trafo.concatenate_matrices( trafo.quaternion_matrix(qOrb), trafo.rotation_matrix(np.pi/2, (1,0,0)), trafo.rotation_matrix(np.pi/2, (0,0,1)) ) return trafo.quaternion_from_matrix(orbFlip)
def compute_Q_frame(self): # Q frame only tilts and yaws, but does not spin. Useful to determine ground contact rot_psi = tfms.rotation_matrix(self._body_euler.x, [0, 0, 1]) init_rot = tfms.rotation_matrix(math.pi / 2, [0, 0, 1]) rot_theta = tfms.rotation_matrix(self._body_euler.y, [0, 1, 0]) rot_phi = tfms.rotation_matrix(self._body_euler.z, [0, 0, 1]) self._world_Q_rot = np.matmul(np.matmul(rot_psi, init_rot), rot_theta)
def dynamics(s): global param, inp, wind, forces, moments m= param['m'] Jx = param['Jx'] Jy = param['Jy'] Jz = param['Jz'] Jxz = param['Jxz'] # Jxz = 0.0 p_n = s[0][0] p_e = s[1][0] p_d = s[2][0] u = s[3][0] v = s[4][0] w = s[5][0] phi = s[6][0] theta = s[7][0] psi = s[8][0] p = s[9][0] q = s[10][0] r = s[11][0] state_dot = np.zeros((12,1)) #[[0.],[0.],[0.],[0.],[0.],[0.],[0.],[0.],[0.],[0.],[0.],[0.]] # origin, xaxis, yaxis, zaxis = (0., 0., 0.), (1., 0., 0.), (0., 1., 0.), (0., 0., 1.) Rx = rotation_matrix(phi,xaxis) Ry = rotation_matrix(theta,yaxis) Rz = rotation_matrix(psi,zaxis) R_V_B = concatenate_matrices(Rz,Ry,Rx) R_V_B = R_V_B[0:3,0:3] sphi = np.sin(phi) cphi = np.cos(phi) stheta = np.sin(theta) ctheta = np.cos(theta) spsi = np.sin(psi) cpsi = np.cos(psi) ttheta = np.tan(theta) secth = 1.0/ctheta # calpha = np.cos(alpha) # salpha = np.sin(alpha) Mat2 = np.matrix([[1.0,sphi*ttheta,cphi*ttheta],[0.,cphi,-sphi],[0.,sphi*secth,cphi*secth]]) J = np.matrix([[Jx, 0., -Jxz],[0., Jy, 0.],[-Jxz, 0., Jz]]) temp1 = R_V_B*np.matrix(s[3:6]) state_dot[0:3] = temp1 temp2 = Mat2*np.matrix(s[9:12]) state_dot[6:9] = temp2 temp3 = np.transpose(-np.cross(np.transpose(np.matrix(s[9:12])),np.transpose(np.matrix(s[3:6])))) + (1.0/m)*forces state_dot[3:6] = temp3 temp4 = np.linalg.pinv(J)*(np.transpose(-np.cross(np.transpose(np.matrix(s[9:12])),np.transpose(J*np.matrix(s[9:12])))) + moments) state_dot[9:12] = temp4 s2 = np.copy(state_dot) return s2
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_cam_object_T(self, object_pose): Tr = tr.translation_matrix( [object_pose[0], object_pose[1], object_pose[2]]) xaxis, yaxis, zaxis = [1, 0, 0], [0, 1, 0], [0, 0, 1] Rx = tr.rotation_matrix(object_pose[3], xaxis) Ry = tr.rotation_matrix(object_pose[4], yaxis) Rz = tr.rotation_matrix(object_pose[5], zaxis) R = tr.concatenate_matrices(Rx, Ry, Rz) T = tr.concatenate_matrices(Tr, R) return T
def set_cam_object_T(self, ball_pose): Tr = tr.translation_matrix([ball_pose[0], ball_pose[1], ball_pose[2]]) # the rotation angles are zero because the detected ball does not have any orientation xaxis, yaxis, zaxis = [1, 0, 0], [0, 1, 0], [0, 0, 1] Rx = tr.rotation_matrix(0, xaxis) Ry = tr.rotation_matrix(0, yaxis) Rz = tr.rotation_matrix(0, zaxis) R = tr.concatenate_matrices(Rx, Ry, Rz) T = tr.concatenate_matrices(Tr, R) return T
def tagTworld(): """ Returns homogeneous transformation that expresses world_p in tag frame. Returns: tag_T_world (numpy.array): homogeneous transformation that expresses world_p in tag cf """ T_x = tr.rotation_matrix(-math.pi / 2, [1, 0, 0]) T_z = tr.rotation_matrix(math.pi / 2, [0, 0, 1]) tag_T_world = np.matmul(T_x, T_z) return tag_T_world
def get_camera_transformation(self, offsets=None, current=False): # R is to fix axes correspondance to gripper Rx = tft.rotation_matrix(pi / 2, (1, 0, 0)) Rz = tft.rotation_matrix(pi / 2, (0, 0, 1)) R = Rz.dot(Rx) if offsets is None: D_e = tft.translation_matrix((0.094, -0.031, 0.0235)) # m else: D_e = tft.translation_matrix(offsets) # m if current: # use current robot position to determine camera T R_e = self.get_self_pose() # use orientation of EE else: R_e = tft.rotation_matrix(-pi / 4, (1, 0, 0)) # rotation around z return R_e.dot(D_e).dot(R)
def bebop2track_transform_odom(bebop_odom): global track_last_known_pos global bebop_last_known_pos global bebop_last_known_hdg global track_last_known_hdg pose = bebop_odom.pose.pose twist = bebop_odom.twist.twist.linear bebop_d_pos = np.array([pose.position.x, pose.position.y, pose.position.z ]) - bebop_last_known_pos bebop_q = np.array([ pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w ]) hdg_bebop = tfs.euler_from_quaternion(bebop_q)[2] track_bebop_dhdg = track_last_known_hdg - bebop_last_known_hdg R_hdg = tfs.rotation_matrix(track_bebop_dhdg, (0, 0, 1))[0:3, 0:3] track_pos = np.matmul(R_hdg, bebop_d_pos) + track_last_known_pos bebop_vel_body = np.array([twist.x, twist.y, twist.z]) R_body2track = tfs.rotation_matrix(hdg_bebop + track_bebop_dhdg, (0, 0, 1))[0:3, 0:3] track_vel_body = np.matmul(R_body2track, bebop_vel_body) track_odom = bebop_odom track_odom.twist.twist.linear.x = track_vel_body[0] track_odom.twist.twist.linear.y = track_vel_body[1] track_odom.twist.twist.linear.z = track_vel_body[2] track_odom.pose.pose.position.x = track_pos[0] track_odom.pose.pose.position.y = track_pos[1] track_odom.pose.pose.position.z = track_pos[2] track_quat = tfs.quaternion_multiply( tfs.quaternion_from_euler(0, 0, track_bebop_dhdg), bebop_q) track_odom.pose.pose.orientation.x = track_quat[0] track_odom.pose.pose.orientation.y = track_quat[1] track_odom.pose.pose.orientation.z = track_quat[2] track_odom.pose.pose.orientation.w = track_quat[3] return track_odom
def __init__(self): rospy.init_node(NODE_NAME) sys.path.append( '~/Documents/RobotSemantics/semseg_ws/src/image_segnet/object_class_pcl/src' ) #import parameters to self for name, default in PARAM_LIST: setattr(self, name, rospy.get_param(PARAM_PREFIX + name, default)) self.clmats = {} self.cis = {} self.imgsC = {} self.timestamps = [] self.counterTS = 0 self.counter = 0 self.clmat_counter = 0 self.pcls = {} # subscribed Topics #rospy.Subscriber(self.in_pcl, PointCloud2, self.pcl_cb) rospy.Subscriber(self.in_viz + '/' + self.camera_used + '/camera_info', CameraInfo, self.ci_cb) rospy.Subscriber( self.in_viz + '/' + self.camera_used + '/' + self.image_spec, CompressedImage, self.image_cb) #rospy.Subscriber(self.in_semseg + '/class_matrix', clmat, self.clmat_cb) #approximate time synchranization of subscribed topics mode_sub = message_filters.Subscriber(self.in_pcl, PointCloud2) penalty_sub = message_filters.Subscriber( self.in_semseg + '/class_matrix', clmat) ts = message_filters.ApproximateTimeSynchronizer( [mode_sub, penalty_sub], 200, 0.5) ts.registerCallback(self.callback) # topic where we publish self.pcl_pub = rospy.Publisher(self.out_pcl, PointCloud2, queue_size=20) self.pcl_pub2 = rospy.Publisher('dyn_pcl_clmat_IG', PointCloud2, queue_size=20) #setting buffer for transformation tfBuffer = tf2_ros.Buffer(rospy.Duration(10)) #tf buffer length self.tf_listener = tf.TransformListener(tfBuffer) self.adjustment = np.dot( tf_help.rotation_matrix(math.pi / 2, (0, 1, 0)), tf_help.rotation_matrix(math.pi, (0, 0, 1))) print "hi"
def mbes_as_cb(self, goal): # Unpack goal p_auv = [ goal.mbes_pose.transform.translation.x, goal.mbes_pose.transform.translation.y, goal.mbes_pose.transform.translation.z ] r_mbes = quaternion_matrix([ goal.mbes_pose.transform.rotation.x, goal.mbes_pose.transform.rotation.y, goal.mbes_pose.transform.rotation.z, goal.mbes_pose.transform.rotation.w ])[0:3, 0:3] # IGL sim ping # The sensor frame on IGL needs to have the z axis pointing opposite from the actual sensor direction R_flip = rotation_matrix(np.pi, (1, 0, 0))[0:3, 0:3] mbes = self.draper.project_mbes(np.asarray(p_auv), r_mbes.dot(R_flip), goal.beams_num.data, self.mbes_angle) mbes = mbes[::-1] # Reverse beams for same order as real pings # Pack result mbes_cloud = pack_cloud(self.map_frame, mbes) result = MbesSimResult() result.sim_mbes = mbes_cloud self.as_ping.set_succeeded(result) self.latest_mbes = result.sim_mbes
def transformation_matrix(rot_angle, rot_dir, trans): # print "rot_angle: ", rot_angle # print "rot_dir: ", rot_dir # print "trans: ", trans return tfs.concatenate_matrices( tfs.rotation_matrix(rot_angle, rot_dir), tfs.translation_matrix(trans))
def cv2ros(rvecs,tvecs): """Transform pose from openCV solvePNP output format to ROS TransformRTStampedWithHeader format. Args: rvecs: in format [rx, ry, rz] units : radians tvecs: in format [tx, ty, tz] units : meters (type: list) Returns: An HTM (type: TransformRTStampedWithHeader) """ # ROS pose ros_pose = TransformRTStampedWithHeader() # ROS position ros_pose.transform.translation.x = tvecs[0] ros_pose.transform.translation.y = tvecs[1] ros_pose.transform.translation.z = tvecs[2] # Ros orientation angle = sqrt(rvecs[0] ** 2 + rvecs[1] ** 2 + rvecs[2] ** 2) direction = [i / angle for i in rvecs[0:3]] np_T = tf.rotation_matrix(angle, direction) np_q = tf.quaternion_from_matrix(np_T) ros_pose.transform.rotation.x = np_q[0] ros_pose.transform.rotation.y = np_q[1] ros_pose.transform.rotation.z = np_q[2] ros_pose.transform.rotation.w = np_q[3] return ros_pose
def perpendicular_vector(v): """ Finds an arbitrary perpendicular vector to *v*. The idea here is finding a perpendicular vector then rotating abitraryly it around v @type v np.array([x,y,z]) """ # for two vectors (x, y, z) and (a, b, c) to be perpendicular, # the following equation has to be fulfilled # 0 = ax + by + cz # x = y = z = 0 is not an acceptable solution if v[0] == v[1] == v[2] == 0: raise ValueError('zero-vector') # If one dimension is zero, this can be solved by setting that to # non-zero and the others to zero. Example: (4, 2, 0) lies in the # x-y-Plane, so (0, 0, 1) is orthogonal to the plane. random_rotation = tr.rotation_matrix(np.random.uniform(0, np.pi),v)[:3,:3] if v[0] == 0: return np.dot(random_rotation,np.array([1, 0, 0])) if v[1] == 0: return np.dot(random_rotation,np.array([0, 1, 0])) if v[2] == 0: return np.dot(random_rotation,np.array([0, 0, 1])) # set a = b = 1 # then the equation simplifies to # c = -(x + y)/z return np.dot(random_rotation,np.array([1, 1, -1.0 * (v[0] + v[1]) / v[2]]))
def get_gripper_transformation(self, yaw): suction_cup = 0.01 # meters, used to offset suction press gripper_offsets = np.array([0.0, -0.1397 + suction_cup, -0.0889, 1]) R = tft.rotation_matrix(pi / 2 + yaw, (0, 0, 1)) # rotation around z p = R.dot(gripper_offsets) D_e = tft.translation_matrix(p[:3]) # m return D_e
def test_transformations(self): from math import pi alpha, beta, gamma = 0, 0, -pi/2. origin, xaxis, yaxis, zaxis = (0, 0, 0), (1, 0, 0), (0, 1, 0), (0, 0, 1) # Rx = tf.rotation_matrix(alpha, xaxis) # Ry = tf.rotation_matrix(beta, yaxis) R = Rz = tf.rotation_matrix(gamma, zaxis) #R = tf.concatenate_matrices(Rx, Ry, Rz) point0 = np.array([0, 1, 0, 0]) print point0 #point1 = R * point0 #point1 = R * point0.T #point1 = np.multiply(R, point0) #point1 = np.multiply(R, point0.T) actual_point1 = R.dot(point0) print 'actual_point1:', actual_point1 expected_point1 = np.array([1, 0, 0, 0]) print 'expected_point1:', expected_point1 #np.alltrue(point1 == ) #self.assertEqual(point1.tolist(), expected_point1.tolist()) self.assertTrue(np.allclose(actual_point1, expected_point1))
def ur2ros(ur_pose): """Transform pose from UR format to ROS Pose format""" # ROS pose ros_pose = Pose() # Position ros_pose.position.x = ur_pose[0] ros_pose.position.y = ur_pose[1] ros_pose.position.z = ur_pose[2] # angle normalized angle = sqrt(ur_pose[3]**2 + ur_pose[4]**2 + ur_pose[5]**2) orient = [i / angle for i in ur_pose[3:6]] #rx ry rz to rotation matrix np_T = tf.rotation_matrix(angle, orient) #rotation matrix to quaternion np_q = tf.quaternion_from_matrix(np_T) ros_pose.orientation.x = np_q[0] ros_pose.orientation.y = np_q[1] ros_pose.orientation.z = np_q[2] ros_pose.orientation.w = np_q[3] return ros_pose
def coordinate_translator(world_coords, odom_coords, focal_point, principal_point): """ Takes in the coordinates of the point in the world and translates it to a point on the image using odometry and lens data """ w_z, w_y, w_x = world_coords w_y *= -1 o_z, o_y, o_theta = odom_coords o_x = 0 o_y *= -1 fx, fy = focal_point cx, cy = principal_point t = (w_x - o_x, w_y - o_y, w_z - o_z) r = rotation_matrix(o_theta, (1, 0, 0)) rx = r[0][:3] ry = r[1][:3] rz = r[2][:3] x = t[0] + (w_x * rx[0]) + (w_y * rx[1]) + (w_z * rx[2]) y = t[1] + (w_x * ry[0]) + (w_y * ry[1]) + (w_z * ry[2]) z = t[2] + (w_x * rz[0]) + (w_y * rz[1]) + (w_z * rz[2]) u = (fx * (x / z)) + cx v = (fy * (y / z)) + cy return [480 - v, 640 - u]
def test_frame3_axis_angle(self, x, y, z, axis, angle): r1 = np.array(spw.frame_axis_angle(x, y, z, axis, angle)).astype(float) r2 = rotation_matrix(angle, np.array(axis)) 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 GetPlaceOnTSRList( T_target, T_ee, Bw_T, Bw_R, avalues = [0] ): T0_w = T_target T0_p = TSRUtil.E2P( T_ee ) Tw_p = linalg.inv( T0_w ) * T0_p Tw_p[0,3] = 0 Tw_p[1,3] = 0 # backoff distance from center of target backoff = -0.03 # determine if EE is perpendicular or parallel to target [x_t, y_t, z_t, t_t] = MatrixToAxes( T_target ) [x_e, y_e, z_e, t_e] = MatrixToAxes( T_ee ) t_backoff = array( [0, 0, backoff * ( 1 - fabs( dot( z_t, z_e ) ) )] ) G_backoff = mat( Rt2G( eye( 3 ), t_backoff) ) tsrList = [] for i in range( len( avalues ) ): th = avalues[i] * pi / 180 Rz = mat( tr.rotation_matrix( th, array( [0, 0, 1.] ) ) ) Tw_p_i = Rz * Tw_p * G_backoff T0_p_i = T0_w * Tw_p_i tsr_i = TSRUtil.PalmToTSR( T0_p_i, T0_w, Bw_T, Bw_R ) tsrList.append( copy.deepcopy( tsr_i ) ) return tsrList
def GetPlaceOnTSRList(T_target, T_ee, Bw_T, Bw_R, avalues=[0]): T0_w = T_target T0_p = TSRUtil.E2P(T_ee) Tw_p = linalg.inv(T0_w) * T0_p Tw_p[0, 3] = 0 Tw_p[1, 3] = 0 # backoff distance from center of target backoff = -0.03 # determine if EE is perpendicular or parallel to target [x_t, y_t, z_t, t_t] = MatrixToAxes(T_target) [x_e, y_e, z_e, t_e] = MatrixToAxes(T_ee) t_backoff = array([0, 0, backoff * (1 - fabs(dot(z_t, z_e)))]) G_backoff = mat(Rt2G(eye(3), t_backoff)) tsrList = [] for i in range(len(avalues)): th = avalues[i] * pi / 180 Rz = mat(tr.rotation_matrix(th, array([0, 0, 1.]))) Tw_p_i = Rz * Tw_p * G_backoff T0_p_i = T0_w * Tw_p_i tsr_i = TSRUtil.PalmToTSR(T0_p_i, T0_w, Bw_T, Bw_R) tsrList.append(copy.deepcopy(tsr_i)) return tsrList
def get_start_guess(self): if 1: rod = matrix2rodrigues(self.base_cam.get_rotation()) t = self.base_cam.get_translation() t.shape= (3,) rod.shape=(3,) return np.array(list(t)+list(rod),dtype=np.float) R = np.eye(4) R[:3,:3] = self.base_cam.get_rotation() angle, direction, point = rotation_from_matrix(R) q = quaternion_about_axis(angle,direction) #q = matrix2quaternion(R) if 1: R2 = rotation_matrix(angle, direction, point) #R2 = quaternion2matrix( q ) try: assert np.allclose(R, R2) except: print print 'R' print R print 'R2' print R2 raise C = self.base_cam.get_camcenter() result = list(C) + list(q) return result
def fourdof_to_matrix(translation, yaw): """ Convert from a Cartesian translation and yaw to a homogeneous transform. """ T = transformations.rotation_matrix(yaw, [0,0,1]) T[0:3,3] = translation return T
def ur2ros(ur_pose): """Transform pose from UR format to ROS Pose format. Args: ur_pose: A pose in UR format [px, py, pz, rx, ry, rz] (type: list) Returns: An HTM (type: Pose). """ # ROS pose ros_pose = Pose() # ROS position ros_pose.position.x = ur_pose[0] ros_pose.position.y = ur_pose[1] ros_pose.position.z = ur_pose[2] # Ros orientation angle = sqrt(ur_pose[3]**2 + ur_pose[4]**2 + ur_pose[5]**2) direction = [i / angle for i in ur_pose[3:6]] np_T = tf.rotation_matrix(angle, direction) np_q = tf.quaternion_from_matrix(np_T) ros_pose.orientation.x = np_q[0] ros_pose.orientation.y = np_q[1] ros_pose.orientation.z = np_q[2] ros_pose.orientation.w = np_q[3] return ros_pose
def fourdof_to_matrix(translation, yaw): """ Convert from a Cartesian translation and yaw to a homogeneous transform. """ T = transformations.rotation_matrix(yaw, [0, 0, 1]) T[0:3, 3] = translation return T
def align_with(self, target): """ Aligns the normal of this triangle to target @param target - 3 dim np.array @return (rotated triangle, rotation matrix) """ target = np.array(target) source = np.cross(self.b - self.a, self.c - self.a) source /= np.linalg.norm(source) rotation = np.eye(3) dot = np.dot(source, target) if not np.isnan(dot): angle = np.arccos(dot) if not np.isnan(angle): cross = np.cross(source, target) cross_norm = np.linalg.norm(cross) if not np.isnan(cross_norm) and not cross_norm < epsilon: cross = cross / cross_norm rotation = tft.rotation_matrix(angle, cross)[:3,:3] return (Triangle(np.dot(rotation, self.a), np.dot(rotation, self.b), np.dot(rotation, self.c)), rotation)
def getWristPose(joint_angle_list, robot, joint_names, intermediate_pose=False): ''' Get the wrist pose for given joint angle configuration. joint_angle_list: List of joint angles to get final wrist pose for kwargs: Other keyword arguments use as required. Return: List of 16 values which represent the joint wrist pose obtained from the End-Effector Transformation matrix using column-major ordering. ''' pose = np.eye(4) intermediate_poses = [] for joint, angle in zip(joint_names, joint_angle_list): R, T, axis = robot.get_joint_pose(joint) A = rotation_matrix(angle, axis) pose = np.matmul(np.matmul(pose, np.matmul(R, T)), A) intermediate_poses.append((pose, axis)) if intermediate_pose: return intermediate_poses else: return pose
def test_frame3_axis_angle(self, x, y, z, axis, angle): r2 = rotation_matrix(angle, np.array(axis)) r2[0, 3] = x r2[1, 3] = y r2[2, 3] = z np.testing.assert_array_almost_equal(w.compile_and_execute(w.frame_axis_angle, [x, y, z, axis, angle]), r2)
def align_with(self, target): """ Aligns the normal of this triangle to target @param target - 3 dim np.array @return (rotated triangle, rotation matrix) """ target = np.array(target) source = np.cross(self.b - self.a, self.c - self.a) source /= np.linalg.norm(source) rotation = np.eye(3) dot = np.dot(source, target) if not np.isnan(dot): angle = np.arccos(dot) if not np.isnan(angle): cross = np.cross(source, target) cross_norm = np.linalg.norm(cross) if not np.isnan(cross_norm) and not cross_norm < epsilon: cross = cross / cross_norm rotation = tft.rotation_matrix(angle, cross)[:3, :3] return (Triangle(np.dot(rotation, self.a), np.dot(rotation, self.b), np.dot(rotation, self.c)), rotation)
def GetManipGraspList( objectName, objectPose, tablePose, actionName ): loginfo( "Generating %s grasp for %s" %( actionName, objectName ) ) graspList = [] grasp = graspit_msgs.msg.Grasp() T0_o = PoseToTransform( objectPose ) To_g_list = [] if "darpadrill" in objectName: # Rx1 has 2 fingers along +ve y-axis # Rx2 has 2 fingers along -ve y-axis Rx1 = tr.rotation_matrix( -pi/2, array( [0, 1., 0] ) ) Rx2 = tr.rotation_matrix( pi/2, array( [0, 1., 0] ) ) for th in arange( 15, 75, 15) * pi/180: Rz1 = tr.rotation_matrix( th, array( [0, 0, 1.] ) ) To_g = mat( Rz1 ) * mat( Rx1 ) To_g_list.append( copy.deepcopy( To_g ) ) Rz2 = tr.rotation_matrix( pi - th, array( [0, 0, 1.] ) ) To_g = mat( Rz2 ) * mat( Rx2 ) To_g_list.append( copy.deepcopy( To_g ) ) G_fin = mat( Rt2G( eye( 3 ), array( [0.,0., -0.07] ) ) ) G_pre = mat( Rt2G( eye( 3 ), array( [0.,0., -0.14] ) ) ) G_tra = mat( Rt2G( eye( 3 ), array( [0.,0., 0.00] ) ) ) for To_g in To_g_list: grasp.pre_grasp_pose = TransformToPose( G_tra *T0_o * To_g * G_pre ) grasp.final_grasp_pose = TransformToPose( T0_o * To_g * G_fin ) grasp.epsilon_quality = 0.1 grasp.volume_quality = 0.1 graspList.append( copy.deepcopy( grasp ) ) else: success = 0 reason = "No trigger grasps for object %s" % objectName return success, graspList, reason loginfo( "Trigger grasps: %d grasps" %( len( graspList ) ) ) success = 1 reason = "Done" return success, graspList, reason
def get_camera_position(self, marker): """ Outputs the position of the camera in the global coordinates """ euler_angles = euler_from_quaternion((marker.pose.pose.orientation.x, marker.pose.pose.orientation.y, marker.pose.pose.orientation.z, marker.pose.pose.orientation.w)) translation = np.array([marker.pose.pose.position.y, -marker.pose.pose.position.x, 0, 1.0]) translation_rotated = rotation_matrix(self.yaw-euler_angles[2], [0,0,1]).dot(translation) xy_yaw = (translation_rotated[0]+self.position[0],translation_rotated[1]+self.position[1],self.yaw-euler_angles[2]) return xy_yaw
def fk(robot, lr, joint_values): pose_mat = robot.GetLink(start_link).GetTransform() R = np.eye(4) for i, j in enumerate(joint_values): rot = tft.rotation_matrix(j, arm_joint_axes[i]) trans = arm_link_trans[i] R[:3,:3] = rot[:3,:3] R[:3,3] = trans pose_mat = np.dot(pose_mat, R) return pose_mat
def rotation_matrix_from_axes(newaxis, oldaxis=Z_AXIS): """ Return the rotation matrix that aligns two vectors. """ oldaxis = tr.unit_vector(oldaxis) newaxis = tr.unit_vector(newaxis) c = np.dot(oldaxis, newaxis) angle = np.arccos(c) if np.isclose(c, -1.0) or np.allclose(newaxis, oldaxis): v = perpendicular_vector(newaxis) else: v = np.cross(oldaxis, newaxis) return tr.rotation_matrix(angle, v)
def calENU(self): if(self.body.isNone() or self.headingAngle is None): return False else: br = self.body.getRotationAroundZ() q = rotation_matrix(br+self.headingAngle, (0,0,1)) t = translation_from_matrix(self.body.getMatrix()) t = translation_matrix(t) self.enu.setMatrix(t).transformByMatrix(q) return True #eoc
def callback(self, data): # rospy.loginfo('callback called') # Set parameters # TODO: get the parameters from the gps transform!!! xoff = -0.45 yoff = 0.0 std = .03 rot = math.pi/2 # Rotate the base pose origin, zaxis = (0, 0, 0), (0, 0, 1) Rz = rotation_matrix(rot, zaxis, origin) pt = numpy.matrix((data.pose.pose.position.x, data.pose.pose.position.y, data.pose.pose.position.z, 0),dtype=numpy.float64).T xy = Rz*pt # Add the angle rotation oldAng = data.pose.pose.orientation ang = euler_from_quaternion([ oldAng.x, oldAng.y, oldAng.z, oldAng.w ]) ang = ang[0], ang[1], ang[2] + rot # Offset the GPS reading by the lever arm offset x = xy[0] + xoff*math.cos(ang[2]) - yoff*math.sin(ang[2]) y = xy[1] + xoff*math.sin(ang[2]) + yoff*math.cos(ang[2]) #x = xy[0] + xoff*math.cos(ang[2]) #y = xy[1] + yoff*math.sin(ang[2]) gps_x = x + random.gauss(0,std) gps_y = y + random.gauss(0,std) print "testing" print xy print x,y print gps_x, gps_y # Populate the angle # Note... GPS doesnt really return a heading!! So we cant use to localize... rememmmmberrrrr # gps_msg.pose.pose.orientation = Quaternion(*quaternion_from_euler(*ang)) self.gps_msg.pose.orientation = Quaternion(*quaternion_from_euler(*ang)) # Populate x,y self.gps_msg.header.stamp = data.header.stamp self.gps_msg.header.frame_id = "map_gps" #self.gps_msg.pose.pose.position.x = gps_x #self.gps_msg.pose.pose.position.y = gps_y self.gps_msg.pose.position.x = gps_x self.gps_msg.pose.position.y = gps_y # Publish self.pub.publish(self.gps_msg) self.pubStat.publish(self.gps_stat)
def convert_pose_inverse_transform(pose): translation = np.zeros((4,1)) translation[0] = -pose.position.x translation[1] = -pose.position.y translation[2] = -pose.position.z translation[3] = 1.0 rotation = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) euler_angle = euler_from_quaternion(rotation) rotation = np.transpose(rotation_matrix(euler_angle[2], [0,0,1])) # the angle is a yaw transformed_translation = rotation.dot(translation) translation = (transformed_translation[0], transformed_translation[1], transformed_translation[2]) rotation = quaternion_from_matrix(rotation) return (translation, rotation)
def get_camera_position(self, marker): """ Outputs the position of the camera in the global coordinates """ translation, rotation = TransformHelpers.convert_pose_inverse_transform(marker.pose.pose) euler_angles = euler_from_quaternion(rotation) # correct for coordinate system convention changes between camera and world # Also, the Neato can't fly! translation = np.array([-translation[1], translation[0], 0, 1]) # correct for the possibility that the marker is rotated relative to the world coordinate frame rot_mat = rotation_matrix(self.yaw, [0,0,1]) translation_rotated = rot_mat.dot(translation) xy_yaw = (translation_rotated[0]+self.position[0], translation_rotated[1]+self.position[1], self.yaw+euler_angles[2]) return xy_yaw
def convert_pose_inverse_transform(pose): """ Helper method to invert a transform (this is built into the tf C++ classes, but ommitted from Python) """ translation = np.zeros((4,1)) translation[0] = -pose.position.x translation[1] = -pose.position.y translation[2] = -pose.position.z translation[3] = 1.0 rotation = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) euler_angle = euler_from_quaternion(rotation) rotation = np.transpose(rotation_matrix(euler_angle[2], [0,0,1])) # the angle is a yaw transformed_translation = rotation.dot(translation) translation = (transformed_translation[0], transformed_translation[1], transformed_translation[2]) rotation = quaternion_from_matrix(rotation) return (translation, rotation)
def addGeometryToEnvironment(endpoint1, endpoint2): #TODO: How do we add in the doodler? radius = 0.0005 with env: prism = orpy.RaveCreateKinBody(env, '') prism.SetName(str(np.random.rand())) #give it a random name - we'll never reference it points_diff = (endpoint2 - endpoint1)[:-1, 3] #difference vector offset = endpoint1[:-1, 3] norm = np.linalg.norm(points_diff, 2) #prism.InitFromBoxes(np.array([[0., 0., 0., radius*2., radius*2., norm]]), True) prism.InitFromBoxes(np.array([[-radius, -radius, 0., radius, radius, norm/2.]]), True) z = np.array([0., 0., norm]) cross = np.cross(z, points_diff) angle = np.arccos(np.dot(z, points_diff) / (norm * norm) ) tr_matrix = tr.rotation_matrix(angle, cross) tr_matrix[:-1, 3] = (endpoint1[:-1, 3] + endpoint2[:-1, 3])/2. env.Add(prism, True) prism.SetTransform(tr_matrix)
def transformWorldToPixels(self, aSpriteCordinates, cameraLocation, cameraAngle): #initialize the array which has elements of coins #initialize the camera coordinate points for each coin image aSpritePixels = [] for aCoord in aSpriteCordinates: #rotational matrix, rotate by self.theta, along the z-axis rotat = rotation_matrix(cameraAngle,[0,0,1]) #translation matrix which maps the location of a robot(in world) to camera coordinate origin trans = np.matrix([[1,0,0,-(cameraLocation[0])],[0,1,0,-(cameraLocation[1])],[0,0,1,-(cameraLocation[2])],[0,0,0,1]]) #this calculates the points of the aCoin in camera coordinate system new_coord = np.dot(rotat, np.dot(trans, aCoord)) #calls the pixelate function which maps the camera coordinate points into 2D screen aSpritePixels.append(self.pixelate(new_coord[:3])) #append to the array which takes an array for each coin return aSpritePixels """
def rotation_matrix_from_axes(newaxis, oldaxis=Z_AXIS): """ Returns the rotation matrix that aligns two vectors. @type newaxis: np.array @param newaxis: The goal axis @type oldaxis: np.array @param oldaxis: The initial axis @rtype: array, shape (3,3) @return: The resulting rotation matrix that aligns the old to the new axis. """ oldaxis = tr.unit_vector(oldaxis) newaxis = tr.unit_vector(newaxis) c = np.dot(oldaxis, newaxis) angle = np.arccos(c) if np.isclose(c, -1.0) or np.allclose(newaxis, oldaxis): v = perpendicular_vector(newaxis) else: v = np.cross(oldaxis, newaxis) return tr.rotation_matrix(angle, v)
def fk(self, joint_values): self.handles = list() pose_mat = self.origin R = np.eye(4) for i, j in enumerate(joint_values[:]): rot = tft.rotation_matrix(j, self.arm_joint_axes[i]) trans = self.arm_link_trans[i] R[:3,:3] = rot[:3,:3] R[:3,3] = trans print('{0}: {1}\n'.format(i,R)) pose_mat = np.dot(pose_mat, R) self.handles += utils.plot_transform(self.env, pose_mat, .3) pose_mat = np.dot(pose_mat, self.gripper_tool_to_sensor) self.handles += utils.plot_transform(self.env, pose_mat, .3) return tfx.pose(pose_mat)
def get_rotation_matrix(angle,axis): """ This function returns a rot matrix (The new frame has the x axis aligned with the GIVEN axis) @type angle : float @param angle : radian @type axis : array([x,y,z]) @param axis : new x-axis (normalized) @type rot_matrix: 3x3 matrix @param rot_matrix: rotation matrix """ old_x = np.array([1,0,0]) v = np.cross(old_x, axis) I = np.eye(3) K = skew(v) c = np.dot(old_x, axis) s = np.linalg.norm(v) R = I + s*K + (1-c)*np.linalg.matrix_power(K,2) R_angle = tr.rotation_matrix(angle, old_x) rot_matrix = np.dot(R, R_angle[:3,:3]) return rot_matrix
def beaconCallback(self,data): # rospy.loginfo('callback called') beacon_msg = Beacon(); # Rotation value rot = math.pi/2 # Rotate the base pose origin, zaxis = (0, 0, 0), (0, 0, 1) Rz = rotation_matrix(rot, zaxis, origin) pt = numpy.matrix((data.pose.pose.position.x, data.pose.pose.position.y, data.pose.pose.position.z, 0),dtype=numpy.float64).T xy = Rz*pt # Add the angle rotation oldAng = data.pose.pose.orientation ang = euler_from_quaternion([ oldAng.x, oldAng.y, oldAng.z, oldAng.w ]) ang = ang[0], ang[1], ang[2] + rot # Offset the base_pose_ground_truth by the specified xoff, yoff sensor_x = xy[0] + self.xoff*math.cos(ang[2]) - self.yoff*math.sin(ang[2]) sensor_y = xy[1] + self.xoff*math.sin(ang[2]) + self.yoff*math.cos(ang[2]) dist_x = sensor_x - self.beacon_x; dist_y = sensor_y - self.beacon_y; dist_true = math.sqrt(dist_x**2 + dist_y**2) dist = dist_true + random.gauss(0,self.std) print "testing" print xy print sensor_x, sensor_y print self.beacon_x, self.beacon_y print dist_x, dist_y print dist_true print dist # Populate the message beacon_msg.header.stamp = data.header.stamp beacon_msg.header.frame_id = "base_ranger_1" beacon_msg.beacon_frame = "beacon_1" beacon_msg.range = dist # Publish self.pub.publish(beacon_msg)
def rotate(x, angle): return transformations.rotation_matrix(angle, axis_world)[:3, :3].dot(x)
def _pose_sending_func(self): """ Continually sends the desired pose to the controller. """ r = rospy.Rate(100) while not rospy.is_shutdown(): try: r.sleep() except rospy.ROSInterruptException: break # get the desired pose as a transform matrix with self._state_lock: ps_des = copy.deepcopy(self._desired_pose_stamped) delta_dist = self._delta_dist if ps_des == None: # no desired pose to publish continue # convert desired pose to transform matrix from goal frame to CONTROLLER_FRAME self._transform_listener.waitForTransform( CONTROLLER_FRAME, ps_des.header.frame_id, rospy.Time(0), rospy.Duration(4.0) ) # trans, rot = self._transform_listener.lookupTransform(CONTROLLER_FRAME, ps_des.header.frame_id, rospy.Time(0)) # torso_H_wrist_des = geom.trans_rot_to_hmat(trans, rot) ps_des.header.stamp = rospy.Time(0) ps_des = self._transform_listener.transformPose(CONTROLLER_FRAME, ps_des) torso_H_wrist_des = conversions.pose_to_mat(ps_des.pose) if self.br is not None: p = ps_des.pose.position q = ps_des.pose.orientation self.br.sendTransform( (p.x, p.y, p.z), (q.x, q.y, q.z, q.w), rospy.Time.now(), "/cci_goal", CONTROLLER_FRAME ) # get the current pose as a transform matrix goal_link = self._hand_description.hand_frame # really shouldn't need to do waitForTransform here, since rospy.Time(0) means "use the most # recent available transform". but without this, tf sometimes raises an ExtrapolationException. -jdb self._transform_listener.waitForTransform(CONTROLLER_FRAME, goal_link, rospy.Time(0), rospy.Duration(4.0)) trans, rot = self._transform_listener.lookupTransform(CONTROLLER_FRAME, goal_link, rospy.Time(0)) torso_H_wrist = geom.trans_rot_to_hmat(trans, rot) # compute difference between actual and desired wrist pose wrist_H_wrist_des = np.dot(linalg.inv(torso_H_wrist), torso_H_wrist_des) trans_err, rot_err = geom.hmat_to_trans_rot(wrist_H_wrist_des) # scale the relative transform such that the translation is equal # to delta_dist. we're assuming that the scaled angular error will # be reasonable scale_factor = delta_dist / linalg.norm(trans_err) if scale_factor > 1.0: scale_factor = 1.0 # don't overshoot scaled_trans_err = scale_factor * trans_err angle_err, direc, point = transformations.rotation_from_matrix(wrist_H_wrist_des) scaled_angle_err = scale_factor * angle_err wrist_H_wrist_control = np.dot( transformations.translation_matrix(scaled_trans_err), transformations.rotation_matrix(scaled_angle_err, direc, point), ) # find incrementally moved pose to send to the controller torso_H_wrist_control = np.dot(torso_H_wrist, wrist_H_wrist_control) desired_pose = conversions.mat_to_pose(torso_H_wrist_control) desired_ps = PoseStamped() desired_ps.header.stamp = rospy.Time(0) desired_ps.header.frame_id = CONTROLLER_FRAME desired_ps.pose = desired_pose if self.br is not None: p = desired_ps.pose.position q = desired_ps.pose.orientation self.br.sendTransform( (p.x, p.y, p.z), (q.x, q.y, q.z, q.w), rospy.Time.now(), "/cci_imm_goal", CONTROLLER_FRAME ) # send incremental pose to controller self._desired_pose_pub.publish(desired_ps)
def rotz(q): return numpy.matrix(tfs.rotation_matrix(q, [0, 0, 1])) def trans(xyz): return numpy.matrix(tfs.translation_matrix(xyz))
def roty(q): return numpy.matrix(tfs.rotation_matrix(q, [0, 1, 0])) def rotz(q): return numpy.matrix(tfs.rotation_matrix(q, [0, 0, 1]))
def viconPoseCallback(self, msg_vicon): if self.adjVicon: self.Tv_w = np.dot(self.poseToTransform(msg_vicon.pose), tr.rotation_matrix(-np.pi/2,(0,0,1))) #print "Found vicon initial frame: {vicon}".format(vicon=self.Tv_w) self.adjVicon = False
def rot_angle_direction(angle, direction): direction = direction/np.linalg.norm(direction) r = tft.rotation_matrix(angle, direction.A1) return np.matrix(r[0:3,0:3])
def run_controller(self): if self.X is None: return self.t = (rospy.Time.now() - self.tbase).to_sec() if self.running_flag and not rospy.get_param('return_to_base'): try: Xref = self.ref_func(self.t) except ValueError: # let's assume this implies t is out of range: if self.t < self.ref_time[0] or self.t > self.ref_time[-1]: rospy.loginfo("Trajectory Complete!") self.running_flag = False self.Einx = 0 self.Einy = 0 if self.first_time: self.first_time = False self.cmd_pub.publish(Twist()) return # if we get here, we can run the controller: cmd = Twist() uff = self.estimate_derivative(self.t) # uff is in the spatial frame... transform to body frame: Rbo = tr.rotation_matrix(self.ref_func(self.t)[2], [0,0,1])[0:2,0:2].T Vb = np.dot(Rbo, uff[0:2]) Vfeedback_p = np.array([ 50*Kx*(Xref[0] - self.X[0]), 50*Ky*(Xref[1] - self.X[1]) ]) Vfb_k = np.dot(Rbo, Vfeedback_p) if self.get_intergral_time== 0: self.time_gap = rospy.Time.now().to_sec() self.get_intergral_time = 1 elif self.get_intergral_time == 1: self.time_gap = rospy.Time.now().to_sec() - self.time_gap self.Einx = self.Einx + (Xref[0] - self.X[0])*self.time_gap self.Einy = self.Einy + (Xref[1] - self.X[1])*self.time_gap self.get_intergral_time = 2 else: self.Einx = self.Einx + (Xref[0] - self.X[0])*self.time_gap self.Einy = self.Einy + (Xref[1] - self.X[1])*self.time_gap Vfeedback_i = np.array([ self.Einx*Kix, self.Einy*Kiy ]) Vfb_i = np.dot(Rbo, Vfeedback_i) cmd.linear.x = Vb[0] + Vfb_k[0]+Vfb_i[0] cmd.linear.y = Vb[1] + Vfb_k[1]+Vfb_i[1] cmd.angular.z = uff[2] + Kth*angle_utils.shortest_angular_distance(self.X[2],Xref[2]) cmd = self.clamp_controls(cmd) rospy.logdebug("t = %f, xd = %f, yd = %f, thd = %f", self.t, cmd.linear.x, cmd.linear.y, cmd.angular.z) self.send_transform(Xref) self.cmd_pub.publish(cmd) else: try: if not rospy.get_param('return_to_base'): Xref = np.array([self.ref_state[-1][0],self.ref_state[-1][1],self.ref_state[-1][2]]) else: self.running_flag = False self.ref_state[-1][0] = 0 self.ref_state[-1][1] = 0 self.ref_state[-1][2] = 0 Xref = np.array([0,0,0]) except IndexError: return except TypeError: return cmd = Twist() Vfeedback = np.array([ 3*Kx*(Xref[0] - self.X[0]), 3*Ky*(Xref[1] - self.X[1]) ]) Rbo = tr.rotation_matrix(Xref[2], [0,0,1])[0:2,0:2].T Vfb = np.dot(Rbo, Vfeedback) cmd.linear.x = Vfb[0] cmd.linear.y = Vfb[1] cmd.angular.z = angle_utils.shortest_angular_distance(self.X[2],Xref[2]) cmd = self.clamp_controls(cmd) cmd = self.low_limit(cmd); rospy.logdebug("t = %f, xd = %f, yd = %f, thd = %f", self.t, cmd.linear.x, cmd.linear.y, cmd.angular.z) if not self.first_time: self.cmd_pub.publish(cmd) if rospy.get_param('return_to_base'): self.cmd_pub.publish(cmd) return
def frame_change(pos,last_pos,angle,last_angle,delta): inv_R = np.linalg.inv(rotation_matrix(last_angle,(0,0,1))) new_pos = np.divide(np.dot(inv_R,np.transpose(np.subtract(pos, last_pos))),delta) new_ori = math.asin(np.dot(inv_R,np.array([math.cos(angle), math.sin(angle), 0,0]))[1])/delta return [new_pos,new_ori]