def __init__(self, body, des, weight):
"""
:type body: pydart.BodyNode
:param des:
:param weight:
"""
super().__init__()
self.body = body
self.des = mm.seq2Vec3(mm.logSO3(des))
self.weight = weight
self.cur = mm.seq2Vec3(mm.logSO3(self.body.world_transform()[:3, :3]))
def processs_body_geom(self):
skel = self.skeleton
for i in range(skel.getJointNum()):
joint = skel.getJoint(i) # type: ym.Joint
self.body_name.append(joint.name)
# print(joint.children)
avg_offset = mm.seq2Vec3(
sum([child.offset
for child in joint.children]) / len(joint.children))
length = mm.length(avg_offset)
length *= 0.9
mass = self.mass_map[joint.name]
width = math.sqrt(mass / 1000. / length * 0.9)
height = width
geom_type = 'box' if self.shape_map[
joint.name] is None else self.shape_map[joint.name][0]
geom_size = [width, height, length] if self.shape_map[
joint.name] is None else self.shape_map[joint.name][1]
offset_T = mm.getSE3ByTransV(.5 * avg_offset)
defaultBoneV = mm.unitZ()
boneR = mm.SO3ToSE3(mm.getSO3FromVectors(defaultBoneV, avg_offset))
boneT = np.dot(offset_T, boneR)
# boneT = offset_T
self.joint_to_body_transf.append(boneT)
self.body_transf.append(np.dot(self.joint_transf[i], boneT))
self.body_mass.append(mass)
self.body_geom_type.append(geom_type)
self.body_geom_size.append(geom_size)
def update_goal_in_local_frame(self, reset=False):
if not reset:
self.prev_goal = self.goal.copy()
body_transform = self.skel.body(0).world_transform()
goal_vector_in_world_frame = self.goal_in_world_frame - body_transform[:
3,
3]
goal_vector_in_world_frame[1] = 0.
radius = mm.length(goal_vector_in_world_frame)
unit_goal_vector_in_world_frame = mm.normalize(
goal_vector_in_world_frame)
root_x_in_world_plane = body_transform[:3, 0]
root_x_in_world_plane[1] = 0.
unit_root_x_in_world_plane = mm.seq2Vec3(
mm.normalize(root_x_in_world_plane))
unit_root_z_in_world_plane = mm.cross(unit_root_x_in_world_plane,
mm.unitY())
# angle = atan2(np.dot(unit_root_x_in_world_plane, unit_goal_vector_in_world_frame), np.dot(unit_root_z_in_world_plane, unit_goal_vector_in_world_frame))
self.goal = radius * np.array([
np.dot(unit_root_x_in_world_plane,
unit_goal_vector_in_world_frame),
np.dot(unit_root_z_in_world_plane, unit_goal_vector_in_world_frame)
])
if reset:
self.prev_goal = self.goal.copy()
def step(self, target):
target = self.config.x_normal.normalize_l(target)
m = self.model
feed_dict = {m.x: [[target]], m.prev_y: self.current_y}
if self.state is not None:
feed_dict[m.initial_state] = self.state
# x : target x, target y => 2
# # y : foot contact=2, root transform(rotation, tx, ty)=3, root_height, joint pos=3*13=39 => 45
# y : foot contact=2, root transform(rotation, tx, ty)=3, root_height, joint pos=3*19=57 => 63
output, self.state, self.current_y = self.sess.run(
[m.generated, m.final_state, m.final_y], feed_dict)
output = output[0][0]
output = self.config.y_normal.de_normalize_l(output)
angles = copy(output[63:])
output = output[:63]
contacts = copy(output[:2])
output = output[2:]
# move root
self.pose = self.pose.transform(output)
root_orientation = mm.getSO3FromVectors(
(self.pose.d[0], 0., self.pose.d[1]), -mm.unitZ())
points = [[0, output[3], 0]]
output = output[4:]
for i in range(len(output) // 3):
points.append(output[i * 3:(i + 1) * 3])
point_offset = mm.seq2Vec3([0., -0.05, 0.])
for i in range(len(points)):
points[i] = mm.seq2Vec3(self.pose.global_point_3d(
points[i])) / 100. + point_offset
orientations = list()
for i in range(len(angles) // 3):
orientations.append(mm.exp(angles[i * 3:(i + 1) * 3]))
return contacts, points, angles, orientations, root_orientation
def update(self):
self.cur = mm.seq2Vec3(mm.logSO3(self.body.world_transform()[:3, :3]))