Esempio n. 1
0
 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)
Esempio n. 2
0
 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
     )
Esempio n. 3
0
 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
Esempio n. 4
0
 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
Esempio n. 5
0
    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
Esempio n. 6
0
    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