def test_to_from_euler(self): """2-way conversion starting from quaternions.""" np.random.seed(0) angles_euler = np.pi * np.random.rand(100, 3) conventions_euler = ["xzx", "xyx", "yxy", "yzy", "zyz", "zxz"] # For Tait-Bryan angles the second angle must be between -pi/2 and pi/2 angles_tb = angles_euler.copy() angles_tb[:, 1] -= np.pi / 2 conventions_tb = ["xzy", "xyz", "yxz", "yzx", "zyx", "zxy"] axis_types = ["extrinsic", "intrinsic"] for convention in conventions_euler: for axis_type in axis_types: out = rowan.to_euler( rowan.from_euler( angles_euler[..., 0], angles_euler[..., 1], angles_euler[..., 2], convention, axis_type, ), convention, axis_type, ) self.assertTrue( np.all( np.logical_or( np.isclose(out - angles_euler, 0), np.isclose(out + angles_euler, 0), )), msg="Failed for convention {}, axis type {}".format( convention, axis_type), ) for convention in conventions_tb: for axis_type in axis_types: out = rowan.to_euler( rowan.from_euler( angles_tb[..., 0], angles_tb[..., 1], angles_tb[..., 2], convention, axis_type, ), convention, axis_type, ) self.assertTrue( np.all( np.logical_or( np.isclose(out - angles_tb, 0), np.isclose(out + angles_tb, 0), )), msg="Failed for convention {}, axis type {}".format( convention, axis_type), )
def test_from_to_euler(self): """2-way conversion starting from Euler angles.""" np.random.seed(0) quats = rowan.normalize(np.random.rand(25, 4)) conventions = [ "xzx", "xyx", "yxy", "yzy", "zyz", "zxz", "xzy", "xyz", "yxz", "yzx", "zyx", "zxy", ] axis_types = ["extrinsic", "intrinsic"] for convention in conventions: for axis_type in axis_types: euler = rowan.to_euler(quats, convention, axis_type) out = rowan.from_euler(euler[..., 0], euler[..., 1], euler[..., 2], convention, axis_type) self.assertTrue( np.all( np.logical_or(np.isclose(out - quats, 0), np.isclose(out + quats, 0))), msg="Failed for convention {}, axis type {}".format( convention, axis_type), )
def render(self, rotation=(1, 0, 0, 0), translation=(0, 0, 0), **kwargs): (positions, orientations, colors) = mesh.unfoldProperties( [self.positions, self.orientations, self.colors]) positions = rowan.rotate(rotation, positions) positions += translation orientations = rowan.multiply(rotation, rowan.normalize(orientations)) rotations = np.degrees(rowan.to_euler(orientations)) lines = [] for (pos, rot, col, alpha) in zip(positions, rotations, colors[:, :3], 1 - colors[:, 3]): lines.append( 'sphere {{ ' '0, 1 scale<{a}, {b}, {c}> ' 'rotate <{rot[2]}, {rot[1]}, {rot[0]}> ' 'translate <{pos[0]}, {pos[1]}, {pos[2]}> ' 'pigment {{ color <{col[0]}, {col[1]}, {col[2]}> transmit {alpha} }} ' '}}'.format(a=self.a, b=self.b, c=self.c, pos=pos, rot=rot, col=col, alpha=alpha)) return lines
def quaternion2z(quaternion: np.ndarray) -> np.ndarray: """Convert a rotation about the z axis to a quaternion. This is a helper for 2D simulations, taking the rotation of a particle about the z axis and converting it to a quaternion. The input angle `theta` is assumed to be in radians. """ return rowan.to_euler(quaternion)[:, 0].astype(np.float32)
def policy(self, state): # set state self.state.position.x = state[0] self.state.position.y = state[1] self.state.position.z = state[2] self.state.velocity.x = state[3] self.state.velocity.y = state[4] self.state.velocity.z = state[5] rpy = np.degrees(rowan.to_euler(state[6:10], 'xyz')) self.state.attitude.roll = rpy[0] self.state.attitude.pitch = -rpy[1] # inverted coordinate system! self.state.attitude.yaw = rpy[2] self.sensors.gyro.x = np.degrees(state[10]) self.sensors.gyro.y = np.degrees(state[11]) self.sensors.gyro.z = np.degrees(state[12]) firm.controllerSJC(self.control, self.setpoint, self.sensors, self.state, self.tick) self.tick += 10 # power distribution thrust = self.control.thrustSI torqueArr = firm.floatArray_frompointer(self.control.torque) torque = np.array([torqueArr[0], torqueArr[1], torqueArr[2]]) thrust_to_torque = 0.006 arm_length = 0.046 thrustpart = 0.25 * thrust yawpart = -0.25 * torque[2] / thrust_to_torque arm = 0.707106781 * arm_length rollpart = 0.25 / arm * torque[0] pitchpart = 0.25 / arm * torque[1] motorForce = np.array([ thrustpart - rollpart - pitchpart + yawpart, thrustpart - rollpart + pitchpart - yawpart, thrustpart + rollpart + pitchpart + yawpart, thrustpart + rollpart - pitchpart - yawpart ]) motorForce = np.clip(motorForce, self.a_min, self.a_max) v = firm.controllerSJCGetq() self.q.append(np.array([v.x, v.y, v.z])) v = firm.controllerSJCGetqr() self.qr.append(np.array([v.x, v.y, v.z])) v = firm.controllerSJCGetomega() self.omega.append(np.array([v.x, v.y, v.z])) v = firm.controllerSJCGetomegar() self.omegar.append(np.array([v.x, v.y, v.z])) # return np.array([0.0, 0.01, 0.01, 0.0]) * 9.81 return motorForce
def add(self, position: np.ndarray, orientation: Optional[np.ndarray]): """Update the state of the dynamics calculations by adding the next values. This updates the motion of the particles, comparing the positions and orientations of the current frame with the previous frame, adding the difference to the total displacement. This approach allows for tracking particles over periodic boundaries, or through larger rotations assuming that there are sufficient frames to capture the information. Each single displacement obeys the minimum image convention, so for large time intervals it is still possible to have missing information. Args: position: The current positions of the particles orientation: The current orientations of the particles represented as a quaternion """ self.delta_translation -= self.box.wrap(self.previous_position - position) if self.previous_orientation is not None and orientation is not None: self.delta_rotation += rowan.to_euler( rowan.divide(orientation, self.previous_orientation) ) self.previous_position = position self.previous_orientation = orientation
def deduce_state(self, s): rpy = np.degrees(rowan.to_euler(rowan.normalize(s[6:10]), 'xyz')) return rpy
def test_to_euler(self): """Test conversion to Euler angles""" v = one self.assertTrue(np.all(rowan.to_euler(v) == np.array([0, 0, 0]))) v = np.array([0.5, 0.5, 0.5, 0.5]) self.assertTrue( np.all(rowan.to_euler(v) == np.array([np.pi / 2, 0, np.pi / 2]))) # More complicated test, checks 2d arrays # and more complex angles self.assertTrue( np.allclose(rowan.to_euler(euler_quaternions, 'zyz', 'intrinsic'), euler_angles)) # Ensure proper errors are raised with self.assertRaises(ValueError): rowan.to_euler(euler_quaternions, 'foo', 'intrinsic') with self.assertRaises(ValueError): rowan.to_euler(euler_quaternions, 'foo', 'extrinsic') with self.assertRaises(ValueError): rowan.to_euler(euler_quaternions, 'zyz', 'bar') with self.assertRaises(ValueError): rowan.to_euler(2 * one) with self.assertRaises(ValueError): rowan.to_euler(zero)
def test_zero_beta(self): """Check cases where beta is 0.""" # Since the Euler calculations are all done using matrices, it's easier # to construct the test cases by directly using matrices as well. We # assume gamma is 0 since, due to gimbal lock, only either alpha+gamma # or alpha-gamma is a relevant parameter, and we just scan the other # possible values. The actual function is defined such that gamma will # always be zero in those cases. We define the matrices using lambda # functions to support sweeping a range of values for alpha and beta, # specifically to test cases where signs flip e.g. cos(0) vs cos(pi). # These sign flips lead to changes in the rotation angles that must be # tested. mats_euler_intrinsic = [ ('xzx', 'intrinsic', lambda alpha, beta: [[np.cos(beta), 0, 0], [0, np.cos(beta) * np.cos(alpha), -np.sin(alpha)], [0, np.cos(beta) * np.sin(alpha), np.cos(alpha)]]), ('xyx', 'intrinsic', lambda alpha, beta: [[np.cos(beta), 0, 0], [0, np.cos(alpha), -np.cos(beta) * np.sin(alpha)], [0, np.sin(alpha), np.cos(beta) * np.cos(alpha)]]), ('yxy', 'intrinsic', lambda alpha, beta: [[np.cos(alpha), 0, np.cos(beta) * np.sin(alpha)], [0, np.cos(beta), 0], [-np.sin(alpha), 0, np.cos(beta) * np.cos(alpha)]]), ('yzy', 'intrinsic', lambda alpha, beta: [[np.cos(beta) * np.cos(alpha), 0, np.sin(alpha)], [0, np.cos(beta), 0], [-np.cos(beta) * np.sin(alpha), 0, np.cos(alpha)]]), ('zyz', 'intrinsic', lambda alpha, beta: [[ np.cos(beta) * np.cos(alpha), -np.sin(alpha), 0 ], [np.cos(beta) * np.sin(alpha), np.cos(beta), 0], [0, 0, np.cos(beta)]]), ('zxz', 'intrinsic', lambda alpha, beta: [[np.cos(alpha), -np.cos(beta) * np.sin(alpha), 0], [np.sin(alpha), np.cos(beta) * np.cos(beta), 0], [0, 0, np.cos(beta)]]), ] mats_tb_intrinsic = [ ('xzy', 'intrinsic', lambda alpha, beta: [[0, -np.sin( beta), 0], [np.sin(beta) * np.cos(alpha), 0, -np.sin( alpha)], [np.sin(beta) * np.sin(alpha), 0, np.cos(alpha)]]), ('xyz', 'intrinsic', lambda alpha, beta: [[0, 0, np.sin( beta)], [np.sin(beta) * np.sin(alpha), np.cos(alpha), 0 ], [-np.sin(beta) * np.cos(alpha), np.sin(alpha), 0]]), ('yxz', 'intrinsic', lambda alpha, beta: [[np.cos(alpha), np.sin(beta) * np.sin(alpha), 0], [0, 0, -np.sin(beta)], [-np.sin(alpha), np.sin(beta) * np.cos(alpha), 0]]), ('yzx', 'intrinsic', lambda alpha, beta: [[0, -np.sin(beta) * np.cos(alpha), np.sin(alpha)], [np.sin(beta), 0, 0], [0, np.sin(beta) * np.sin(alpha), np.cos(alpha)]]), ('zyx', 'intrinsic', lambda alpha, beta: [[ 0, -np.sin(alpha), np.sin(beta) * np.cos(alpha) ], [0, np.cos(alpha), np.sin(beta) * np.sin(alpha)], [-np.sin(beta), 0, 0]]), ('zxy', 'intrinsic', lambda alpha, beta: [[ np.cos(alpha), 0, np.sin(beta) * np.sin(alpha) ], [np.sin(alpha), 0, -np.sin(beta) * np.cos(alpha)], [0, -1, 0]]), ] # Extrinsic rotations can be tested identically to intrinsic rotations # in the case of proper Euler angles. mats_euler_extrinsic = [(m[0], 'extrinsic', m[2]) for m in mats_euler_intrinsic] # For Tait-Bryan angles, extrinsic rotations axis order must be # reversed (since axes 1 and 3 are not identical), but more # importantly, due to the sum/difference of alpha and gamma that # arises, we need to test the negative of alpha to catch the dangerous # cases. In practice we get the same results since we're sweeping alpha # values in the tests below, but it's useful to set this up precisely. mats_tb_extrinsic = [(m[0][::-1], 'extrinsic', lambda alpha, beta: m[2] (-alpha, beta)) for m in mats_tb_intrinsic] # Since angle representations may not be unique, checking that # quaternions are equal may not work. Instead we perform rotations and # check that they are identical. For simplicity, we rotate the # simplest vector with all 3 components (otherwise tests won't catch # the problem because there's no component to rotate). test_vector = [1, 1, 1] mats_intrinsic = (mats_euler_intrinsic, mats_tb_intrinsic) mats_extrinsic = (mats_euler_extrinsic, mats_tb_extrinsic) # The beta angles are different for proper Euler angles and Tait-Bryan # angles because the relevant beta terms will be sines and cosines, # respectively. all_betas = ((0, np.pi), (np.pi / 2, -np.pi / 2)) alphas = (0, np.pi / 2, np.pi, 3 * np.pi / 2) for mats in (mats_intrinsic, mats_extrinsic): for betas, mat_set in zip(all_betas, mats): for convention, axis_type, mat_func in mat_set: quaternions = [] for beta in betas: for alpha in alphas: mat = mat_func(alpha, beta) if np.linalg.det(mat) == -1: # Some of these will be improper rotations. continue quat = rowan.from_matrix(mat) quaternions.append(quat) euler = rowan.to_euler(quat, convention, axis_type) converted = rowan.from_euler(*euler, convention=convention, axis_type=axis_type) correct_rotation = rowan.rotate(quat, test_vector) test_rotation = rowan.rotate( converted, test_vector) self.assertTrue(np.allclose(correct_rotation, test_rotation, atol=1e-6), msg=""" Failed for convention {}, axis type {}, alpha = {}, beta = {}. Expected quaternion: {}. Calculated: {}. Expected vector: {}. Calculated vector: {}.""".format( convention, axis_type, alpha, beta, quat, converted, correct_rotation, test_rotation)) # For completeness, also test with broadcasting. quaternions = np.asarray(quaternions).reshape(-1, 4) all_euler = rowan.to_euler(quaternions, convention, axis_type) converted = rowan.from_euler(all_euler[..., 0], all_euler[..., 1], all_euler[..., 2], convention, axis_type) self.assertTrue( np.allclose(rowan.rotate(quaternions, test_vector), rowan.rotate(converted, test_vector), atol=1e-6))
def render(self): """Render all the shapes in this scene. :returns: HTML string contents to be displayed """ canvas_id = 'zdog_{}'.format(self.CANVAS_INDEX) illo_id = 'illo_{}'.format(self.CANVAS_INDEX) Scene.CANVAS_INDEX += 1 html_lines = [] js_lines = [] euler = -rowan.to_euler( self.rotation, convention='xyz', axis_type='intrinsic') translation = self.translation*(1, -1, 1) pan_cfg = self.get_feature_config('pan') pan = pan_cfg.get('value', True) if pan_cfg is not None else False js_lines.append(""" let {illo_id} = new Zdog.Illustration({{ element: '#{canvas_id}', zoom: {zoom}, dragRotate: {rotation_enabled}, rotate: {{x: {angle[0]}, y: {angle[1]}, z: {angle[2]}}}, translate: {{x: {pos[0]}, y: {pos[1]}, z: {pos[2]}}}, }}); """.format( illo_id=illo_id, canvas_id=canvas_id, zoom=self.zoom*self.pixel_scale, angle=euler, pos=translation, rotation_enabled=('false' if pan else 'true'))) config = self.get_feature_config('ambient_light') ambient_light = 0 if config is None else config.get('value', .4) config = self.get_feature_config('directional_light') directional_light = ([(0, 0, 0)] if config is None else config.get('value', [(0, 0, 0)])) directional_light = np.atleast_2d(directional_light) shapeIndex = 0 for i, prim in enumerate(self._primitives): js_lines.extend(prim.render( rotation=self.rotation, illo_id=illo_id, name_suffix=i, ambient_light=ambient_light, directional_light=directional_light)) (width, height) = map(int, self.size_pixels) html_lines.append(""" <canvas id="{canvas_id}" width="{width}" height="{height}"></canvas> """.format(canvas_id=canvas_id, width=width, height=height)) html_lines.append("""<script> var fill_{canvas_id} = function() {{ """.format(canvas_id=canvas_id)) html_lines.append(LOCAL_HELPER_SCRIPT) html_lines.extend(js_lines) pan_snippet = """ new Zdog.Dragger({{ startElement: {illo_id}.element, onDragStart: function( pointer, moveX, moveY) {{ this.lastX = 0; this.lastY = 0; }}, onDragMove: function( pointer, moveX, moveY ) {{ let deltax = moveX - this.lastX; let deltay = moveY - this.lastY; let scale = 1.0/{illo_id}.zoom; {illo_id}.translate.x += deltax*scale; {illo_id}.translate.y += deltay*scale; this.lastX = moveX; this.lastY = moveY; }} }});""".format(illo_id=illo_id) if pan: html_lines.append(pan_snippet) html_lines.append(""" let this_canvas = document.querySelector("#{canvas_id}"); """.format(canvas_id=canvas_id)) html_lines.append(""" let animate_{canvas_id} = function() {{ if(is_in_view(this_canvas)) {{ {illo_id}.updateRenderGraph(); }} if(document.contains(this_canvas)) {{ requestAnimationFrame(animate_{canvas_id}); }} }}; animate_{canvas_id}();""".format(canvas_id=canvas_id, illo_id=illo_id)) # remove the global reference to this function after using it html_lines.append('fill_{canvas_id} = null;'.format(canvas_id=canvas_id)) html_lines.append('};') # end of fill_{canvas_id} # now call fill_{canvas_id}, possibly after loading zdog html_lines.append(""" if (typeof Zdog == 'undefined') {{ var script = document.createElement('script'); script.addEventListener('load', fill_{canvas_id}, false); script.src = 'https://unpkg.com/zdog@1/dist/zdog.dist.min.js'; document.getElementsByTagName('head')[0].appendChild(script); }} else fill_{canvas_id}(); """.format(canvas_id=canvas_id)) html_lines.append('</script>') return '\n'.join(html_lines)
def f(self, X, Z, A, t, logentry=None): # x = X[0] # y = X[1] # z = X[2] # xdot = X[3] # ydot = X[4] # zdot = X[5] # qw = X[6] # qx = X[7] # qy = X[8] # qz = X[9] # wx = X[10] # wy = X[11] # wz = X[12] a = Z**2 # a = A J = self.J dxdt = np.zeros(13) q = X[6:10] q = rowan.normalize(q) omega = X[10:] B0 = self.B0 * self.params['C_T'] # B0 = self.B0 eta = np.dot(B0, a) f_u = np.array([0, 0, eta[0]]) tau_u = np.array([eta[1], eta[2], eta[3]]) dxdt[0:3] = X[3:6] # dxdt[3:6] = -9.81 + rowan.rotate(q, f_u) / self.params['m'] dxdt[3:6] = np.array([0., 0., -9.81 ]) + rowan.rotate(q, f_u) / self.params['m'] Vinf = self.params['Vwind_mean'] - np.linalg.norm(X[3:6]) Vinf_B = rowan.rotate(rowan.inverse(q), Vinf) Vz_B = np.array([0.0, 0.0, Vinf_B[2]]) Vs_B = Vinf_B - Vz_B alpha = np.arcsin(np.linalg.norm(Vz_B) / np.linalg.norm(Vinf_B)) n = np.sqrt( np.multiply(a, self.B0[0, :]) / (self.params['C_T'] * self.params['rho'] * self.params['D']**4)) Fs_B = (Vs_B / np.linalg.norm(Vs_B) ) * self.params['C_s'] * self.params['rho'] * sum( n**self.params['k1']) * (np.linalg.norm(Vinf)**( 2 - self.params['k1'])) * (self.params['D']**( 2 + self.params['k1'])) * ( (np.pi / 2)**2 - alpha**2) * (alpha + self.params['k2']) # Fs_B = [0,0,0] dxdt[3:6] += rowan.rotate(q, Fs_B) / self.mass qnew = rowan.calculus.integrate(q, omega, self.ave_dt) if qnew[0] < 0: qnew = -qnew dxdt[6:10] = (qnew - q) / self.ave_dt dxdt[10:] = 1 / J * (np.cross(J * omega, omega) + tau_u) dxdt[10:] += 1 / J * np.cross( np.array([0.0, 0.0, self.params['D'] / 4]), Fs_B) euler_o = rowan.to_euler(q, 'xyz') if logentry is not None: logentry['f_u'] = f_u logentry['tau'] = tau_u logentry['euler_o'] = euler_o return dxdt.reshape((len(dxdt), 1))
def write(self, trajectory, file=sys.stdout): """Serialize a trajectory into pos-format and write it to file. :param trajectory: The trajectory to serialize :type trajectory: :class:`~garnett.trajectory.Trajectory` :param file: A file-like object.""" def _write(msg, end='\n'): file.write(msg + end) for i, frame in enumerate(trajectory): # data section if frame.data is not None: header_keys = frame.data_keys _write('#[data] ', end='') _write(' '.join(header_keys)) columns = list() for key in header_keys: columns.append(frame.data[key]) rows = np.array(columns).transpose() for row in rows: _write(' '.join(row)) _write('#[done]') # boxMatrix and rotation box_matrix = np.array(frame.box.get_box_matrix()) if self._rotate and frame.view_rotation is not None: for i in range(3): box_matrix[:, i] = rowan.rotate(frame.view_rotation, box_matrix[:, i]) if frame.view_rotation is not None and not self._rotate: angles = rowan.to_euler(frame.view_rotation, axis_type='extrinsic', convention='xyz') * 180 / math.pi _write('rotation ' + ' '.join((str(_num(_)) for _ in angles))) _write('boxMatrix ', end='') _write(' '.join((str(_num(v)) for v in box_matrix.flatten()))) # shape defs try: if len(frame.types) != len(frame.type_shapes): raise ValueError( "Unequal number of types and type_shapes.") for name, type_shape in zip(frame.types, frame.type_shapes): _write('def {} "{}"'.format(name, type_shape.pos_string)) except AttributeError: # If AttributeError is raised because the frame does not contain # shape information, fill them all with the default shape for name in frame.types: logger.info("No shape defined for '{}'. " "Using fallback definition.".format(name)) _write('def {} "{}"'.format( name, DEFAULT_SHAPE_DEFINITION.pos_string)) # Orientations must be provided for all particles # If the frame does not have orientations, identity quaternions are used orientation = getattr(frame, 'orientation', np.array([[1, 0, 0, 0]] * frame.N)) for typeid, pos, rot in zip(frame.typeid, frame.position, orientation): name = frame.types[typeid] _write(name, end=' ') try: shapedef = frame.shapedef.get(name) except AttributeError: shapedef = DEFAULT_SHAPE_DEFINITION if self._rotate and frame.view_rotation is not None: pos = rowan.rotate(frame.view_rotation, pos) rot = rowan.multiply(frame.view_rotation, rot) if isinstance(shapedef, SphereShape): _write(' '.join((str(_num(v)) for v in pos))) elif isinstance(shapedef, ArrowShape): # The arrow shape actually has two position vectors of # three elements since it has start.{x,y,z} and end.{x,y,z}. # That is, "rot" is not an accurate variable name, since it # does not represent a quaternion. _write(' '.join( (str(_num(v)) for v in chain(pos, rot[:3])))) else: _write(' '.join((str(_num(v)) for v in chain(pos, rot)))) _write('eof') logger.debug("Wrote frame {}.".format(i + 1)) logger.info("Wrote {} frames.".format(i + 1))
def policy(self, state): # current state p = state[0:3] v = state[3:6] q = state[6:10] omega = state[10:13] R = rowan.to_matrix(q) # position controller # p_tilde = p - self.p_d # v_tilde = v - self.v_d # v_r = self.v_d - self.lambda_p * p_tilde # v_r_dot = self.a_d - self.lambda_p * v_tilde # s_v = v - v_r # f_r = self.mass * v_r_dot \ # - self.Kv * s_v \ # - self.Kp * p_tilde # thrust = np.linalg.norm(f_r) # qr = mkvec( # asinf((F_d.x * sinf(yaw) - F_d.y * cosf(yaw)) / control->thrustSI), # atanf((F_d.x * cosf(yaw) + F_d.y * sinf(yaw)) / F_d.z), # desiredYaw); # position controller pos_e = self.p_d - p vel_e = self.v_d - v F_d = self.mass * (self.a_d + self.Kpos_D * vel_e + self.Kpos_P * pos_e) print(F_d) thrust = np.linalg.norm(F_d) yaw = 0 rpy_d = np.array([ np.arcsin((F_d[0] * np.sin(yaw) - F_d[1] * np.cos(yaw)) / thrust), np.arctan((F_d[0] * np.cos(yaw) + F_d[1] * np.sin(yaw)) / F_d[2]), yaw]) R_d = rowan.to_matrix(rowan.from_euler(*rpy_d)) print(rpy_d) print(R_d) # attitude controller # rotation error Rtilde = self.R_d.T @ R qtilde_0 = 1/2 * np.sqrt(1 + np.trace(Rtilde)) qtilde_v = 1 / (4 * qtilde_0) * veemap(Rtilde - Rtilde.T) omega_r = Rtilde.T @ self.omega_d - 2 * self.lambda_a * qtilde_v print(omega_r) if self.omega_r_last is not None: omega_r_dot = (omega_r - self.omega_r_last) / 0.01 else: omega_r_dot = np.zeros(3) self.omega_r_last = omega_r s_omega = omega - omega_r torque = self.J * omega_r_dot \ - np.cross(self.J * omega, omega_r) \ - self.K_att * s_omega \ - self.kq * qtilde_v # power distribution thrust_to_torque = 0.006 arm_length = 0.046 thrustpart = 0.25 * thrust yawpart = -0.25 * torque[2] / thrust_to_torque arm = 0.707106781 * arm_length rollpart = 0.25 / arm * torque[0] pitchpart = 0.25 / arm * torque[1] motorForce = np.array([ thrustpart - rollpart - pitchpart + yawpart, thrustpart - rollpart + pitchpart - yawpart, thrustpart + rollpart + pitchpart + yawpart, thrustpart + rollpart - pitchpart - yawpart ]) motorForce = np.clip(motorForce, self.a_min, self.a_max) # logging self.qr.append(np.degrees(rpy_d)) self.q.append(np.degrees(rowan.to_euler(q, 'xyz'))) self.omegar.append(omega_r) self.omega.append(omega) return motorForce
def policy(self, state): # current state p = state[0:3] v = state[3:6] q = state[6:10] omega = state[10:13] R = rowan.to_matrix(q) # position controller pos_e = self.p_d - p vel_e = self.v_d - v F_d = self.mass * (self.a_d + self.Kpos_D * vel_e + self.Kpos_P * pos_e) print(F_d) thrust = np.linalg.norm(F_d) yaw = 0 q_d = np.array([ np.arcsin((F_d[0] * np.sin(yaw) - F_d[1] * np.cos(yaw)) / thrust), np.arctan((F_d[0] * np.cos(yaw) + F_d[1] * np.sin(yaw)) / F_d[2]), yaw]) if self.q_d_last is not None: q_d_dot = (q_d - self.q_d_last) / 0.01 else: q_d_dot = np.zeros(3) self.q_d_last = q_d # attitude controller q = rowan.to_euler(q, 'xyz') omega_r = Zinv(q) @ (q_d_dot +self.lambda_att * (q_d -q)) if self.omega_r_last is not None: omega_r_dot = (omega_r - self.omega_r_last) / 0.01 else: omega_r_dot = np.zeros(3) self.omega_r_last = omega_r torque = self.J * omega_r_dot \ - np.cross(self.J * omega, omega_r) \ - self.K_att * (omega - omega_r) # power distribution thrust_to_torque = 0.006 arm_length = 0.046 thrustpart = 0.25 * thrust yawpart = -0.25 * torque[2] / thrust_to_torque arm = 0.707106781 * arm_length rollpart = 0.25 / arm * torque[0] pitchpart = 0.25 / arm * torque[1] motorForce = np.array([ thrustpart - rollpart - pitchpart + yawpart, thrustpart - rollpart + pitchpart - yawpart, thrustpart + rollpart + pitchpart + yawpart, thrustpart + rollpart - pitchpart - yawpart ]) motorForce = np.clip(motorForce, self.a_min, self.a_max) # logging self.qr.append(np.degrees(q_d)) self.q.append(np.degrees(q)) self.omegar.append(omega_r) self.omega.append(omega) return motorForce
def render(self, rotation=(1, 0, 0, 0), name_suffix='', illo_id='illo', ambient_light=0.4, directional_light=[], stroke=False, outline=False, **kwargs): # in the zdog coordinate system, x is to the right, y is down, # and z is toward you lines = [] stroke = stroke or 'false' (vertices, faces) = geometry.convexHull(self.vertices) face_normals = [] face_paths = [] for face in faces: r01 = vertices[face[1]] - vertices[face[0]] r12 = vertices[face[2]] - vertices[face[1]] normal = np.cross(r01, r12) normal /= np.linalg.norm(normal) face_normals.append(normal) path = ', '.join('{{x: {}, y: {}, z: {}}}'.format(*v) for v in vertices[face] * (1, -1, 1)) face_paths.append(path) face_normals = np.array(face_normals, dtype=np.float32) orientations_euler = rowan.to_euler(self.orientations, convention='xyz', axis_type='intrinsic') particles = zip(*mesh.unfoldProperties([ self.positions * (1, -1, 1), self.orientations, -orientations_euler, self.colors * 255 ])) for i, (position, orientation, eulers, color) in enumerate(particles): group_index = 'convexPoly_{}_{}'.format(name_suffix, i) # full rotation to apply to vectors from base orientation # to final scene orientation full_rotation = rowan.multiply(rotation, orientation) lines.append(""" let {group_index} = new Zdog.Group({{ addTo: {illo_id}, rotate: {{x: {angle[0]}, y: {angle[1]}, z: {angle[2]}}}, translate: {{x: {pos[0]}, y: {pos[1]}, z: {pos[2]}}}, updateSort: true, }});""".format(group_index=group_index, illo_id=illo_id, angle=eulers, pos=position)) for (face_path, normal) in zip(face_paths, face_normals): rotated_normal = rowan.rotate(full_rotation, normal) light = ambient_light for direction in directional_light: light += max(0, -np.dot(rotated_normal, direction)) light = np.clip(light, 0, 1) (r, g, b) = map(int, light * color[:3]) # RGB components are 0-255, A component is a float 0-1 face_color = '"rgba({}, {}, {}, {})"'.format( r, g, b, color[3] / 255) lines.append(""" new Zdog.Shape({{ addTo: {group_index}, color: {face_color}, path: [{path}], fill: true, backface: true, stroke: {stroke}, }}); """.format(group_index=group_index, face_color=face_color, path=face_path, stroke=stroke)) if outline: outline_color = '"rgba(0, 0, 0, {})"'.format(color[3] / 255) lines.append(""" new Zdog.Shape({{ addTo: {group_index}, color: {color}, path: [{path}], fill: false, stroke: {stroke}, }}); """.format(group_index=group_index, color=outline_color, path=face_path, stroke=outline)) return lines
def policy(self, X, pd=np.zeros(3), vd=np.zeros(3), ad=np.zeros(3), jd=np.zeros(3), sd=np.zeros(3), logentry=None, time=None, K_i=0, K_p=0, K_d=0): # print('baseline: X = ', X) p_e = pd - X[:3] v_e = vd - X[3:6] print(X) int_p_e = self.integrate_error(p_e, time) F_d = (self.K_i * int_p_e + self.K_p * p_e + self.K_d * v_e) * self.mass # TODO: add integral term F_d[2] += self.g * self.mass T_d = np.linalg.norm(F_d) yaw_d = 0 roll_d = np.arcsin( (F_d[0] * np.sin(yaw_d) - F_d[1] * np.cos(yaw_d)) / T_d) pitch_d = np.arctan( (F_d[0] * np.cos(yaw_d) + F_d[1] * np.sin(yaw_d)) / F_d[2]) euler_d = np.array([roll_d, pitch_d, yaw_d]) q = rowan.normalize(X[6:10]) euler = rowan.to_euler(q, 'xyz') att_e = -(euler - euler_d) th_r = rowan.normalize( rowan.from_euler(att_e[0], att_e[1], att_e[2], 'xyz')) th_d = rowan.normalize( rowan.from_euler(euler_d[0], euler_d[1], euler_d[2], 'xyz')) att_v_e = -X[10:] torque = self.Att_p * att_e + self.Att_d * att_v_e torque[0] *= self.J[0] torque[1] *= self.J[1] torque[2] *= self.J[2] Jomega = np.array( [self.J[0] * X[10], self.J[1] * X[11], self.J[2] * X[12]]) torque -= np.cross(Jomega, X[10:]) yawpart = -0.25 * torque[2] / self.t2t rollpart = 0.25 / self.arm * torque[0] #/ self.t2t pitchpart = 0.25 / self.arm * torque[1] #/ self.t2t thrustpart = 0.25 * T_d motorForce = np.array([ thrustpart - rollpart - pitchpart + yawpart, thrustpart - rollpart + pitchpart - yawpart, thrustpart + rollpart + pitchpart + yawpart, thrustpart + rollpart - pitchpart - yawpart ]) Fc = motorForce / self.params['C_T'] omega = np.sqrt(np.maximum(Fc, self.params['motor_min_speed']) ) # Ensure each prop has a positive thrust omega = np.minimum( omega, self.params['motor_max_speed']) # Maximum rotation speed # print('baseline: omega = ', omega) # print('T', T_r) # print('tau',tau_r) # x_error = pd[0] - X[0] # y_error = pd[1] - X[1] # z_error = pd[2] - X[2] # # vx_error = vd[0] - X[3] # vy_error = vd[1] - X[4] # vz_error = vd[2] - X[5] # # # print('baseline: x_error = ', x_error) # # print('baseline: y_error = ', y_error) # # ax_d = ad[0] # ay_d = ad[1] # az_d = ad[2] # # self.int_x_error += self.params.dt_posctrl * x_error # self.int_y_error += self.params.dt_posctrl * y_error # self.int_z_error += self.params.dt_posctrl * z_error # # ax_r = self.params['K_p_x'] * x_error + self.params['K_d_x'] * vx_error + self.params[ # 'K_i_x'] * self.int_x_error + ax_d # ay_r = self.params['K_p_y'] * y_error + self.params['K_d_y'] * vy_error + self.params[ # 'K_i_y'] * self.int_y_error + ay_d # az_r = self.params['K_p_z'] * z_error + self.params['K_d_z'] * vz_error + self.params[ # 'K_i_z'] * self.int_z_error + az_d # # print('baseline: x PDI terms:',K_p_x * x_error, K_d_x * vx_error, K_i_x * intx_error, ax_d) # # print('baseline: y PDI terms:',K_p_y * y_error, K_d_y * vy_error, K_i_y * inty_error, ay_d) # # jx_d = jd[0] # jy_d = jd[1] # jz_d = jd[2] # # jx_r = K_p_x * vx_error + K_d_x * (ax_d - ax_r) + K_i_x * x_error + jx_d # # jy_r = K_p_y * vy_error + K_d_y * (ay_d - ay_r) + K_i_y * y_error + jy_d # # sx_d = sd[0] # sy_d = sd[1] # sz_d = sd[2] # # sx_r = K_p_x * (ax_d - ax_r) + K_d_x * (jx_d - jx_r) + K_i_x * vx_error + sx_d # # sy_r = K_p_y * (ay_d - ay_r) + K_d_y * (jy_d - jy_r) + K_i_y * vy_error + sy_d # # # print('baseline: ay_r = ', ay_r) # # Fx_r = ax_r * self.params['m'] # Fy_r = (ay_r + self.params['g']) * self.params['m'] # Fz_r = az_r * self.params['m'] # # Fy_r = np.maximum(Fy_r, 0.10 * self.params['g'] * self.params['m']) # # # # print('baseline: Fx_r, Fy_r = ', Fx_r, Fy_r) # # T_r = np.minimum(np.sqrt(Fy_r ** 2 + Fx_r ** 2 + Fz_r ** 2), 125.) # / np.cos(X[2]) # if T_r > 124.: # warn('thrust gets too high') # # print('T_r = ', T_r) # T_d = self.params['m'] * np.sqrt(ax_d ** 2 + (ay_d + self.params['g']) ** 2 + az_d ** 2) # # # th = X[2] # th_r = # th_d = # # mat = np.array([[-np.sin(th_d), -T_d * np.cos(th_d)], #TODO: use th_d, T_d # [np.cos(th_d), -T_d * np.sin(th_d)]]) # b = self.params['m'] * np.array([jx_d, jy_d, jz_d])# # v = np.linalg.solve(mat, b) # T_d_dot = v[0] # w_d = v[1] # # w = X[5] # # mat = np.array([[-np.sin(th_d), -T_d * np.cos(th_d)], # [np.cos(th_d), -T_d * np.sin(th_d)]]) # b = self.params['m'] * np.array([sx_d, sy_d]) - np.array( # [-2. * T_d_dot * np.cos(th_d) * w_d + T_d * np.sin(th_d) * (w_d ** 2), # -2. * T_d_dot * np.sin(th_d) * w_d - T_d * np.cos(th_d) * (w_d ** 2)]) # # v = np.linalg.solve(mat, b) # # T_d_ddot = v[0] # alpha_d = v[1] # # print('baseline_pos: Tdot, w_d, alpha_d', Tdot, w_d, alpha_d) w_d = 0 alpha_d = 0 if logentry is not None: logentry['th_d'] = th_d logentry['th_r'] = th_r logentry['T_d'] = T_d logentry['euler'] = euler logentry['att_e'] = att_e # logentry['Fx_r'] = Fx_r # logentry['Fy_r'] = Fy_r # logentry['Fz_r'] = Fz_r # logentry['int_x_error'] = self.int_x_error # logentry['int_y_error'] = self.int_y_error # logentry['int_y_error'] = self.int_z_error return T_d, th_d, w_d, alpha_d, omega, motorForce