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
