def _update(self): """Update the lower unilateral inequality matrix and vector.""" self._A_ineq = np.zeros(4 * len(self.contacts), 6 * len(self.contacts)) for i, contact in enumerate(self.contacts): rot = get_matrix_from_quaternion(self.model.get_orientation(contact)).T rot = block_diag((rot, rot)) self._A_ineq[i*4:(i+1)*4, i*6:(i+1)*6] = self._friction_matrix.dot(rot) self._b_upper_bound = np.zeros(4 * len(self.contacts))
def _update(self): """Update the lower unilateral inequality matrix and vector.""" rotations = [] for contact in self.contacts: link = self.model.get_link_id(contact) rot = get_matrix_from_quaternion( self.model.get_orientation(link)).T # (3,3) rotations.append(block_diag((rot, rot))) # (6,6) self._A_ineq = block_diag(rotations) # (M*6,M*6) self._b_lower_bound = np.concatenate( [self._vector for _ in self.contacts]) # (M*6,)
def set_propeller_velocities(self, velocities, max_velocity=True, forces=True): """ Set the joint velocities and apply the thrust force on the propeller link corresponding to the given joint id(s). Args: velocities (np.array[float[4]]): velocity of each propeller forces (float, np.float[N], None, bool): maximum motor torques / forces. If True, it will apply the default maximum force values. max_velocity (float, bool, None): if True, it will make sure that the given velocity(ies) are below their authorized maximum value(s) (inferred from the URDF, or set previously by the user). If you already did the check outside the method or if you don't want limits, set this variable to False. Returns: None """ if len(velocities) != 4: raise ValueError("Expecting a velocity for each propeller") joint_ids = self.joints # call parent method super(Quadcopter, self).set_joint_velocities(velocities, joint_ids, forces, max_velocity) # if the simulator can not simulate air, calculate thrust force of the given joints, and apply it on the link if not self.simulator.simulate_gas_dynamics(): for jnt, d, v in zip(joint_ids, self.propeller_directions, velocities): if max_velocity and v > self.max_velocity: v = self.max_velocity # compute propeller speed v0 state = self.sim.get_link_state( self.id, jnt, compute_velocity=True ) # , compute_forward_kinematics=True) R = get_matrix_from_quaternion(state[1]) linear_velocity = np.array(state[-2]) propeller_up_vec = R.dot(np.array([0., 0., 1.])) v0 = linear_velocity.dot(propeller_up_vec) # v0 = 0 # static thrust # compute thrust f = self.calculate_thrust_force(v * d, self.area, self.propeller_pitch, v0) # f = self.mass * self.gravity / 4. # apply force in the simulation self.apply_external_force(force=[0, 0, f], link_id=jnt, position=(0., 0., 0.))
def _update_frame(self, line_idx, position, orientation): """Update the lines that compose the frames.""" pos = position rot = get_matrix_from_quaternion(orientation) # (3,3) # set data for x, y, z axes (NOTE: there is no .set_data() for 3 dim data...) for i in range(3): new_pos = pos + rot[:, i]/10. line = self._lines[line_idx+i] line.set_data([pos[0], new_pos[0]], [pos[1], new_pos[1]]) line.set_3d_properties([pos[2], new_pos[2]]) line_idx += 3 return line_idx
def set_propeller_velocities(self, velocities, max_velocity=True, forces=True): """ Set the joint velocities and apply the thrust force on the propeller link corresponding to the given joint id(s). Args: velocities (float[4]): velocity of each propeller forces (float, np.float[N], None, bool): maximum motor torques / forces. If True, it will apply the default maximum force values. max_velocity (float, bool, None): if True, it will make sure that the given velocity(ies) are below their authorized maximum value(s) (inferred from the URDF, or set previously by the user). If you already did the check outside the method or if you don't want limits, set this variable to False. Returns: None """ joint_id = 7 # call parent method super(Techpod, self).set_joint_velocities(velocities, joint_id, forces, max_velocity) # calculate thrust force of the given joints, and apply it on the link if max_velocity and velocities > self.max_velocity: velocities = self.max_velocity # compute propeller speed v0 state = self.sim.get_link_state( self.id, joint_id, compute_velocity=True) # , compute_forward_kinematics=True) R = get_matrix_from_quaternion(state[1]) linear_velocity = np.array(state[-2]) propeller_up_vec = R.dot(np.array([0., 1., 0.])) v0 = linear_velocity.dot(propeller_up_vec) # v0 = 0 # static thrust # compute thrust f = self.calculate_thrust_force(velocities, self.area, self.propeller_pitch) # , v0) # apply force in the simulation # f = 20 self.apply_external_force(force=[0, 0, -f], link_id=joint_id, position=(0., 0., 0.))
def _update(self, x=None): """ Update the task by computing the A matrix and b vector that will be used by the task solver. """ # get jacobians expressed in the world frame jacobian = self.model.get_jacobian(link=self.distal_link) jdotqdot = self.model.compute_JdotQdot(link=self.distal_link) # express jacobians in the distal/contact link frame orientation = self.model.get_orientation( link=self.distal_link) # shape: (4,) rot = get_matrix_from_quaternion(orientation).T # shape: (3,3) rot = block_diag(rot, rot) # shape: (6,6) jacobian = rot.dot(jacobian) jdotqdot = rot.dot(jdotqdot) # set A and b self._A = np.dot(self.contact_matrix, jacobian) self._b = np.dot(self.contact_matrix, jdotqdot)
def _read(self): """Read the orientation state data.""" self.data = get_matrix_from_quaternion( self.robot.get_base_orientation())[:, self._base_axis]
def _get_states(self): """Return the state.""" axis = get_matrix_from_quaternion(self.body.orientation)[self.dim] return np.dot(axis, self.axis)
def rotation_matrix(self): """Return the orientation as a rotation matrix.""" return get_matrix_from_quaternion(self.orientation)
def _update(self): """Update the upper unilateral inequality matrix and vector.""" rot = get_matrix_from_quaternion(self.model.get_orientation(self._link)).T rot = block_diag((rot, rot)) self._A_ineq = self._zmp_matrix.dot(rot) # (4,6)
def get_symbolic_equations_of_motion(self, verbose=False): """ This returns the symbolic equation of motions of the robot (using the URDF). Internally, this used the `sympy.mechanics` module. """ # gravity and time g, t = sympy.symbols('g t') # create the world inertial frame of reference and its origin world_frame = mechanics.ReferenceFrame('Fw') world_origin = mechanics.Point('Pw') world_origin.set_vel(world_frame, mechanics.Vector(0)) # create the base frame (its position, orientation and velocities) + generalized coordinates and speeds base_id = -1 # Check if the robot has a fixed base and create the generalized coordinates and speeds based on that, # as well the base position, orientation and velocities if self.has_fixed_base(): # generalized coordinates q(t) and speeds dq(t) q = mechanics.dynamicsymbols('q:{}'.format(len(self.joints))) dq = mechanics.dynamicsymbols('dq:{}'.format(len(self.joints))) pos, orn = self.get_base_pose() lin_vel, ang_vel = [0, 0, 0], [0, 0, 0] # 0 because fixed base joint_id = 0 else: # generalized coordinates q(t) and speeds dq(t) q = mechanics.dynamicsymbols('q:{}'.format(7 + len(self.joints))) dq = mechanics.dynamicsymbols('dq:{}'.format(6 + len(self.joints))) pos, orn = q[:3], q[3:7] lin_vel, ang_vel = dq[:3], dq[3:6] joint_id = 7 # set the position, orientation and velocities of the base base_frame = world_frame.orientnew('Fb', 'Quaternion', [orn[3], orn[0], orn[1], orn[2]]) base_frame.set_ang_vel( world_frame, ang_vel[0] * world_frame.x + ang_vel[1] * world_frame.y + ang_vel[2] * world_frame.z) base_origin = world_origin.locatenew( 'Pb', pos[0] * world_frame.x + pos[1] * world_frame.y + pos[2] * world_frame.z) base_origin.set_vel( world_frame, lin_vel[0] * world_frame.x + lin_vel[1] * world_frame.y + lin_vel[2] * world_frame.z) # inputs u(t) (applied torques) u = mechanics.dynamicsymbols('u:{}'.format(len(self.joints))) joint_id_u = 0 # kinematics differential equations kd_eqs = [q[i].diff(t) - dq[i] for i in range(len(self.joints))] # define useful lists/dicts for later bodies, loads = [], [] frames = {base_id: (base_frame, base_origin)} # frames = {base_id: (worldFrame, worldOrigin)} # go through each joint/link (each link is associated to a joint) for link_id in range(self.num_links): # get useful information about joint/link kinematics and dynamics from simulator info = self.sim.get_dynamics_info(self.id, link_id) mass, local_inertia_diagonal = info[0], info[2] info = self.sim.get_link_state(self.id, link_id) local_inertial_frame_position, local_inertial_frame_orientation = info[ 2], info[3] # worldLinkFramePosition, worldLinkFrameOrientation = info[4], info[5] info = self.sim.get_joint_info(self.id, link_id) joint_name, joint_type = info[1:3] # jointDamping, jointFriction = info[6:8] link_name, joint_axis_in_local_frame, parent_frame_position, parent_frame_orientation, \ parent_idx = info[-5:] xl, yl, zl = joint_axis_in_local_frame # get previous references parent_frame, parent_point = frames[parent_idx] # create a reference frame with its origin for each joint # set frame orientation if joint_type == self.sim.JOINT_REVOLUTE: R = get_matrix_from_quaternion(parent_frame_orientation) R1 = get_symbolic_matrix_from_axis_angle( joint_axis_in_local_frame, q[joint_id]) R = R1.dot(R) frame = parent_frame.orientnew('F' + str(link_id), 'DCM', sympy.Matrix(R)) else: x, y, z, w = parent_frame_orientation # orientation of the joint in parent CoM inertial frame frame = parent_frame.orientnew('F' + str(link_id), 'Quaternion', [w, x, y, z]) # set frame angular velocity ang_vel = 0 if joint_type == self.sim.JOINT_REVOLUTE: ang_vel = dq[joint_id] * (xl * frame.x + yl * frame.y + zl * frame.z) frame.set_ang_vel(parent_frame, ang_vel) # create origin of the reference frame # set origin position x, y, z = parent_frame_position # position of the joint in parent CoM inertial frame pos = x * parent_frame.x + y * parent_frame.y + z * parent_frame.z if joint_type == self.sim.JOINT_PRISMATIC: pos += q[joint_id] * (xl * frame.x + yl * frame.y + zl * frame.z) origin = parent_point.locatenew('P' + str(link_id), pos) # set origin velocity if joint_type == self.sim.JOINT_PRISMATIC: vel = dq[joint_id] * (xl * frame.x + yl * frame.y + zl * frame.z) origin.set_vel(world_frame, vel.express(world_frame)) else: origin.v2pt_theory(parent_point, world_frame, parent_frame) # define CoM frame and position (and velocities) wrt the local link frame x, y, z, w = local_inertial_frame_orientation com_frame = frame.orientnew('Fc' + str(link_id), 'Quaternion', [w, x, y, z]) com_frame.set_ang_vel(frame, mechanics.Vector(0)) x, y, z = local_inertial_frame_position com = origin.locatenew('C' + str(link_id), x * frame.x + y * frame.y + z * frame.z) com.v2pt_theory(origin, world_frame, frame) # define com particle # com_particle = mechanics.Particle('Pa' + str(linkId), com, mass) # bodies.append(com_particle) # save # frames[linkId] = (frame, origin) # frames[linkId] = (frame, origin, com_frame, com) frames[link_id] = (com_frame, com) # define mass and inertia ixx, iyy, izz = local_inertia_diagonal inertia = mechanics.inertia(com_frame, ixx, iyy, izz, ixy=0, iyz=0, izx=0) inertia = (inertia, com) # define rigid body associated to frame body = mechanics.RigidBody(link_name, com, frame, mass, inertia) bodies.append(body) # define dynamical forces/torques acting on the body # gravity force applied on the CoM force = (com, -mass * g * world_frame.z) loads.append(force) # if prismatic joint, compute force if joint_type == self.sim.JOINT_PRISMATIC: force = (origin, u[joint_id_u] * (xl * frame.x + yl * frame.y + zl * frame.z)) # force = (com, u[jointIdU] * (x * frame.x + y * frame.y + z * frame.z) - mass * g * worldFrame.z) loads.append(force) # if revolute joint, compute torque if joint_type == self.sim.JOINT_REVOLUTE: v = (xl * frame.x + yl * frame.y + zl * frame.z) # torqueOnPrevBody = (parentFrame, - u[jointIdU] * v) torque_on_prev_body = (parent_frame, -u[joint_id_u] * v) torque_on_curr_body = (frame, u[joint_id_u] * v) loads.append(torque_on_prev_body) loads.append(torque_on_curr_body) # if joint is not fixed increment the current joint id if joint_type != self.sim.JOINT_FIXED: joint_id += 1 joint_id_u += 1 if verbose: print("\nLink name with type: {} - {}".format( link_name, self.get_joint_types(joint_ids=link_id))) print("------------------------------------------------------") print("Position of joint frame wrt parent frame: {}".format( origin.pos_from(parent_point))) print("Orientation of joint frame wrt parent frame: {}".format( frame.dcm(parent_frame))) print("Linear velocity of joint frame wrt parent frame: {}". format(origin.vel(world_frame).express(parent_frame))) print("Angular velocity of joint frame wrt parent frame: {}". format(frame.ang_vel_in(parent_frame))) print("------------------------------------------------------") print("Position of joint frame wrt world frame: {}".format( origin.pos_from(world_origin))) print("Orientation of joint frame wrt world frame: {}".format( frame.dcm(world_frame).simplify())) print("Linear velocity of joint frame wrt world frame: {}". format(origin.vel(world_frame))) print("Angular velocity of joint frame wrt parent frame: {}". format(frame.ang_vel_in(world_frame))) print("------------------------------------------------------") # print("Local position of CoM wrt joint frame: {}".format(com.pos_from(origin))) # print("Local linear velocity of CoM wrt joint frame: {}".format(com.vel(worldFrame).express(frame))) # print("Local angular velocity of CoM wrt joint frame: {}".format(com_frame.ang_vel_in(frame))) # print("------------------------------------------------------") if joint_type == self.sim.JOINT_PRISMATIC: print("Input value (force): {}".format(loads[-1])) elif joint_type == self.sim.JOINT_REVOLUTE: print( "Input value (torque on previous and current bodies): {} and {}" .format(loads[-2], loads[-1])) print("") if verbose: print("Summary:") print("Generalized coordinates: {}".format(q)) print("Generalized speeds: {}".format(dq)) print("Inputs: {}".format(u)) print("Kinematic differential equations: {}".format(kd_eqs)) print("Bodies: {}".format(bodies)) print("Loads: {}".format(loads)) print("") # TODO: 1. account for external forces applied on different rigid-bodies (e.g. contact forces) # TODO: 2. account for constraints (e.g. holonomic, non-holonomic, etc.) # Get the Equation of Motion (EoM) using Kane's method kane = mechanics.KanesMethod(world_frame, q_ind=q, u_ind=dq, kd_eqs=kd_eqs) kane.kanes_equations(bodies=bodies, loads=loads) # get mass matrix and force vector (after simplifying) such that :math:`M(x,t) \dot{x} = f(x,t)` M = sympy.trigsimp(kane.mass_matrix_full) f = sympy.trigsimp(kane.forcing_full) # mechanics.find_dynamicsymbols(M) # mechanics.find_dynamicsymbols(f) # save useful info for future use (by other methods) constants = [g] constant_values = [9.81] parameters = (dict(zip(constants, constant_values))) self.symbols = { 'q': q, 'dq': dq, 'kane': kane, 'parameters': parameters } # linearize # parameters = dict(zip(constants, constant_values)) # M_, A_, B_, u_ = kane.linearize() # A_ = A_.subs(parameters) # B_ = B_.subs(parameters) # M_ = kane.mass_matrix_full.subs(parameters) # self.symbols = {'A_': A_, 'B_': B_, 'M_': M_} # return M_, A_, B_, u_ return M, f