def disturb_pose(self, pose, noise):
""" uniform disturb pose
pose
noise float, deviation angle in radius
"""
new_pose = pose.copy()
noise3d = noise / np.sqrt(3)
new_pose[:3] += uniform(-noise3d, noise3d, 3)
for jstar, jdof in zip(self.pos_start, self.joint_dof):
if jdof == 1:
new_pose[jstar] += uniform(-noise, noise)
elif jdof == 4:
noise_quat = Quaternion.fromExpMap(
uniform(-noise3d, noise3d, 3))
ori_quat = Quaternion.fromWXYZ(pose[jstar:jstar + 4])
new_quat = noise_quat.mul(ori_quat)
new_pose[jstar:jstar + 4] = new_quat.wxyz()
elif jdof == 0:
pass
else:
assert (False and "not support jdof other than 1 and 4")
return new_pose
def world_frame(self):
""" position returns a 3x6 matrix
where row is [x, y, z] column is m1 m2 m3 m4 origin h
"""
origin = self.state[0:3]
quat = Quaternion(self.state[6:10])
rot = quat.as_rotation_matrix()
wHb = np.r_[np.c_[rot, origin], np.array([[0, 0, 0, 1]])]
quadBodyFrame = params.body_frame.T
quadWorldFrame = wHb.dot(quadBodyFrame)
world_frame = quadWorldFrame[0:3]
return world_frame
def get_link_states(self):
pose, vel = self.get_pose()
self.skeleton.set_pose(pose)
self.skeleton.set_vel(vel)
states = self.get_link_states_by_ids(self.skeleton.get_link_ids())
pos_array = []
rot_array = []
for s in states:
pos_array.append(s[0:3])
rot_array.append(s[3:7])
# belly
#pos_root = np.array(pos_array[0])
#pos_chest = np.array(pos_array[1])
#pos_belly = 0.5*(pos_root + pos_chest)
#states.append([pos_belly[0], pos_belly[1], pos_belly[2], 0, 0, 0, 1])
root_pos = np.array(self.skeleton.get_joint_global_pos(0))
root_rot = Quaternion.fromXYZW(rot_array[0])
root_pos += root_rot.rotate(np.array([0,0.205,0]))
states.append(root_pos.tolist() + [0, 0, 0, 1])
# joints
states.append(self.skeleton.get_joint_global_pos(3) + [0, 0, 0, 1])
states.append(self.skeleton.get_joint_global_pos(9) + [0, 0, 0, 1])
states.append(self.skeleton.get_joint_global_pos(6) + [0, 0, 0, 1])
states.append(self.skeleton.get_joint_global_pos(12) + [0, 0, 0, 1])
# states.append(self.skeleton.get_joint_global_pos(4) + [0, 0, 0, 1])
# states.append(self.skeleton.get_joint_global_pos(10) + [0, 0, 0, 1])
# states.append(self.skeleton.get_joint_global_pos(7) + [0, 0, 0, 1])
# states.append(self.skeleton.get_joint_global_pos(13) + [0, 0, 0, 1])
# neck
neck_pos = np.array(self.skeleton.get_joint_global_pos(2))
head_rot = Quaternion.fromXYZW(rot_array[2])
neck_pos += head_rot.rotate(np.array([0,0.02,0]))
states.append(neck_pos.tolist() + [0, 0, 0, 1])
# clavicle
chest_rot = Quaternion.fromXYZW(rot_array[1])
j_chest_pos = np.array(self.skeleton.get_joint_global_pos(1))
r_clavicle_pos = chest_rot.rotate(np.array([-0.011, 0.24, 0.095])) + j_chest_pos
r_clavicle_rot = chest_rot.mul(Quaternion.fromEulerZYX([-1.64, -0.21, 0.0338])).xyzw()
states.append(r_clavicle_pos.tolist() + r_clavicle_rot.tolist())
l_clavicle_pos = chest_rot.rotate(np.array([-0.011, 0.24, -0.095])) + j_chest_pos
l_clavicle_rot = chest_rot.mul(Quaternion.fromEulerZYX([1.64, 0.21, 0.0338])).xyzw()
states.append(l_clavicle_pos.tolist() + l_clavicle_rot.tolist())
return states
def disturb_root_pose(self, pose, pos_noise, ori_noise):
""" uniform disturb pose root
"""
new_pose = pose.copy()
noise3d = pos_noise / np.sqrt(3)
new_pose[:3] += uniform(-noise3d, noise3d, 3)
noise3d = ori_noise / np.sqrt(3)
noise_quat = Quaternion.fromExpMap(uniform(-noise3d, noise3d, 3))
ori_quat = Quaternion.fromWXYZ(pose[3:7])
new_quat = noise_quat.mul(ori_quat)
new_pose[3:7] = new_quat.wxyz()
return new_pose
def get_link_states(self):
pos_stick1, rot_stick1 = self.sim_client.getBasePositionAndOrientation(
self.stick1)
pos_stick2, rot_stick2 = self.sim_client.getBasePositionAndOrientation(
self.stick2)
pos_bar, rot_bar = self.sim_client.getBasePositionAndOrientation(
self.object_id)
rot_bar = Quaternion.fromXYZW(rot_bar).mul(
Quaternion.fromXYZW([np.sin(np.pi / 4), 0, 0,
np.cos(np.pi / 4)]))
return [
pos_stick1 + rot_stick1,
pos_stick2 + rot_stick2,
np.array(pos_bar).tolist() + rot_bar.xyzw().tolist(),
]
def change_dir(frames):
# change direction to x+ axis, starting position at origin
ori_pos = frames[0, 1:4].copy()
ori_pos[1] = 0
vel_dir = frames[-1, 1:4] - frames[0, 1:4]
heading_theta = np.arctan2(-vel_dir[2], vel_dir[0])
inv_rot = Quaternion.fromAngleAxis(-heading_theta, np.array([0, 1, 0]))
for i in range(frames.shape[0]):
frame = frames[i]
frames[i, 1:4] = inv_rot.rotate(frame[1:4] - ori_pos)
root_rot = Quaternion.fromWXYZ(frame[4:8])
new_rot = inv_rot.mul(root_rot)
frames[i, 4:8] = new_rot.pos_wxyz()
return frames
def buildHeadingTrans(self, rot):
""" Build the rotation that rotate to local coordinate
rot np.array of float, 4 dim, (w, x, y, z) quat of coordinate orientation
"""
theta = self._kin_core.getHeadingTheta(rot)
inv_rot = Quaternion.fromExpMap(np.array([0, -theta, 0]))
return inv_rot
def imageDataCallback(self, msg):
trans_msg = msg.relative_pose.position # NED translation
rot_msg = msg.relative_pose.orientation # orientation in quaternion
# these come in as body to body, need camera to camera
translation = np.array([trans_msg.x, trans_msg.y, trans_msg.z])
rotation = Quaternion(
np.array([rot_msg.w, rot_msg.x, rot_msg.y, rot_msg.z]))
# convert from body to camera frame
translation = self.R_bc @ translation
features_next = np.array(msg.features_next).reshape(-1, 2).T
features_prev = np.array(msg.features_prev).reshape(-1, 2).T
#dt = msg.dt
if np.linalg.norm(translation) > 0.05:
# points in the next camera frame
p_next = vt.triangulate_points(features_prev, features_next,
translation, rotation,
self.triang_method, self.Kc_inv)
p_inertial = self.current_att.R.T @ self.R_bc.T @ p_next + self.current_pos.reshape(
(3, 1))
self.publish_pointCloud(p_inertial.T)
def __init__(self, profile=False):
self.profile = profile
self.update_frequency = 20.0
self.datum = coordinates.Simulation_Datum
# displacement in x (East), y (North), z (Altitude)
self.displacement = np.array((0.0, 0.0, -5))
self.velocity = np.array((0.0, 0.0, 0.0))
# Rotation applied from East,North,Altitude coordinate system to the vehicle
# euler angles = (pitch, roll, -yaw)
self.orientation = Quaternion.fromEuler(
(math.radians(0), math.radians(0), math.radians(0)))
#print math.degrees(calculatePitch(self.orientation))
#assert(44 < math.degrees(calculatePitch(self.orientation)) < 46)
#print math.degrees(calculateYaw(self.orientation))
#assert(89 < math.degrees(calculateYaw(self.orientation)) < 91)
# In the vehicle-local coordinate system:
self.angular_velocity = np.array(
(0.0, 0.0, 0.0)) # about x, about y, about z
self.motor_states = MotorStates()
self.update_lock = threading.Lock()
self.thread = threading.Thread(target=self.runLoopWrapper)
self.thread.daemon = True
self.keep_going = True
rospy.Subscriber("/control/motors", ctrl_msgs.MotorDemand,
self.onMotorStateMessage)
def action_to_targ_pose(self, action):
""" Converte action to PD controller target pose
Inputs:
action np.array of float, action which DeepMimicSim can take in
Outputs:
pose np.array of float, pose of character
"""
assert (action.size == self.dof - 7)
targ_pose = np.zeros(self.dof)
targ_pose[3] = 1
for i in range(1, self.num_joints):
dof = self.joint_dof[i]
p_off = self.pos_start[i]
a_off = self.act_start[i]
if dof == 1: # revolute
targ_pose[p_off] = action[a_off]
elif dof == 4: # spherical
angle = action[a_off]
axis = action[a_off + 1:a_off + 4]
quata = Quaternion.fromAngleAxis(angle, axis)
targ_pose[p_off:p_off + 4] = quata.wxyz()
assert (np.isfinite(sum(targ_pose))), embed() #(action, targ_pose)
return targ_pose
def post_update(self):
self.t += self._sim_step
if self.checkpoint:
pose, vel = self.character.get_pose()
self.recorder.append('character', pose)
if self.checkpoint and self.t > self.cut_off:
pose, vel = self.character.get_pose()
pose, vel = self._skeleton.toLocalFrame(pose, vel)
with open('data/states/%s.npy' % self.name, 'wb') as f:
np.save(f, pose)
np.save(f, vel)
frames = self.recorder.get('character')
last_frame = frames[-1]
inv_ori_rot = self._skeleton.buildHeadingTrans(
np.array(last_frame[3:7]))
offset = np.array(last_frame[0:3])
offset[1] = 0
for frame in frames:
pos = np.array(frame[0:3])
rot = Quaternion.fromWXYZ(np.array(frame[3:7]))
newpos = inv_ori_rot.rotate(pos - offset)
newrot = inv_ori_rot.mul(rot)
frame[0:3] = newpos.tolist()
frame[3:7] = newrot.wxyz().tolist()
self.recorder.save('data/records/%s.yaml' % self.name)
print(self.t)
print(pose)
print(vel)
print('finish data saving')
input()
def stateCallback(self, msg):
position = msg.pose.pose.position # NED position in i frame
orient = msg.pose.pose.orientation # orientation in quaternion
self.current_pos = np.array([position.x, position.y, position.z])
self.current_att = Quaternion(
np.array([orient.w, orient.x, orient.y, orient.z]))
def targ_pose_to_exp(self, pose):
""" Convert target pose to exponential map, used as initialization of
Actor reference memory
Inputs:
pose np.array of float, pose of character
Outputs:
exp np.array of float, action represented in exponential map
"""
assert (pose.size == self.dof)
expmap = np.zeros(self.expdof)
for i in range(1, self.num_joints):
dof = self.joint_dof[i]
p_off = self.pos_start[i]
e_off = self.exp_start[i]
if dof == 1: # revolute
expmap[e_off] = pose[p_off]
elif dof == 4: # spherical
quata = Quaternion.fromWXYZ(pose[p_off:p_off + 4])
expmap[e_off:e_off + 3] = quata.expmap()
assert (np.isfinite(sum(expmap))), embed() #(action, targ_pose)
return expmap
def exp_to_targ_pose_old(self, expmap):
""" Convert action represented in exponential map to angle-axis action
which deepmimic_sim can take in
Inputs:
expmap np.array of float, action represented in exponential map
Outputs:
pose np.array of float, pose of character
"""
assert (expmap.size == self.expdof)
pose = np.zeros(self.dof)
for i in range(1, self.num_joints):
dof = self.joint_dof[i]
e_off = self.exp_start[i]
p_off = self.pos_start[i]
if dof == 1: # revolute
pose[p_off] = expmap[e_off]
elif dof == 4: # spherical
quata = Quaternion.fromExpMap(expmap[e_off:e_off + 3])
pose[p_off:p_off + 4] = quata.wxyz()
assert (np.isfinite(sum(pose))), embed() #(action, targ_pose)
return pose
def exp_to_action(self, expmap):
""" Convert action represented in exponential map to angle-axis action
which deepmimic_sim can take in
Inputs:
expmap np.array of float, action represented in exponential map
Outputs:
action np.array of float, action for deepmimic environment
"""
assert (expmap.size == self.expdof)
action = np.zeros(self.dof - 7)
for i in range(1, self.num_joints):
dof = self.joint_dof[i]
e_off = self.exp_start[i]
a_off = self.act_start[i]
if dof == 1: # revolute
action[a_off] = expmap[e_off]
elif dof == 4: # spherical
quata = Quaternion.fromExpMap(expmap[e_off:e_off + 3])
action[a_off:a_off + 4] = quata.angaxis()
assert (np.isfinite(sum(action))), embed() #(action, targ_pose)
return action
def computeVel(self, pose0, pose1, dt, padding=True):
""" Compute velocity between two poses
Inputs:
pose0 np.array of float, start pose
pose1 np.array of float, end pose
dt float, duraction between two poses
Outputs:
avg_vel np.array of float, vel (w/ or w/o padding)
"""
assert (pose0.size == self.dof)
assert (pose1.size == self.dof)
avg_vel = np.zeros_like(pose0)
root0 = pose0[:3]
root1 = pose1[:3]
avg_vel[:3] = (root1 - root0) / dt
offset = 3
# root angular velocity is in world coordinate
dof = self.joint_dof[0]
quat0 = Quaternion.fromWXYZ(pose0[offset:offset + dof])
quat1 = Quaternion.fromWXYZ(pose1[offset:offset + dof])
avg_vel[offset:offset + 3] = computeAngVel(quat0, quat1, dt)
offset += dof
# other joints
for i in range(1, self.num_joints):
dof = self.joint_dof[i]
if dof == 1: # revolute
theta0 = pose0[offset]
theta1 = pose1[offset]
avg_vel[offset] = (theta1 - theta0) / dt
elif dof == 4: # spherical
quat0 = Quaternion.fromWXYZ(pose0[offset:offset + dof])
quat1 = Quaternion.fromWXYZ(pose1[offset:offset + dof])
avg_vel[offset:offset + 3] = computeAngVelRel(quat0, quat1, dt)
offset += dof
if padding is False:
avg_vel = avg_vel[self.comp2pad]
return avg_vel
def calc_command(self, att_des: quat.Quaternion, ang_vel_des,
att: quat.Quaternion):
"""Compute the angular velocity command from current attitude
and desired attitude. Returns 1D numpy array of length 3. att_des and
att are quaternions equivalent to rotations from inertial to body
"""
q_des2body = att_des.inverse(
) @ att # return quaternion rotation from desired to body
feedforward = q_des2body.rotp(
ang_vel_des) #rotate ang_vel_des from des frame to body frame
error = att_des.box_minus(
att
) #returns Axis-Angle representation of how much you should rotate about each of the body frame axes
feedback = self.kp * error
ang_vel_cmd = self.saturate(feedforward + feedback)
return ang_vel_cmd
def test_as_v_thta(self):
x = [1.0, 0.0, 0.0]
q = Quaternion.from_v_theta(x, np.pi / 2)
expected_v = np.array(x)
expected_theta = np.pi / 2
actual_v, actual_theta = q.as_v_theta()
print actual_v, actual_theta, expected_v
self.assertTrue(np.array_equal(expected_v, actual_v))
self.assertEqual(expected_theta, actual_theta)
def get_link_states(self):
angle = self.__wallAngle / 180 * np.pi
offset = np.array([np.cos(angle), 0, np.sin(angle)])
l_bar_pos = np.array(
self.__basePos) - offset * (3.0 - self.__vis_offset)
l_bar_pos[1] = 0
r_bar_pos = np.array(
self.__basePos) + offset * (3.0 + self.__vis_offset)
r_bar_pos[1] = 0
t_bar_quat = Quaternion.fromXYZW(self.__baseOri).mul(
Quaternion.fromAngleAxis(np.pi / 2, np.array([0, 0, 1])))
t_bar_pos = np.array(self.__basePos) + offset * self.__vis_offset
t_bar_pos[1] = self.height - 0.02
return [
l_bar_pos.tolist() + self.__baseOri,
r_bar_pos.tolist() + self.__baseOri,
t_bar_pos.tolist() + t_bar_quat.xyzw().tolist()
]
def calc_reward(self):
if self.is_fail:
return 0
count, pose, vel = self._mocap.slerp(self.t)
vel[0] *= self.preset.speed_multiplier; vel[2] *= self.preset.speed_multiplier
self._curr_kin_pose = pose
self._curr_kin_vel = vel
local_sim_pose, local_sim_vel = self._skeleton.toLocalFrame(self._curr_sim_pose, self._curr_sim_vel)
local_kin_pose, local_kin_vel = self._skeleton.toLocalFrame(self._curr_kin_pose, self._curr_kin_vel)
reward = self._skeleton.get_reward(local_sim_pose, local_sim_vel, local_kin_pose, local_kin_vel)
if self.is_episode_end():
pose, vel = self.character.get_pose()
pose, vel = self._skeleton.toLocalFrame(pose, vel)
bonus = 10.0*self._skeleton.get_reward2(local_sim_pose, local_sim_vel, local_kin_pose, local_kin_vel)
bonus *= np.exp(-1.0 * np.mean(np.abs(vel[6:] - local_kin_vel[6:])))
bonus *= np.exp(-1.0 * np.mean(np.abs(vel[3:6] - self.preset.angular_v)))
bonus *= bell(vel[2], self.preset.linear_v_z, 1.0)
if self.checkpoint:
print('v[2]', vel[2])
print(pose)
print(vel)
print('writing to %s ...' % (self.checkpoint + '.npy'))
with open(self.checkpoint + '.npy', 'wb') as f:
np.save(f, pose)
np.save(f, vel)
print('processing recording, please wait ...')
frames = self.motion_recorder.get('character')
last_frame = frames[-1]
inv_ori_rot = self._skeleton.buildHeadingTrans(np.array(last_frame[3:7]))
offset = np.array(last_frame[0:3])
offset[1] = 0
for frame in frames:
pos = np.array(frame[0:3])
rot = Quaternion.fromWXYZ(np.array(frame[3:7]))
newpos = inv_ori_rot.rotate(pos - offset)
newrot = inv_ori_rot.mul(rot)
frame[0:3] = newpos.tolist()
frame[3:7] = newrot.wxyz().tolist()
self.motion_recorder.save(self.checkpoint + '.yaml')
print('writing done')
return bonus + reward
return reward
def calc_command(self, att_des: quat.Quaternion, ang_vel_des,
att: quat.Quaternion):
"""Compute the angular velocity command from current attitude
and desired attitude. Returns 1D numpy array of length 3.
"""
q_des2body = att_des @ att
feedforward = q_des2body.rotp(ang_vel_des)
error = att_des.inverse().box_minus(att)
feedback = self.kp * error
ang_vel_cmd = self.saturate(feedforward + feedback)
return ang_vel_cmd
def state_dot(self, state, t, F, M, Fi_s):
x, y, z, xdot, ydot, zdot, qw, qx, qy, qz, p, q, r = state
quat = np.array([qw,qx,qy,qz])
bRw = Quaternion(quat).as_rotation_matrix() # world to body rotation matrix
wRb = bRw.T # orthogonal matrix inverse = transpose
# acceleration - Newton's second law of motion
#accel = (1.0 / params.mass) * (wRb.dot(np.array([[0, 0, F]]).T) - np.array([[0, 0, params.mass * params.g]]).T)
rotor_drag = True
if(rotor_drag):
v = np.array([[xdot],[ydot],[zdot]])
vw = np.array([[0],[0],[0]]) # wind velocity
omega = np.array([[p],[q],[r]])
Fa = self.total_rotor_drag(v, vw, omega, Fi_s, wRb) # total rotor drag in body frame
else:
Fa = np.array([[0],[0],[0]])
accel = -params.g*params.e3 + (F/params.mass)*np.dot(wRb,params.e3) + np.dot(wRb, Fa)
# angular velocity - using quternion
# http://www.euclideanspace.com/physics/kinematics/angularvelocity/
K_quat = 2.0; # this enforces the magnitude 1 constraint for the quaternion
quaterror = 1.0 - (qw**2 + qx**2 + qy**2 + qz**2)
qdot = (-1.0/2) * np.array([[0, -p, -q, -r],
[p, 0, -r, q],
[q, r, 0, -p],
[r, -q, p, 0]]).dot(quat) + K_quat * quaterror * quat;
# angular acceleration - Euler's equation of motion
# https://en.wikipedia.org/wiki/Euler%27s_equations_(rigid_body_dynamics)
omega = np.array([p,q,r])
pqrdot = params.invI.dot( M.flatten() - np.cross(omega, params.I.dot(omega)) )
state_dot = np.zeros(13)
state_dot[0] = xdot
state_dot[1] = ydot
state_dot[2] = zdot
state_dot[3] = accel[0][0]
state_dot[4] = accel[1][0]
state_dot[5] = accel[2][0]
state_dot[6] = qdot[0]
state_dot[7] = qdot[1]
state_dot[8] = qdot[2]
state_dot[9] = qdot[3]
state_dot[10] = pqrdot[0]
state_dot[11] = pqrdot[1]
state_dot[12] = pqrdot[2]
return state_dot
def generalized_falling_rwd(self):
avg_penalty = self.offset_penalty / self.num_step
rwd = 1.0 - np.clip(
np.power(avg_penalty / self.offset_penalty_coeff, 2), 0, 1)
avg_ang_penalty = self.angv_penalty / self.num_step
rwd *= np.exp(-self.angv_penalty_coeff * avg_ang_penalty)
if rwd > 0:
q = Quaternion.fromWXYZ(self.root_quat)
min_angle = np.pi
for qb in self.q_bases:
q_base = Quaternion.fromWXYZ(np.array(qb))
q_diff = q.mul(q_base.conjugate())
angle = np.abs(q_diff.angle())
min_angle = np.min([min_angle, angle])
rwd *= np.clip(min_angle / (np.pi / 2), 0.01, 1)
#print(min_angle)
#print(self.root_quat)
if self.head_contact:
rwd *= 0.7
return rwd
def normalize_target_pose(self, pose):
"""
normalize given pose to make quaternions norm as 1
"""
assert (pose.size == self.dof)
norm_pose = pose.copy()
for i in range(self.num_joints):
dof = self.joint_dof[i]
p_off = self.pos_start[i]
if dof == 4: # spherical
quata = Quaternion.fromWXYZ(pose[p_off:p_off + 4])
norm_pose[p_off:p_off + 4] = quata.pos_wxyz()
assert (np.isfinite(sum(norm_pose))), embed() #(action, targ_pose)
return norm_pose
def update_imu(self, gyroscope, accelerometer):
"""
Perform one update step with data from a IMU sensor array
:param gyroscope: A three-element array containing the gyroscope data in radians per second.
:param accelerometer: A three-element array containing the accelerometer data. Can be any unit since a normalized value is used.
"""
q = self.quaternion
gyroscope = np.array(gyroscope, dtype=float).flatten()
accelerometer = np.array(accelerometer, dtype=float).flatten()
# Normalise accelerometer measurement
if norm(accelerometer) is 0:
warnings.warn("accelerometer is zero")
return
accelerometer /= norm(accelerometer)
# Gradient descent algorithm corrective step
f = np.array([
2 * (q[1] * q[3] - q[0] * q[2]) - accelerometer[0],
2 * (q[0] * q[1] + q[2] * q[3]) - accelerometer[1],
2 * (0.5 - q[1]**2 - q[2]**2) - accelerometer[2]
])
j = np.array([[-2 * q[2], 2 * q[3], -2 * q[0], 2 * q[1]],
[2 * q[1], 2 * q[0], 2 * q[3], 2 * q[2]],
[0, -4 * q[1], -4 * q[2], 0]])
step = j.T.dot(f)
step /= norm(step) # normalise step magnitude
# Compute rate of change of quaternion
qdot = (q * Quaternion(0, gyroscope[0], gyroscope[1],
gyroscope[2])) * 0.5 - self.beta * step.T
# Integrate to yield quaternion
q += qdot * self.samplePeriod
self.quaternion = Quaternion(q / norm(q)) # normalise quaternion
def __init__(self, info):
"""
Inputs:
info json item load from character file
"""
self._pos = np.array(
[info["AttachX"], info["AttachY"], info["AttachZ"]])
self._name = info["Name"]
self._parent_id = info["Parent"]
self._parent = None
self._type_name = info['Type']
self._type = NAME2TYPE[info['Type']]
self._dof = Dof[self._type]
self._expdof = ExpDof[self._type]
self._w = info["DiffWeight"]
self._is_end_effector = info["IsEndEffector"]
if self._type is JointType.REVOLUTE:
self._torque_lim = [info["TorqueLim"]]
elif self._type is JointType.SPHERE:
if "TorqueLimX" not in info:
self._torque_lim = [info["TorqueLim"]] * 4
else:
self._torque_lim = [
info["TorqueLimX"], info["TorqueLimY"], info["TorqueLimZ"],
0
]
else:
self._torque_lim = [0] * 4
if self._type is JointType.REVOLUTE:
self.limlow = np.array([info["LimLow0"]])
self.limhigh = np.array([info["LimHigh0"]])
elif self._type is JointType.SPHERE:
self.limlow = -3.14 * np.ones(3)
self.limhigh = 3.14 * np.ones(3)
else:
self.limlow = np.array([])
self.limhigh = np.array([])
self.pose = np.zeros(self._dof)
self.pose2quat = POSE2QUAT[self._type]
self.quat = Quaternion(1, 0, 0, 0)
self.vel = np.zeros(self._dof)
self.vel2omg = VEL2OMG[self._type]
self.omg = np.zeros(3)
def state_dot(self, state, t, F, M):
x, y, z, xdot, ydot, zdot, qw, qx, qy, qz, p, q, r = state
quat = np.array([qw, qx, qy, qz])
bRw = Quaternion(
quat).as_rotation_matrix() # world to body rotation matrix
self.R = bRw
wRb = bRw.T # orthogonal matrix inverse = transpose
# acceleration - Newton's second law of motion
accel = 1.0 / params.mass * (wRb.dot(np.array(
[[0, 0, F]]).T) - np.array([[0, 0, params.mass * params.g]]).T)
# angular velocity - using quternion
# http://www.euclideanspace.com/physics/kinematics/angularvelocity/
K_quat = 2.0
# this enforces the magnitude 1 constraint for the quaternion
quaterror = 1.0 - (qw**2 + qx**2 + qy**2 + qz**2)
qdot = (-1.0 / 2) * np.array([[0, -p, -q, -r], [p, 0, -r, q],
[q, r, 0, -p], [r, -q, p, 0]
]).dot(quat) + K_quat * quaterror * quat
# angular acceleration - Euler's equation of motion
# https://en.wikipedia.org/wiki/Euler%27s_equations_(rigid_body_dynamics)
omega = np.array([p, q, r])
pqrdot = params.invI.dot(M.flatten() -
np.cross(omega, params.I.dot(omega)))
state_dot = np.zeros(13)
state_dot[0] = xdot
state_dot[1] = ydot
state_dot[2] = zdot
state_dot[3] = accel[0]
state_dot[4] = accel[1]
state_dot[5] = accel[2]
state_dot[6] = qdot[0]
state_dot[7] = qdot[1]
state_dot[8] = qdot[2]
state_dot[9] = qdot[3]
state_dot[10] = pqrdot[0]
state_dot[11] = pqrdot[1]
state_dot[12] = pqrdot[2]
return state_dot
def toLocalFrame(self, pose, vel, ori_pos=None, ori_rot=None):
""" Convert pose and vel from world frame to local frame,
the local frame heading direction is rotated x-axis
Inputs:
pose np.array of float, character pose
vel np.array of float, character vel w/ padding
ori_pos np.array of float, 3 dim, position of local coordinate origin
ori_rot np.array of float, 4 dim, (w, x, y, z) quat of local coordinate orientation
Outputs:
local_pose
local_vel
"""
if ori_pos is None:
ori_pos = pose[:3]
if ori_rot is None:
ori_rot = pose[3:7]
# heading theta
inv_ori_rot = self.buildHeadingTrans(ori_rot)
local_pos = pose.copy()
local_vel = vel.copy()
# ignore y difference, because local cooridnate shares xoz plane with world
local_pos[0] -= ori_pos[0] # root x pos
local_pos[2] -= ori_pos[2] # root y pos
ori_rot = Quaternion.fromWXYZ(ori_rot)
ori_rot = inv_ori_rot.mul(ori_rot)
local_pos[3:7] = ori_rot.pos_wxyz() # root orientation
local_vel[:3] = inv_ori_rot.rotate(vel[:3]) # root velocity
local_vel[3:6] = inv_ori_rot.rotate(vel[3:6]) # root angular velocity
return local_pos, local_vel
def targ_pose_to_action(self, pose):
""" Convert desired pose to action
Inputs:
pose np.array of float, pose of character
Outputs:
action np.array of float, action which DeepMimicSim can take in
"""
assert (pose.size == self.dof)
action = np.zeros(self.dof - 7)
for i in range(1, self.num_joints):
dof = self.joint_dof[i]
p_off = self.pos_start[i]
a_off = self.act_start[i]
if dof == 1: # revolute
action[a_off] = pose[p_off]
elif dof == 4: # spherical
quata = Quaternion.fromWXYZ(pose[p_off:p_off + 4])
action[a_off:a_off + 4] = quata.angaxis()
assert (np.isfinite(sum(action))), embed() #(action, targ_pose)
return action
class MadgwickAHRS:
samplePeriod = 0.01
quaternion = Quaternion(1, 0, 0, 0)
beta = 1
def __init__(self, sampleperiod=None, quaternion=None, beta=None):
"""
Initialize the class with the given parameters.
:param sampleperiod: The sample period
:param quaternion: Initial quaternion
:param beta: Algorithm gain beta
:return:
"""
if sampleperiod is not None:
self.samplePeriod = sampleperiod
if quaternion is not None:
self.quaternion = quaternion
if beta is not None:
self.beta = beta
def update(self, gyroscope, accelerometer, magnetometer):
"""
Perform one update step with data from a AHRS sensor array
:param gyroscope: A three-element array containing the gyroscope data in radians per second.
:param accelerometer: A three-element array containing the accelerometer data. Can be any unit since a normalized value is used.
:param magnetometer: A three-element array containing the magnetometer data. Can be any unit since a normalized value is used.
:return:
"""
q = self.quaternion
gyroscope = np.array(gyroscope, dtype=float).flatten()
accelerometer = np.array(accelerometer, dtype=float).flatten()
magnetometer = np.array(magnetometer, dtype=float).flatten()
# Normalise accelerometer measurement
if norm(accelerometer) is 0:
warnings.warn("accelerometer is zero")
return
accelerometer /= norm(accelerometer)
# Normalise magnetometer measurement
if norm(magnetometer) is 0:
warnings.warn("magnetometer is zero")
return
magnetometer /= norm(magnetometer)
h = q * (Quaternion(0, magnetometer[0], magnetometer[1],
magnetometer[2]) * q.conj())
b = np.array([0, norm(h[1:3]), 0, h[3]])
# Gradient descent algorithm corrective step
f = np.array([
2 * (q[1] * q[3] - q[0] * q[2]) - accelerometer[0],
2 * (q[0] * q[1] + q[2] * q[3]) - accelerometer[1],
2 * (0.5 - q[1]**2 - q[2]**2) - accelerometer[2],
2 * b[1] * (0.5 - q[2]**2 - q[3]**2) + 2 * b[3] *
(q[1] * q[3] - q[0] * q[2]) - magnetometer[0],
2 * b[1] * (q[1] * q[2] - q[0] * q[3]) + 2 * b[3] *
(q[0] * q[1] + q[2] * q[3]) - magnetometer[1],
2 * b[1] * (q[0] * q[2] + q[1] * q[3]) + 2 * b[3] *
(0.5 - q[1]**2 - q[2]**2) - magnetometer[2]
])
j = np.array([[-2 * q[2], 2 * q[3], -2 * q[0], 2 * q[1]],
[2 * q[1], 2 * q[0], 2 * q[3], 2 * q[2]],
[0, -4 * q[1], -4 * q[2], 0],
[
-2 * b[3] * q[2], 2 * b[3] * q[3],
-4 * b[1] * q[2] - 2 * b[3] * q[0],
-4 * b[1] * q[3] + 2 * b[3] * q[1]
],
[
-2 * b[1] * q[3] + 2 * b[3] * q[1],
2 * b[1] * q[2] + 2 * b[3] * q[0],
2 * b[1] * q[1] + 2 * b[3] * q[3],
-2 * b[1] * q[0] + 2 * b[3] * q[2]
],
[
2 * b[1] * q[2], 2 * b[1] * q[3] - 4 * b[3] * q[1],
2 * b[1] * q[0] - 4 * b[3] * q[2], 2 * b[1] * q[1]
]])
step = j.T.dot(f)
step /= norm(step) # normalise step magnitude
# Compute rate of change of quaternion
qdot = (q * Quaternion(0, gyroscope[0], gyroscope[1],
gyroscope[2])) * 0.5 - self.beta * step.T
# Integrate to yield quaternion
q += qdot * self.samplePeriod
self.quaternion = Quaternion(q / norm(q)) # normalise quaternion
def update_imu(self, gyroscope, accelerometer):
"""
Perform one update step with data from a IMU sensor array
:param gyroscope: A three-element array containing the gyroscope data in radians per second.
:param accelerometer: A three-element array containing the accelerometer data. Can be any unit since a normalized value is used.
"""
q = self.quaternion
gyroscope = np.array(gyroscope, dtype=float).flatten()
accelerometer = np.array(accelerometer, dtype=float).flatten()
# Normalise accelerometer measurement
if norm(accelerometer) is 0:
warnings.warn("accelerometer is zero")
return
accelerometer /= norm(accelerometer)
# Gradient descent algorithm corrective step
f = np.array([
2 * (q[1] * q[3] - q[0] * q[2]) - accelerometer[0],
2 * (q[0] * q[1] + q[2] * q[3]) - accelerometer[1],
2 * (0.5 - q[1]**2 - q[2]**2) - accelerometer[2]
])
j = np.array([[-2 * q[2], 2 * q[3], -2 * q[0], 2 * q[1]],
[2 * q[1], 2 * q[0], 2 * q[3], 2 * q[2]],
[0, -4 * q[1], -4 * q[2], 0]])
step = j.T.dot(f)
step /= norm(step) # normalise step magnitude
# Compute rate of change of quaternion
qdot = (q * Quaternion(0, gyroscope[0], gyroscope[1],
gyroscope[2])) * 0.5 - self.beta * step.T
# Integrate to yield quaternion
q += qdot * self.samplePeriod
self.quaternion = Quaternion(q / norm(q)) # normalise quaternion