def forward_dynamics(self, action):
if len(action) != self.action_dim:
raise ValueError('incorrect action dimension: expected %d but got '
'%d' % (self.action_dim, len(action)))
lb, ub = self.action_bounds
action = np.clip(action, lb, ub)
for ctrl, act in zip(self.extra_data.controls, action):
if ctrl.typ == "force":
for name in ctrl.bodies:
body = find_body(self.world, name)
direction = np.array(ctrl.direction)
direction = direction / np.linalg.norm(direction)
world_force = body.GetWorldVector(direction * act)
world_point = body.GetWorldPoint(ctrl.anchor)
body.ApplyForce(world_force, world_point, wake=True)
elif ctrl.typ == "torque":
assert ctrl.joint
joint = find_joint(self.world, ctrl.joint)
joint.motorEnabled = True
# forces the maximum allowed torque to be taken
if act > 0:
joint.motorSpeed = 1e5
else:
joint.motorSpeed = -1e5
joint.maxMotorTorque = abs(act)
else:
raise NotImplementedError
self.before_world_step(action)
self.world.Step(self.extra_data.timeStep,
self.extra_data.velocityIterations,
self.extra_data.positionIterations)
def forward_dynamics(self, action):
if len(action) != self.action_dim:
raise ValueError('incorrect action dimension: expected %d but got '
'%d' % (self.action_dim, len(action)))
lb, ub = self.action_bounds
action = np.clip(action, lb, ub)
for ctrl, act in zip(self.extra_data.controls, action):
if ctrl.typ == "force":
for name in ctrl.bodies:
body = find_body(self.world, name)
direction = np.array(ctrl.direction)
direction = direction / np.linalg.norm(direction)
world_force = body.GetWorldVector(direction * act)
world_point = body.GetWorldPoint(ctrl.anchor)
body.ApplyForce(world_force, world_point, wake=True)
elif ctrl.typ == "torque":
assert ctrl.joint
joint = find_joint(self.world, ctrl.joint)
joint.motorEnabled = True
# forces the maximum allowed torque to be taken
if act > 0:
joint.motorSpeed = 1e5
else:
joint.motorSpeed = -1e5
joint.maxMotorTorque = abs(act)
else:
raise NotImplementedError
self.before_world_step(action)
self.world.Step(
self.extra_data.timeStep,
self.extra_data.velocityIterations,
self.extra_data.positionIterations
)
def _compute_com_pos_vel(self, *com):
com_key = ",".join(sorted(com))
if com_key in self._cached_coms:
return self._cached_coms[com_key]
total_mass_quant = 0
total_mass = 0
for body_name in com:
body = find_body(self.world, body_name)
total_mass_quant += body.mass * np.array(
list(body.worldCenter) + list(body.linearVelocity))
total_mass += body.mass
com_quant = total_mass_quant / total_mass
self._cached_coms[com_key] = com_quant
return com_quant
def _compute_com_pos_vel(self, *com):
com_key = ",".join(sorted(com))
if com_key in self._cached_coms:
return self._cached_coms[com_key]
total_mass_quant = 0
total_mass = 0
for body_name in com:
body = find_body(self.world, body_name)
total_mass_quant += body.mass * np.array(
list(body.worldCenter) + list(body.linearVelocity))
total_mass += body.mass
com_quant = total_mass_quant / total_mass
self._cached_coms[com_key] = com_quant
return com_quant
def get_raw_obs(self):
"""
Return the unfiltered & noiseless observation. By default, it computes
based on the declarations in the xml file.
"""
if self._cached_obs is not None:
return self._cached_obs
obs = []
for state in self.extra_data.states:
new_obs = None
if state.body:
body = find_body(self.world, state.body)
if state.local is not None:
l = state.local
position = body.GetWorldPoint(l)
linearVel = body.GetLinearVelocityFromLocalPoint(l)
# now I wish I could write angle = error "not supported"
else:
position = body.position
linearVel = body.linearVelocity
if state.to is not None:
to = find_body(self.world, state.to)
if state.typ == "xpos":
new_obs = position[0]
elif state.typ == "ypos":
new_obs = position[1]
elif state.typ == "xvel":
new_obs = linearVel[0]
elif state.typ == "yvel":
new_obs = linearVel[1]
elif state.typ == "apos":
new_obs = body.angle
elif state.typ == "avel":
new_obs = body.angularVelocity
elif state.typ == "dist":
new_obs = np.linalg.norm(position - to.position)
elif state.typ == "angle":
diff = to.position - position
abs_angle = np.arccos(
diff.dot((0, 1)) / np.linalg.norm(diff))
new_obs = body.angle + abs_angle
else:
raise NotImplementedError
elif state.joint:
joint = find_joint(self.world, state.joint)
if state.typ == "apos":
new_obs = joint.angle
elif state.typ == "avel":
new_obs = joint.speed
else:
raise NotImplementedError
elif state.com:
com_quant = self._compute_com_pos_vel(*state.com)
if state.typ == "xpos":
new_obs = com_quant[0]
elif state.typ == "ypos":
new_obs = com_quant[1]
elif state.typ == "xvel":
new_obs = com_quant[2]
elif state.typ == "yvel":
new_obs = com_quant[3]
else:
print(state.typ)
# orientation and angular velocity of the whole body is not
# supported
raise NotImplementedError
else:
raise NotImplementedError
if state.transform is not None:
if state.transform == "id":
pass
elif state.transform == "sin":
new_obs = np.sin(new_obs)
elif state.transform == "cos":
new_obs = np.cos(new_obs)
else:
raise NotImplementedError
obs.append(new_obs)
self._cached_obs = np.array(obs)
return self._cached_obs
def get_raw_obs(self):
"""
Return the unfiltered & noiseless observation. By default, it computes
based on the declarations in the xml file.
"""
if self._cached_obs is not None:
return self._cached_obs
obs = []
for state in self.extra_data.states:
new_obs = None
if state.body:
body = find_body(self.world, state.body)
if state.local is not None:
l = state.local
position = body.GetWorldPoint(l)
linearVel = body.GetLinearVelocityFromLocalPoint(l)
# now I wish I could write angle = error "not supported"
else:
position = body.position
linearVel = body.linearVelocity
if state.to is not None:
to = find_body(self.world, state.to)
if state.typ == "xpos":
new_obs = position[0]
elif state.typ == "ypos":
new_obs = position[1]
elif state.typ == "xvel":
new_obs = linearVel[0]
elif state.typ == "yvel":
new_obs = linearVel[1]
elif state.typ == "apos":
new_obs = body.angle
elif state.typ == "avel":
new_obs = body.angularVelocity
elif state.typ == "dist":
new_obs = np.linalg.norm(position - to.position)
elif state.typ == "angle":
diff = to.position - position
abs_angle = np.arccos(
diff.dot((0, 1)) / np.linalg.norm(diff))
new_obs = body.angle + abs_angle
else:
raise NotImplementedError
elif state.joint:
joint = find_joint(self.world, state.joint)
if state.typ == "apos":
new_obs = joint.angle
elif state.typ == "avel":
new_obs = joint.speed
else:
raise NotImplementedError
elif state.com:
com_quant = self._compute_com_pos_vel(*state.com)
if state.typ == "xpos":
new_obs = com_quant[0]
elif state.typ == "ypos":
new_obs = com_quant[1]
elif state.typ == "xvel":
new_obs = com_quant[2]
elif state.typ == "yvel":
new_obs = com_quant[3]
else:
print(state.typ)
# orientation and angular velocity of the whole body is not
# supported
raise NotImplementedError
else:
raise NotImplementedError
if state.transform is not None:
if state.transform == "id":
pass
elif state.transform == "sin":
new_obs = np.sin(new_obs)
elif state.transform == "cos":
new_obs = np.cos(new_obs)
else:
raise NotImplementedError
obs.append(new_obs)
self._cached_obs = np.array(obs)
return self._cached_obs