diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp
index 0b3df749e..91dd9328b 100644
--- a/examples/SharedMemory/PhysicsClientC_API.cpp
+++ b/examples/SharedMemory/PhysicsClientC_API.cpp
@@ -1056,6 +1056,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3JointControlCommandInit2Internal(b3S
command->m_sendDesiredStateCommandArgument.m_Kp[i] = 0;
command->m_sendDesiredStateCommandArgument.m_Kd[i] = 0;
command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 0;
+ command->m_sendDesiredStateCommandArgument.m_damping[i] = 0;
}
command->m_sendDesiredStateCommandArgument.m_desiredStateQ[3] = 1;
return (b3SharedMemoryCommandHandle)command;
@@ -1103,6 +1104,22 @@ B3_SHARED_API int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle,
return 0;
}
+B3_SHARED_API int b3JointControlSetKpMultiDof(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double* kps, int dofCount)
+{
+ struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
+ b3Assert(command);
+ if ((dofIndex >= 0) && (dofIndex < MAX_DEGREE_OF_FREEDOM ) && dofCount >= 0 && dofCount <= 4)
+ {
+ for (int dof = 0; dof < dofCount; dof++)
+ {
+ command->m_sendDesiredStateCommandArgument.m_Kp[dofIndex + dof] = kps[dof];
+ command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KP;
+ command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex + dof] |= SIM_DESIRED_STATE_HAS_KP;
+ }
+ }
+ return 0;
+}
+
B3_SHARED_API int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
@@ -1116,6 +1133,22 @@ B3_SHARED_API int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle,
return 0;
}
+B3_SHARED_API int b3JointControlSetKdMultiDof(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double* kds, int dofCount)
+{
+ struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
+ b3Assert(command);
+ if ((dofIndex >= 0) && (dofIndex < MAX_DEGREE_OF_FREEDOM ) && dofCount >= 0 && dofCount <= 4)
+ {
+ for (int dof = 0; dof < dofCount; dof++)
+ {
+ command->m_sendDesiredStateCommandArgument.m_Kd[dofIndex + dof] = kds[dof];
+ command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KD;
+ command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex + dof] |= SIM_DESIRED_STATE_HAS_KD;
+ }
+ }
+ return 0;
+}
+
B3_SHARED_API int b3JointControlSetMaximumVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double maximumVelocity)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
@@ -1174,6 +1207,35 @@ B3_SHARED_API int b3JointControlSetDesiredForceTorqueMultiDof(b3SharedMemoryComm
return 0;
}
+B3_SHARED_API int b3JointControlSetDamping(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value)
+{
+ struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
+ b3Assert(command);
+ if ((dofIndex >= 0) && (dofIndex < MAX_DEGREE_OF_FREEDOM))
+ {
+ command->m_sendDesiredStateCommandArgument.m_damping[dofIndex] = value;
+ command->m_updateFlags |= SIM_DESIRED_STATE_HAS_DAMPING;
+ command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_DAMPING;
+ }
+ return 0;
+}
+
+B3_SHARED_API int b3JointControlSetDampingMultiDof(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double* damping, int dofCount)
+{
+ struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
+ b3Assert(command);
+ if ((dofIndex >= 0) && (dofIndex < MAX_DEGREE_OF_FREEDOM ) && dofCount >= 0 && dofCount <= 4)
+ {
+ for (int dof = 0; dof < dofCount; dof++)
+ {
+ command->m_sendDesiredStateCommandArgument.m_damping[dofIndex+dof] = damping[dof];
+ command->m_updateFlags |= SIM_DESIRED_STATE_HAS_DAMPING;
+ command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex + dof] |= SIM_DESIRED_STATE_HAS_DAMPING;
+ }
+ }
+ return 0;
+}
+
B3_SHARED_API int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h
index 196c8d8dd..779509069 100644
--- a/examples/SharedMemory/PhysicsClientC_API.h
+++ b/examples/SharedMemory/PhysicsClientC_API.h
@@ -484,7 +484,9 @@ extern "C"
B3_SHARED_API int b3JointControlSetDesiredPositionMultiDof(b3SharedMemoryCommandHandle commandHandle, int qIndex, const double* position, int dofCount);
B3_SHARED_API int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
+ B3_SHARED_API int b3JointControlSetKpMultiDof(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double* kps, int dofCount);
B3_SHARED_API int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
+ B3_SHARED_API int b3JointControlSetKdMultiDof(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double* kds, int dofCount);
B3_SHARED_API int b3JointControlSetMaximumVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double maximumVelocity);
///Only use when controlMode is CONTROL_MODE_VELOCITY
@@ -494,6 +496,8 @@ extern "C"
B3_SHARED_API int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
B3_SHARED_API int b3JointControlSetDesiredForceTorqueMultiDof(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double* forces, int dofCount);
+ B3_SHARED_API int b3JointControlSetDamping(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
+ B3_SHARED_API int b3JointControlSetDampingMultiDof(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double* damping, int dofCount);
///Only use if when controlMode is CONTROL_MODE_TORQUE,
B3_SHARED_API int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index e3b4ced37..50e88f083 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -7153,8 +7153,8 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct
motor->setRhsClamp(clientCmd.m_sendDesiredStateCommandArgument.m_rhsClamp[velIndex]);
}
bool hasDesiredPosOrVel = false;
- btScalar kp = 0.f;
- btScalar kd = 0.f;
+ btVector3 kp(0, 0, 0);
+ btVector3 kd(0, 0, 0);
btVector3 desiredVelocity(0, 0, 0);
if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_QDOT) != 0)
{
@@ -7163,7 +7163,7 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex + 0],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex + 1],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex + 2]);
- kd = 0.1;
+ kd.setValue(0.1, 0.1, 0.1);
}
btQuaternion desiredPosition(0, 0, 0, 1);
if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[posIndex] & SIM_DESIRED_STATE_HAS_Q) != 0)
@@ -7174,38 +7174,122 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex + 1],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex + 2],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex + 3]);
- kp = 0.1;
+ kp.setValue(0.1, 0.1, 0.1);
}
if (hasDesiredPosOrVel)
{
+ bool useMultiDof = true;
+
if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_KP) != 0)
{
- kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex];
+ kp.setValue(
+ clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex + 0],
+ clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex + 0],
+ clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex + 0]);
+ }
+ if (((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex+0] & SIM_DESIRED_STATE_HAS_KP) != 0) &&
+ ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex+1] & SIM_DESIRED_STATE_HAS_KP) != 0) &&
+ ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex+2] & SIM_DESIRED_STATE_HAS_KP) != 0)
+ )
+ {
+ kp.setValue(
+ clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex + 0],
+ clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex + 1],
+ clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex + 2]);
+ } else
+ {
+ useMultiDof = false;
}
if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_KD) != 0)
{
- kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex];
+ kd.setValue(
+ clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex + 0],
+ clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex + 0],
+ clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex + 0]);
}
- motor->setVelocityTarget(desiredVelocity, kd);
- //todo: instead of clamping, combine the motor and limit
- //and combine handling of limit force and motor force.
+ if (((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex+0] & SIM_DESIRED_STATE_HAS_KD) != 0) &&
+ ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex+1] & SIM_DESIRED_STATE_HAS_KD) != 0) &&
+ ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex+2] & SIM_DESIRED_STATE_HAS_KD) != 0))
+ {
+ kd.setValue(
+ clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex + 0],
+ clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex + 1],
+ clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex + 2]);
+ } else
+ {
+ useMultiDof = false;
+ }
- //clamp position
- //if (mb->getLink(link).m_jointLowerLimit <= mb->getLink(link).m_jointUpperLimit)
- //{
- // btClamp(desiredPosition, mb->getLink(link).m_jointLowerLimit, mb->getLink(link).m_jointUpperLimit);
- //}
- motor->setPositionTarget(desiredPosition, kp);
+ btVector3 maxImp(
+ 1000000.f * m_data->m_physicsDeltaTime,
+ 1000000.f * m_data->m_physicsDeltaTime,
+ 1000000.f * m_data->m_physicsDeltaTime);
- btScalar maxImp = 1000000.f * m_data->m_physicsDeltaTime;
+ if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
+ {
+ maxImp.setValue(
+ clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex + 0] * m_data->m_physicsDeltaTime,
+ clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex + 0] * m_data->m_physicsDeltaTime,
+ clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex + 0] * m_data->m_physicsDeltaTime);
+ }
- if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE) != 0)
- maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex] * m_data->m_physicsDeltaTime;
+ if (((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex+0] & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) &&
+ ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex+1] & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) &&
+ ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex+2] & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0))
+ {
+ maxImp.setValue(
+ clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex + 0] * m_data->m_physicsDeltaTime,
+ clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex + 1] * m_data->m_physicsDeltaTime,
+ clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex + 2] * m_data->m_physicsDeltaTime);
+ } else
+ {
+ useMultiDof = false;
+ }
+
+ if (useMultiDof)
+ {
+ motor->setVelocityTargetMultiDof(desiredVelocity, kd);
+ motor->setPositionTargetMultiDof(desiredPosition, kp);
+ motor->setMaxAppliedImpulseMultiDof(maxImp);
+ } else
+ {
+ motor->setVelocityTarget(desiredVelocity, kd[0]);
+ //todo: instead of clamping, combine the motor and limit
+ //and combine handling of limit force and motor force.
- motor->setMaxAppliedImpulse(maxImp);
+ //clamp position
+ //if (mb->getLink(link).m_jointLowerLimit <= mb->getLink(link).m_jointUpperLimit)
+ //{
+ // btClamp(desiredPosition, mb->getLink(link).m_jointLowerLimit, mb->getLink(link).m_jointUpperLimit);
+ //}
+ motor->setPositionTarget(desiredPosition, kp[0]);
+ motor->setMaxAppliedImpulse(maxImp[0]);
+ }
+
+ btVector3 damping(1.f, 1.f, 1.f);
+ if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_DAMPING) != 0) {
+ if (
+ (clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex+0] & SIM_DESIRED_STATE_HAS_DAMPING)&&
+ (clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex+1] & SIM_DESIRED_STATE_HAS_DAMPING)&&
+ (clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex+2] & SIM_DESIRED_STATE_HAS_DAMPING)
+ )
+ {
+ damping.setValue(
+ clientCmd.m_sendDesiredStateCommandArgument.m_damping[velIndex + 0],
+ clientCmd.m_sendDesiredStateCommandArgument.m_damping[velIndex + 1],
+ clientCmd.m_sendDesiredStateCommandArgument.m_damping[velIndex + 2]);
+ } else
+ {
+ damping.setValue(
+ clientCmd.m_sendDesiredStateCommandArgument.m_damping[velIndex + 0],
+ clientCmd.m_sendDesiredStateCommandArgument.m_damping[velIndex + 0],
+ clientCmd.m_sendDesiredStateCommandArgument.m_damping[velIndex + 0]);
+ }
+ }
+ motor->setDamping(damping);
}
numMotors++;
}
diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h
index 311379d79..655d13c05 100644
--- a/examples/SharedMemory/SharedMemoryCommands.h
+++ b/examples/SharedMemory/SharedMemoryCommands.h
@@ -465,6 +465,8 @@ struct SendDesiredStateArgs
//or the maximum applied force/torque for the PD/motor/constraint to reach the desired velocity in CONTROL_MODE_VELOCITY and CONTROL_MODE_POSITION_VELOCITY_PD mode
//indexed by degree of freedom, 6 dof base, and then dofs for each link
double m_desiredStateForceTorque[MAX_DEGREE_OF_FREEDOM];
+
+ double m_damping[MAX_DEGREE_OF_FREEDOM];
};
enum EnumSimDesiredStateUpdateFlags
@@ -475,6 +477,7 @@ enum EnumSimDesiredStateUpdateFlags
SIM_DESIRED_STATE_HAS_KP = 8,
SIM_DESIRED_STATE_HAS_MAX_FORCE = 16,
SIM_DESIRED_STATE_HAS_RHS_CLAMP = 32,
+ SIM_DESIRED_STATE_HAS_DAMPING = 64,
};
enum EnumSimParamUpdateFlags
diff --git a/examples/pybullet/examples/__init__.py b/examples/pybullet/examples/__init__.py
new file mode 100644
index 000000000..8b1378917
--- /dev/null
+++ b/examples/pybullet/examples/__init__.py
@@ -0,0 +1 @@
+
diff --git a/examples/pybullet/examples/biped2d_pybullet.py b/examples/pybullet/examples/biped2d_pybullet.py
index acf7ebeed..7ddf7b976 100644
--- a/examples/pybullet/examples/biped2d_pybullet.py
+++ b/examples/pybullet/examples/biped2d_pybullet.py
@@ -7,7 +7,6 @@ dt = 1e-3
iters = 2000
import pybullet_data
-p.setAdditionalSearchPath(pybullet_data.getDataPath())
physicsClient = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.resetSimulation()
diff --git a/examples/pybullet/gym/pybullet_data/TwoJointRobot_w_fixedJoints.urdf b/examples/pybullet/gym/pybullet_data/TwoJointRobot_w_fixedJoints.urdf
new file mode 100644
index 000000000..4053835eb
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/TwoJointRobot_w_fixedJoints.urdf
@@ -0,0 +1,182 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/examples/pybullet/gym/pybullet_data/TwoJointRobot_wo_fixedJoints.urdf b/examples/pybullet/gym/pybullet_data/TwoJointRobot_wo_fixedJoints.urdf
new file mode 100644
index 000000000..bd0dec215
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/TwoJointRobot_wo_fixedJoints.urdf
@@ -0,0 +1,110 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/examples/pybullet/gym/pybullet_data/ball.vtk b/examples/pybullet/gym/pybullet_data/ball.vtk
new file mode 100644
index 000000000..a5007a034
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/ball.vtk
@@ -0,0 +1,5681 @@
+# vtk DataFile Version 2.0
+ball_, Created by Gmsh
+ASCII
+DATASET UNSTRUCTURED_GRID
+POINTS 600 double
+-0.983023465 -0.0964612812 0.156077221
+-0.983023465 0.0964612812 -0.156077221
+-0.934172392 0.303531051 0.187592313
+-0.934172332 -0.303530991 -0.187592283
+-0.851981342 -0.0839028731 -0.516805649
+-0.851981342 0.0839028731 0.516805649
+-0.851980925 -0.499768227 0.156077191
+-0.851980925 0.499768227 -0.156077191
+-0.850650847 -0.276393682 0.447213203
+-0.850650847 0.276393682 -0.447213203
+-0.738584518 -0.432902843 -0.516805708
+-0.738584518 0.432902843 0.516805708
+-0.73858422 -0.655845523 -0.156077221
+-0.73858422 0.655845523 0.156077221
+-0.639950216 -0.568661451 0.51680553
+-0.639950216 -0.207932755 0.739748478
+-0.639950216 0.207932755 -0.739748478
+-0.639950216 0.568661451 -0.51680553
+-0.577350318 -0.187592313 -0.794654369
+-0.577350318 0.187592313 0.794654369
+-0.57735014 -0.794654608 0.187592268
+-0.57735014 0.794654608 -0.187592268
+-0.525731206 -0.723606944 -0.447213262
+-0.525731206 0.723606944 0.447213262
+-0.395510972 -0.544374168 -0.739748478
+-0.395510972 0.544374168 0.739748478
+-0.395510912 -0.905102789 -0.156077221
+-0.395510912 0.905102789 0.156077221
+-0.356822073 -0.491123736 0.79465431
+-0.356822073 0.491123736 -0.79465431
+-0.343073279 -0.784354985 0.51680547
+-0.343073279 0.784354985 -0.51680547
+-0.343073219 -0.111471429 0.932670832
+-0.343073219 0.111471429 -0.932670832
+-0.2104855876170211 -0.964206637039642 0.1581990558083625
+-0.212030917 -0.291835517 -0.932670832
+-0.212030917 0.291835517 0.932670832
+-0.212030917 0.964718938 -0.156077191
+-0.183480024 -0.836209893 -0.516805649
+-0.183480024 0.836209893 0.516805649
+0 -0.982246935 -0.187592283
+0 -0.894427359 0.447213143
+0 -0.67288357 0.739748418
+0.003206644697647349 -0.6069040358667548 -0.793866965776601
+0 -0.360728621 0.932670832
+0 0 -1
+0 0 1
+0 0.360728621 -0.932670832
+0 0.607062101 0.794654369
+0 0.67288357 -0.739748418
+0 0.894427359 -0.447213143
+0 0.982246935 0.187592283
+0.183480024 -0.836209893 -0.516805649
+0.183480024 0.836209893 0.516805649
+0.212030917 -0.964718938 0.156077191
+0.212030917 -0.291835517 -0.932670832
+0.212030917 0.291835517 0.932670832
+0.212030917 0.964718938 -0.156077191
+0.343073219 -0.111471429 0.932670832
+0.343073219 0.111471429 -0.932670832
+0.343073279 -0.784354985 0.51680547
+0.343073279 0.784354985 -0.51680547
+0.356822073 -0.491123736 0.79465431
+0.356822073 0.491123736 -0.79465431
+0.395510912 -0.905102789 -0.156077221
+0.395510912 0.905102789 0.156077221
+0.395510972 -0.544374168 -0.739748478
+0.395510972 0.544374168 0.739748478
+0.525731206 -0.723606944 -0.447213262
+0.525731206 0.723606944 0.447213262
+0.57735014 -0.794654608 0.187592268
+0.57735014 0.794654608 -0.187592268
+0.577350318 -0.187592313 -0.794654369
+0.577350318 0.187592313 0.794654369
+0.639950216 -0.568661451 0.51680553
+0.639950216 -0.207932755 0.739748478
+0.639950216 0.207932755 -0.739748478
+0.6403071758249252 0.5681663036852607 -0.5166876297130185
+0.73858422 -0.655845523 -0.156077221
+0.73858422 0.655845523 0.156077221
+0.738584518 -0.432902843 -0.516805708
+0.738584518 0.432902843 0.516805708
+0.850650847 -0.276393682 0.447213203
+0.850650847 0.276393682 -0.447213203
+0.851980925 -0.499768227 0.156077191
+0.851980925 0.499768227 -0.156077191
+0.851981342 -0.0839028731 -0.516805649
+0.851981342 0.0839028731 0.516805649
+0.934172332 -0.303530991 -0.187592283
+0.934172332 0.303530991 0.187592283
+0.983023465 -0.0964612812 0.156077221
+0.983023465 0.0964612812 -0.156077221
+0.5711142457352039 -0.02117318796408544 0.5475993502453641
+-0.1834416395507999 0.7396783497444386 0.3181062812107694
+0.2106439939202896 -0.2207277428413223 0.4082221439441702
+-0.0539340503386703 0.1205623207765116 0.4836333150598941
+0.2628914088751474 0.7773567891547469 0.09515773339548624
+0.03625912260493044 0.1154490571841422 0.01428072891263531
+0.04533024469518119 0.01305404249175379 0.830501261727119
+0.1985279596222446 0.1026376486066588 0.4134011017099528
+0.2142576018926516 0.2515904542794398 0.6479455601266221
+0.03591460286896146 0.4648677758371071 -0.6570336955526989
+0.5671390851245302 0.5015167065943134 -0.1302515747102941
+-0.4336614400972409 0.6553851050556312 0.01835453816623948
+-0.7093788513188323 0.02333083593137917 0.4009020437778424
+0.3304153126852112 0.3344430393663131 0.4341447817148135
+0.2835700190185853 -0.1109561501157584 -0.03835100901018595
+0.3384894052038657 0.5443598947507846 -0.4660342040261232
+-0.5534428405667881 0.233243177347242 0.4775208239879267
+0.3754426255119463 0.1029385152098835 -0.6464035637317401
+0.4981870246588242 0.6568525523351662 0.03962368739938198
+-0.3224051846001322 0.4144807316899718 0.1313140478558114
+-0.6478232497017437 -0.3106068971239328 0.01838391805257134
+-0.6576204209938942 -0.2352120512243483 0.431309676011668
+-0.05520070934685202 0.3843844699857725 0.5403667230515509
+-0.7319644942494914 -0.1507752387397017 0.1790669502670221
+-0.6189952662241086 0.01441484356622188 -0.4292031591776315
+-0.6262186005325446 -0.1021573130552927 -0.08428654763266503
+-0.6149963655872288 0.1445403524573829 -0.1438974409017034
+-0.7148900485634544 0.2730783119567216 -0.3026754607577274
+-0.7649453896417203 0.2504548103851916 0.09555797780578199
+-0.3715645582539543 0.1330924452813407 0.3239746822760231
+-0.6615658498360117 0.4804724661556226 -0.1782035107984122
+-0.6505392321170821 0.4100942048988089 0.3356868961443531
+0.336318519474418 0.64955921672115 0.141676610967413
+-0.2373043190629461 0.3628706261497389 -0.7212426055441383
+-0.4720367219822728 -0.2703656731060821 -0.4799947601815256
+-0.4838010172770574 -0.4084105827317038 0.02536967576873089
+-0.4820695438460503 -0.3369545734327805 0.2368150044206616
+-0.05526386895500314 -0.6265972416112141 0.5202839866714712
+-0.4578607499769821 -0.1491368316814568 -0.2444009752709367
+-0.4725026890114479 -0.2375671541793563 -0.06095126386351808
+-0.5195546414777057 -0.1755190582509266 0.1209235603722047
+-0.5124729844160486 -0.1633519413558985 0.3519691378163123
+-0.4528539662618702 -0.01654279695323081 -0.5802020170398763
+-0.4008637964178123 0.03833967835098375 -0.2780189568137055
+-0.4045298128199117 0.002479879083573726 -0.006952714925801346
+-0.5016139410945001 0.005830520154651217 0.2427433809520513
+-0.08387994752248742 0.6831638754551344 -0.5090009524768978
+0.09979297173020235 0.6214399756317296 -0.5394454270132362
+-0.5114964260553124 0.2451501439357156 -0.3032535448403038
+-0.5084862441056747 0.2942826463714892 -0.0130907342464313
+-0.5126741910570937 0.2936280836507524 0.1874216510559829
+-0.4823199432566514 0.4941810202203625 -0.3367534291859899
+-0.3772934083386824 -0.5828600711068156 0.05591038990918762
+0.31959038478937 0.6140451281085459 0.4487754251285493
+-0.479509208666884 0.08909312439915772 0.6399726514442271
+-0.5987352168435675 0.484251969256761 0.6021395588401608
+0.5668924718157011 -0.4886889568034963 -0.6283779956613038
+0.639950216 -0.388297103 0.628277004
+0.7459657790000001 -0.06201494095 0.6282770635
+0.7592375082896597 0.03835589427941914 -0.6142001436623598
+-0.289495498 -0.6902920305 -0.6282770635
+-0.289495498 0.6902920305 0.6282770635
+0.2890391134520873 0.6930274692624179 0.6252725821525187
+-0.1954342843662734 -0.7343660690537938 0.6146913268860001
+-0.9175452116087988 0.2925245629145609 -0.1630626342597246
+0.917502195 0.2981147541 -0.156077206
+0 -0.291835517 -0.932670832
+-0.005512644538268026 0.2950789991623015 0.9312507298938767
+0.004756329446631069 0.9636913804870406 -0.1603331621849719
+0.1796931993833944 0.2270906263641855 -0.9332463212503043
+0.2839543240347723 0.8721681798631183 0.3355386774168226
+-0.289495468 -0.870656341 -0.336441435
+-0.2824861689252822 0.8686566398371635 0.3475001356528549
+-0.9088421251404275 -0.001564412629376344 -0.3594565782347531
+-0.9203312242053348 -0.003164966157839524 0.3264860119713744
+-0.09774266951051733 0.5555167174787097 0.05788528891157628
+-0.7959907918282177 -0.08211319339975402 -0.2053910711192425
+0.03999856928576947 -0.7162839095211453 0.3099013009429171
+-0.202249908345953 -0.5058539092504294 0.1161823685420006
+-0.3148245980164619 -0.486917647454533 0.3203107592281709
+-0.2982953732069865 -0.5180797069493416 0.5498719423954977
+-0.3594062555305886 -0.3476011575009336 -0.6363012624107233
+-0.4028820318916886 -0.3603563062649521 -0.3178933311462525
+-0.3832569634790784 -0.3515629954201686 -0.1608643060189855
+-0.4216489709933123 -0.3683521042712592 0.5299835001681149
+-0.3195340547884118 -0.140458944668357 -0.6767753179600076
+-0.2872190492198003 -0.173107228026045 -0.1073851967440021
+-0.3380772476234242 -0.2306023291143007 0.1015223234543294
+-0.2354645404631943 -0.01847107181770858 -0.3779481679535365
+-0.2339023084221015 -0.01296643920890605 -0.1988671494631802
+-0.4987959292068802 0.5466275984665788 0.3553392545012639
+-0.2993944553011925 0.1160085292182497 -0.5451195709408356
+-0.3180663905624121 0.1816410244317394 -0.3429709835741863
+-0.2984496076861362 0.1731311801956928 -0.1197266002465337
+0.4687659597810016 -0.2609182020629837 0.6456947938158271
+-0.4055380501868786 0.3360296110081572 -0.1690284861777329
+-0.3933493850310362 0.3799269361927073 0.4612899972994972
+-0.6111316600192891 0.5629118916990601 0.2058136799792555
+-0.1835444972768308 -0.2503244747191174 0.6894847407095083
+-0.3286465144478981 0.5109703139291987 -0.4955986563887662
+-0.004524729044751769 0.5061287162879585 -0.4068364833826668
+-0.2086130206014155 0.6253776096440031 0.1785871149235653
+0.07445582966821258 0.7328882912340795 0.1563893347189518
+-0.2415260834301355 0.6855613573510219 -0.1635365387199929
+-0.1928500407563954 -0.7485795663723692 -0.08958998919100135
+-0.2440533359827937 -0.761435917282856 0.1477089760778446
+-0.1323263255053054 -0.4702260860235108 -0.4985941389931162
+-0.22737749859913 0.4085551242737154 -0.1336398432333173
+-0.2222732982258658 -0.2445988685511035 -0.4900209146071922
+-0.1866052001617161 -0.3328028942391639 -0.3051094150744377
+-0.2137502620263282 -0.3303845535191539 -0.03786939554681432
+-0.1395435470476703 -0.1430048503387833 -0.7860101332846771
+-0.1661517739966091 -0.2049543828386332 -0.6361077629093909
+-0.1017630055389408 -0.1670464911194232 -0.3764157164282813
+-0.1109262933335437 -0.1818459499504256 -0.175842010364428
+-0.2161513648114311 -0.1313699938541844 0.1374374024081151
+-0.7811637926694741 0.03047292067867113 -0.02077792430430652
+-0.1418259415916119 -0.02690292354447475 -0.5424026029815623
+-0.1662287378662191 0.1595605701931951 -0.4695541301881394
+-0.1728767079413297 0.1908071514581589 0.0615239831543104
+-0.1248750586000535 0.3373311231905142 -0.5554664407645996
+-0.14774927995634 0.3063479780977638 -0.2971590655764376
+0.317116356473119 0.2636782785377943 -0.517452462149712
+-0.2394625555723072 0.2531091923781731 0.6835180344331175
+-0.1679730860372385 0.6368995083228546 -0.3448914412206089
+-0.1837756206218147 0.8016555747753288 0.001376073021788904
+0.3451586896859973 0.4434648907403473 0.5896671438737594
+0.003912050757982196 -0.6827793661450405 -0.2655034195677201
+0.05117827517194013 -0.4435365454492594 -0.6820128265394876
+-0.193753047436954 0.4669909734707379 0.3562881914225061
+-0.09133017258798415 -0.3710555026643473 0.2211949880926304
+-0.03705517642456098 -0.3967684449657486 0.5575378871330299
+0.004882513238944297 -0.09563858312228475 0.4646605164348893
+-0.06034179915165584 -0.2072440902334313 0.8115716909884921
+-0.01502779929022804 -0.1349339192206479 -0.5198216229754721
+-0.06170726596266114 -0.03300790193827988 -0.3521591664732798
+-0.04024251802528552 0.005316680619917384 -0.1720644173805807
+-0.05170363143209005 0.1443521662120487 -0.3084341261872872
+-0.01322903563459284 0.2892392261133912 -0.1423042447860729
+-0.006197885268519417 0.7257559254287651 -0.117604568649597
+0.00766263881206371 0.6713927509871588 0.3773690346367958
+-0.2765102001377094 -0.1450045556428195 0.3700566231624965
+0.2934780115174567 -0.2895974752147772 -0.6869405692235921
+-0.0646204301881949 0.7678706524307314 -0.322313732124793
+0.1983578642321666 -0.6692184654722374 -0.3806422137534514
+0.1896536071268709 -0.7783794014092489 -0.1543701154216221
+0.04814811832475203 -0.306957222624095 -0.3920233494555808
+0.130872993428573 -0.2614789390432524 -0.224930457969755
+0.05389397376507032 -0.2123135609868312 -0.0456044035096165
+0.1511164622406309 -0.1940120025982684 -0.5486948695672719
+0.08753891237885909 -0.1517419059418617 0.2085053703494517
+0.1573315106118656 -0.2063105176207875 0.7159593163722718
+0.1550574248229858 0.04460639168602056 -0.1632815974459763
+-0.09138953766989316 -0.4558122872772558 -0.2134181647417048
+0.2200403617095288 0.07018008050263533 0.1855545431154388
+0.1745720918276096 0.2964164757364657 -0.6869932450671804
+0.1552374192338686 0.2846877060298754 -0.2720285722263412
+0.5793485990688858 -0.4281363848214862 0.159228937020492
+0.1842995827528208 0.380858358573093 0.2335724355465266
+0.4560445866155982 0.1596898570065349 0.5987060162239723
+0.1897142363072325 0.6801377880530463 -0.1902732043843415
+0.1901803121038665 0.8420330977104334 -0.01077757151453724
+0.3735835590265989 -0.533816093193585 -0.3508145004491201
+-0.2333532313517916 0.1770127404954969 0.4648197664473095
+0.4181266969831226 -0.6618380482497811 0.1445372622865072
+0.3540133290923644 -0.6599343730878077 0.3758436212261005
+0.3333915829314644 -0.515513798024383 -0.5694349617782053
+0.2972094522201416 -0.5540869081361335 -0.1452966914102986
+0.2485136095544038 -0.6516874642731798 0.03570321982604948
+0.4160373989187208 -0.4753541308402521 0.3172453979902219
+0.2504742106226169 -0.3318321328184352 -0.4555861371925892
+0.3482752365168776 -0.3103663015204521 0.5103815503896095
+0.363152181432955 -0.09509648738396842 -0.3986768381978469
+-0.06840482657418301 0.05308915763389212 -0.8176348399056567
+0.7133486872755079 -0.1421207717732033 -0.6548609400718425
+0.7136162202406232 0.139291053558023 0.6556126973517143
+-0.7106270713040959 -0.1410901777337968 -0.6583856211814568
+-0.7240001684667472 0.1373968881268417 0.6443478150910714
+0.657967418 -0.310247578 -0.6557300385
+0.657967418 0.310247578 0.6557300385
+0.3779418925513292 -0.01593005867563534 -0.167519838453102
+-0.6392793337542173 0.2794427660700018 0.688823162263437
+-0.5065672941465678 0.5379795210271793 -0.6419662140897575
+0.4983861445 0.5298925935 -0.65572992
+0.2932328291895358 -0.06287549989074649 0.5654605252776161
+0.349947676 0.6377393605 -0.6557298899999999
+0.3106772736351038 0.1522421239459671 -0.2847040964404556
+0.3619016076579907 0.1233667417260152 -0.0445398112121507
+0.3684048064067006 0.2305894720953906 0.2839597151092496
+-0.3487589772923571 0.638136180858714 -0.6557480128856027
+-0.4871324826293567 0.3638301270017998 0.7677279552997338
+-0.9585979285 0.1035348849 0.171834767
+0.608650267 0.01017022099999999 -0.7672014235
+0.608650267 -0.01017022099999999 0.7672014235
+-0.6024376177446233 -0.003711788266030136 0.7714069934328189
+-0.6042447734847533 0.006687585073746893 -0.7701420626251527
+0.3849066454986819 0.3232513751465138 -0.3587201583300489
+0.349947646 -0.3012975825 0.863662571
+0.3483476961081615 0.3176425081266358 -0.8582847161038692
+-0.349947646 -0.3012975825 0.863662571
+-0.349947646 0.3012975825 -0.863662571
+-0.3946906175 -0.239713915 -0.8636626005
+-0.09671645922550712 -0.455556329131489 -0.8609885293111011
+-0.1060154585 0.449448809 0.8636626005
+0.1060154585 -0.449448809 -0.8636626005
+0.2912864408393545 0.4360741369068572 0.03666708387193633
+0.1060154585 0.449448809 0.8636626005
+0.9585978985000001 -0.1035348549 -0.171834752
+0.1784110365 -0.4259261785 0.863662571
+0.1784110365 0.4259261785 -0.863662571
+-0.1784110365 -0.4259261785 0.863662571
+-0.1784110365 0.4259261785 -0.863662571
+0.4485607805444057 0.0274412363049529 0.8699508680135298
+0.4602117685 -0.03806044200000001 -0.8636626005
+0.197755486 -0.5757181345 -0.7672014235
+0.1875170285645086 0.572125542956834 0.7717238126591888
+-0.197755486 -0.5757181345 -0.7672014235
+-0.197755486 0.5757181345 0.7672014235
+-0.4983861445 -0.3495282455 0.767201394
+0.500733827029852 -0.3429494372899287 0.76820165259275
+0.4984777918937785 0.3440639152388975 -0.7690320795564272
+-0.4983861445 0.3495282455 -0.767201394
+-0.893076867 0.19371696205 0.352198981
+0.893076837 0.19371693205 0.352198966
+-0.893076837 -0.19371693205 -0.352198966
+0.197755456 -0.9436748619999999 -0.171834752
+0.197755456 0.9436748619999999 0.171834752
+-0.1821617266157461 -0.9467164087685083 -0.1730772900272592
+-0.197755456 0.9436748619999999 0.171834752
+0.1784110365 -0.582003653 0.7672013639999999
+0.2040914871099653 0.5689224150578829 -0.7711529358179858
+-0.1784110365 -0.582003653 0.7672013639999999
+-0.1706458392315522 0.5873889545945985 -0.7646290951570205
+-0.7146655325 -0.6472114175 0.1718347295
+-0.3946905285 -0.879686773 0.1718347295
+-0.3826992263243234 0.885549985338254 -0.1672278852813028
+0.4151318255287663 -0.8705468821272369 0.1688179381681522
+0.3816176964913821 0.8846414226634638 -0.1740180681064438
+-0.091740012 -0.909228414 -0.352198966
+-0.091740012 0.909228414 0.352198966
+0.091740012 -0.909228414 -0.352198966
+0.1089846745012238 0.8993400783200778 0.3711736160872028
+0.6605850764739363 0.7218724255144597 -0.02082643400756409
+0.6051372878400938 -0.6836471993845313 0.3532258537403473
+0.4583870346619611 -0.4209981570445642 -0.1711038550931559
+0.408350076952553 -0.5098359751664326 0.01900175426181287
+0.608650178 0.6816580295 -0.352198899
+-0.608650178 -0.6816580295 0.352198899
+0.45648501097621 -0.3834349647647116 -0.5239930356657895
+-0.1042226204554116 0.9733574426023197 0.01329700120380936
+0.1060154585 -0.9734829365 -0.015757546
+-0.4932432547533117 -0.8454431013474896 0.02391297713720811
+0.541956875235316 -0.3767605104076648 0.4856263302575095
+-0.486430526 0.8498786985 -0.01575752350000001
+0.486430526 0.8498786985 -0.01575752350000001
+0.48061420171132 -0.2281208781151917 -0.2153453641230161
+0.4284248677084959 -0.1138731139101116 0.2768188947161023
+0.4891608441455692 0.04285741068612015 -0.2392536176845577
+0.5028450662469833 0.1744824366837791 -0.3552556460455714
+0.4918009675321204 0.1992321909831743 0.03932374300364145
+0.4173683137353702 0.1629270449975743 0.1505109990808487
+0.5887669952330052 0.2351015203098686 -0.5092344915592217
+0.4725694558703502 0.3409692333818932 -0.2300329801651423
+0.5014632353682689 0.330522856036409 0.5277466676480008
+0.5887046726367641 0.4133518205028979 -0.3564640791626207
+0.5031963258450896 0.4082414396977185 0.1108730627916911
+0.2436600679212088 0.1449343977100464 -0.8027510085861623
+0.2661692629564029 -0.4161860961116939 0.6778322933520385
+0.5389336922228024 0.1297001439188301 -0.6793540746793117
+0.6190312172756546 -0.4853355389423176 -0.03823594414065377
+0.6213611290860127 -0.3654585732207788 -0.347839611365611
+0.670070540631135 -0.3304850044928141 0.3075176279985078
+-0.6312346450457121 -0.4270283138376362 -0.1407213056076125
+0.6730006361713489 -0.2300717412324852 -0.04015617222070676
+0.6324119311658877 -0.1685265550881164 0.47278684240569
+0.6718515317701345 -0.05261279391738263 -0.3476589343402736
+0.6546114519167675 0.01675368673195545 -0.08995678553058238
+0.5666475744692999 0.01576403308009006 0.1090301993966335
+0.7263519144247657 0.2057950395874256 -0.2835024210938008
+0.6292624093999444 0.1466102513984335 0.3296682045884995
+0.6539040382194233 0.4373423551522446 0.2973447396401895
+-0.7643904657784308 -0.3270157704006694 0.1507235781129556
+-0.3983099956439372 0.6820166389351584 0.1935158435411442
+0.1454606949276961 0.3560515065805233 0.4574558148114424
+-0.01767048188895652 0.3492544118967844 0.06334264290988761
+0.7646550963318656 -0.06248067496257166 0.1786710643635689
+0.7930174579354851 0.1693865451645832 0.003272865232918417
+0.01099307050411781 0.02194760905610892 0.6477401853065817
+-0.1771759356058517 0.03015440844192887 0.2473680276008317
+-0.5356149458936418 -0.00801003630796797 0.4864425417428122
+0.1872786676121908 0.2172773256198141 0.03050623571169319
+-0.3987986696634189 -0.4946354289051259 -0.4439894573386592
+-0.5888927087817538 0.4747743064772978 0.04104831404680311
+-0.4694703441665588 0.5798300713339117 -0.1603835011744431
+-0.8294116489724226 0.4943369061879145 0.1673151208721621
+-0.8285092954478688 -0.4943369061879149 -0.167315120872162
+0.08601788431546994 -0.4744118786476427 -0.4369039278327513
+0.4268144290850234 -0.7056612795120653 -0.09119225644388079
+-0.235911498660543 0.2496733507810339 0.2926911520456563
+0.04659252556510694 -0.5220086048051136 -0.03190513921165003
+-0.7464064032657043 -0.5811252752524934 -0.255439270284655
+-0.7257250063463581 -0.5604577921412572 -0.3346302417443242
+-0.746406405436403 0.5811252774217315 0.2554392724872807
+-0.7257250063463581 0.560457792141257 0.3346302417443243
+-0.005212278065876854 0.1920171302444726 0.2672071089672988
+0.01121553754032911 0.4522189791379762 0.2930005482030935
+0.2839230991990829 0.4622075315499078 -0.3082127476279448
+-0.4946697047593434 -0.3557726340191656 -0.7662271305074406
+-0.7138890554715793 0.6476333650493602 -0.1727963078701259
+0.5625564905082746 0.01892208994219046 -0.5555981537279906
+-0.09297651214053022 -0.0615660465312691 0.01339996495471615
+-0.8038254879010522 -0.01152003924077081 0.1729691168589109
+-0.2579234662149842 -0.5521401609060735 -0.2660064486766345
+0.7006944775450153 0.3370390934389887 -0.1608815100660374
+0.7914465127578063 -0.2741122939862151 -0.5140873673583262
+0.7888851699703932 0.2780931631132194 0.5168056818287339
+-0.7952829299999999 0.25840285805 0.5168056785
+0.4915117175 -0.159702092 0.836209655
+0.4915117175 0.159702092 -0.836209655
+-0.320482407164663 -0.4333159822567259 -0.8217963374496685
+0.3037709445 0.4181048425 0.836209655
+-0.01265687939189167 0.8375532314178935 0.5137773422349819
+-0.4952171247940293 0.6753533752031967 -0.5140018615193953
+-0.9824696345558724 -0.01113430460937868 0.01032097863238671
+0.983023465 0 0
+0.2984464834261905 -0.9359684011346607 -0.0001635690412845059
+-0.3037709145 -0.9349108635 -1.499999999210466e-08
+-0.3037709145 0.9349108635 1.499999999210466e-08
+-0.7952825725 -0.5778068750000001 -1.499999999210466e-08
+-0.273800906277004 -0.8767913403461751 0.3346302417443243
+-0.4754992136879836 0.6456701396731745 0.5683147406859165
+0.4713001299770969 -0.6411723922906573 -0.5765449053642733
+0.7424826560371693 -0.237850575465973 0.5980659213001208
+0.00551055714916493 -0.7645400515392027 0.6175047879190813
+0.632157862 -0.5782548935 -0.482009485
+0.632157862 0.5782548935 0.482009485
+-0.632157862 -0.5782548935 -0.482009485
+-0.273800906277004 0.8767913403461751 -0.3346302417443242
+-0.6443169789210386 0.5642532597966963 0.4817704764478887
+-0.8529292054160453 0.09169119062812325 -0.478939318374926
+0.852032273325431 -0.08033786692739198 0.4834552999970668
+0.8513160945 0.09624540444999999 -0.482009426
+-0.8513160945 -0.09624540444999999 0.482009426
+-0.3515619224043152 0.7829670991881906 0.4779156773553317
+0.354605615 0.7799084185 0.4820094555
+-0.1648899015864478 -0.2546360153139223 0.9278156620360043
+0 -0.1803643105 0.9663354159999999
+-0.916837156 -0.1864274816 0.301645212
+0.916837156 0.1864274816 -0.301645212
+-0.1715366095 0.0557357145 -0.9663354159999999
+0.1715366095 -0.0557357145 0.9663354159999999
+-0.1648899015864478 0.2546360153139221 -0.9278156620360043
+0.7453005315000001 -0.4225275665 0.4820093665
+-0.7453005315000001 0.4225275665 -0.4820093665
+0.7453005315000001 0.4225275665 -0.4820093665
+-0.1060154585 -0.1459177585 -0.9663354159999999
+-0.1173073419684945 0.157376424596409 0.9630408543723834
+0.109959547888504 -0.1576493649126376 -0.9636288252903217
+0.118687913334654 0.1555536665238542 0.9628678827087409
+0.1715366395 0.839391172 -0.4820093065
+-0.1715366395 0.839391172 -0.4820093065
+-0.460621059 -0.8143548665 -0.3016452415
+0.460621059 0.8143548665 0.3016452415
+0.632157713 0.6897262335000001 0.3016452415
+-0.632157713 0.6897262335000001 0.3016452415
+-0.1060154585 -0.9295731485000001 0.301645167
+-0.1060154585 0.9295731485000001 -0.301645167
+0.8513158860000001 0.3880809545 -0.301645197
+-0.8513158860000001 -0.3880809545 0.301645197
+-0.470377170221815 0.3308692980874088 -0.5346461516515045
+-0.1378971466884762 -0.05872417041574372 0.8245819610452255
+-0.6636097440568927 -0.4820470659935153 0.1441924221159884
+-0.09924075389872836 -0.2639529564994283 0.9372760024704234
+0.3428052246626204 0.3101314782608706 -0.7114897127700051
+-0.09924075389872844 0.2639529564994281 -0.9372760024704236
+0.2497212303585584 0.4759065639172987 -0.634785469991164
+-0.5018582142373347 0.7913357212282524 -0.2936758927309688
+-0.1836056077381358 -0.6567713801098206 -0.4817652775425123
+-0.2470453184003564 0.6526313165075527 -0.5021591169140639
+0.1776571780252803 0.5609861185411928 0.3505695105209232
+-0.06475380565392276 0.6107809085531266 0.5638166510217938
+0.2735253005670124 -0.8762404997642916 0.3346302417443243
+0.2708187524123817 0.8746241813427392 -0.3400836120249706
+0.6171031044747036 0.2521419060127235 0.1499066624088571
+0.4764939378534095 0.4593821606547799 0.3433921458186807
+0.7257250063463581 -0.5604577921412572 -0.3346302417443242
+0.7257250063463581 0.560457792141257 0.3346302417443243
+0.7001310961221709 -0.6616508216673254 0.1759751935031104
+-0.4840866932637082 -0.6121700368985139 -0.1501069607555149
+0.489348674378097 -0.3619183235336728 -0.7671171736051364
+0.1440253661317507 0.3895705948950923 -0.5116461636673226
+0.4946697047593433 0.3557726340191655 0.7662271305074408
+0.7464064032657043 -0.5811252752524936 -0.2554392702846551
+0.8285092954478688 -0.4943369061879149 -0.167315120872162
+0.7464064032657043 0.5811252752524934 0.2554392702846552
+0.829411599676636 0.4943369061879145 0.1673151208721621
+-0.3526508796440742 0.6262207041656306 -0.3288568676190132
+-0.0310745620977699 0.4969617889406759 -0.1761210085106677
+0.9074384118554124 -0.2394964387055004 -0.2895170798756234
+0.9174609659449906 -5.551115123125783e-17 0.3346302417443243
+-0.9411115744202567 -0.1064871788113919 -0.2265040935376754
+0.2076585869786285 -0.6593725678856036 0.2099765396705036
+-0.9248774506368126 -0.2767315414789368 0.1481069821184871
+-0.9608377633517635 -0.1905018186427999 -5.551115123125783e-17
+-0.9598400361758898 0.1938991361849407 0.001978364046988118
+-0.9009246315621086 0.3824345339992005 0.04567887866379172
+-0.8941131900717569 -0.3991747520142159 -0.02009175298266276
+-0.8468161346461875 -0.3613128783487702 -0.3346302417443242
+-0.764674133349585 0.03626511617036607 -0.6086059259490402
+-0.7488125245903672 -0.05809673420800637 0.6252838165775659
+-0.7470301189491562 -0.533868944883471 0.3346302417443243
+-0.7470301189491562 0.533868944883471 -0.3346302417443242
+-0.8001267099090277 0.5711394866640385 -0.01333475398259278
+-0.7371909513941939 -0.5034384621332837 -0.4053005277811758
+-0.6706594551458225 -0.32955793745861 -0.6338583417611625
+-0.8309969453659541 0.3731133554126599 0.3590941679249
+-0.7404349180221135 0.5072093490563344 0.391480111932217
+-0.657984249270295 -0.7252353702830132 0.01572114045120625
+0.1716397495004564 -0.520493295642469 0.4114520704611611
+-0.5633540339641707 -0.7831576632145403 -0.156077221
+-0.657984249270295 0.7252353702830132 -0.01572114045120619
+-0.5954559207000718 0.759834284989988 0.156077221
+-0.4946697047593434 -0.6742138297313394 0.5168055006382355
+-0.639950216 0.4085518317838012 -0.6157588733492543
+-0.5998918291693639 0.6967399145036977 -0.336298137399878
+-0.5237505581750362 -0.502706610838997 -0.6564133188224507
+0.01571896257416014 -0.6311969310402367 -0.506967405792985
+-0.3668821572994054 0.5028341068058905 -0.03428236487469615
+-0.2791390631868537 0.09909319471286901 0.9308043434985912
+-0.2737977646360331 -0.1017366076864576 -0.932670832
+-5.551115123125783e-17 -0.964718938 0.156077191
+-0.1241942773737781 -0.762168056508469 -0.606583644747047
+-0.0100337821037552 0.2805783147871246 -0.785522469281193
+0.1648899015864477 -0.7264596185416647 0.6325962642028151
+0.2395466931977362 -0.3847973404532145 0.1987252323797588
+0.1527941100557161 -0.2485787627082534 0.932883326713121
+-0.5150830027579768 -0.225458908235314 -0.6414400992740774
+-0.1648899015864478 0.7264596185416647 -0.6325962642028151
+0.1648899015864477 0.7264596185416647 -0.6325962642028151
+0.1120997896845222 0.9729512646430574 0.005333115449487641
+0.2905600608752268 -0.8710022476126034 -0.3346302417443242
+0.2733518164848321 0.08333433799291456 0.9340806351506195
+-0.2186876832761552 0.1355551725632875 -0.7514080414631757
+0.5883533537861093 0.7646147491588572 0.1573500293575232
+0.657984249270295 -0.7252353702830132 0.01572114045120628
+-0.283151721891854 -0.2697271334462735 -0.7677817901486564
+0.3923587771881114 0.643044977255366 -0.2358683228539508
+-0.4406098886950756 -0.6391326817605969 0.3190304651144633
+0.639950216 0.4085518317838012 -0.6157588733492545
+0.7429325497058085 -0.5022358480759658 -0.3926503031839971
+0.8149411764884267 -0.3823966170795073 -0.3882821740004315
+0.7385844337468173 0.4959351597049497 0.414817375227283
+0.8893080805939757 -0.4106472579246842 -5.551115123125783e-17
+0.7397023528267623 -0.5327068054158335 0.3516701384263012
+0.9049514139222521 -0.3367420363845999 0.1560772031267084
+0.7453150012885376 0.5372878022233248 -0.331485378105409
+0.8468161346461875 0.3613128783487702 0.3346302417443243
+0.9574021488411723 0.2026759276143504 0.00969656983529274
+0.8941131900717569 0.3991747520142158 0.02009175298266304
+-0.957709450127873 -0.1150537272727258 -0.1655405539330506
+-0.4845984888036726 -0.5261166943379729 0.6692604834886484
+-0.3506172103503078 -0.6234596794029414 0.6692604834886484
+-0.4753746993073021 0.7937918318749986 0.3346302417443243
+-0.4840505109308432 -0.7014906573711918 0.1445905231917857
+0.2707633919962071 -0.7081297721928731 -0.6163457072825573
+0.2720567874572617 -0.1070947872842641 -0.932670832
+0.4606355048509909 -0.8086249801719713 -0.3147570083320652
+0.3506172103503078 -0.6234596794029414 0.6692604834886484
+0.4096770803439219 -0.761789534681282 -0.4708113795533366
+-0.5608864967343244 -0.5002524898810411 0.2796298810577725
+0.6115251132961432 0.5648777306174195 0.1603831698017203
+0.3112910443022733 0.005653668058812759 0.7497519303946486
+-0.326223368851062 -0.090716640529712 0.6409736602035725
+0.5603757281700371 -0.5518103791561476 -0.2623215125048247
+0.110531317716949 -0.06212769157402307 -0.7090846736271308
+0.7066528525432456 0.3857452961775781 0.06466039034313326
+-0.2870284024040223 0.3993456525073666 -0.3603438608047557
+0.4073544778967688 -0.316613713703684 0.004045726126147522
+-0.7885700713375572 -0.2499228280367138 -0.07471496532949348
+0.4402160867874776 -0.1218990893552541 -0.6831289812200559
+0.6646971797773117 0.2583486777982067 0.4739581945577929
+0.2738513407524434 0.3723478719915778 -0.1410164250591326
+-0.6097271098386507 0.05789107466758828 0.07196260058945804
+0.6233549523511848 0.4703431892808756 0.5916864408666598
+-0.2252612501639656 -0.6585131146906121 0.3735728487626422
+-0.1952039515635032 -0.4206871829507813 -0.675228105140974
+0.1112955484387936 0.5127300343494182 0.6032043578325359
+0.4251068484948444 -0.2312900141676994 -0.8520679447903731
+0.4535650605864477 0.2229142033935338 0.8414200303719319
+0.4482113831635415 -0.5098491617575793 0.7088642783123628
+0.06209713868688906 -0.6846150787542344 -0.7006190068735234
+0.0375335915419611 0.2478879406402632 0.7891168376389351
+0.9674071119541455 0.03415884376465241 0.161696170936081
+-0.6465554288694874 -0.2733596096316241 -0.3223698881350392
+-0.6342918131897257 -0.5058841359687427 -0.2738551387910891
+-0.346614458255896 0.6520192716236507 0.3646333695986975
+0.04578829671120705 -0.1403897385186038 -0.2562642746637507
+0.2476581831353243 -0.3872670587268629 -0.2083650025354314
+-0.3003935458321194 -0.1749983686001565 -0.3188893171939637
+-0.727709338623766 0.1540042504308398 0.2424727027908384
+0.1159640558706719 -0.07081091511933212 -0.4108053950654383
+0.5452822968143677 0.2416746102773027 -0.1075963602108601
+0.003220736745420392 -0.2941661607341402 -0.6318213160745906
+0.02250441915388015 0.1375631176366774 -0.5374998591515358
+-0.6090663507680694 0.3444698098995285 -0.1186274894911475
+0.2198027106802337 -0.3850323513976263 -0.003955119313183036
+-0.2539815602932239 -0.1830537221569612 0.9302432470180022
+-0.2539815602932239 0.1830537221569611 -0.9302432470180022
+
+CELLS 2536 12680
+4 386 189 123 142
+4 231 167 297 194
+4 221 114 215 255
+4 232 397 194 471
+4 371 315 377 89
+4 233 380 255 95
+4 297 124 252 194
+4 179 136 137 207
+4 202 391 240 222
+4 0 494 495 115
+4 176 223 233 190
+4 0 115 495 403
+4 212 192 595 482
+4 0 415 403 495
+4 367 440 370 368
+4 106 279 348 369
+4 90 377 299 88
+4 489 297 573 252
+4 125 281 191 324
+4 112 131 585 117
+4 268 585 10 506
+4 251 285 563 92
+4 87 491 432 371
+4 216 192 235 231
+4 373 494 115 570
+4 371 369 378 377
+4 139 49 530 101
+4 576 539 197 326
+4 472 39 232 93
+4 22 383 453 163
+4 9 156 119 7
+4 337 249 361 569
+4 186 276 263 243
+4 532 332 237 236
+4 525 60 41 169
+4 69 67 218 427
+4 265 595 534 203
+4 269 381 104 108
+4 3 498 387 570
+4 3 570 499 168
+4 581 344 559 263
+4 168 499 585 570
+4 337 249 526 261
+4 578 218 100 375
+4 402 240 244 106
+4 211 228 229 97
+4 415 208 120 591
+4 218 427 575 476
+4 129 323 172 553
+4 569 526 348 242
+4 50 235 192 252
+4 505 10 585 428
+4 469 518 332 38
+4 436 162 145 53
+4 484 542 541 362
+4 368 440 378 91
+4 309 295 472 48
+4 570 387 499 364
+4 206 239 240 391
+4 267 271 251 73
+4 89 567 475 548
+4 5 166 314 104
+4 5 104 434 166
+4 201 200 238 388
+4 329 124 96 252
+4 205 226 592 238
+4 10 126 383 517
+4 17 414 191 274
+4 75 150 366 92
+4 328 70 64 389
+4 197 539 144 555
+4 111 121 390 188
+4 9 1 168 119
+4 532 236 237 259
+4 504 497 122 384
+4 9 168 1 165
+4 11 108 123 147
+4 151 86 401 266
+4 220 262 388 306
+4 85 334 562 79
+4 437 225 190 598
+4 36 282 215 19
+4 22 24 428 383
+4 23 587 422 182
+4 208 115 570 117
+4 242 597 106 569
+4 27 217 374 345
+4 27 374 320 164
+4 315 584 377 89
+4 377 584 378 89
+4 520 215 98 462
+4 32 46 462 520
+4 564 121 255 233
+4 33 134 177 534
+4 34 196 197 522
+4 34 457 522 197
+4 270 266 264 367
+4 537 204 577 203
+4 153 472 221 93
+4 36 520 215 583
+4 36 295 583 215
+4 86 367 401 266
+4 86 367 433 401
+4 98 379 225 462
+4 360 76 284 410
+4 360 410 284 305
+4 162 65 96 318
+4 194 162 96 318
+4 234 270 72 571
+4 234 270 571 264
+4 111 193 587 221
+4 201 174 175 590
+4 86 367 266 406
+4 333 194 51 318
+4 437 225 302 190
+4 437 225 44 302
+4 56 412 580 583
+4 57 231 252 253
+4 57 329 253 252
+4 58 563 409 243
+4 97 230 382 244
+4 234 579 571 72
+4 401 571 305 284
+4 56 580 533 583
+4 68 560 565 423
+4 68 426 423 565
+4 67 427 575 218
+4 51 93 320 217
+4 196 319 418 26
+4 454 110 562 124
+4 454 455 562 535
+4 454 455 476 562
+4 125 461 212 191
+4 324 125 212 191
+4 57 50 451 252
+4 252 474 451 57
+4 422 182 587 221
+4 80 148 362 270
+4 571 284 72 305
+4 401 266 571 284
+4 57 235 50 252
+4 88 362 485 365
+4 65 96 253 329
+4 377 88 546 365
+4 90 88 546 377
+4 280 371 476 105
+4 401 433 353 367
+4 321 223 44 300
+4 300 359 321 223
+4 429 470 488 31
+4 452 31 470 429
+4 429 216 488 470
+4 452 470 216 429
+4 270 264 266 571
+4 479 249 256 335
+4 250 476 297 124
+4 250 476 124 471
+4 215 114 379 95
+4 496 497 120 156
+4 496 156 208 1
+4 192 398 107 252
+4 25 282 188 215
+4 366 186 424 75
+4 67 145 218 154
+4 120 142 141 384
+4 162 145 471 124
+4 368 299 490 91
+4 367 91 86 440
+4 398 102 356 538
+4 241 264 234 566
+4 575 476 81 572
+4 427 81 575 476
+4 467 107 277 530
+4 404 480 453 196
+4 151 284 401 360
+4 151 401 353 360
+4 23 430 182 422
+4 450 533 583 56
+4 98 533 583 450
+4 169 391 170 222
+4 62 289 359 300
+4 141 118 136 574
+4 25 309 215 153
+4 153 188 215 25
+4 23 456 182 430
+4 200 205 209 180
+4 360 410 109 312
+4 360 410 305 109
+4 76 360 312 410
+4 313 125 461 534
+4 53 145 471 162
+4 16 313 461 534
+4 16 534 461 134
+4 349 347 367 368
+4 347 365 367 368
+4 140 187 568 143
+4 143 187 568 385
+4 129 510 222 223
+4 156 7 497 122
+4 367 490 86 91
+4 101 530 467 322
+4 49 322 530 101
+4 489 231 252 192
+4 231 235 252 192
+4 118 120 156 208
+4 204 173 577 200
+4 204 594 200 577
+4 251 572 371 355
+4 572 407 548 81
+4 496 120 208 156
+4 0 166 115 403
+4 166 115 439 0
+4 176 552 172 28
+4 16 534 134 33
+4 12 420 509 480
+4 420 127 112 463
+4 385 21 122 512
+4 147 282 188 25
+4 432 82 424 366
+4 495 3 168 570
+4 498 112 420 364
+4 353 76 83 433
+4 324 49 101 303
+4 523 38 469 152
+4 243 276 379 563
+4 449 566 45 447
+4 443 524 125 534
+4 443 125 524 303
+4 295 298 578 48
+4 30 539 576 326
+4 57 231 531 160
+4 116 16 445 461
+4 497 384 120 596
+4 497 120 384 386
+4 161 290 358 59
+4 236 388 589 254
+4 164 39 93 331
+4 4 165 168 116
+4 165 316 4 168
+4 283 314 120 2
+4 28 223 190 302
+4 145 476 454 69
+4 186 359 263 581
+4 311 359 186 581
+4 194 318 253 531
+4 72 270 266 571
+4 179 202 178 207
+4 270 266 367 406
+4 389 565 254 259
+4 300 527 243 44
+4 44 527 243 225
+4 168 208 570 117
+4 256 337 249 361
+4 461 183 568 184
+4 413 53 232 333
+4 98 243 379 563
+4 98 563 379 100
+4 161 524 45 358
+4 420 463 373 6
+4 420 6 325 463
+4 420 480 364 127
+4 6 463 460 502
+4 6 502 325 463
+4 325 20 555 539
+4 347 365 361 362
+4 336 361 362 347
+4 347 367 365 362
+4 528 116 126 134
+4 190 223 224 243
+4 207 178 402 202
+4 65 110 124 346
+4 346 535 65 110
+4 116 117 585 130
+4 570 585 112 364
+4 570 117 112 585
+4 112 127 364 131
+4 230 192 489 248
+4 117 136 116 118
+4 115 137 132 574
+4 115 137 133 132
+4 531 194 231 253
+4 194 318 531 51
+4 499 585 316 168
+4 119 122 461 503
+4 120 142 384 386
+4 503 122 461 143
+4 122 384 596 385
+4 229 592 595 278
+4 595 248 214 482
+4 212 482 101 192
+4 132 179 137 233
+4 132 179 136 137
+4 223 263 510 94
+4 103 217 193 167
+4 116 119 140 461
+4 116 140 184 461
+4 591 574 403 208
+4 314 283 120 591
+4 166 314 591 283
+4 403 166 591 283
+4 522 260 54 342
+4 422 221 587 153
+4 587 153 221 93
+4 93 587 153 435
+4 348 365 249 569
+4 232 578 471 53
+4 368 369 378 475
+4 242 348 246 99
+4 26 196 343 418
+4 177 204 203 209
+4 199 568 489 213
+4 382 230 573 248
+4 230 248 489 573
+4 144 171 128 561
+4 144 127 128 170
+4 317 332 40 237
+4 117 136 131 130
+4 126 200 177 134
+4 127 364 131 175
+4 127 202 175 131
+4 127 144 175 202
+4 130 134 135 590
+4 131 179 136 132
+4 131 178 202 175
+4 131 175 130 178
+4 270 340 362 264
+4 270 367 264 362
+4 347 362 264 367
+4 136 135 118 185
+4 136 130 135 181
+4 120 123 142 386
+4 137 108 142 591
+4 137 233 380 121
+4 189 384 374 142
+4 137 233 133 132
+4 134 183 200 177
+4 135 185 140 118
+4 118 140 596 187
+4 118 136 185 141
+4 140 187 143 596
+4 140 568 461 143
+4 482 398 214 107
+4 143 568 191 488
+4 141 385 384 519
+4 170 245 202 391
+4 222 240 242 391
+4 167 232 221 397
+4 232 221 397 114
+4 504 512 384 122
+4 302 225 243 190
+4 598 462 564 32
+4 232 53 162 333
+4 242 224 94 99
+4 144 175 202 404
+4 172 223 171 176
+4 181 228 227 229
+4 233 121 255 380
+4 173 177 200 204
+4 225 98 243 379
+4 175 178 590 130
+4 202 222 207 179
+4 202 222 179 170
+4 142 111 121 211
+4 130 135 181 590
+4 128 233 133 176
+4 177 209 183 200
+4 136 178 181 402
+4 136 207 402 380
+4 489 248 192 398
+4 183 534 209 177
+4 183 209 534 210
+4 185 402 211 136
+4 297 110 124 357
+4 512 345 103 385
+4 187 111 185 199
+4 187 185 184 199
+4 111 199 211 185
+4 233 564 190 224
+4 233 190 223 224
+4 222 242 233 223
+4 568 187 184 199
+4 187 199 519 111
+4 585 174 364 383
+4 586 364 383 585
+4 263 186 344 366
+4 153 39 472 93
+4 232 472 413 39
+4 114 232 472 578
+4 48 472 153 39
+4 188 153 215 221
+4 188 255 390 221
+4 221 153 215 114
+4 188 215 255 221
+4 175 201 590 178
+4 178 201 590 206
+4 401 571 109 305
+4 297 252 538 573
+4 141 185 187 111
+4 141 111 187 519
+4 576 170 171 222
+4 576 169 41 457
+4 198 388 594 200
+4 198 201 245 388
+4 201 245 206 202
+4 201 245 588 206
+4 501 15 113 381
+4 204 594 203 566
+4 205 227 180 206
+4 205 227 209 180
+4 206 180 181 227
+4 207 242 402 380
+4 203 566 209 204
+4 209 534 210 595
+4 180 227 229 181
+4 180 227 209 229
+4 327 21 468 385
+4 516 400 503 122
+4 564 462 379 215
+4 222 526 391 242
+4 242 99 246 396
+4 401 360 305 109
+4 185 213 230 229
+4 185 211 402 228
+4 211 402 97 396
+4 369 368 365 106
+4 106 348 279 246
+4 564 215 95 255
+4 233 95 564 224
+4 3 387 499 570
+4 364 174 480 383
+4 495 403 115 208
+4 495 208 115 570
+4 208 117 574 115
+4 403 208 574 115
+4 380 255 396 390
+4 133 381 104 113
+4 381 104 108 137
+4 133 137 104 381
+4 197 169 196 170
+4 542 541 362 80
+4 200 590 201 205
+4 174 201 200 590
+4 193 587 221 93
+4 295 472 578 114
+4 139 467 482 101
+4 482 107 214 467
+4 404 174 175 201
+4 229 592 278 244
+4 409 186 92 75
+4 163 196 404 453
+4 163 196 453 26
+4 246 280 348 352
+4 219 391 40 237
+4 12 586 480 22
+4 169 522 391 493
+4 251 280 105 371
+4 422 188 153 25
+4 280 357 475 352
+4 594 234 262 220
+4 594 262 388 220
+4 201 238 588 245
+4 245 391 238 239
+4 170 202 222 391
+4 245 391 206 202
+4 527 438 98 225
+4 462 98 438 225
+4 402 244 228 97
+4 142 211 121 136
+4 137 121 136 142
+4 218 145 578 154
+4 154 307 578 218
+4 307 412 100 218
+4 227 592 595 229
+4 228 229 97 244
+4 402 106 244 97
+4 402 228 240 206
+4 595 524 212 247
+4 229 244 248 230
+4 47 524 49 303
+4 247 595 214 482
+4 212 482 247 101
+4 351 475 368 593
+4 212 595 247 482
+4 230 382 244 248
+4 146 255 564 121
+4 474 538 451 61
+4 198 388 518 220
+4 43 198 518 220
+4 469 43 198 518
+4 397 396 376 250
+4 566 265 595 524
+4 595 265 534 524
+4 47 466 45 524
+4 561 176 171 128
+4 192 451 139 50
+4 267 150 92 87
+4 267 92 572 87
+4 232 471 114 397
+4 18 268 528 134
+4 287 18 134 268
+4 408 273 108 11
+4 271 81 407 572
+4 269 19 146 273
+4 274 281 461 29
+4 540 275 107 77
+4 556 258 388 236
+4 63 275 277 467
+4 306 556 258 388
+4 529 31 281 470
+4 474 538 252 451
+4 36 25 215 282
+4 218 145 375 578
+4 578 307 100 218
+4 237 219 391 259
+4 237 389 417 64
+4 120 2 496 283
+4 151 76 284 360
+4 285 75 92 150
+4 302 323 28 223
+4 19 32 286 146
+4 286 146 269 19
+4 169 257 493 510
+4 289 62 359 311
+4 312 290 465 63
+4 388 594 238 262
+4 198 388 200 201
+4 238 262 241 592
+4 239 106 588 240
+4 391 526 597 242
+4 391 260 597 526
+4 526 510 94 223
+4 291 598 564 32
+4 92 186 366 75
+4 432 424 150 366
+4 75 149 186 424
+4 310 32 564 15
+4 435 23 587 422
+4 435 422 587 153
+4 592 262 241 264
+4 43 294 220 296
+4 588 264 244 106
+4 295 48 578 472
+4 296 158 234 55
+4 296 55 234 66
+4 56 583 298 412
+4 159 56 583 298
+4 236 388 254 258
+4 110 562 124 357
+4 102 110 357 562
+4 321 223 42 44
+4 202 402 207 222
+4 202 206 402 240
+4 73 285 304 251
+4 304 73 251 580
+4 154 307 218 67
+4 246 348 279 352
+4 114 472 153 309
+4 99 100 105 375
+4 75 311 186 149
+4 248 278 214 288
+4 29 125 461 313
+4 461 274 29 313
+4 29 292 125 313
+4 246 382 352 279
+4 250 167 194 297
+4 13 512 384 504
+4 62 359 321 300
+4 559 359 321 62
+4 107 61 77 277
+4 107 77 61 538
+4 325 509 555 20
+4 326 197 421 34
+4 217 37 327 419
+4 539 14 339 514
+4 328 256 389 417
+4 51 93 331 320
+4 331 194 93 51
+4 331 333 194 51
+4 232 53 471 162
+4 334 79 535 562
+4 339 539 30 20
+4 539 339 325 20
+4 376 199 489 230
+4 376 199 167 489
+4 65 318 253 96
+4 172 28 553 323
+4 576 223 172 129
+4 217 341 320 51
+4 217 51 531 341
+4 467 107 530 139
+4 27 374 513 345
+4 179 222 207 233
+4 179 222 233 128
+4 170 222 179 128
+4 254 565 423 362
+4 259 336 254 589
+4 259 565 254 336
+4 260 337 259 597
+4 260 389 259 337
+4 260 597 526 337
+4 260 337 256 389
+4 337 597 526 569
+4 329 538 71 346
+4 262 347 264 589
+4 262 589 254 347
+4 262 362 264 347
+4 589 347 336 254
+4 514 576 30 539
+4 183 180 209 210
+4 183 184 180 210
+4 426 80 148 362
+4 589 264 106 347
+4 589 336 347 569
+4 597 569 337 259
+4 360 353 312 109
+4 43 306 220 518
+4 441 534 203 265
+4 348 369 365 106
+4 279 351 297 352
+4 279 352 369 351
+4 352 297 280 357
+4 296 234 220 306
+4 294 577 220 594
+4 158 594 220 234
+4 297 351 279 593
+4 280 371 348 352
+4 119 140 461 122
+4 122 140 461 143
+4 201 205 206 588
+4 590 205 206 201
+4 55 579 234 66
+4 288 354 398 573
+4 288 107 356 353
+4 16 134 461 116
+4 375 218 105 145
+4 239 589 106 240
+4 214 312 353 109
+4 63 247 467 301
+4 247 101 467 301
+4 482 467 247 101
+4 401 360 284 305
+4 571 72 284 266
+4 359 263 510 223
+4 594 241 262 234
+4 594 566 241 234
+4 88 490 365 299
+4 310 552 176 28
+4 561 14 502 339
+4 472 48 53 413
+4 590 206 180 181
+4 590 178 206 181
+4 389 256 361 337
+4 259 532 236 254
+4 413 472 232 53
+4 496 208 415 1
+4 336 361 569 337
+4 186 92 276 409
+4 347 264 349 367
+4 328 256 417 54
+4 42 155 323 129
+4 44 42 323 223
+4 264 367 350 349
+4 129 223 323 42
+4 279 369 368 351
+4 348 369 279 352
+4 92 87 371 572
+4 351 593 368 279
+4 350 370 353 356
+4 350 367 353 370
+4 351 352 369 475
+4 348 344 363 366
+4 352 371 369 475
+4 366 344 363 424
+4 351 475 369 368
+4 348 352 371 369
+4 572 315 87 371
+4 593 567 102 357
+4 476 372 81 572
+4 103 111 167 193
+4 374 554 182 587
+4 225 243 98 527
+4 119 118 140 596
+4 144 245 404 202
+4 169 473 257 60
+4 375 471 250 397
+4 41 169 54 522
+4 251 92 348 371
+4 111 211 199 376
+4 371 491 377 315
+4 593 370 405 378
+4 368 490 367 91
+4 368 369 377 378
+4 74 559 261 257
+4 116 16 134 287
+4 500 16 116 287
+4 371 372 548 475
+4 567 102 85 405
+4 136 380 402 211
+4 402 396 211 380
+4 217 167 231 194
+4 124 476 454 145
+4 436 162 454 145
+4 385 327 103 195
+4 454 562 476 124
+4 476 562 357 124
+4 137 380 136 121
+4 558 532 389 254
+4 64 532 389 558
+4 123 142 188 108
+4 132 233 128 179
+4 173 177 204 537
+4 594 238 262 241
+4 114 375 471 578
+4 114 100 375 578
+4 389 532 237 259
+4 83 433 370 353
+4 367 353 370 433
+4 251 105 218 355
+4 83 440 370 433
+4 370 440 367 433
+4 100 105 218 251
+4 112 131 117 132
+4 592 241 566 264
+4 573 382 376 230
+4 489 252 231 297
+4 46 98 438 462
+4 46 438 464 462
+4 402 106 97 242
+4 536 361 479 84
+4 536 479 361 256
+4 215 98 462 379
+4 278 350 214 288
+4 288 350 353 356
+4 288 353 350 214
+4 288 354 350 356
+4 278 350 288 354
+4 233 380 95 224
+4 560 258 254 423
+4 560 254 565 423
+4 116 16 9 445
+4 16 431 116 9
+4 445 9 116 119
+4 134 528 177 126
+4 116 130 135 136
+4 309 472 153 48
+4 172 223 176 28
+4 342 417 237 260
+4 260 389 417 237
+4 396 97 382 246
+4 218 476 145 69
+4 408 314 507 591
+4 179 136 207 178
+4 95 396 375 99
+4 396 250 397 375
+4 211 97 402 228
+4 230 211 97 376
+4 98 442 243 563
+4 98 442 563 533
+4 344 74 261 363
+4 74 261 363 545
+4 335 545 261 74
+4 115 137 104 133
+4 348 366 363 377
+4 32 520 146 19
+4 391 260 259 597
+4 288 356 107 398
+4 422 147 182 188
+4 430 147 182 422
+4 430 123 182 147
+4 515 461 274 17
+4 17 191 143 461
+4 298 578 307 100
+4 242 569 106 348
+4 263 92 276 186
+4 289 359 243 186
+4 106 368 365 347
+4 4 116 500 431
+4 504 7 122 497
+4 176 233 133 564
+4 199 167 489 195
+4 13 189 456 395
+4 400 512 504 122
+4 400 7 122 504
+4 20 539 30 326
+4 89 475 378 371
+4 430 11 123 147
+4 564 233 255 95
+4 566 264 214 592
+4 214 264 278 592
+4 566 109 214 264
+4 222 402 242 240
+4 240 106 597 589
+4 97 106 382 246
+4 480 404 174 175
+4 480 174 404 383
+4 480 144 404 175
+4 354 593 350 405
+4 350 593 370 405
+4 367 593 350 349
+4 350 370 593 367
+4 278 349 350 593
+4 308 577 411 24
+4 173 411 577 24
+4 304 409 58 563
+4 126 134 130 590
+4 585 126 174 383
+4 90 584 416 377
+4 559 257 510 261
+4 44 464 438 225
+4 109 410 305 358
+4 388 201 245 238
+4 104 591 108 137
+4 202 240 402 222
+4 258 254 340 262
+4 254 340 262 362
+4 136 121 380 211
+4 390 221 396 211
+4 158 203 447 35
+4 524 247 566 358
+4 358 109 247 566
+4 592 264 278 244
+4 376 297 382 573
+4 160 217 341 37
+4 375 471 145 105
+4 301 524 49 47
+4 498 570 112 364
+4 337 526 249 569
+4 597 106 569 589
+4 66 148 258 423
+4 154 436 145 53
+4 377 348 92 371
+4 480 127 144 175
+4 364 480 174 175
+4 364 175 127 480
+4 70 257 328 60
+4 391 259 589 597
+4 245 388 391 219
+4 205 588 227 206
+4 85 102 71 547
+4 255 95 396 114
+4 89 378 549 584
+4 116 184 134 461
+4 461 183 184 134
+4 450 583 159 56
+4 448 583 36 159
+4 520 448 583 36
+4 174 590 130 175
+4 93 435 39 164
+4 254 558 532 560
+4 251 100 276 563
+4 200 383 201 174
+4 195 327 103 217
+4 217 419 327 345
+4 252 398 573 489
+4 250 194 471 124
+4 124 297 252 538
+4 411 173 399 24
+4 596 187 143 385
+4 385 488 568 143
+4 385 187 568 199
+4 385 519 187 199
+4 28 190 176 291
+4 94 348 261 526
+4 94 348 526 242
+4 371 548 89 475
+4 526 249 348 261
+4 142 136 141 185
+4 520 564 146 215
+4 116 184 135 134
+4 132 131 179 127
+4 106 368 272 279
+4 382 106 244 279
+4 106 244 279 272
+4 94 263 348 276
+4 403 115 574 137
+4 571 305 557 109
+4 566 557 109 571
+4 245 388 219 518
+4 479 249 361 256
+4 479 84 361 249
+4 196 245 170 391
+4 404 198 201 245
+4 211 221 396 376
+4 564 215 255 146
+4 566 571 264 234
+4 566 264 571 109
+4 396 246 382 250
+4 566 557 571 234
+4 85 405 157 550
+4 405 567 378 550
+4 85 157 405 459
+4 560 556 236 258
+4 461 212 534 125
+4 142 211 136 185
+4 267 407 87 572
+4 572 371 548 315
+4 409 311 186 75
+4 285 75 409 92
+4 16 292 534 33
+4 411 293 537 35
+4 294 203 35 577
+4 307 412 218 67
+4 257 335 261 74
+4 551 415 168 1
+4 256 335 261 257
+4 237 317 64 417
+4 34 196 40 418
+4 422 147 188 25
+4 76 353 83 446
+4 541 426 362 80
+4 427 81 476 372
+4 543 427 372 81
+4 229 192 248 595
+4 388 258 306 262
+4 388 258 262 254
+4 87 432 150 92
+4 87 491 371 315
+4 410 290 465 312
+4 434 15 8 113
+4 38 22 469 152
+4 38 469 22 163
+4 436 145 454 69
+4 69 145 67 436
+4 527 438 46 98
+4 524 47 161 45
+4 434 113 8 439
+4 45 441 447 265
+4 442 58 527 243
+4 82 444 424 363
+4 445 503 461 17
+4 515 461 17 445
+4 446 356 353 83
+4 46 98 448 450
+4 557 566 449 55
+4 41 473 169 60
+4 107 451 61 530
+4 50 452 138 235
+4 50 452 235 458
+4 78 565 477 68
+4 79 478 455 562
+4 455 79 562 535
+4 458 235 50 57
+4 545 84 363 82
+4 387 586 505 499
+4 585 499 586 364
+4 548 372 487 567
+4 129 525 169 510
+4 525 169 510 257
+4 583 114 578 100
+4 412 583 298 100
+4 357 476 372 562
+4 583 578 298 100
+4 160 217 531 341
+4 85 334 102 562
+4 272 347 264 349
+4 106 264 272 347
+4 92 251 267 572
+4 275 540 214 312
+4 540 214 353 107
+4 119 1 118 156
+4 275 107 214 540
+4 1 118 156 208
+4 1 208 168 118
+4 9 1 119 156
+4 408 123 11 108
+4 507 408 591 123
+4 583 379 100 98
+4 583 114 379 215
+4 520 98 215 583
+4 429 195 458 217
+4 129 223 172 323
+4 172 223 28 323
+4 219 237 332 236
+4 250 194 124 297
+4 231 194 252 253
+4 598 225 190 462
+4 462 225 464 598
+4 507 314 120 591
+4 460 128 113 561
+4 463 561 509 144
+4 287 134 116 268
+4 456 554 182 374
+4 93 232 193 194
+4 469 577 198 43
+4 202 402 206 178
+4 506 126 116 585
+4 109 465 358 247
+4 333 162 194 318
+4 525 510 223 359
+4 247 482 214 467
+4 49 324 192 138
+4 49 529 324 138
+4 167 193 221 232
+4 472 221 232 114
+4 263 366 344 348
+4 263 366 348 92
+4 587 554 435 164
+4 467 139 530 101
+4 116 130 134 135
+4 500 287 116 268
+4 5 104 269 501
+4 286 146 501 269
+4 334 535 346 110
+4 345 513 512 103
+4 461 515 274 313
+4 356 338 538 102
+4 559 525 359 510
+4 561 339 502 325
+4 547 71 338 102
+4 267 285 92 150
+4 282 273 108 146
+4 296 158 220 234
+4 163 219 196 330
+4 164 331 93 320
+4 495 168 208 570
+4 415 208 495 168
+4 415 208 403 495
+4 120 574 591 208
+4 496 415 208 120
+4 291 190 176 564
+4 291 564 176 310
+4 524 125 534 212
+4 270 340 234 481
+4 270 148 340 481
+4 313 125 534 292
+4 148 258 340 481
+4 234 481 340 258
+4 470 216 488 191
+4 138 470 192 216
+4 203 204 577 594
+4 467 322 277 63
+4 470 191 324 192
+4 324 470 192 138
+4 482 139 192 107
+4 49 139 192 101
+4 167 232 397 194
+4 129 525 510 223
+4 188 111 182 221
+4 519 103 111 167
+4 454 535 562 110
+4 142 188 121 111
+4 439 373 115 113
+4 498 373 112 570
+4 373 494 439 115
+4 270 264 234 340
+4 285 73 267 251
+4 100 251 218 483
+4 580 251 483 73
+4 296 66 234 306
+4 298 412 100 307
+4 383 200 173 126
+4 126 177 200 173
+4 302 223 243 44
+4 258 148 340 423
+4 190 223 243 302
+4 186 263 344 581
+4 559 263 510 359
+4 522 169 54 493
+4 387 498 364 570
+4 362 485 361 565
+4 548 567 487 89
+4 382 297 352 279
+4 234 579 557 571
+4 119 596 140 122
+4 136 207 380 137
+4 26 453 480 196
+4 6 373 460 463
+4 213 229 192 230
+4 229 192 230 248
+4 206 391 240 202
+4 74 344 559 581
+4 115 104 113 133
+4 534 177 183 134
+4 534 183 461 134
+4 42 129 223 525
+4 42 425 155 129
+4 42 425 129 525
+4 522 196 391 40
+4 391 260 522 342
+4 522 342 40 391
+4 391 237 342 40
+4 391 342 237 260
+4 328 256 54 473
+4 126 590 200 134
+4 343 144 509 555
+4 293 528 399 177
+4 579 481 234 66
+4 100 483 218 412
+4 458 195 235 231
+4 195 235 429 458
+4 57 231 235 252
+4 197 457 169 576
+4 302 190 28 291
+4 294 594 158 203
+4 296 294 220 158
+4 189 374 456 182
+4 199 185 213 230
+4 476 280 297 357
+4 485 362 361 365
+4 116 118 135 140
+4 116 140 119 118
+4 383 198 577 200
+4 13 456 189 513
+4 199 488 195 489
+4 385 199 488 195
+4 385 519 199 195
+4 13 189 384 103
+4 13 103 384 512
+4 103 512 385 384
+4 242 348 106 246
+4 195 216 231 489
+4 65 124 96 329
+4 573 398 538 102
+4 398 252 573 538
+4 384 385 103 519
+4 233 380 242 207
+4 16 313 515 461
+4 16 445 461 515
+4 279 382 297 573
+4 336 361 347 569
+4 29 461 125 281
+4 577 220 43 294
+4 294 43 577 308
+4 177 534 209 203
+4 209 203 595 566
+4 15 146 564 381
+4 15 564 146 32
+4 396 242 97 246
+4 107 356 538 398
+4 261 344 363 348
+4 5 104 408 269
+4 348 377 369 371
+4 89 371 378 377
+4 585 130 174 126
+4 585 175 174 130
+4 368 416 91 378
+4 309 295 114 472
+4 170 245 144 202
+4 388 391 238 245
+4 34 418 197 196
+4 34 418 326 197
+4 197 418 343 196
+4 197 418 326 343
+4 222 510 391 526
+4 174 383 201 404
+4 108 215 146 255
+4 255 95 114 215
+4 230 229 211 185
+4 97 229 230 244
+4 116 135 118 136
+4 168 119 118 116
+4 593 378 567 475
+4 116 135 184 140
+4 137 121 142 108
+4 108 121 142 188
+4 269 273 108 408
+4 271 407 267 572
+4 294 577 411 308
+4 223 243 44 300
+4 322 101 49 301
+4 44 323 302 223
+4 65 124 329 346
+4 40 196 319 418
+4 217 419 320 341
+4 576 539 144 197
+4 496 283 415 120
+4 551 495 3 168
+4 495 415 168 551
+4 194 252 253 96
+4 513 456 374 554
+4 15 552 176 310
+4 506 528 399 18
+4 506 528 126 517
+4 52 38 332 518
+4 85 71 102 334
+4 15 310 176 564
+4 15 564 176 113
+4 16 134 287 33
+4 275 107 467 214
+4 149 186 344 581
+4 344 263 261 559
+4 546 365 88 544
+4 570 373 112 115
+4 378 157 550 549
+4 195 235 231 216
+4 383 480 453 404
+4 22 480 453 383
+4 558 254 389 565
+4 559 261 344 74
+4 17 414 143 191
+4 414 143 191 488
+4 35 293 537 521
+4 232 472 578 53
+4 518 582 38 523
+4 302 437 190 291
+4 316 3 168 492
+4 491 584 377 315
+4 432 150 92 366
+4 434 104 113 115
+4 434 113 104 501
+4 151 433 353 401
+4 151 86 433 401
+4 450 583 448 159
+4 520 98 583 448
+4 518 219 332 236
+4 427 372 478 543
+4 428 393 586 505
+4 426 565 477 541
+4 430 395 123 508
+4 156 122 596 119
+4 9 119 168 116
+4 503 7 119 122
+4 459 370 405 356
+4 373 460 128 113
+4 463 127 509 420
+4 420 509 480 127
+4 548 567 475 372
+4 212 101 524 125
+4 212 324 101 125
+4 303 101 324 125
+4 301 247 101 524
+4 524 301 247 358
+4 561 509 144 555
+4 328 389 64 417
+4 329 124 538 346
+4 346 110 124 538
+4 537 399 173 177
+4 399 528 173 177
+4 411 399 537 293
+4 580 100 483 251
+4 73 271 251 483
+4 167 193 111 221
+4 121 255 390 188
+4 115 403 104 137
+4 374 587 93 164
+4 374 587 193 93
+4 374 193 587 111
+4 587 374 554 164
+4 389 532 259 254
+4 289 359 186 311
+4 581 344 149 74
+4 167 232 194 193
+4 193 221 232 93
+4 211 396 97 376
+4 230 573 489 376
+4 489 376 297 167
+4 114 471 232 578
+4 223 94 224 243
+4 297 194 252 231
+4 376 167 397 250
+4 489 231 167 297
+4 106 365 348 569
+4 198 245 518 388
+4 472 153 221 114
+4 162 65 454 124
+4 138 216 192 235
+4 138 192 139 50
+4 570 117 585 168
+4 131 175 364 585
+4 374 103 513 345
+4 343 480 144 196
+4 480 509 511 343
+4 423 560 258 556
+4 45 466 265 524
+4 9 431 116 165
+4 9 168 165 116
+4 327 385 468 195
+4 103 327 385 345
+4 103 217 327 345
+4 330 40 196 319
+4 163 330 196 319
+4 469 219 330 332
+4 315 548 572 407
+4 283 403 415 591
+4 129 41 169 525
+4 250 105 280 476
+4 99 250 105 280
+4 463 502 325 561
+4 409 92 276 563
+4 563 92 276 251
+4 18 506 528 268
+4 528 506 116 268
+4 528 268 116 134
+4 269 146 108 273
+4 165 492 316 168
+4 186 149 344 424
+4 186 344 366 424
+4 15 564 113 381
+4 440 86 367 433
+4 342 417 260 54
+4 54 256 417 260
+4 238 589 239 391
+4 367 91 440 368
+4 378 370 157 440
+4 118 156 596 119
+4 503 9 119 7
+4 434 501 15 113
+4 16 500 116 431
+4 533 98 100 563
+4 583 98 100 533
+4 271 251 355 572
+4 483 251 355 271
+4 445 503 119 461
+4 563 304 409 285
+4 498 112 373 420
+4 420 463 112 373
+4 373 112 128 463
+4 471 145 105 476
+4 529 281 324 470
+4 596 141 384 120
+4 384 385 141 596
+4 496 2 120 497
+4 255 221 396 390
+4 406 367 490 86
+4 406 362 490 367
+4 297 593 102 357
+4 297 593 573 102
+4 447 521 203 441
+4 450 533 442 98
+4 456 23 182 554
+4 23 182 554 587
+4 50 529 138 452
+4 525 425 129 41
+4 139 50 451 530
+4 547 459 356 446
+4 460 14 502 561
+4 401 264 571 266
+4 353 151 76 433
+4 75 150 424 366
+4 266 367 401 264
+4 22 24 383 152
+4 145 124 162 454
+4 163 383 404 469
+4 196 522 391 169
+4 450 98 448 583
+4 449 566 447 158
+4 264 109 350 401
+4 264 367 401 350
+4 401 367 353 350
+4 401 109 350 353
+4 370 440 459 157
+4 458 231 57 160
+4 4 116 585 268
+4 401 360 109 353
+4 214 264 109 350
+4 278 350 264 214
+4 214 109 353 350
+4 231 217 531 160
+4 531 194 217 231
+4 244 272 278 279
+4 244 278 272 264
+4 272 278 349 264
+4 14 561 539 339
+4 587 23 435 554
+4 568 213 192 489
+4 404 198 245 518
+4 242 97 402 396
+4 95 255 396 380
+4 257 335 60 70
+4 364 12 480 420
+4 12 586 364 480
+4 458 452 235 429
+4 569 361 347 365
+4 117 132 574 115
+4 117 115 112 132
+4 560 236 254 258
+4 563 304 285 251
+4 317 342 237 40
+4 188 221 422 153
+4 536 78 64 389
+4 470 281 324 191
+4 516 143 414 488
+4 516 488 414 468
+4 257 335 74 60
+4 328 257 473 60
+4 421 326 576 197
+4 468 31 488 414
+4 534 265 466 524
+4 534 524 466 443
+4 163 219 404 196
+4 469 219 404 163
+4 22 383 163 469
+4 383 404 453 163
+4 587 435 93 164
+4 562 567 487 372
+4 562 102 567 357
+4 289 243 359 300
+4 142 188 111 182
+4 182 111 142 374
+4 508 386 123 507
+4 300 359 223 243
+4 537 173 577 204
+4 411 399 173 537
+4 537 577 173 411
+4 580 100 412 483
+4 488 470 191 414
+4 414 470 191 281
+4 553 576 514 172
+4 523 43 469 518
+4 523 518 469 38
+4 342 317 237 417
+4 361 389 78 536
+4 536 361 389 256
+4 78 361 565 389
+4 189 103 13 513
+4 12 480 509 511
+4 343 144 480 509
+4 462 438 464 225
+4 441 45 466 265
+4 458 235 57 231
+4 522 41 169 457
+4 455 478 427 372
+4 476 427 372 455
+4 430 456 182 395
+4 430 395 182 123
+4 68 565 477 426
+4 428 586 22 383
+4 12 428 393 586
+4 22 586 428 12
+4 401 151 266 284
+4 291 564 310 32
+4 16 313 534 292
+4 304 533 563 58
+4 598 46 464 462
+4 598 225 464 437
+4 599 441 466 534
+4 599 534 466 443
+4 534 441 466 265
+4 557 566 45 449
+4 520 46 98 448
+4 32 564 520 462
+4 291 598 190 564
+4 199 489 568 488
+4 568 488 489 216
+4 488 195 489 216
+4 489 195 167 231
+4 397 194 471 250
+4 354 398 102 356
+4 470 192 216 191
+4 568 191 216 192
+4 385 516 143 122
+4 142 111 384 374
+4 348 369 377 365
+4 375 471 397 114
+4 396 397 114 375
+4 335 249 545 479
+4 270 406 367 362
+4 537 204 203 177
+4 590 180 206 205
+4 136 207 178 402
+4 49 139 138 192
+4 247 214 109 465
+4 214 465 312 109
+4 374 384 103 111
+4 391 522 260 493
+4 194 96 253 318
+4 402 106 242 240
+4 245 239 238 588
+4 588 592 239 238
+4 206 227 228 588
+4 580 100 251 563
+4 473 257 493 169
+4 328 257 256 473
+4 592 239 262 264
+4 239 264 589 262
+4 240 597 106 242
+4 588 264 106 239
+4 128 112 115 132
+4 498 495 494 570
+4 494 495 115 570
+4 373 128 112 115
+4 365 367 368 490
+4 368 377 365 299
+4 368 416 299 91
+4 128 127 179 170
+4 131 179 202 178
+4 194 217 51 531
+4 179 137 233 207
+4 343 197 196 144
+4 326 197 343 555
+4 35 537 577 203
+4 101 482 139 192
+4 482 398 107 192
+4 420 325 509 463
+4 226 238 594 241
+4 226 592 241 566
+4 493 261 257 256
+4 503 7 122 400
+4 463 112 128 127
+4 463 144 127 128
+4 539 144 561 171
+4 539 555 561 144
+4 260 256 337 261
+4 585 383 586 428
+4 363 249 348 365
+4 385 195 103 519
+4 128 133 132 115
+4 249 365 361 569
+4 112 132 127 131
+4 132 179 128 127
+4 128 112 132 127
+4 117 136 574 132
+4 117 132 131 136
+4 132 137 136 574
+4 381 146 269 501
+4 357 372 567 562
+4 586 480 383 364
+4 586 22 383 480
+4 171 170 576 144
+4 539 576 144 171
+4 126 590 130 174
+4 68 565 558 78
+4 389 78 558 565
+4 144 170 197 196
+4 184 229 181 180
+4 416 91 378 549
+4 378 416 549 584
+4 542 362 490 406
+4 116 445 119 461
+4 135 180 181 590
+4 568 184 210 213
+4 184 213 185 229
+4 127 170 144 202
+4 564 379 224 95
+4 199 213 489 230
+4 213 230 192 489
+4 84 361 249 544
+4 84 546 544 363
+4 122 21 400 512
+4 516 400 122 21
+4 388 391 589 238
+4 524 566 45 358
+4 95 375 100 99
+4 451 50 192 252
+4 283 0 415 403
+4 493 391 526 260
+4 493 261 260 526
+4 215 146 520 19
+4 53 145 578 471
+4 154 145 578 53
+4 244 279 278 248
+4 382 280 297 250
+4 415 208 168 1
+4 382 352 297 280
+4 99 276 251 100
+4 379 276 99 100
+4 379 224 99 276
+4 248 573 279 278
+4 246 280 352 382
+4 507 2 120 314
+4 386 2 120 507
+4 107 398 538 252
+4 316 585 10 4
+4 261 256 337 249
+4 262 254 362 347
+4 254 347 336 362
+4 119 156 122 7
+4 260 337 526 261
+4 278 349 264 350
+4 354 350 593 278
+4 354 356 405 350
+4 288 356 398 354
+4 352 297 357 351
+4 297 357 351 593
+4 357 475 352 351
+4 507 11 123 508
+4 250 167 397 194
+4 8 14 460 113
+4 15 14 176 552
+4 517 10 428 383
+4 426 541 362 565
+4 151 360 353 76
+4 361 544 365 249
+4 371 369 475 378
+4 367 368 370 593
+4 157 378 405 370
+4 548 315 371 89
+4 326 539 555 20
+4 516 122 385 21
+4 521 534 177 203
+4 441 521 203 534
+4 152 24 577 308
+4 59 305 358 557
+4 136 181 135 185
+4 135 184 181 180
+4 137 574 142 136
+4 118 140 187 185
+4 118 141 185 187
+4 140 187 184 568
+4 141 142 185 111
+4 142 111 211 185
+4 108 146 269 381
+4 501 113 104 381
+4 599 534 292 33
+4 38 330 469 163
+4 469 330 219 163
+4 40 522 196 34
+4 521 203 537 35
+4 218 69 145 67
+4 248 398 489 573
+4 210 180 209 229
+4 390 111 221 211
+4 191 568 212 192
+4 248 288 214 398
+4 27 374 217 320
+4 463 144 128 561
+4 117 130 116 136
+4 39 232 93 331
+4 574 591 137 142
+4 35 203 294 158
+4 43 296 220 306
+4 122 143 596 140
+4 123 182 188 142
+4 36 159 583 295
+4 534 595 212 210
+4 534 524 212 595
+4 37 217 458 160
+4 531 253 231 57
+4 252 329 474 57
+4 65 329 253 57
+4 135 134 180 590
+4 135 180 134 184
+4 250 471 105 476
+4 140 461 568 184
+4 143 568 461 191
+4 410 59 358 290
+4 410 305 358 59
+4 469 198 404 518
+4 469 383 198 577
+4 469 383 404 198
+4 581 359 559 62
+4 475 371 372 476
+4 476 371 372 572
+4 200 383 198 201
+4 383 404 198 201
+4 233 380 224 242
+4 432 377 366 92
+4 82 377 366 432
+4 178 206 202 201
+4 200 177 209 204
+4 148 66 258 481
+4 560 565 558 68
+4 553 514 552 172
+4 172 514 552 176
+4 190 243 224 379
+4 190 243 379 225
+4 190 225 379 462
+4 200 204 209 226
+4 190 462 379 564
+4 328 70 256 257
+4 200 226 594 204
+4 190 379 224 564
+4 178 181 402 206
+4 181 206 227 228
+4 335 70 256 479
+4 376 167 250 297
+4 206 588 239 245
+4 228 244 240 588
+4 240 244 106 588
+4 579 305 571 72
+4 113 133 564 176
+4 113 381 564 133
+4 589 106 569 347
+4 275 312 465 63
+4 540 107 356 77
+4 126 173 383 517
+4 232 162 471 194
+4 74 444 545 363
+4 333 162 232 194
+4 536 361 84 78
+4 565 485 361 78
+4 355 476 572 371
+4 371 572 548 372
+4 572 372 81 548
+4 69 455 476 454
+4 487 85 562 79
+4 485 362 484 565
+4 485 362 542 484
+4 368 377 299 416
+4 488 31 470 414
+4 414 31 470 281
+4 556 518 582 52
+4 556 52 236 518
+4 518 556 388 236
+4 306 518 556 388
+4 556 518 306 582
+4 459 370 356 83
+4 545 84 249 363
+4 545 479 249 84
+4 363 84 249 544
+4 547 102 459 85
+4 528 177 126 173
+4 80 406 270 362
+4 131 178 130 136
+4 485 88 365 544
+4 490 365 362 88
+4 407 87 572 315
+4 99 396 375 250
+4 0 166 403 283
+4 251 371 105 355
+4 168 4 116 585
+4 592 244 588 264
+4 244 264 272 106
+4 408 5 314 104
+4 567 405 378 593
+4 507 386 123 120
+4 181 228 229 185
+4 230 97 211 229
+4 561 113 176 128
+4 378 584 377 416
+4 183 461 212 534
+4 183 210 534 212
+4 136 185 402 181
+4 528 126 517 173
+4 11 273 108 147
+4 198 220 594 388
+4 209 534 595 203
+4 514 172 539 176
+4 185 228 229 211
+4 97 382 230 376
+4 199 230 211 185
+4 199 211 230 376
+4 29 281 125 324
+4 385 568 488 199
+4 220 234 262 306
+4 421 30 576 326
+4 595 524 247 566
+4 595 214 566 247
+4 302 225 44 243
+4 324 191 212 192
+4 237 389 259 260
+4 443 47 303 524
+4 208 117 118 574
+4 168 208 117 118
+4 337 361 389 565
+4 532 64 237 317
+4 589 259 336 569
+4 259 569 337 336
+4 312 353 540 214
+4 525 257 510 559
+4 525 257 559 60
+4 279 272 349 368
+4 59 358 161 45
+4 297 573 593 279
+4 117 130 131 585
+4 14 113 176 561
+4 149 444 74 344
+4 365 544 361 485
+4 91 378 157 440
+4 368 370 378 440
+4 567 405 85 550
+4 71 61 338 538
+4 481 234 270 72
+4 251 285 92 267
+4 271 572 575 81
+4 271 483 575 355
+4 92 371 432 377
+4 85 102 459 405
+4 494 439 115 0
+4 480 144 196 404
+4 404 144 196 245
+4 201 175 202 178
+4 404 245 201 202
+4 364 499 586 387
+4 500 268 116 4
+4 420 127 364 112
+4 117 118 574 136
+4 141 120 596 118
+4 574 141 118 120
+4 460 6 494 373
+4 138 192 50 235
+4 563 276 409 243
+4 563 285 409 92
+4 247 467 465 63
+4 247 467 214 465
+4 10 383 585 428
+4 499 585 586 505
+4 506 585 10 126
+4 508 11 123 430
+4 392 586 387 12
+4 392 393 586 12
+4 580 533 100 563
+4 394 189 386 13
+4 563 304 251 580
+4 394 395 189 13
+4 514 552 176 14
+4 555 197 343 144
+4 326 539 197 555
+4 293 399 528 18
+4 186 243 289 409
+4 289 186 409 311
+4 469 22 383 152
+4 490 367 362 365
+4 119 1 168 118
+4 215 309 114 153
+4 21 345 512 385
+4 385 327 21 345
+4 385 21 468 516
+4 58 243 289 527
+4 58 243 409 289
+4 301 63 247 290
+4 465 63 290 247
+4 301 247 358 290
+4 301 101 49 524
+4 540 107 353 356
+4 149 444 344 424
+4 15 113 176 14
+4 33 441 599 534
+4 326 343 20 555
+4 301 290 358 161
+4 524 301 358 161
+4 345 27 217 419
+4 469 152 577 308
+4 469 308 577 43
+4 552 553 172 28
+4 139 451 107 530
+4 452 529 138 470
+4 50 529 49 138
+4 49 50 139 530
+4 293 177 134 528
+4 293 399 537 177
+4 580 533 563 304
+4 41 30 576 421
+4 452 529 470 31
+4 516 488 385 143
+4 516 385 488 468
+4 564 520 146 32
+4 152 383 577 24
+4 188 182 123 147
+4 316 4 168 585
+4 104 408 108 591
+4 408 104 314 591
+4 521 35 447 203
+4 435 153 93 39
+4 513 456 189 374
+4 82 546 363 377
+4 82 84 363 546
+4 225 44 464 437
+4 527 44 438 225
+4 506 116 126 528
+4 439 494 373 460
+4 8 460 439 113
+4 439 460 373 113
+4 439 113 115 434
+4 32 46 598 462
+4 532 332 317 237
+4 520 46 462 98
+4 93 194 217 51
+4 562 476 372 455
+4 524 47 466 443
+4 443 125 292 534
+4 303 125 524 101
+4 25 36 215 309
+4 376 297 573 489
+4 526 348 249 569
+4 94 263 261 348
+4 419 341 217 37
+4 217 419 27 320
+4 390 380 211 396
+4 380 224 396 95
+4 396 224 99 95
+4 472 48 413 39
+4 53 48 472 578
+4 358 557 45 59
+4 451 538 107 61
+4 129 169 576 222
+4 129 576 223 222
+4 510 129 222 169
+4 212 192 101 324
+4 324 192 101 49
+4 311 62 359 581
+4 47 301 524 161
+4 570 499 585 364
+4 303 324 29 125
+4 322 63 467 301
+4 301 101 467 322
+4 234 481 258 66
+4 434 104 115 166
+4 166 403 591 104
+4 314 166 591 104
+4 465 247 290 358
+4 76 353 446 540
+4 312 540 353 76
+4 70 389 256 536
+4 580 412 100 583
+4 580 100 533 583
+4 256 335 257 70
+4 303 292 443 125
+4 61 71 329 538
+4 65 454 124 110
+4 312 410 109 465
+4 579 72 234 481
+4 518 582 52 38
+4 258 306 262 234
+4 540 356 353 446
+4 463 325 509 561
+4 325 555 509 561
+4 294 203 577 594
+4 312 275 465 214
+4 331 333 232 194
+4 331 413 232 333
+4 331 232 93 194
+4 331 413 39 232
+4 324 529 470 138
+4 158 566 594 234
+4 294 594 220 158
+4 159 298 583 295
+4 484 78 565 477
+4 484 485 565 78
+4 476 69 427 455
+4 486 79 487 562
+4 486 79 562 478
+4 548 543 372 81
+4 483 251 218 355
+4 506 528 517 399
+4 78 389 558 64
+4 271 355 575 572
+4 148 340 362 270
+4 423 148 340 362
+4 33 521 177 293
+4 520 462 564 215
+4 55 449 158 566
+4 158 566 234 55
+4 158 566 447 203
+4 527 442 243 98
+4 446 459 356 83
+4 374 217 103 345
+4 374 217 193 103
+4 469 383 577 152
+4 133 233 128 132
+4 485 542 362 88
+4 90 299 377 416
+4 378 549 550 89
+4 92 371 87 432
+4 370 83 459 440
+4 168 551 492 3
+4 551 1 168 492
+4 1 492 165 168
+4 364 387 12 420
+4 498 420 387 364
+4 386 504 384 497
+4 394 508 386 123
+4 394 508 123 395
+4 15 8 113 14
+4 5 434 104 501
+4 501 381 104 269
+4 104 269 108 408
+4 512 385 384 122
+4 501 15 381 146
+4 15 146 501 286
+4 491 432 377 82
+4 173 517 399 24
+4 218 483 575 67
+4 122 516 143 503
+4 517 173 383 24
+4 334 110 102 562
+4 334 562 535 110
+4 553 30 514 576
+4 374 27 554 164
+4 468 488 31 429
+4 163 319 196 26
+4 197 457 421 34
+4 376 167 221 397
+4 111 221 376 167
+4 376 396 397 221
+4 263 186 366 92
+4 155 576 553 129
+4 576 155 553 30
+4 576 155 30 41
+4 380 242 396 224
+4 217 458 429 37
+4 303 29 292 125
+4 64 532 237 389
+4 525 223 42 321
+4 566 557 234 55
+4 566 45 358 557
+4 107 61 277 530
+4 280 475 371 352
+4 475 357 476 372
+4 280 475 357 476
+4 568 489 192 216
+4 558 254 565 560
+4 257 74 559 60
+4 114 375 100 95
+4 97 396 382 376
+4 531 318 253 65
+4 531 65 253 57
+4 61 329 474 538
+4 221 114 396 397
+4 111 199 519 167
+4 297 102 110 357
+4 297 102 573 538
+4 386 189 142 384
+4 424 363 366 82
+4 222 242 207 233
+4 207 222 402 242
+4 540 77 356 446
+4 71 338 102 538
+4 571 109 264 401
+4 484 541 477 565
+4 484 362 541 565
+4 485 544 361 84
+4 92 572 371 251
+4 366 348 92 377
+4 77 547 356 446
+4 550 85 567 487
+4 99 250 246 396
+4 99 280 246 250
+4 546 377 82 90
+4 172 576 171 223
+4 146 108 255 121
+4 248 398 214 482
+4 229 278 595 248
+4 371 491 432 377
+4 229 278 248 244
+4 392 387 586 505
+4 392 393 505 586
+4 566 45 265 524
+4 486 548 372 487
+4 486 543 478 372
+4 486 543 372 548
+4 155 129 41 576
+4 41 129 155 425
+4 217 167 194 193
+4 166 115 434 439
+4 431 116 165 4
+4 545 82 363 444
+4 503 445 119 9
+4 570 498 495 3
+4 377 365 299 88
+4 90 491 584 377
+4 491 82 377 90
+4 316 168 3 499
+4 499 316 585 10
+4 156 497 596 122
+4 363 249 365 544
+4 373 128 460 463
+4 311 186 149 581
+4 523 152 469 308
+4 523 308 469 43
+4 53 307 578 154
+4 53 48 578 307
+4 338 356 77 547
+4 524 247 101 212
+4 325 555 561 539
+4 4 268 585 10
+4 176 561 539 14
+4 327 217 195 429
+4 429 468 488 195
+4 429 327 468 195
+4 460 14 561 113
+4 77 107 277 275
+4 275 107 277 467
+4 320 374 93 164
+4 217 374 193 93
+4 217 374 93 320
+4 217 231 458 160
+4 463 509 127 144
+4 188 182 422 221
+4 562 478 455 372
+4 486 562 487 372
+4 486 562 372 478
+4 189 182 456 395
+4 394 123 386 189
+4 394 395 123 189
+4 223 359 321 525
+4 559 321 359 525
+4 355 572 476 575
+4 499 585 505 10
+4 583 298 578 295
+4 295 114 583 215
+4 295 578 583 114
+4 52 556 236 560
+4 66 423 258 556
+4 211 376 111 221
+4 188 111 221 390
+4 263 348 276 92
+4 348 276 92 251
+4 99 224 94 276
+4 99 251 276 348
+4 374 193 111 103
+4 423 426 148 362
+4 423 426 362 565
+4 539 339 561 325
+4 380 242 402 396
+4 396 224 242 99
+4 105 218 476 145
+4 576 171 223 222
+4 166 104 115 403
+4 251 371 348 280
+4 99 251 105 100
+4 459 157 405 370
+4 366 363 377 82
+4 485 84 361 78
+4 141 111 384 142
+4 141 111 519 384
+4 385 187 519 141
+4 596 187 385 141
+4 28 190 223 176
+4 176 190 233 564
+4 252 192 398 489
+4 99 348 94 242
+4 106 279 369 368
+4 242 94 224 223
+4 519 167 195 103
+4 246 106 382 279
+4 99 94 348 276
+4 591 403 574 137
+4 283 415 120 591
+4 415 208 591 403
+4 197 169 170 576
+4 576 170 222 169
+4 404 219 245 196
+4 121 390 380 211
+4 376 396 382 250
+4 376 382 297 250
+4 175 174 364 585
+4 116 585 126 130
+4 116 126 134 130
+4 99 280 251 348
+4 581 263 559 359
+4 105 355 476 218
+4 371 355 476 105
+4 95 396 114 375
+4 547 338 356 102
+4 106 347 365 569
+4 379 243 224 276
+4 382 97 244 106
+4 375 250 471 105
+4 110 535 65 454
+4 26 480 511 343
+4 511 26 453 480
+4 27 554 513 374
+4 38 469 330 332
+4 49 139 50 138
+4 383 577 173 200
+4 383 577 24 173
+4 254 532 236 560
+4 532 52 332 236
+4 560 532 236 52
+4 66 234 306 258
+4 514 172 576 539
+4 64 389 70 536
+4 343 555 509 20
+4 17 503 461 143
+4 274 281 191 461
+4 17 143 414 516
+4 143 503 516 17
+4 539 176 14 514
+4 214 288 353 107
+4 398 107 288 214
+4 63 275 467 465
+4 467 275 214 465
+4 235 452 216 429
+4 429 195 216 235
+4 429 488 216 195
+4 197 421 457 576
+4 457 41 576 421
+4 234 264 262 340
+4 355 575 476 218
+4 517 528 173 399
+4 457 197 169 522
+4 451 252 192 107
+4 451 538 252 107
+4 54 169 41 473
+4 525 60 169 257
+4 521 293 537 177
+4 521 203 177 537
+4 557 305 571 579
+4 557 358 109 305
+4 55 557 234 579
+4 520 19 36 215
+4 215 282 188 108
+4 215 282 108 146
+4 76 353 312 360
+4 196 219 40 330
+4 129 41 576 169
+4 93 193 217 194
+4 327 37 217 429
+4 243 359 263 186
+4 409 276 186 243
+4 463 128 460 561
+4 463 561 460 502
+4 194 124 252 96
+4 10 585 383 126
+4 218 483 355 575
+4 67 145 154 436
+4 108 282 188 147
+4 108 273 282 147
+4 18 293 134 528
+4 33 134 18 293
+4 474 252 538 329
+4 537 411 35 577
+4 294 577 35 411
+4 577 594 200 198
+4 577 220 198 43
+4 577 220 594 198
+4 109 465 410 358
+4 410 358 465 290
+4 13 189 386 384
+4 386 13 384 504
+4 387 12 586 364
+4 80 406 362 542
+4 408 123 507 11
+4 215 282 146 19
+4 18 33 287 134
+4 33 177 134 293
+4 358 557 109 566
+4 542 490 362 88
+4 467 322 530 277
+4 538 61 338 77
+4 339 514 30 539
+4 538 102 71 110
+4 71 334 346 110
+4 346 110 538 71
+4 298 578 48 307
+4 343 26 480 196
+4 303 524 49 101
+4 412 483 218 67
+4 348 363 365 377
+4 350 356 405 370
+4 562 567 85 487
+4 562 102 85 567
+4 475 593 357 567
+4 475 372 567 357
+4 137 233 207 380
+4 184 181 229 185
+4 203 594 158 566
+4 144 170 576 197
+4 184 185 213 199
+4 568 184 213 199
+4 219 589 259 236
+4 219 237 236 259
+4 588 227 228 592
+4 228 229 244 592
+4 227 592 229 228
+4 228 588 592 244
+4 224 99 95 379
+4 379 100 99 95
+4 236 254 589 259
+4 473 257 256 493
+4 238 262 592 239
+4 589 238 239 262
+4 239 589 264 106
+4 239 592 588 264
+4 585 168 117 116
+4 99 246 280 348
+4 242 106 97 246
+4 248 573 288 398
+4 278 279 593 573
+4 246 382 250 280
+4 250 280 297 476
+4 261 363 249 348
+4 597 526 569 242
+4 162 124 471 194
+4 162 124 194 96
+4 216 192 231 489
+4 195 217 231 458
+4 215 98 379 583
+4 510 391 526 493
+4 510 261 493 526
+4 564 215 379 95
+4 573 398 102 354
+4 310 28 176 291
+4 104 403 591 137
+4 196 169 391 170
+4 368 593 378 370
+4 363 546 544 365
+4 490 299 368 365
+4 221 114 255 396
+4 144 170 128 171
+4 127 202 131 179
+4 170 127 179 202
+4 131 178 136 179
+4 135 181 184 185
+4 135 184 140 185
+4 185 140 187 184
+4 468 385 488 195
+4 368 377 416 378
+4 206 588 240 239
+4 183 210 568 184
+4 210 184 180 229
+4 200 226 209 205
+4 588 205 227 592
+4 209 210 229 595
+4 210 229 212 213
+4 488 191 216 568
+4 238 226 592 241
+4 226 566 595 592
+4 595 278 214 248
+4 482 248 398 192
+4 200 226 238 594
+4 200 209 183 180
+4 120 574 141 142
+4 142 374 189 182
+4 122 385 596 143
+4 126 383 200 174
+4 590 134 180 200
+4 134 180 183 184
+4 183 461 568 212
+4 461 568 212 191
+4 375 218 100 105
+4 578 145 375 471
+4 268 116 585 506
+4 74 344 444 363
+4 424 444 344 363
+4 475 368 593 378
+4 354 102 593 405
+4 102 405 567 593
+4 171 128 223 222
+4 171 128 176 223
+4 128 223 222 233
+4 176 223 128 233
+4 222 526 242 223
+4 223 242 94 526
+4 221 472 232 93
+4 510 263 261 94
+4 526 261 94 510
+4 99 251 280 105
+4 261 263 344 348
+4 561 176 539 171
+4 176 171 172 539
+4 113 176 128 133
+4 137 381 133 233
+4 381 564 133 233
+4 381 233 121 564
+4 402 181 185 228
+4 265 566 595 203
+4 209 226 566 595
+4 226 204 209 566
+4 594 226 566 204
+4 220 518 306 388
+4 245 239 206 391
+4 589 597 259 569
+4 402 240 228 244
+4 206 228 240 588
+4 237 260 259 391
+4 566 247 214 109
+4 196 391 219 245
+4 208 574 118 120
+4 329 96 253 252
+4 156 596 120 118
+4 497 596 120 156
+4 259 389 565 337
+4 404 175 202 201
+4 573 593 354 102
+4 297 110 102 538
+4 130 590 181 178
+4 118 187 596 141
+4 206 181 402 228
+4 386 497 120 2
+4 487 89 567 550
+4 476 357 297 124
+4 123 189 182 142
+4 374 182 111 587
+4 182 111 587 221
+4 519 199 195 167
+4 330 219 40 332
+4 40 219 237 332
+4 522 54 260 493
+4 223 263 94 243
+4 260 417 389 256
+4 113 133 128 115
+4 189 374 103 513
+4 384 519 103 111
+4 189 103 374 384
+4 54 493 169 473
+4 369 368 377 365
+4 200 205 180 590
+4 236 219 589 388
+4 388 589 391 219
+4 493 260 261 256
+4 111 376 199 167
+4 205 226 227 592
+4 205 201 238 588
+4 209 205 226 227
+4 200 205 201 238
+4 200 594 238 388
+4 205 226 238 200
+4 588 205 592 238
+4 222 169 391 510
+4 254 565 362 336
+4 124 297 538 110
+4 249 545 261 335
+4 256 249 261 335
+4 249 545 363 261
+4 357 593 475 351
+4 108 188 215 255
+4 248 288 573 278
+4 278 354 288 573
+4 120 591 142 123
+4 591 123 120 507
+4 167 195 217 231
+4 103 167 195 217
+4 391 589 240 597
+4 240 597 242 391
+4 404 518 245 219
+4 469 518 404 219
+4 354 356 102 405
+4 168 117 116 118
+4 547 356 459 102
+4 405 102 459 356
+4 565 361 362 336
+4 567 89 475 378
+4 89 550 378 567
+4 574 141 142 136
+4 200 180 183 134
+4 226 592 595 227
+4 209 595 227 226
+4 209 227 595 229
+4 595 214 592 566
+4 595 592 214 278
+4 108 137 121 381
+4 183 212 568 210
+4 388 254 262 589
+4 388 589 262 238
+4 353 356 370 83
+4 229 212 192 595
+4 595 192 248 482
+4 210 212 229 595
+4 213 229 212 192
+4 10 506 126 517
+4 212 213 192 568
+4 210 213 212 568
+4 254 423 340 362
+4 262 362 340 264
+4 196 219 391 40
+4 539 171 172 576
+4 108 255 121 188
+4 226 566 241 594
+4 381 564 121 146
+4 108 121 146 381
+4 130 178 181 136
+4 241 262 234 264
+4 272 106 347 368
+4 272 347 349 368
+4 112 131 364 585
+4 585 130 131 175
+4 337 336 361 565
+4 259 565 336 337
+4 99 250 375 105
+4 223 526 510 222
+4 408 123 108 591
+4 461 281 191 125
+4 497 122 384 596
+4 405 378 157 550
+4 329 252 538 124
+4 349 593 278 279
+4 279 593 368 349
+4 391 589 239 240
+4 273 19 146 282
+4 309 36 215 295
+4 189 123 182 395
+4 306 258 556 66
+4 46 442 527 98
+4 450 442 46 98
+4 563 379 100 276
+4 69 218 476 427
+4 24 517 428 383
+4 359 223 243 263
+4 538 338 356 77
+4 107 356 77 538
+4 145 476 471 124
+4 211 121 390 111
+4 373 113 128 115
+4 363 546 365 377
+4 126 174 200 590
+4 45 566 265 447
+4 447 566 265 203
+4 570 115 112 117
+4 373 494 570 498
+4 243 263 94 276
+4 224 243 94 276
+4 96 162 124 65
+4 54 256 260 493
+4 54 493 473 256
+4 328 70 389 256
+4 197 522 196 169
+4 233 242 224 223
+4 349 367 593 368
+4 169 510 493 391
+4 144 196 245 170
+4 510 257 493 261
+4 219 259 589 391
+4 137 233 121 381
+4 379 100 95 114
+4 583 379 114 100
+4 170 222 128 171
+4 573 354 593 278
+4 509 144 480 127
+4 210 184 229 213
+4 121 255 380 390
+4 559 263 261 510
+4 272 349 278 279
+4 382 279 248 573
+4 244 279 248 382
+4 258 234 262 340
+4 258 340 254 423
+4 371 475 280 476
+4 108 142 591 123
+4 120 591 574 142
+4 505 585 586 428
+4 108 188 123 147
+4 451 192 139 107
+4 235 452 138 216
+4 452 470 138 216
+4 549 91 378 157
+4 498 373 6 420
+4 6 494 373 498
+4 256 70 536 479
+4 447 441 203 265
+4 442 58 243 563
+4 442 58 563 533
+4 155 553 323 129
+4 576 129 172 553
+4 469 518 219 332
+4 236 52 332 518
+4 518 236 388 219
+4 582 43 518 306
+4 523 582 43 518
+4 512 513 13 103
+4 107 139 467 482
+4 521 33 177 534
+4 441 521 534 33
+4 32 146 15 286
+4 414 281 191 274
+4 17 191 461 274
+4 334 102 110 71
+4 267 271 572 251
+4 309 295 215 114
+4 300 289 243 527
+4 291 437 190 598
+4 190 462 564 598
+4 443 534 292 599
+4 453 480 22 12
+4 12 453 480 511
+
+CELL_TYPES 2536
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
diff --git a/examples/pybullet/gym/pybullet_data/checker_grid.jpg b/examples/pybullet/gym/pybullet_data/checker_grid.jpg
new file mode 100644
index 000000000..416550484
Binary files /dev/null and b/examples/pybullet/gym/pybullet_data/checker_grid.jpg differ
diff --git a/examples/pybullet/gym/pybullet_data/cloth_z_up.mtl b/examples/pybullet/gym/pybullet_data/cloth_z_up.mtl
new file mode 100644
index 000000000..8b68eeec8
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/cloth_z_up.mtl
@@ -0,0 +1,13 @@
+# Blender MTL File: 'None'
+# Material Count: 1
+
+newmtl None
+Ns 94.117647
+Ka 1.000000 1.000000 1.000000
+Kd 0.640000 0.640000 0.640000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.000000
+d 1.000000
+illum 2
+map_Kd cube.png
diff --git a/examples/pybullet/gym/pybullet_data/cloth_z_up.obj b/examples/pybullet/gym/pybullet_data/cloth_z_up.obj
new file mode 100644
index 000000000..45cc723a7
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/cloth_z_up.obj
@@ -0,0 +1,89 @@
+# Blender v2.79 (sub 0) OBJ File: ''
+# www.blender.org
+mtllib cloth_z_up.mtl
+o Plane_Plane.001
+v 1.000000 -0.500000 -0.000000
+v 0.500000 -1.000000 -0.000000
+v 0.500000 -0.500000 -0.000000
+v 0.000000 -0.500000 -0.000000
+v -0.500000 -1.000000 -0.000000
+v -0.500000 -0.500000 -0.000000
+v -0.000000 0.500000 0.000000
+v -0.500000 0.000000 -0.000000
+v -0.500000 0.500000 0.000000
+v 1.000000 0.500000 0.000000
+v 0.500000 0.000000 0.000000
+v 0.500000 0.500000 0.000000
+v 0.000000 0.000000 0.000000
+v 0.500000 1.000000 0.000000
+v -0.000000 1.000000 0.000000
+v 1.000000 1.000000 0.000000
+v -1.000000 0.000000 -0.000000
+v -1.000000 0.500000 0.000000
+v -0.500000 1.000000 0.000000
+v -1.000000 1.000000 0.000000
+v -1.000000 -1.000000 -0.000000
+v -1.000000 -0.500000 -0.000000
+v 0.000000 -1.000000 -0.000000
+v 1.000000 0.000000 0.000000
+v 1.000000 -1.000000 -0.000000
+vt 0.976031 1.084981
+vt 0.738016 1.669965
+vt 0.738016 1.084981
+vt 0.499998 1.084981
+vt 0.261984 1.669965
+vt 0.261984 1.084981
+vt 0.499998 -0.084982
+vt 0.261984 0.500000
+vt 0.261984 -0.084982
+vt 0.976031 -0.084982
+vt 0.738016 0.500000
+vt 0.738016 -0.084982
+vt 0.499998 0.500000
+vt 0.738016 -0.669965
+vt 0.499998 -0.669965
+vt 0.976031 -0.669965
+vt 0.023969 0.500000
+vt 0.023969 -0.084982
+vt 0.261984 -0.669965
+vt 0.023969 -0.669965
+vt 0.023969 1.669965
+vt 0.023969 1.084981
+vt 0.499998 1.669965
+vt 0.976031 0.500000
+vt 0.976031 1.669965
+vn 0.0000 0.0000 -1.0000
+usemtl None
+s 1
+f 1/1/1 2/2/1 3/3/1
+f 4/4/1 5/5/1 6/6/1
+f 7/7/1 8/8/1 9/9/1
+f 10/10/1 11/11/1 12/12/1
+f 12/12/1 13/13/1 7/7/1
+f 14/14/1 7/7/1 15/15/1
+f 16/16/1 12/12/1 14/14/1
+f 9/9/1 17/17/1 18/18/1
+f 19/19/1 18/18/1 20/20/1
+f 15/15/1 9/9/1 19/19/1
+f 6/6/1 21/21/1 22/22/1
+f 8/8/1 22/22/1 17/17/1
+f 13/13/1 6/6/1 8/8/1
+f 3/3/1 23/23/1 4/4/1
+f 11/11/1 4/4/1 13/13/1
+f 24/24/1 3/3/1 11/11/1
+f 1/1/1 25/25/1 2/2/1
+f 4/4/1 23/23/1 5/5/1
+f 7/7/1 13/13/1 8/8/1
+f 10/10/1 24/24/1 11/11/1
+f 12/12/1 11/11/1 13/13/1
+f 14/14/1 12/12/1 7/7/1
+f 16/16/1 10/10/1 12/12/1
+f 9/9/1 8/8/1 17/17/1
+f 19/19/1 9/9/1 18/18/1
+f 15/15/1 7/7/1 9/9/1
+f 6/6/1 5/5/1 21/21/1
+f 8/8/1 6/6/1 22/22/1
+f 13/13/1 4/4/1 6/6/1
+f 3/3/1 2/2/1 23/23/1
+f 11/11/1 3/3/1 4/4/1
+f 24/24/1 1/1/1 3/3/1
diff --git a/examples/pybullet/gym/pybullet_data/cloth_z_up.urdf b/examples/pybullet/gym/pybullet_data/cloth_z_up.urdf
new file mode 100644
index 000000000..72b61d2f6
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/cloth_z_up.urdf
@@ -0,0 +1,32 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/examples/pybullet/gym/pybullet_data/configs/__init__.py b/examples/pybullet/gym/pybullet_data/configs/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/examples/pybullet/gym/pybullet_data/configs/laikago_gym_config.gin b/examples/pybullet/gym/pybullet_data/configs/laikago_gym_config.gin
new file mode 100644
index 000000000..3a7095680
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs/laikago_gym_config.gin
@@ -0,0 +1,80 @@
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_config
+import pybullet_envs.minitaur.envs_v2.sensors.robot_sensors
+
+UPPER_BOUND = 6.28318548203
+LOWER_BOUND = -6.28318548203
+SIM_TIME_STEP = 0.001
+NUM_ACTION_REPEAT = 2
+NUM_MOTORS = 12
+NOISY_READING = True
+
+locomotion_gym_config.SimulationParameters.sim_time_step_s = %SIM_TIME_STEP
+locomotion_gym_config.SimulationParameters.num_action_repeat = %NUM_ACTION_REPEAT
+locomotion_gym_config.SimulationParameters.enable_rendering = False
+locomotion_gym_config.LocomotionGymConfig.simulation_parameters = @locomotion_gym_config.SimulationParameters()
+
+robot_sensors.IMUSensor.channels = ["R", "P", "dR", "dP"]
+robot_sensors.IMUSensor.noisy_reading = %NOISY_READING
+robot_sensors.IMUSensor.lower_bound = [-6.28318548203, -6.28318548203,
+ -6283.18554688, -6283.18554688]
+robot_sensors.IMUSensor.upper_bound = [6.28318548203, 6.28318548203,
+ 6283.18554688, 6283.18554688]
+
+robot_sensors.MotorAngleSensor.num_motors = %NUM_MOTORS
+robot_sensors.MotorAngleSensor.noisy_reading = %NOISY_READING
+robot_sensors.MotorAngleSensor.lower_bound = -6.28318548203
+robot_sensors.MotorAngleSensor.upper_bound = 6.28318548203
+
+sensors = [@robot_sensors.IMUSensor(), @robot_sensors.MotorAngleSensor()]
+
+Act0/locomotion_gym_config.ScalarField.name = "motor_angle_0"
+Act0/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act0/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act1/locomotion_gym_config.ScalarField.name = "motor_angle_1"
+Act1/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act1/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act2/locomotion_gym_config.ScalarField.name = "motor_angle_2"
+Act2/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act2/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act3/locomotion_gym_config.ScalarField.name = "motor_angle_3"
+Act3/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act3/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act4/locomotion_gym_config.ScalarField.name = "motor_angle_4"
+Act4/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act4/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act5/locomotion_gym_config.ScalarField.name = "motor_angle_5"
+Act5/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act5/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act6/locomotion_gym_config.ScalarField.name = "motor_angle_6"
+Act6/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act6/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act7/locomotion_gym_config.ScalarField.name = "motor_angle_7"
+Act7/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act7/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act8/locomotion_gym_config.ScalarField.name = "motor_angle_8"
+Act8/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act8/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act9/locomotion_gym_config.ScalarField.name = "motor_angle_9"
+Act9/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act9/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act10/locomotion_gym_config.ScalarField.name = "motor_angle_10"
+Act10/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act10/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act11/locomotion_gym_config.ScalarField.name = "motor_angle_11"
+Act11/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act11/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+
+
+locomotion_gym_config.LocomotionGymConfig.actions = [
+ @Act0/locomotion_gym_config.ScalarField(),
+ @Act1/locomotion_gym_config.ScalarField(),
+ @Act2/locomotion_gym_config.ScalarField(),
+ @Act3/locomotion_gym_config.ScalarField(),
+ @Act4/locomotion_gym_config.ScalarField(),
+ @Act5/locomotion_gym_config.ScalarField(),
+ @Act6/locomotion_gym_config.ScalarField(),
+ @Act7/locomotion_gym_config.ScalarField(),
+ @Act8/locomotion_gym_config.ScalarField(),
+ @Act9/locomotion_gym_config.ScalarField(),
+ @Act10/locomotion_gym_config.ScalarField(),
+ @Act11/locomotion_gym_config.ScalarField()]
diff --git a/examples/pybullet/gym/pybullet_data/configs/laikago_gym_env.gin b/examples/pybullet/gym/pybullet_data/configs/laikago_gym_env.gin
new file mode 100644
index 000000000..0b3b1ce95
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs/laikago_gym_env.gin
@@ -0,0 +1,116 @@
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_env
+import pybullet_envs.minitaur.envs_v2.scenes.simple_scene
+import pybullet_envs.minitaur.envs_v2.tasks.simple_locomotion_task
+import pybullet_envs.minitaur.robots.laikago
+
+URDF_ROOT = "urdf/"
+ABDUCTION_P_GAIN = 220.0
+ABDUCTION_D_GAIN = 0.3
+HIP_P_GAIN = 220.0
+HIP_D_GAIN = 2.0
+KNEE_P_GAIN = 220.0
+KNEE_D_GAIN = 2.0
+
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_config
+import pybullet_envs.minitaur.envs_v2.sensors.robot_sensors
+
+UPPER_BOUND = 6.28318548203
+LOWER_BOUND = -6.28318548203
+SIM_TIME_STEP = 0.001
+NUM_ACTION_REPEAT = 2
+NUM_MOTORS = 12
+NOISY_READING = True
+
+locomotion_gym_config.SimulationParameters.sim_time_step_s = %SIM_TIME_STEP
+locomotion_gym_config.SimulationParameters.num_action_repeat = %NUM_ACTION_REPEAT
+locomotion_gym_config.SimulationParameters.enable_rendering = False
+locomotion_gym_config.LocomotionGymConfig.simulation_parameters = @locomotion_gym_config.SimulationParameters()
+
+robot_sensors.IMUSensor.channels = ["R", "P", "dR", "dP"]
+robot_sensors.IMUSensor.noisy_reading = %NOISY_READING
+robot_sensors.IMUSensor.lower_bound = [-6.28318548203, -6.28318548203,
+ -6283.18554688, -6283.18554688]
+robot_sensors.IMUSensor.upper_bound = [6.28318548203, 6.28318548203,
+ 6283.18554688, 6283.18554688]
+
+robot_sensors.MotorAngleSensor.num_motors = %NUM_MOTORS
+robot_sensors.MotorAngleSensor.noisy_reading = %NOISY_READING
+robot_sensors.MotorAngleSensor.lower_bound = -6.28318548203
+robot_sensors.MotorAngleSensor.upper_bound = 6.28318548203
+
+sensors = [@robot_sensors.IMUSensor(), @robot_sensors.MotorAngleSensor()]
+
+Act0/locomotion_gym_config.ScalarField.name = "motor_angle_0"
+Act0/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act0/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act1/locomotion_gym_config.ScalarField.name = "motor_angle_1"
+Act1/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act1/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act2/locomotion_gym_config.ScalarField.name = "motor_angle_2"
+Act2/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act2/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act3/locomotion_gym_config.ScalarField.name = "motor_angle_3"
+Act3/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act3/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act4/locomotion_gym_config.ScalarField.name = "motor_angle_4"
+Act4/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act4/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act5/locomotion_gym_config.ScalarField.name = "motor_angle_5"
+Act5/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act5/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act6/locomotion_gym_config.ScalarField.name = "motor_angle_6"
+Act6/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act6/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act7/locomotion_gym_config.ScalarField.name = "motor_angle_7"
+Act7/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act7/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act8/locomotion_gym_config.ScalarField.name = "motor_angle_8"
+Act8/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act8/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act9/locomotion_gym_config.ScalarField.name = "motor_angle_9"
+Act9/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act9/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act10/locomotion_gym_config.ScalarField.name = "motor_angle_10"
+Act10/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act10/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act11/locomotion_gym_config.ScalarField.name = "motor_angle_11"
+Act11/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act11/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+
+
+locomotion_gym_config.LocomotionGymConfig.actions = [
+ @Act0/locomotion_gym_config.ScalarField(),
+ @Act1/locomotion_gym_config.ScalarField(),
+ @Act2/locomotion_gym_config.ScalarField(),
+ @Act3/locomotion_gym_config.ScalarField(),
+ @Act4/locomotion_gym_config.ScalarField(),
+ @Act5/locomotion_gym_config.ScalarField(),
+ @Act6/locomotion_gym_config.ScalarField(),
+ @Act7/locomotion_gym_config.ScalarField(),
+ @Act8/locomotion_gym_config.ScalarField(),
+ @Act9/locomotion_gym_config.ScalarField(),
+ @Act10/locomotion_gym_config.ScalarField(),
+ @Act11/locomotion_gym_config.ScalarField()]
+
+
+
+laikago.Laikago.urdf_root = %URDF_ROOT
+laikago.Laikago.time_step = %SIM_TIME_STEP
+laikago.Laikago.action_repeat = %NUM_ACTION_REPEAT
+laikago.Laikago.self_collision_enabled = False
+laikago.Laikago.control_latency = 0.002
+laikago.Laikago.pd_latency = 0.0
+laikago.Laikago.motor_kp = [%ABDUCTION_P_GAIN, %HIP_P_GAIN, %KNEE_P_GAIN,
+ %ABDUCTION_P_GAIN, %HIP_P_GAIN, %KNEE_P_GAIN,
+ %ABDUCTION_P_GAIN, %HIP_P_GAIN, %KNEE_P_GAIN,
+ %ABDUCTION_P_GAIN, %HIP_P_GAIN, %KNEE_P_GAIN]
+laikago.Laikago.motor_kd = [%ABDUCTION_D_GAIN, %HIP_D_GAIN, %KNEE_D_GAIN,
+ %ABDUCTION_D_GAIN, %HIP_D_GAIN, %KNEE_D_GAIN,
+ %ABDUCTION_D_GAIN, %HIP_D_GAIN, %KNEE_D_GAIN,
+ %ABDUCTION_D_GAIN, %HIP_D_GAIN, %KNEE_D_GAIN]
+laikago.Laikago.sensors = %sensors
+
+locomotion_gym_env.LocomotionGymEnv.gym_config = @locomotion_gym_config.LocomotionGymConfig()
+locomotion_gym_env.LocomotionGymEnv.robot_class = @laikago.Laikago
+locomotion_gym_env.LocomotionGymEnv.scene = @simple_scene.SimpleScene()
+locomotion_gym_env.LocomotionGymEnv.task = @simple_locomotion_task.SimpleForwardTask()
diff --git a/examples/pybullet/gym/pybullet_data/configs/laikago_mpc_example_flat.gin b/examples/pybullet/gym/pybullet_data/configs/laikago_mpc_example_flat.gin
new file mode 100644
index 000000000..df735f43f
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs/laikago_mpc_example_flat.gin
@@ -0,0 +1,149 @@
+#-*-Python-*-
+
+# NOTE: Should be run with >=10CPU for decent performance.
+
+import pybullet_envs.minitaur.agents.baseline_controller.torque_stance_leg_controller
+import pybullet_envs.minitaur.envs_v2.env_loader
+import pybullet_envs.minitaur.envs_v2.env_wrappers.mpc_locomotion_wrapper
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_config
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_env
+import pybullet_envs.minitaur.envs_v2.scenes.simple_scene
+import pybullet_envs.minitaur.envs_v2.sensors.camera_sensor
+import pybullet_envs.minitaur.envs_v2.sensors.imu_sensor
+import pybullet_envs.minitaur.envs_v2.sensors.last_action_sensor
+import pybullet_envs.minitaur.envs_v2.sensors.toe_position_sensor
+import pybullet_envs.minitaur.envs_v2.tasks.simple_locomotion_task
+import pybullet_envs.minitaur.envs_v2.tasks.terminal_conditions
+import pybullet_envs.minitaur.envs_v2.utilities.noise_generators
+import pybullet_envs.minitaur.robots.hybrid_motor_model
+import pybullet_envs.minitaur.robots.laikago_v2
+import pybullet_envs.minitaur.robots.robot_config
+
+
+# Configure the dynamic robot
+
+SIM_TIME_STEP = 0.001
+NUM_ACTION_REPEAT = 4 # Control frequency will be 100 Hz
+
+
+########################################
+# Configure the sensors
+########################################
+imu_sensor.IMUSensor.channels = [
+ %imu_sensor.IMUChannel.ROLL,
+ %imu_sensor.IMUChannel.PITCH,
+ %imu_sensor.IMUChannel.ROLL_RATE,
+ %imu_sensor.IMUChannel.PITCH_RATE
+]
+
+imu_sensor.IMUSensor.lower_bound = [-6.28318548203, -6.28318548203, -6283.18554688, -6283.18554688]
+imu_sensor.IMUSensor.upper_bound = [6.28318548203, 6.28318548203, 6283.18554688, 6283.18554688]
+
+# Add noise to the IMU sensor and toe position sensor
+IMUNoise/noise_generators.NormalNoise.scale = (0.025, 0.025, 0.1, 0.1)
+TOENoise/noise_generators.NormalNoise.scale = (0.0025, 0.0025, 0.005, 0.0025, 0.0025, 0.005, 0.0025, 0.0025, 0.005, 0.0025, 0.0025, 0.005)
+imu_sensor.IMUSensor.noise_generator = @IMUNoise/noise_generators.NormalNoise()
+toe_position_sensor.ToePositionSensor.noise_generator = @TOENoise/noise_generators.NormalNoise()
+
+frontCamera/camera_sensor.CameraSensor.camera_translation_from_base = (0.197, 0.0, -0.115)
+frontCamera/camera_sensor.CameraSensor.camera_rotation_from_base = (-0.4996018, 0.4999998, 0.4999998, 0.5003982)
+frontCamera/camera_sensor.CameraSensor.parent_link_id = -1
+frontCamera/camera_sensor.CameraSensor.resolution = (32, 24)
+frontCamera/camera_sensor.CameraSensor.sensor_latency = 0.03
+frontCamera/camera_sensor.CameraSensor.name = "frontCam"
+frontCamera/camera_sensor.CameraSensor.fov_degree = 75
+frontCamera/camera_sensor.CameraSensor.camera_mode = %sim_camera.CameraMode.DEPTH
+frontCamera/camera_sensor.CameraSensor.camera_update_frequency_hz = 30.0
+
+rearCamera/camera_sensor.CameraSensor.camera_translation_from_base = (-0.092, 0.0, -0.105)
+rearCamera/camera_sensor.CameraSensor.camera_rotation_from_base = (-0.4996018, 0.4999998, 0.4999998, 0.5003982)
+rearCamera/camera_sensor.CameraSensor.parent_link_id = -1
+rearCamera/camera_sensor.CameraSensor.resolution = (32, 24)
+rearCamera/camera_sensor.CameraSensor.sensor_latency = 0.03
+rearCamera/camera_sensor.CameraSensor.name = "rearCam"
+rearCamera/camera_sensor.CameraSensor.fov_degree = 75
+rearCamera/camera_sensor.CameraSensor.camera_mode = %sim_camera.CameraMode.DEPTH
+rearCamera/camera_sensor.CameraSensor.camera_update_frequency_hz = 30.0
+
+sensors = [@imu_sensor.IMUSensor(), @last_action_sensor.LastActionSensor(), @toe_position_sensor.ToePositionSensor(), @frontCamera/camera_sensor.CameraSensor(), @rearCamera/camera_sensor.CameraSensor()]
+laikago_v2.Laikago.sensors = %sensors
+
+
+########################################
+# Specify the motor model and its parameters
+########################################
+LAIKAGO_MOTOR_ANGLE_UPPER_LIMITS = 6.28318548203
+LAIKAGO_MOTOR_ANGLE_LOWER_LIMITS = -6.28318548203
+laikago/robot_config.MotorLimits.angle_lower_limits = %LAIKAGO_MOTOR_ANGLE_LOWER_LIMITS
+laikago/robot_config.MotorLimits.angle_upper_limits = %LAIKAGO_MOTOR_ANGLE_UPPER_LIMITS
+laikago/robot_config.MotorLimits.torque_lower_limits = -30
+laikago/robot_config.MotorLimits.torque_upper_limits = 30
+laikago_v2.Laikago.motor_limits = @laikago/robot_config.MotorLimits()
+laikago_v2.Laikago.motor_control_mode = %robot_config.MotorControlMode.HYBRID
+laikago_v2.Laikago.motor_model_class = @hybrid_motor_model.HybridMotorModel
+locomotion_gym_env.LocomotionGymEnv.robot_class = @laikago_v2.Laikago
+hybrid_motor_model.HybridMotorModel.kp = 250
+hybrid_motor_model.HybridMotorModel.kd = (0.3, 2.0, 2.0, 0.3, 2.0, 2.0, 0.3, 2.0, 2.0, 0.3, 2.0, 2.0)
+
+
+########################################
+# Setup the terrain randomization and simulation parameters
+########################################
+
+locomotion_gym_config.LocomotionGymConfig.simulation_parameters = @locomotion_gym_config.SimulationParameters()
+locomotion_gym_env.LocomotionGymEnv.robot_class = @laikago_v2.Laikago
+locomotion_gym_env.LocomotionGymEnv.task = @simple_locomotion_task.SimpleForwardTask()
+locomotion_gym_env.LocomotionGymEnv.gym_config = @locomotion_gym_config.LocomotionGymConfig()
+locomotion_gym_env.LocomotionGymEnv.scene = @simple_scene.SimpleScene()
+locomotion_gym_config.SimulationParameters.sim_time_step_s = %SIM_TIME_STEP
+locomotion_gym_config.SimulationParameters.num_action_repeat = %NUM_ACTION_REPEAT
+
+
+########################################
+# Setup the task and terminal condition parameters
+########################################
+terminal_conditions.maxstep_terminal_condition.max_step = 2000
+terminal_conditions.default_terminal_condition_for_laikago_v2.max_roll = 0.25
+terminal_conditions.default_terminal_condition_for_laikago_v2.max_pitch = 1.0
+terminal_conditions.default_terminal_condition_for_laikago_v2.min_height = 0.15
+terminal_conditions.default_terminal_condition_for_laikago_v2.enforce_foot_contacts = True
+
+# Setup the terminal condition
+terminal_conditions.logical_any_terminal_condition.conditions = [
+ @terminal_conditions.default_terminal_condition_for_laikago_v2,
+ @terminal_conditions.maxstep_terminal_condition,
+ ]
+
+simple_locomotion_task.SimpleForwardTask.terminal_condition = @terminal_conditions.logical_any_terminal_condition
+
+env_loader.load.wrapper_classes = [
+ @mpc_locomotion_wrapper.MPCLocomotionWrapper,
+]
+
+########################################
+# Configure the MPC-related parameters
+########################################
+torque_stance_leg_controller.TorqueStanceLegController.qp_weights = (5, 5, 0.2, 0, 0, 10, 0.5, 0.5, 0.2, 0.2, 0.2, 0.1, 0)
+torque_stance_leg_controller.TorqueStanceLegController.body_inertia = (0.183375, 0, 0, 0, 0.6267, 0, 0, 0, 0.636175)
+torque_stance_leg_controller.TorqueStanceLegController.friction_coeffs = (0.45, 0.45, 0.45, 0.45)
+
+########################################
+# Configure the foothold wrapper parameters and action space
+########################################
+mpc_locomotion_wrapper.MPCLocomotionWrapper.foot_friction_coeff = 1.0
+mpc_locomotion_wrapper.MPCLocomotionWrapper.locomotion_gait = %mpc_locomotion_wrapper.Gait.TROT
+mpc_locomotion_wrapper.MPCLocomotionWrapper.control_frequency=20
+mpc_locomotion_wrapper.MPCLocomotionWrapper.target_horizontal_com_velocity_heuristic = @mpc_locomotion_wrapper.InverseRaibertTargetHorizontalComVelocityHeuristic()
+mpc_locomotion_wrapper.InverseRaibertTargetHorizontalComVelocityHeuristic.gains = (-0.25, -0.1)
+
+mpc_locomotion_wrapper.MPCLocomotionWrapper.swing_target_action_range = ((-0.05, -0.05, -0.03), (0.1, 0.05, 0.03))
+mpc_locomotion_wrapper.MPCLocomotionWrapper.pitch_action_range = (-0.2, 0.2)
+mpc_locomotion_wrapper.MPCLocomotionWrapper.roll_action_range = (-0.05, 0.05)
+mpc_locomotion_wrapper.MPCLocomotionWrapper.base_velocity_action_range = ((-0.05, -0.05), (0.05, 0.05))
+mpc_locomotion_wrapper.MPCLocomotionWrapper.base_twist_action_range = (-0.2, 0.2)
+mpc_locomotion_wrapper.MPCLocomotionWrapper.base_height_action_range = (0.42, 0.48)
+mpc_locomotion_wrapper.MPCLocomotionWrapper.swing_clearance_action_range = (0.05, 0.1)
+
+imu_based_com_velocity_estimator.IMUBasedCOMVelocityEstimator.use_sensor_interface = False
+
+
diff --git a/examples/pybullet/gym/pybullet_data/configs/laikago_mpc_stepstone.gin b/examples/pybullet/gym/pybullet_data/configs/laikago_mpc_stepstone.gin
new file mode 100644
index 000000000..2644d3b7b
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs/laikago_mpc_stepstone.gin
@@ -0,0 +1,142 @@
+#-*-Python-*-
+
+# NOTE: Should be run with >=10CPU for decent performance.
+
+import pybullet_envs.minitaur.agents.baseline_controller.torque_stance_leg_controller
+import pybullet_envs.minitaur.envs_v2.env_loader
+import pybullet_envs.minitaur.envs_v2.env_wrappers.mpc_locomotion_wrapper
+import pybullet_envs.minitaur.envs_v2.env_wrappers.observation_dictionary_to_array_wrapper
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_config
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_env
+import pybullet_envs.minitaur.envs_v2.scenes.random_stepstone_scene
+import pybullet_envs.minitaur.envs_v2.sensors.imu_sensor
+import pybullet_envs.minitaur.envs_v2.sensors.last_action_sensor
+import pybullet_envs.minitaur.envs_v2.sensors.toe_position_sensor
+import pybullet_envs.minitaur.envs_v2.tasks.simple_locomotion_task
+import pybullet_envs.minitaur.envs_v2.tasks.terminal_conditions
+import pybullet_envs.minitaur.envs_v2.utilities.noise_generators
+import pybullet_envs.minitaur.robots.hybrid_motor_model
+import pybullet_envs.minitaur.robots.laikago_v2
+import pybullet_envs.minitaur.robots.robot_config
+
+
+# Configure the dynamic robot
+
+SIM_TIME_STEP = 0.001
+NUM_ACTION_REPEAT = 4 # Control frequency will be 100 Hz
+
+
+########################################
+# Configure the sensors
+########################################
+imu_sensor.IMUSensor.channels = [
+ %imu_sensor.IMUChannel.ROLL,
+ %imu_sensor.IMUChannel.PITCH,
+ %imu_sensor.IMUChannel.ROLL_RATE,
+ %imu_sensor.IMUChannel.PITCH_RATE
+]
+
+imu_sensor.IMUSensor.lower_bound = [-6.28318548203, -6.28318548203, -6283.18554688, -6283.18554688]
+imu_sensor.IMUSensor.upper_bound = [6.28318548203, 6.28318548203, 6283.18554688, 6283.18554688]
+
+# Add noise to the IMU sensor and toe position sensor
+# IMUNoise/noise_generators.NormalNoise.scale = (0.025, 0.025, 0.1, 0.1)
+# TOENoise/noise_generators.NormalNoise.scale = (0.0025, 0.0025, 0.005, 0.0025, 0.0025, 0.005, 0.0025, 0.0025, 0.005, 0.0025, 0.0025, 0.005)
+# imu_sensor.IMUSensor.noise_generator = @IMUNoise/noise_generators.NormalNoise()
+# toe_position_sensor.ToePositionSensor.noise_generator = @TOENoise/noise_generators.NormalNoise()
+
+
+sensors = [@imu_sensor.IMUSensor(), @last_action_sensor.LastActionSensor(), @toe_position_sensor.ToePositionSensor()]
+laikago_v2.Laikago.sensors = %sensors
+
+
+########################################
+# Specify the motor model and its parameters
+########################################
+LAIKAGO_MOTOR_ANGLE_UPPER_LIMITS = 6.28318548203
+LAIKAGO_MOTOR_ANGLE_LOWER_LIMITS = -6.28318548203
+laikago/robot_config.MotorLimits.angle_lower_limits = %LAIKAGO_MOTOR_ANGLE_LOWER_LIMITS
+laikago/robot_config.MotorLimits.angle_upper_limits = %LAIKAGO_MOTOR_ANGLE_UPPER_LIMITS
+laikago/robot_config.MotorLimits.torque_lower_limits = -30
+laikago/robot_config.MotorLimits.torque_upper_limits = 30
+laikago_v2.Laikago.motor_limits = @laikago/robot_config.MotorLimits()
+laikago_v2.Laikago.motor_control_mode = %robot_config.MotorControlMode.HYBRID
+laikago_v2.Laikago.motor_model_class = @hybrid_motor_model.HybridMotorModel
+locomotion_gym_env.LocomotionGymEnv.robot_class = @laikago_v2.Laikago
+hybrid_motor_model.HybridMotorModel.kp = 250
+hybrid_motor_model.HybridMotorModel.kd = (0.3, 2.0, 2.0, 0.3, 2.0, 2.0, 0.3, 2.0, 2.0, 0.3, 2.0, 2.0)
+
+
+########################################
+# Setup the terrain randomization and simulation parameters
+########################################
+
+locomotion_gym_config.LocomotionGymConfig.simulation_parameters = @locomotion_gym_config.SimulationParameters()
+locomotion_gym_env.LocomotionGymEnv.robot_class = @laikago_v2.Laikago
+locomotion_gym_env.LocomotionGymEnv.task = @simple_locomotion_task.SimpleForwardTask()
+locomotion_gym_env.LocomotionGymEnv.gym_config = @locomotion_gym_config.LocomotionGymConfig()
+locomotion_gym_env.LocomotionGymEnv.scene = @random_stepstone_scene.RandomStepstoneScene()
+locomotion_gym_config.SimulationParameters.sim_time_step_s = %SIM_TIME_STEP
+locomotion_gym_config.SimulationParameters.num_action_repeat = %NUM_ACTION_REPEAT
+
+random_stepstone_scene.RandomStepstoneScene.random_seed = 13
+random_stepstone_scene.RandomStepstoneScene.gap_length_lower_bound = 0.07
+random_stepstone_scene.RandomStepstoneScene.gap_length_upper_bound = 0.15
+random_stepstone_scene.RandomStepstoneScene.stone_width = 0.6
+
+########################################
+# Setup the task and terminal condition parameters
+########################################
+terminal_conditions.maxstep_terminal_condition.max_step = 2000
+terminal_conditions.default_terminal_condition_for_laikago_v2.max_roll = 0.25
+terminal_conditions.default_terminal_condition_for_laikago_v2.max_pitch = 1.0
+terminal_conditions.default_terminal_condition_for_laikago_v2.min_height = 0.15
+terminal_conditions.default_terminal_condition_for_laikago_v2.enforce_foot_contacts = True
+
+# Setup the terminal condition
+terminal_conditions.logical_any_terminal_condition.conditions = [
+ @terminal_conditions.default_terminal_condition_for_laikago_v2,
+ @terminal_conditions.maxstep_terminal_condition,
+ ]
+
+ simple_locomotion_task.SimpleForwardTask.terminal_condition = @terminal_conditions.logical_any_terminal_condition
+simple_locomotion_task.SimpleForwardTask.clip_velocity = 0.0015
+
+time_ordered_buffer.TimeOrderedBuffer.error_on_duplicate_timestamp = False
+time_ordered_buffer.TimeOrderedBuffer.replace_value_on_duplicate_timestamp = True
+time_ordered_buffer.TimeOrderedBuffer.error_on_timestamp_reversal = False
+
+
+observation_dictionary_to_array_wrapper.ObservationDictionaryToArrayWrapper.observation_excluded = ('frontCam', 'rearCam')
+
+env_loader.load.wrapper_classes = [
+ @mpc_locomotion_wrapper.MPCLocomotionWrapper,
+ @observation_dictionary_to_array_wrapper.ObservationDictionaryToArrayWrapper,
+]
+
+########################################
+# Configure the MPC-related parameters
+########################################
+torque_stance_leg_controller.TorqueStanceLegController.qp_weights = (5, 5, 0.2, 0, 0, 10, 0.5, 0.5, 0.2, 0.2, 0.2, 0.1, 0)
+torque_stance_leg_controller.TorqueStanceLegController.body_inertia = (0.183375, 0, 0, 0, 0.6267, 0, 0, 0, 0.636175)
+torque_stance_leg_controller.TorqueStanceLegController.friction_coeffs = (0.45, 0.45, 0.45, 0.45)
+
+########################################
+# Configure the foothold wrapper parameters and action space
+########################################
+mpc_locomotion_wrapper.MPCLocomotionWrapper.foot_friction_coeff = 1.0
+mpc_locomotion_wrapper.MPCLocomotionWrapper.locomotion_gait = %mpc_locomotion_wrapper.Gait.TROT
+mpc_locomotion_wrapper.MPCLocomotionWrapper.control_frequency=20
+mpc_locomotion_wrapper.MPCLocomotionWrapper.target_horizontal_com_velocity_heuristic = @mpc_locomotion_wrapper.InverseRaibertTargetHorizontalComVelocityHeuristic()
+mpc_locomotion_wrapper.InverseRaibertTargetHorizontalComVelocityHeuristic.gains = (-0.25, -0.1)
+
+mpc_locomotion_wrapper.MPCLocomotionWrapper.swing_target_action_range = ((-0.05, -0.05, -0.03), (0.1, 0.05, 0.03))
+mpc_locomotion_wrapper.MPCLocomotionWrapper.pitch_action_range = (-0.2, 0.2)
+mpc_locomotion_wrapper.MPCLocomotionWrapper.roll_action_range = (-0.05, 0.05)
+mpc_locomotion_wrapper.MPCLocomotionWrapper.base_velocity_action_range = ((-0.05, -0.05), (0.05, 0.05))
+mpc_locomotion_wrapper.MPCLocomotionWrapper.base_twist_action_range = (-0.2, 0.2)
+mpc_locomotion_wrapper.MPCLocomotionWrapper.base_height_action_range = (0.42, 0.48)
+mpc_locomotion_wrapper.MPCLocomotionWrapper.swing_clearance_action_range = (0.05, 0.1)
+
+imu_based_com_velocity_estimator.IMUBasedCOMVelocityEstimator.use_sensor_interface = False
+
diff --git a/examples/pybullet/gym/pybullet_data/configs/laikago_reactive.gin b/examples/pybullet/gym/pybullet_data/configs/laikago_reactive.gin
new file mode 100644
index 000000000..b4d6f8ba0
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs/laikago_reactive.gin
@@ -0,0 +1,133 @@
+#-*-Python-*-
+
+# NOTE: Should be run with >=10CPU for decent performance.
+
+import pybullet_envs.minitaur.envs_v2.env_loader
+import pybullet_envs.minitaur.envs_v2.env_wrappers.observation_dictionary_to_array_wrapper
+import pybullet_envs.minitaur.envs_v2.env_wrappers.simple_openloop
+import pybullet_envs.minitaur.envs_v2.env_wrappers.trajectory_generator_wrapper_env
+
+
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_env
+import pybullet_envs.minitaur.envs_v2.scenes.simple_scene
+import pybullet_envs.minitaur.envs_v2.tasks.simple_locomotion_task
+import pybullet_envs.minitaur.robots.laikago
+
+URDF_ROOT = "urdf/"
+ABDUCTION_P_GAIN = 220.0
+ABDUCTION_D_GAIN = 0.3
+HIP_P_GAIN = 220.0
+HIP_D_GAIN = 2.0
+KNEE_P_GAIN = 220.0
+KNEE_D_GAIN = 2.0
+
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_config
+import pybullet_envs.minitaur.envs_v2.sensors.robot_sensors
+
+UPPER_BOUND = 6.28318548203
+LOWER_BOUND = -6.28318548203
+SIM_TIME_STEP = 0.001
+NUM_ACTION_REPEAT = 2
+NUM_MOTORS = 12
+NOISY_READING = True
+
+locomotion_gym_config.SimulationParameters.sim_time_step_s = %SIM_TIME_STEP
+locomotion_gym_config.SimulationParameters.num_action_repeat = %NUM_ACTION_REPEAT
+locomotion_gym_config.SimulationParameters.enable_rendering = False
+locomotion_gym_config.LocomotionGymConfig.simulation_parameters = @locomotion_gym_config.SimulationParameters()
+
+robot_sensors.IMUSensor.channels = ["R", "P", "dR", "dP"]
+robot_sensors.IMUSensor.noisy_reading = %NOISY_READING
+robot_sensors.IMUSensor.lower_bound = [-6.28318548203, -6.28318548203,
+ -6283.18554688, -6283.18554688]
+robot_sensors.IMUSensor.upper_bound = [6.28318548203, 6.28318548203,
+ 6283.18554688, 6283.18554688]
+
+robot_sensors.MotorAngleSensor.num_motors = %NUM_MOTORS
+robot_sensors.MotorAngleSensor.noisy_reading = %NOISY_READING
+robot_sensors.MotorAngleSensor.lower_bound = -6.28318548203
+robot_sensors.MotorAngleSensor.upper_bound = 6.28318548203
+
+sensors = [@robot_sensors.IMUSensor(), @robot_sensors.MotorAngleSensor()]
+
+Act0/locomotion_gym_config.ScalarField.name = "motor_angle_0"
+Act0/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act0/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act1/locomotion_gym_config.ScalarField.name = "motor_angle_1"
+Act1/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act1/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act2/locomotion_gym_config.ScalarField.name = "motor_angle_2"
+Act2/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act2/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act3/locomotion_gym_config.ScalarField.name = "motor_angle_3"
+Act3/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act3/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act4/locomotion_gym_config.ScalarField.name = "motor_angle_4"
+Act4/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act4/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act5/locomotion_gym_config.ScalarField.name = "motor_angle_5"
+Act5/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act5/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act6/locomotion_gym_config.ScalarField.name = "motor_angle_6"
+Act6/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act6/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act7/locomotion_gym_config.ScalarField.name = "motor_angle_7"
+Act7/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act7/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act8/locomotion_gym_config.ScalarField.name = "motor_angle_8"
+Act8/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act8/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act9/locomotion_gym_config.ScalarField.name = "motor_angle_9"
+Act9/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act9/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act10/locomotion_gym_config.ScalarField.name = "motor_angle_10"
+Act10/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act10/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act11/locomotion_gym_config.ScalarField.name = "motor_angle_11"
+Act11/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act11/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+
+
+locomotion_gym_config.LocomotionGymConfig.actions = [
+ @Act0/locomotion_gym_config.ScalarField(),
+ @Act1/locomotion_gym_config.ScalarField(),
+ @Act2/locomotion_gym_config.ScalarField(),
+ @Act3/locomotion_gym_config.ScalarField(),
+ @Act4/locomotion_gym_config.ScalarField(),
+ @Act5/locomotion_gym_config.ScalarField(),
+ @Act6/locomotion_gym_config.ScalarField(),
+ @Act7/locomotion_gym_config.ScalarField(),
+ @Act8/locomotion_gym_config.ScalarField(),
+ @Act9/locomotion_gym_config.ScalarField(),
+ @Act10/locomotion_gym_config.ScalarField(),
+ @Act11/locomotion_gym_config.ScalarField()]
+
+
+
+laikago.Laikago.urdf_root = %URDF_ROOT
+laikago.Laikago.time_step = %SIM_TIME_STEP
+laikago.Laikago.action_repeat = %NUM_ACTION_REPEAT
+laikago.Laikago.self_collision_enabled = False
+laikago.Laikago.control_latency = 0.002
+laikago.Laikago.pd_latency = 0.0
+laikago.Laikago.motor_kp = [%ABDUCTION_P_GAIN, %HIP_P_GAIN, %KNEE_P_GAIN,
+ %ABDUCTION_P_GAIN, %HIP_P_GAIN, %KNEE_P_GAIN,
+ %ABDUCTION_P_GAIN, %HIP_P_GAIN, %KNEE_P_GAIN,
+ %ABDUCTION_P_GAIN, %HIP_P_GAIN, %KNEE_P_GAIN]
+laikago.Laikago.motor_kd = [%ABDUCTION_D_GAIN, %HIP_D_GAIN, %KNEE_D_GAIN,
+ %ABDUCTION_D_GAIN, %HIP_D_GAIN, %KNEE_D_GAIN,
+ %ABDUCTION_D_GAIN, %HIP_D_GAIN, %KNEE_D_GAIN,
+ %ABDUCTION_D_GAIN, %HIP_D_GAIN, %KNEE_D_GAIN]
+laikago.Laikago.sensors = %sensors
+
+locomotion_gym_env.LocomotionGymEnv.gym_config = @locomotion_gym_config.LocomotionGymConfig()
+locomotion_gym_env.LocomotionGymEnv.robot_class = @laikago.Laikago
+locomotion_gym_env.LocomotionGymEnv.scene = @simple_scene.SimpleScene()
+locomotion_gym_env.LocomotionGymEnv.task = @simple_locomotion_task.SimpleForwardTask()
+
+
+
+trajectory_generator_wrapper_env.TrajectoryGeneratorWrapperEnv.trajectory_generator = @simple_openloop.LaikagoPoseOffsetGenerator()
+env_loader.load.wrapper_classes = [
+ @observation_dictionary_to_array_wrapper.ObservationDictionaryToArrayWrapper,
+ @trajectory_generator_wrapper_env.TrajectoryGeneratorWrapperEnv]
diff --git a/examples/pybullet/gym/pybullet_data/configs/laikago_walk_static_gait.gin b/examples/pybullet/gym/pybullet_data/configs/laikago_walk_static_gait.gin
new file mode 100644
index 000000000..2b6cd680d
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs/laikago_walk_static_gait.gin
@@ -0,0 +1,65 @@
+import pybullet_envs.minitaur.envs_v2.env_loader
+import pybullet_envs.minitaur.envs_v2.env_wrappers.simple_openloop
+import pybullet_envs.minitaur.envs_v2.env_wrappers.trajectory_generator_wrapper_env
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_config
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_env
+import pybullet_envs.minitaur.envs_v2.scenes.simple_scene
+import pybullet_envs.minitaur.envs_v2.sensors.imu_sensor
+import pybullet_envs.minitaur.envs_v2.sensors.motor_angle_sensor
+import pybullet_envs.minitaur.envs_v2.tasks.simple_locomotion_task
+import pybullet_envs.minitaur.robots.hybrid_motor_model
+import pybullet_envs.minitaur.robots.laikago_v2
+import pybullet_envs.minitaur.robots.robot_config
+
+
+# Specify the gym env parameters
+SIM_TIME_STEP = 0.001
+NUM_ACTION_REPEAT = 4
+locomotion_gym_config.SimulationParameters.sim_time_step_s = %SIM_TIME_STEP
+locomotion_gym_config.SimulationParameters.num_action_repeat = %NUM_ACTION_REPEAT
+locomotion_gym_config.SimulationParameters.enable_rendering = False
+locomotion_gym_config.LocomotionGymConfig.simulation_parameters = @locomotion_gym_config.SimulationParameters()
+locomotion_gym_env.LocomotionGymEnv.gym_config = @locomotion_gym_config.LocomotionGymConfig()
+
+# Specify the world.
+URDF_ROOT = ""
+scene_base.SceneBase.data_root = %URDF_ROOT
+locomotion_gym_env.LocomotionGymEnv.scene = @simple_scene.SimpleScene()
+
+# Specify the task.
+locomotion_gym_env.LocomotionGymEnv.task = @simple_locomotion_task.SimpleForwardTask()
+
+# Specify the sensors. Sensors determine the observation space.
+# Sensors can either be mounted on robots (see below), or passed to envs
+# i.e. like ambient sensors, or provided by tasks (task specific measures).
+imu_sensor.IMUSensor.channels = [
+ %imu_sensor.IMUChannel.ROLL,
+ %imu_sensor.IMUChannel.PITCH,
+ %imu_sensor.IMUChannel.ROLL_RATE,
+ %imu_sensor.IMUChannel.PITCH_RATE,
+]
+
+imu_sensor.IMUSensor.lower_bound = [-6.28318548203, -6.28318548203,
+ -6283.18554688, -6283.18554688]
+imu_sensor.IMUSensor.upper_bound = [6.28318548203, 6.28318548203,
+ 6283.18554688, 6283.18554688]
+
+# We use the default confirugration for MotorAngleSensor, which reads limits from the robot.
+SENSORS = [@imu_sensor.IMUSensor(), @motor_angle_sensor.MotorAngleSensor()]
+
+# Specify the motor limits, and motor control mode.
+LAIKAGO_MOTOR_ANGLE_UPPER_LIMITS = 6.28318548203
+LAIKAGO_MOTOR_ANGLE_LOWER_LIMITS = -6.28318548203
+laikago/robot_config.MotorLimits.angle_lower_limits = %LAIKAGO_MOTOR_ANGLE_LOWER_LIMITS
+laikago/robot_config.MotorLimits.angle_upper_limits = %LAIKAGO_MOTOR_ANGLE_UPPER_LIMITS
+laikago_v2.Laikago.motor_limits = @laikago/robot_config.MotorLimits()
+laikago_v2.Laikago.motor_control_mode = %robot_config.MotorControlMode.POSITION
+laikago_v2.Laikago.motor_model_class = @hybrid_motor_model.HybridMotorModel
+locomotion_gym_env.LocomotionGymEnv.robot_class = @laikago_v2.Laikago
+
+# Specify the motor model parameters. Notice that we don't need to specify the control mode or motor limits here.
+hybrid_motor_model.HybridMotorModel.kp = 2400
+hybrid_motor_model.HybridMotorModel.kd = 5
+
+# Finally, mount sensors specified above to the Laikago.
+laikago_v2.Laikago.sensors = %SENSORS
diff --git a/examples/pybullet/gym/pybullet_data/configs/minitaur_gym_config.gin b/examples/pybullet/gym/pybullet_data/configs/minitaur_gym_config.gin
new file mode 100644
index 000000000..4b268980d
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs/minitaur_gym_config.gin
@@ -0,0 +1,48 @@
+#-*-Python-*-
+
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_config
+import pybullet_envs.minitaur.envs_v2.sensors.imu_sensor
+import pybullet_envs.minitaur.envs_v2.sensors.motor_angle_sensor
+import pybullet_envs.minitaur.robots.minitaur_motor_model_v2
+import pybullet_envs.minitaur.robots.minitaur_v2
+import pybullet_envs.minitaur.robots.robot_config
+
+
+UPPER_BOUND = 1.0
+LOWER_BOUND = -1.0
+SIM_TIME_STEP = 0.001
+NUM_ACTION_REPEAT = 20
+NUM_MOTORS = 8
+NOISY_READING = True
+SENSOR_LATENCY = 0.02
+
+locomotion_gym_config.SimulationParameters.sim_time_step_s = %SIM_TIME_STEP
+locomotion_gym_config.SimulationParameters.num_action_repeat = %NUM_ACTION_REPEAT
+locomotion_gym_config.SimulationParameters.enable_rendering = False
+locomotion_gym_config.LocomotionGymConfig.simulation_parameters = @locomotion_gym_config.SimulationParameters()
+
+robot_config.MotorLimits.angle_lower_limits = %LOWER_BOUND
+robot_config.MotorLimits.angle_upper_limits = %UPPER_BOUND
+
+minitaur_v2.Minitaur.motor_control_mode = %robot_config.MotorControlMode.POSITION
+minitaur_v2.Minitaur.motor_limits = @robot_config.MotorLimits()
+minitaur_v2.Minitaur.motor_model_class = @minitaur_motor_model_v2.MinitaurMotorModel
+minitaur_motor_model_v2.MinitaurMotorModel.pd_latency = 0.003
+minitaur_motor_model_v2.MinitaurMotorModel.kp = 1.0
+minitaur_motor_model_v2.MinitaurMotorModel.kd = 0.015
+
+imu_sensor.IMUSensor.channels = [
+ %imu_sensor.IMUChannel.ROLL,
+ %imu_sensor.IMUChannel.PITCH,
+ %imu_sensor.IMUChannel.ROLL_RATE,
+ %imu_sensor.IMUChannel.PITCH_RATE
+]
+imu_sensor.IMUSensor.lower_bound = [-6.28318548203, -6.28318548203,
+ -6283.18554688, -6283.18554688]
+imu_sensor.IMUSensor.upper_bound = [6.28318548203, 6.28318548203,
+ 6283.18554688, 6283.18554688]
+
+motor_angle_sensor.MotorAngleSensor.sensor_latency = %SENSOR_LATENCY
+motor_angle_sensor.MotorAngleSensor.lower_bound = -6.28318548203
+motor_angle_sensor.MotorAngleSensor.upper_bound = 6.28318548203
+minitaur_v2.Minitaur.sensors = [@imu_sensor.IMUSensor(), @motor_angle_sensor.MotorAngleSensor()]
diff --git a/examples/pybullet/gym/pybullet_data/configs/minitaur_gym_env.gin b/examples/pybullet/gym/pybullet_data/configs/minitaur_gym_env.gin
new file mode 100644
index 000000000..4b5140f0f
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs/minitaur_gym_env.gin
@@ -0,0 +1,63 @@
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_env
+import pybullet_envs.minitaur.envs_v2.scenes.simple_scene
+import pybullet_envs.minitaur.envs_v2.tasks.simple_locomotion_task
+import pybullet_envs.minitaur.robots.minitaur_v2
+import pybullet_envs.minitaur.robots.robot_config
+import pybullet_envs.minitaur.robots.robot_urdf_loader
+
+URDF_ROOT = "urdf/"
+#-*-Python-*-
+
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_config
+import pybullet_envs.minitaur.envs_v2.sensors.imu_sensor
+import pybullet_envs.minitaur.envs_v2.sensors.motor_angle_sensor
+import pybullet_envs.minitaur.robots.minitaur_motor_model_v2
+import pybullet_envs.minitaur.robots.minitaur_v2
+import pybullet_envs.minitaur.robots.robot_config
+
+
+UPPER_BOUND = 1.0
+LOWER_BOUND = -1.0
+SIM_TIME_STEP = 0.001
+NUM_ACTION_REPEAT = 20
+NUM_MOTORS = 8
+NOISY_READING = True
+SENSOR_LATENCY = 0.02
+
+locomotion_gym_config.SimulationParameters.sim_time_step_s = %SIM_TIME_STEP
+locomotion_gym_config.SimulationParameters.num_action_repeat = %NUM_ACTION_REPEAT
+locomotion_gym_config.SimulationParameters.enable_rendering = False
+locomotion_gym_config.LocomotionGymConfig.simulation_parameters = @locomotion_gym_config.SimulationParameters()
+
+robot_config.MotorLimits.angle_lower_limits = %LOWER_BOUND
+robot_config.MotorLimits.angle_upper_limits = %UPPER_BOUND
+
+minitaur_v2.Minitaur.motor_control_mode = %robot_config.MotorControlMode.POSITION
+minitaur_v2.Minitaur.motor_limits = @robot_config.MotorLimits()
+minitaur_v2.Minitaur.motor_model_class = @minitaur_motor_model_v2.MinitaurMotorModel
+minitaur_motor_model_v2.MinitaurMotorModel.pd_latency = 0.003
+minitaur_motor_model_v2.MinitaurMotorModel.kp = 1.0
+minitaur_motor_model_v2.MinitaurMotorModel.kd = 0.015
+
+imu_sensor.IMUSensor.channels = [
+ %imu_sensor.IMUChannel.ROLL,
+ %imu_sensor.IMUChannel.PITCH,
+ %imu_sensor.IMUChannel.ROLL_RATE,
+ %imu_sensor.IMUChannel.PITCH_RATE
+]
+imu_sensor.IMUSensor.lower_bound = [-6.28318548203, -6.28318548203,
+ -6283.18554688, -6283.18554688]
+imu_sensor.IMUSensor.upper_bound = [6.28318548203, 6.28318548203,
+ 6283.18554688, 6283.18554688]
+
+motor_angle_sensor.MotorAngleSensor.sensor_latency = %SENSOR_LATENCY
+motor_angle_sensor.MotorAngleSensor.lower_bound = -6.28318548203
+motor_angle_sensor.MotorAngleSensor.upper_bound = 6.28318548203
+minitaur_v2.Minitaur.sensors = [@imu_sensor.IMUSensor(), @motor_angle_sensor.MotorAngleSensor()]
+
+
+locomotion_gym_env.LocomotionGymEnv.gym_config = @locomotion_gym_config.LocomotionGymConfig()
+locomotion_gym_env.LocomotionGymEnv.robot_class = @minitaur_v2.Minitaur
+locomotion_gym_env.LocomotionGymEnv.scene = @simple_scene.SimpleScene()
+locomotion_gym_env.LocomotionGymEnv.task = @simple_locomotion_task.SimpleForwardTask()
+
diff --git a/examples/pybullet/gym/pybullet_data/configs_v2/__init__.py b/examples/pybullet/gym/pybullet_data/configs_v2/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/examples/pybullet/gym/pybullet_data/configs_v2/base/__init__.py b/examples/pybullet/gym/pybullet_data/configs_v2/base/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/examples/pybullet/gym/pybullet_data/configs_v2/base/laikago_reactive.gin b/examples/pybullet/gym/pybullet_data/configs_v2/base/laikago_reactive.gin
new file mode 100644
index 000000000..030bf4d8f
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs_v2/base/laikago_reactive.gin
@@ -0,0 +1,80 @@
+import pybullet_envs.minitaur.envs_v2.env_loader
+import pybullet_envs.minitaur.envs_v2.env_wrappers.observation_dictionary_to_array_wrapper
+import pybullet_envs.minitaur.envs_v2.env_wrappers.simple_openloop
+import pybullet_envs.minitaur.envs_v2.env_wrappers.trajectory_generator_wrapper_env
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_config
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_env
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_env
+import pybullet_envs.minitaur.envs_v2.scenes.simple_scene
+import pybullet_envs.minitaur.envs_v2.sensors.accelerometer_sensor
+import pybullet_envs.minitaur.envs_v2.sensors.imu_sensor
+import pybullet_envs.minitaur.envs_v2.sensors.motor_angle_sensor
+import pybullet_envs.minitaur.envs_v2.tasks.simple_locomotion_task
+import pybullet_envs.minitaur.envs_v2.tasks.terminal_conditions
+import pybullet_envs.minitaur.robots.hybrid_motor_model
+import pybullet_envs.minitaur.robots.laikago_v2
+import pybullet_envs.minitaur.robots.time_ordered_buffer
+
+
+# Specify the gym env parameters
+SIM_TIME_STEP = 0.001
+NUM_ACTION_REPEAT = 2
+
+locomotion_gym_config.SimulationParameters.sim_time_step_s = %SIM_TIME_STEP
+locomotion_gym_config.SimulationParameters.num_action_repeat = %NUM_ACTION_REPEAT
+locomotion_gym_config.SimulationParameters.enable_rendering = False
+locomotion_gym_config.LocomotionGymConfig.simulation_parameters = @locomotion_gym_config.SimulationParameters()
+locomotion_gym_env.LocomotionGymEnv.gym_config = @locomotion_gym_config.LocomotionGymConfig()
+
+
+# Specify the robot
+locomotion_gym_env.LocomotionGymEnv.robot_class = @laikago_v2.Laikago
+LAIKAGO_MOTOR_ANGLE_UPPER_LIMITS = 6.28318548203
+LAIKAGO_MOTOR_ANGLE_LOWER_LIMITS = -6.28318548203
+laikago/robot_config.MotorLimits.angle_lower_limits = %LAIKAGO_MOTOR_ANGLE_LOWER_LIMITS
+laikago/robot_config.MotorLimits.angle_upper_limits = %LAIKAGO_MOTOR_ANGLE_UPPER_LIMITS
+laikago_v2.Laikago.motor_limits = @laikago/robot_config.MotorLimits()
+laikago_v2.Laikago.motor_control_mode = %robot_config.MotorControlMode.POSITION
+
+# Specify the motor model parameters. Notice that we don't need to specify the control mode
+# and motor limits here, as they will be passed from the robot interface.
+hybrid_motor_model.HybridMotorModel.kp = 220
+hybrid_motor_model.HybridMotorModel.kd = (0.3, 2.0, 2.0, 0.3, 2.0, 2.0, 0.3, 2.0, 2.0, 0.3, 2.0, 2.0)
+
+# This will make sure the hybrid motor model does not throw error during reset, when the timestamp is alwasy zero.
+time_ordered_buffer.TimeOrderedBuffer.error_on_duplicate_timestamp = False
+time_ordered_buffer.TimeOrderedBuffer.replace_value_on_duplicate_timestamp = True
+
+# Add the sensors
+laikago_v2.Laikago.sensors = [@imu_sensor.IMUSensor(), @motor_angle_sensor.MotorAngleSensor(), @accelerometer_sensor.AccelerometerSensor()]
+imu_sensor.IMUSensor.channels = [
+ %imu_sensor.IMUChannel.ROLL,
+ %imu_sensor.IMUChannel.PITCH,
+ %imu_sensor.IMUChannel.ROLL_RATE,
+ %imu_sensor.IMUChannel.PITCH_RATE,
+]
+
+imu_sensor.IMUSensor.lower_bound = [-6.28318548203, -6.28318548203,
+ -6283.18554688, -6283.18554688]
+imu_sensor.IMUSensor.upper_bound = [6.28318548203, 6.28318548203,
+ 6283.18554688, 6283.18554688]
+
+# Specify the scene
+locomotion_gym_env.LocomotionGymEnv.scene = @simple_scene.SimpleScene()
+simple_scene.SimpleScene.data_root = "third_party/bullet/data"
+
+# Specify the task.
+locomotion_gym_env.LocomotionGymEnv.task = @simple_locomotion_task.SimpleForwardTask()
+simple_locomotion_task.SimpleForwardTask.terminal_condition = @terminal_conditions.default_terminal_condition_for_laikago
+
+accelerometer_sensor.AccelerometerSensor.lower_bound = [-50.0, -50.0, -50.0]
+accelerometer_sensor.AccelerometerSensor.upper_bound = [50.0, 50.0, 50.0]
+accelerometer_sensor.AccelerometerSensor.sensor_latency = 0.01
+imu_sensor.IMUSensor.sensor_latency = 0.1
+
+
+# Define the wrappers needed
+env_loader.load.wrapper_classes = [
+ @trajectory_generator_wrapper_env.TrajectoryGeneratorWrapperEnv,
+]
+trajectory_generator_wrapper_env.TrajectoryGeneratorWrapperEnv.trajectory_generator = @simple_openloop.LaikagoPoseOffsetGenerator()
diff --git a/examples/pybullet/gym/pybullet_data/configs_v2/base/laikago_with_imu.gin b/examples/pybullet/gym/pybullet_data/configs_v2/base/laikago_with_imu.gin
new file mode 100644
index 000000000..03d7c3308
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs_v2/base/laikago_with_imu.gin
@@ -0,0 +1,66 @@
+#import pybullet_data
+#MYPATH = pybullet_data.getDataPath()+'/configs_v2/robots/laikago.gin'
+#MYPATH = 'D:/dev/bullet3/examples/pybullet\gym/pybullet_data/configs_v2/robots/laikago.gin'
+#include '$MYPATH/ambiguous.gin'
+
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_config
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_env
+import pybullet_envs.minitaur.envs_v2.scenes.scene_base
+import pybullet_envs.minitaur.envs_v2.scenes.simple_scene
+import pybullet_envs.minitaur.envs_v2.sensors.imu_sensor
+import pybullet_envs.minitaur.envs_v2.sensors.motor_angle_sensor
+import pybullet_envs.minitaur.robots.hybrid_motor_model
+import pybullet_envs.minitaur.robots.laikago_v2
+import pybullet_envs.minitaur.robots.robot_config
+
+URDF_ROOT = ""
+UPPER_BOUND = 6.28318548203
+LOWER_BOUND = -6.28318548203
+SIM_TIME_STEP = 0.001
+NUM_ACTION_REPEAT = 2
+
+locomotion_gym_config.SimulationParameters.sim_time_step_s = %SIM_TIME_STEP
+locomotion_gym_config.SimulationParameters.num_action_repeat = %NUM_ACTION_REPEAT
+locomotion_gym_config.SimulationParameters.enable_rendering = False
+locomotion_gym_config.LocomotionGymConfig.simulation_parameters = @locomotion_gym_config.SimulationParameters()
+
+imu_sensor.IMUSensor.channels = [
+ %imu_sensor.IMUChannel.ROLL,
+ %imu_sensor.IMUChannel.PITCH,
+ %imu_sensor.IMUChannel.ROLL_RATE,
+ %imu_sensor.IMUChannel.PITCH_RATE,
+]
+
+imu_sensor.IMUSensor.lower_bound = [-6.28318548203, -6.28318548203,
+ -6283.18554688, -6283.18554688]
+imu_sensor.IMUSensor.upper_bound = [6.28318548203, 6.28318548203,
+ 6283.18554688, 6283.18554688]
+
+# We use the default confirugration for MotorAngleSensor, which reads limits from the robot.
+SENSORS = [@imu_sensor.IMUSensor(), @motor_angle_sensor.MotorAngleSensor()]
+locomotion_gym_config.SimulationParameters.sim_time_step_s = %SIM_TIME_STEP
+locomotion_gym_config.SimulationParameters.num_action_repeat = %NUM_ACTION_REPEAT
+locomotion_gym_config.SimulationParameters.enable_rendering = False
+locomotion_gym_config.LocomotionGymConfig.simulation_parameters = @locomotion_gym_config.SimulationParameters()
+locomotion_gym_env.LocomotionGymEnv.gym_config = @locomotion_gym_config.LocomotionGymConfig()
+
+# Specify the scene.
+scene_base.SceneBase.data_root = %URDF_ROOT
+locomotion_gym_env.LocomotionGymEnv.scene = @simple_scene.SimpleScene()
+
+# Specify the motor limits, and motor control mode.
+laikago/robot_config.MotorLimits.angle_lower_limits = %LOWER_BOUND
+laikago/robot_config.MotorLimits.angle_upper_limits = %UPPER_BOUND
+laikago_v2.Laikago.motor_limits = @laikago/robot_config.MotorLimits()
+laikago_v2.Laikago.motor_control_mode = %robot_config.MotorControlMode.POSITION
+laikago_v2.Laikago.motor_model_class = @hybrid_motor_model.HybridMotorModel
+locomotion_gym_env.LocomotionGymEnv.robot_class = @laikago_v2.Laikago
+
+# Specify the motor model parameters. Notice that we don't need to specify the control mode or motor limits here.
+hybrid_motor_model.HybridMotorModel.kp = 220
+hybrid_motor_model.HybridMotorModel.kd = 2
+
+laikago_v2.Laikago.sensors = %SENSORS
+
+
+laikago_v2.Laikago.sensors = [@imu_sensor.IMUSensor()]
diff --git a/examples/pybullet/gym/pybullet_data/configs_v2/base/mini_cheetah_with_imu.gin b/examples/pybullet/gym/pybullet_data/configs_v2/base/mini_cheetah_with_imu.gin
new file mode 100644
index 000000000..d7ebe63f7
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs_v2/base/mini_cheetah_with_imu.gin
@@ -0,0 +1,9 @@
+import pybullet_data as pd
+
+include pd.getDataPath()+'/configs_v2/robots/mini_cheetah.gin'
+include pd.getDataPath()+'/configs_v2/sensors/imu.gin'
+
+#include 'robotics/reinforcement_learning/minitaur/envs_v2/configs_v2/robots/mini_cheetah.gin'
+#include 'robotics/reinforcement_learning/minitaur/envs_v2/configs_v2/sensors/imu.gin'
+
+mini_cheetah.MiniCheetah.sensors = [@robot_sensors.IMUSensor()]
diff --git a/examples/pybullet/gym/pybullet_data/configs_v2/robots/__init__.py b/examples/pybullet/gym/pybullet_data/configs_v2/robots/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/examples/pybullet/gym/pybullet_data/configs_v2/robots/laikago.gin b/examples/pybullet/gym/pybullet_data/configs_v2/robots/laikago.gin
new file mode 100644
index 000000000..a639c83c9
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs_v2/robots/laikago.gin
@@ -0,0 +1,58 @@
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_config
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_env
+import pybullet_envs.minitaur.envs_v2.scenes.scene_base
+import pybullet_envs.minitaur.envs_v2.scenes.simple_scene
+import pybullet_envs.minitaur.envs_v2.sensors.imu_sensor
+import pybullet_envs.minitaur.envs_v2.sensors.motor_angle_sensor
+import pybullet_envs.minitaur.robots.hybrid_motor_model
+import pybullet_envs.minitaur.robots.laikago_v2
+import pybullet_envs.minitaur.robots.robot_config
+
+URDF_ROOT = ""
+UPPER_BOUND = 6.28318548203
+LOWER_BOUND = -6.28318548203
+SIM_TIME_STEP = 0.001
+NUM_ACTION_REPEAT = 2
+
+locomotion_gym_config.SimulationParameters.sim_time_step_s = %SIM_TIME_STEP
+locomotion_gym_config.SimulationParameters.num_action_repeat = %NUM_ACTION_REPEAT
+locomotion_gym_config.SimulationParameters.enable_rendering = False
+locomotion_gym_config.LocomotionGymConfig.simulation_parameters = @locomotion_gym_config.SimulationParameters()
+
+imu_sensor.IMUSensor.channels = [
+ %imu_sensor.IMUChannel.ROLL,
+ %imu_sensor.IMUChannel.PITCH,
+ %imu_sensor.IMUChannel.ROLL_RATE,
+ %imu_sensor.IMUChannel.PITCH_RATE,
+]
+
+imu_sensor.IMUSensor.lower_bound = [-6.28318548203, -6.28318548203,
+ -6283.18554688, -6283.18554688]
+imu_sensor.IMUSensor.upper_bound = [6.28318548203, 6.28318548203,
+ 6283.18554688, 6283.18554688]
+
+# We use the default confirugration for MotorAngleSensor, which reads limits from the robot.
+SENSORS = [@imu_sensor.IMUSensor(), @motor_angle_sensor.MotorAngleSensor()]
+locomotion_gym_config.SimulationParameters.sim_time_step_s = %SIM_TIME_STEP
+locomotion_gym_config.SimulationParameters.num_action_repeat = %NUM_ACTION_REPEAT
+locomotion_gym_config.SimulationParameters.enable_rendering = False
+locomotion_gym_config.LocomotionGymConfig.simulation_parameters = @locomotion_gym_config.SimulationParameters()
+locomotion_gym_env.LocomotionGymEnv.gym_config = @locomotion_gym_config.LocomotionGymConfig()
+
+# Specify the scene.
+scene_base.SceneBase.data_root = %URDF_ROOT
+locomotion_gym_env.LocomotionGymEnv.scene = @simple_scene.SimpleScene()
+
+# Specify the motor limits, and motor control mode.
+laikago/robot_config.MotorLimits.angle_lower_limits = %LOWER_BOUND
+laikago/robot_config.MotorLimits.angle_upper_limits = %UPPER_BOUND
+laikago_v2.Laikago.motor_limits = @laikago/robot_config.MotorLimits()
+laikago_v2.Laikago.motor_control_mode = %robot_config.MotorControlMode.POSITION
+laikago_v2.Laikago.motor_model_class = @hybrid_motor_model.HybridMotorModel
+locomotion_gym_env.LocomotionGymEnv.robot_class = @laikago_v2.Laikago
+
+# Specify the motor model parameters. Notice that we don't need to specify the control mode or motor limits here.
+hybrid_motor_model.HybridMotorModel.kp = 220
+hybrid_motor_model.HybridMotorModel.kd = 2
+
+laikago_v2.Laikago.sensors = %SENSORS
diff --git a/examples/pybullet/gym/pybullet_data/configs_v2/robots/mini_cheetah.gin b/examples/pybullet/gym/pybullet_data/configs_v2/robots/mini_cheetah.gin
new file mode 100644
index 000000000..7433da2f8
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs_v2/robots/mini_cheetah.gin
@@ -0,0 +1,98 @@
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_config
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_env
+import pybullet_envs.minitaur.robots.mini_cheetah
+import pybullet_data as pd
+
+URDF_ROOT = pd.getDataPath()+"/urdf/"
+
+ABDUCTION_P_GAIN = 100.0
+ABDUCTION_D_GAIN = 1.0
+HIP_P_GAIN = 30
+HIP_D_GAIN = 2.0
+KNEE_P_GAIN = 50
+KNEE_D_GAIN = 2.0
+
+
+UPPER_BOUND = 6.28318548203
+LOWER_BOUND = -6.28318548203
+SIM_TIME_STEP = 0.001
+NUM_ACTION_REPEAT = 2
+NUM_MOTORS = 12
+NOISY_READING = True
+
+locomotion_gym_config.SimulationParameters.sim_time_step_s = %SIM_TIME_STEP
+locomotion_gym_config.SimulationParameters.num_action_repeat = %NUM_ACTION_REPEAT
+locomotion_gym_config.SimulationParameters.enable_rendering = False
+locomotion_gym_config.LocomotionGymConfig.simulation_parameters = @locomotion_gym_config.SimulationParameters()
+
+Act0/locomotion_gym_config.ScalarField.name = "motor_angle_0"
+Act0/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act0/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act1/locomotion_gym_config.ScalarField.name = "motor_angle_1"
+Act1/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act1/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act2/locomotion_gym_config.ScalarField.name = "motor_angle_2"
+Act2/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act2/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act3/locomotion_gym_config.ScalarField.name = "motor_angle_3"
+Act3/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act3/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act4/locomotion_gym_config.ScalarField.name = "motor_angle_4"
+Act4/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act4/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act5/locomotion_gym_config.ScalarField.name = "motor_angle_5"
+Act5/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act5/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act6/locomotion_gym_config.ScalarField.name = "motor_angle_6"
+Act6/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act6/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act7/locomotion_gym_config.ScalarField.name = "motor_angle_7"
+Act7/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act7/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act8/locomotion_gym_config.ScalarField.name = "motor_angle_8"
+Act8/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act8/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act9/locomotion_gym_config.ScalarField.name = "motor_angle_9"
+Act9/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act9/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act10/locomotion_gym_config.ScalarField.name = "motor_angle_10"
+Act10/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act10/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act11/locomotion_gym_config.ScalarField.name = "motor_angle_11"
+Act11/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act11/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+
+
+locomotion_gym_config.LocomotionGymConfig.actions = [
+ @Act0/locomotion_gym_config.ScalarField(),
+ @Act1/locomotion_gym_config.ScalarField(),
+ @Act2/locomotion_gym_config.ScalarField(),
+ @Act3/locomotion_gym_config.ScalarField(),
+ @Act4/locomotion_gym_config.ScalarField(),
+ @Act5/locomotion_gym_config.ScalarField(),
+ @Act6/locomotion_gym_config.ScalarField(),
+ @Act7/locomotion_gym_config.ScalarField(),
+ @Act8/locomotion_gym_config.ScalarField(),
+ @Act9/locomotion_gym_config.ScalarField(),
+ @Act10/locomotion_gym_config.ScalarField(),
+ @Act11/locomotion_gym_config.ScalarField()]
+
+locomotion_gym_env.LocomotionGymEnv.gym_config = @locomotion_gym_config.LocomotionGymConfig()
+
+mini_cheetah.MiniCheetah.urdf_root = %URDF_ROOT
+mini_cheetah.MiniCheetah.time_step = %SIM_TIME_STEP
+mini_cheetah.MiniCheetah.action_repeat = %NUM_ACTION_REPEAT
+mini_cheetah.MiniCheetah.self_collision_enabled = False
+mini_cheetah.MiniCheetah.control_latency = 0.002
+mini_cheetah.MiniCheetah.pd_latency = 0.0
+mini_cheetah.MiniCheetah.motor_kp = [%ABDUCTION_P_GAIN, %HIP_P_GAIN, %KNEE_P_GAIN,
+ %ABDUCTION_P_GAIN, %HIP_P_GAIN, %KNEE_P_GAIN,
+ %ABDUCTION_P_GAIN, %HIP_P_GAIN, %KNEE_P_GAIN,
+ %ABDUCTION_P_GAIN, %HIP_P_GAIN, %KNEE_P_GAIN]
+mini_cheetah.MiniCheetah.motor_kd = [%ABDUCTION_D_GAIN, %HIP_D_GAIN, %KNEE_D_GAIN,
+ %ABDUCTION_D_GAIN, %HIP_D_GAIN, %KNEE_D_GAIN,
+ %ABDUCTION_D_GAIN, %HIP_D_GAIN, %KNEE_D_GAIN,
+ %ABDUCTION_D_GAIN, %HIP_D_GAIN, %KNEE_D_GAIN]
+
+locomotion_gym_env.LocomotionGymEnv.robot_class = @mini_cheetah.MiniCheetah
+
diff --git a/examples/pybullet/gym/pybullet_data/configs_v2/robots/minitaur.gin b/examples/pybullet/gym/pybullet_data/configs_v2/robots/minitaur.gin
new file mode 100644
index 000000000..091483380
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs_v2/robots/minitaur.gin
@@ -0,0 +1,31 @@
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_config
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_env
+import pybullet_envs.minitaur.robots.minitaur_motor_model_v2
+import pybullet_envs.minitaur.robots.minitaur_v2
+
+URDF_ROOT = "robotics/reinforcement_learning/minitaur/robots/data/urdf/"
+
+UPPER_BOUND = 6.28318548203
+LOWER_BOUND = -6.28318548203
+SIM_TIME_STEP = 0.001
+NUM_ACTION_REPEAT = 6
+NUM_MOTORS = 8
+NOISY_READING = True
+
+locomotion_gym_config.SimulationParameters.sim_time_step_s = %SIM_TIME_STEP
+locomotion_gym_config.SimulationParameters.num_action_repeat = %NUM_ACTION_REPEAT
+locomotion_gym_config.SimulationParameters.enable_rendering = False
+locomotion_gym_config.LocomotionGymConfig.simulation_parameters = @locomotion_gym_config.SimulationParameters()
+locomotion_gym_env.LocomotionGymEnv.gym_config = @locomotion_gym_config.LocomotionGymConfig()
+
+minitaur_v2.Minitaur.motor_control_mode = %robot_config.MotorControlMode.POSITION
+minitaur_v2.Minitaur.motor_limits = @robot_config.MotorLimits()
+minitaur_v2.Minitaur.motor_model_class = @minitaur_motor_model_v2.MinitaurMotorModel
+minitaur_motor_model_v2.MinitaurMotorModel.pd_latency = 0.003
+minitaur_motor_model_v2.MinitaurMotorModel.kp = 1.0
+minitaur_motor_model_v2.MinitaurMotorModel.kd = 0.015
+
+locomotion_gym_env.LocomotionGymEnv.robot_class = @minitaur_v2.Minitaur
+
+robot_config.MotorLimits.angle_lower_limits = %LOWER_BOUND
+robot_config.MotorLimits.angle_upper_limits = %UPPER_BOUND
diff --git a/examples/pybullet/gym/pybullet_data/configs_v2/scenes/__init__.py b/examples/pybullet/gym/pybullet_data/configs_v2/scenes/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/examples/pybullet/gym/pybullet_data/configs_v2/scenes/simple_scene.gin b/examples/pybullet/gym/pybullet_data/configs_v2/scenes/simple_scene.gin
new file mode 100644
index 000000000..6cb12467a
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs_v2/scenes/simple_scene.gin
@@ -0,0 +1,4 @@
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_env
+import pybullet_envs.minitaur.envs_v2.scenes.simple_scene
+
+locomotion_gym_env.LocomotionGymEnv.scene = @simple_scene.SimpleScene()
diff --git a/examples/pybullet/gym/pybullet_data/configs_v2/scenes/stair_scene.gin b/examples/pybullet/gym/pybullet_data/configs_v2/scenes/stair_scene.gin
new file mode 100644
index 000000000..134f5774f
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs_v2/scenes/stair_scene.gin
@@ -0,0 +1,3 @@
+import pybullet_envs.minitaur.envs_v2.scenes.stair_scene
+# Specify the scene (overwrite the setting from laikago_reactive.gin)
+locomotion_gym_env.LocomotionGymEnv.scene = @stair_scene.StairScene()
diff --git a/examples/pybullet/gym/pybullet_data/configs_v2/tasks/__init__.py b/examples/pybullet/gym/pybullet_data/configs_v2/tasks/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/examples/pybullet/gym/pybullet_data/configs_v2/tasks/fwd_task.gin b/examples/pybullet/gym/pybullet_data/configs_v2/tasks/fwd_task.gin
new file mode 100644
index 000000000..316431710
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs_v2/tasks/fwd_task.gin
@@ -0,0 +1,4 @@
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_env
+import pybullet_envs.minitaur.envs_v2.tasks.simple_locomotion_task
+
+locomotion_gym_env.LocomotionGymEnv.task = @simple_locomotion_task.SimpleForwardTask()
diff --git a/examples/pybullet/gym/pybullet_data/configs_v2/tasks/fwd_task_laikago.gin b/examples/pybullet/gym/pybullet_data/configs_v2/tasks/fwd_task_laikago.gin
new file mode 100644
index 000000000..59a50e4e7
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs_v2/tasks/fwd_task_laikago.gin
@@ -0,0 +1,8 @@
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_env
+import pybullet_envs.minitaur.envs_v2.tasks.simple_locomotion_task
+
+locomotion_gym_env.LocomotionGymEnv.task = @simple_locomotion_task.SimpleForwardTask()
+
+simple_locomotion_task.SimpleForwardTask.energy_penalty_coef = 0.002
+simple_locomotion_task.SimpleForwardTask.min_com_height = 0.3
+simple_locomotion_task.SimpleForwardTask.clip_velocity = 0.002
diff --git a/examples/pybullet/gym/pybullet_data/configs_v2/tasks/fwd_task_no_termination.gin b/examples/pybullet/gym/pybullet_data/configs_v2/tasks/fwd_task_no_termination.gin
new file mode 100644
index 000000000..5fc8189ca
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs_v2/tasks/fwd_task_no_termination.gin
@@ -0,0 +1,11 @@
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_env
+import pybullet_envs.minitaur.envs_v2.tasks.simple_locomotion_task
+import pybullet_envs.minitaur.envs_v2.tasks.terminal_conditions
+
+terminal_conditions.maxstep_terminal_condition.max_step = 1500
+# Setup the terminal condition to not to terminate early when the robot falls.
+terminal_conditions.logical_any_terminal_condition.conditions = [
+ @terminal_conditions.maxstep_terminal_condition,
+ ]
+simple_locomotion_task.SimpleForwardTask.terminal_condition = @terminal_conditions.logical_any_terminal_condition
+locomotion_gym_env.LocomotionGymEnv.task = @simple_locomotion_task.SimpleForwardTask()
diff --git a/examples/pybullet/gym/pybullet_data/configs_v2/tasks/fwd_task_no_termination_simplified.gin b/examples/pybullet/gym/pybullet_data/configs_v2/tasks/fwd_task_no_termination_simplified.gin
new file mode 100644
index 000000000..c7d9630d5
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs_v2/tasks/fwd_task_no_termination_simplified.gin
@@ -0,0 +1,12 @@
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_env
+import pybullet_envs.minitaur.envs_v2.tasks.simple_locomotion_task
+import pybullet_envs.minitaur.envs_v2.tasks.terminal_conditions
+
+terminal_conditions.maxstep_terminal_condition.max_step = 1500
+simple_locomotion_task.SimpleForwardTask.terminal_condition = @terminal_conditions.default_terminal_condition_for_laikago_v2
+
+simple_locomotion_task.SimpleForwardTask.divide_with_dt = True
+simple_locomotion_task.SimpleForwardTask.clip_velocity = 0.4
+simple_locomotion_task.SimpleForwardTask.energy_penalty_coef = 0
+
+locomotion_gym_env.LocomotionGymEnv.task = @simple_locomotion_task.SimpleForwardTask()
diff --git a/examples/pybullet/gym/pybullet_data/configs_v2/wrappers/__init__.py b/examples/pybullet/gym/pybullet_data/configs_v2/wrappers/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/examples/pybullet/gym/pybullet_data/configs_v2/wrappers/pmtg_wrapper.gin b/examples/pybullet/gym/pybullet_data/configs_v2/wrappers/pmtg_wrapper.gin
new file mode 100644
index 000000000..f9466a316
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs_v2/wrappers/pmtg_wrapper.gin
@@ -0,0 +1,18 @@
+import pybullet_envs.minitaur.envs_v2.env_loader
+import pybullet_envs.minitaur.envs_v2.env_wrappers.observation_dictionary_to_array_wrapper
+import pybullet_envs.minitaur.envs_v2.env_wrappers.pmtg_wrapper_env
+
+pmtg_wrapper_env.PmtgWrapperEnv.action_filter_enable = True
+pmtg_wrapper_env.PmtgWrapperEnv.action_filter_enable = 1
+pmtg_wrapper_env.PmtgWrapperEnv.intensity_upper_bound = 1.0
+pmtg_wrapper_env.PmtgWrapperEnv.max_delta_time = 4.0
+pmtg_wrapper_env.PmtgWrapperEnv.min_delta_time = 2.0
+pmtg_wrapper_env.PmtgWrapperEnv.residual_range = 0.35
+pmtg_wrapper_env.PmtgWrapperEnv.integrator_coupling_mode = "all coupled"
+pmtg_wrapper_env.PmtgWrapperEnv.walk_height_coupling_mode = "all coupled"
+pmtg_wrapper_env.PmtgWrapperEnv.variable_swing_stance_ratio = 1
+pmtg_wrapper_env.PmtgWrapperEnv.init_gait = "walk"
+
+env_loader.load.wrapper_classes = [
+ @pmtg_wrapper_env.PmtgWrapperEnv,
+ @observation_dictionary_to_array_wrapper.ObservationDictionaryToArrayWrapper]
diff --git a/examples/pybullet/gym/pybullet_data/configs_v2/wrappers/pmtg_wrapper_dict.gin b/examples/pybullet/gym/pybullet_data/configs_v2/wrappers/pmtg_wrapper_dict.gin
new file mode 100644
index 000000000..74251e31c
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs_v2/wrappers/pmtg_wrapper_dict.gin
@@ -0,0 +1,6 @@
+import pybullet_envs.minitaur.envs_v2.env_loader
+import pybullet_envs.minitaur.envs_v2.env_wrappers.pmtg_wrapper_env
+
+pmtg_wrapper_env.PmtgWrapperEnv.action_filter_enable = True
+
+env_loader.load.wrapper_classes = [@pmtg_wrapper_env.PmtgWrapperEnv]
diff --git a/examples/pybullet/gym/pybullet_data/configs_v2/wrappers/pmtg_wrapper_laikago.gin b/examples/pybullet/gym/pybullet_data/configs_v2/wrappers/pmtg_wrapper_laikago.gin
new file mode 100644
index 000000000..8f46b1591
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs_v2/wrappers/pmtg_wrapper_laikago.gin
@@ -0,0 +1,10 @@
+import pybullet_envs.minitaur.envs_v2.env_loader
+import pybullet_envs.minitaur.envs_v2.env_wrappers.observation_dictionary_to_array_wrapper
+import pybullet_envs.minitaur.envs_v2.env_wrappers.pmtg_wrapper_env
+
+pmtg_wrapper_env.PmtgWrapperEnv.action_filter_enable = True
+pmtg_wrapper_env.PmtgWrapperEnv.action_filter_high_cut = 0.5
+
+env_loader.load.wrapper_classes = [
+ @pmtg_wrapper_env.PmtgWrapperEnv,
+ @observation_dictionary_to_array_wrapper.ObservationDictionaryToArrayWrapper]
diff --git a/examples/pybullet/gym/pybullet_data/configs_v2/wrappers/pmtg_wrapper_simplified_env.gin b/examples/pybullet/gym/pybullet_data/configs_v2/wrappers/pmtg_wrapper_simplified_env.gin
new file mode 100644
index 000000000..f6736cb09
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs_v2/wrappers/pmtg_wrapper_simplified_env.gin
@@ -0,0 +1,18 @@
+import pybullet_envs.minitaur.envs_v2.env_loader
+import pybullet_envs.minitaur.envs_v2.env_wrappers.observation_dictionary_to_array_wrapper
+import pybullet_envs.minitaur.envs_v2.env_wrappers.pmtg_wrapper_env
+
+
+pmtg_wrapper_env.PmtgWrapperEnv.init_leg_phase_offsets = [0, 0.5, 0.5, 0]
+pmtg_wrapper_env.PmtgWrapperEnv.integrator_coupling_mode = 'all coupled'
+pmtg_wrapper_env.PmtgWrapperEnv.intensity_upper_bound = 0.5
+pmtg_wrapper_env.PmtgWrapperEnv.max_delta_time = 4
+pmtg_wrapper_env.PmtgWrapperEnv.min_delta_time = 1
+pmtg_wrapper_env.PmtgWrapperEnv.residual_range = 0.2
+pmtg_wrapper_env.PmtgWrapperEnv.variable_swing_stance_ratio = False
+pmtg_wrapper_env.PmtgWrapperEnv.walk_height_coupling_mode = 'null'
+pmtg_wrapper_env.PmtgWrapperEnv.action_filter_high_cut = 0.5
+
+env_loader.load.wrapper_classes = [
+ @pmtg_wrapper_env.PmtgWrapperEnv,
+ @observation_dictionary_to_array_wrapper.ObservationDictionaryToArrayWrapper]
diff --git a/examples/pybullet/gym/pybullet_data/cube.urdf b/examples/pybullet/gym/pybullet_data/cube.urdf
new file mode 100644
index 000000000..83207f9b6
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/cube.urdf
@@ -0,0 +1,32 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/examples/pybullet/gym/pybullet_data/cube_collisionfilter.urdf b/examples/pybullet/gym/pybullet_data/cube_collisionfilter.urdf
new file mode 100644
index 000000000..1e4add6fe
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/cube_collisionfilter.urdf
@@ -0,0 +1,29 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/examples/pybullet/gym/pybullet_data/pickup2.zip b/examples/pybullet/gym/pybullet_data/pickup2.zip
new file mode 100644
index 000000000..31405cfbd
Binary files /dev/null and b/examples/pybullet/gym/pybullet_data/pickup2.zip differ
diff --git a/examples/pybullet/gym/pybullet_data/sphere_1cm.urdf b/examples/pybullet/gym/pybullet_data/sphere_1cm.urdf
new file mode 100644
index 000000000..e5d3b838c
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/sphere_1cm.urdf
@@ -0,0 +1,22 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/examples/pybullet/gym/pybullet_data/sphere_transparent.urdf b/examples/pybullet/gym/pybullet_data/sphere_transparent.urdf
new file mode 100644
index 000000000..3dfd98034
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/sphere_transparent.urdf
@@ -0,0 +1,32 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/examples/pybullet/gym/pybullet_data/stone.mtl b/examples/pybullet/gym/pybullet_data/stone.mtl
new file mode 100644
index 000000000..70d3ba1da
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/stone.mtl
@@ -0,0 +1,10 @@
+# Blender MTL File: 'None'
+# Material Count: 1
+
+newmtl None
+Ns 0
+Ka 0.000000 0.000000 0.000000
+Kd 0.8 0.8 0.8
+Ks 0.8 0.8 0.8
+d 1
+illum 2
diff --git a/examples/pybullet/gym/pybullet_data/stone.obj b/examples/pybullet/gym/pybullet_data/stone.obj
new file mode 100644
index 000000000..0fbe8c5f3
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/stone.obj
@@ -0,0 +1,32 @@
+# Blender v2.78 (sub 0) OBJ File: ''
+# www.blender.org
+mtllib stone.mtl
+o Cube
+v -0.246350 -0.246483 -0.000624
+v -0.151407 -0.176325 0.172867
+v -0.246350 0.249205 -0.000624
+v -0.151407 0.129477 0.172867
+v 0.249338 -0.246483 -0.000624
+v 0.154395 -0.176325 0.172867
+v 0.249338 0.249205 -0.000624
+v 0.154395 0.129477 0.172867
+vn -0.8772 0.0000 0.4801
+vn 0.0000 0.8230 0.5680
+vn 0.8772 0.0000 0.4801
+vn 0.0000 -0.9271 0.3749
+vn 0.0000 0.0000 -1.0000
+vn 0.0000 0.0000 1.0000
+usemtl None
+s off
+f 1//1 4//1 3//1
+f 4//2 7//2 3//2
+f 8//3 5//3 7//3
+f 6//4 1//4 5//4
+f 7//5 1//5 3//5
+f 4//6 6//6 8//6
+f 1//1 2//1 4//1
+f 4//2 8//2 7//2
+f 8//3 6//3 5//3
+f 6//4 2//4 1//4
+f 7//5 5//5 1//5
+f 4//6 2//6 6//6
diff --git a/examples/pybullet/gym/pybullet_data/teddy_large.urdf b/examples/pybullet/gym/pybullet_data/teddy_large.urdf
new file mode 100644
index 000000000..2dc7d7879
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/teddy_large.urdf
@@ -0,0 +1,32 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/examples/pybullet/gym/pybullet_data/terrain.obj b/examples/pybullet/gym/pybullet_data/terrain.obj
new file mode 100644
index 000000000..d8f4e204a
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/terrain.obj
@@ -0,0 +1,2583 @@
+o Terrain
+v -15.0 -15.0 0.0
+v -14.0 -15.0 0.0811990914301
+v -13.0 -15.0 0.0877441126682
+v -12.0 -15.0 0.0136176013718
+v -11.0 -15.0 -0.073028869825
+v -10.0 -15.0 -0.0925329348946
+v -9.0 -15.0 -0.0269626463596
+v -8.0 -15.0 0.0633969748938
+v -7.0 -15.0 0.0954697098
+v -6.0 -15.0 0.0397680337972
+v -5.0 -15.0 -0.0524961890791
+v -4.0 -15.0 -0.0964956578146
+v -3.0 -15.0 -0.0517774637679
+v -2.0 -15.0 0.040544691683
+v -1.0 -15.0 0.095590244582
+v 0.0 -15.0 0.0627505674493
+v 1.0 -15.0 -0.0277816920072
+v 2.0 -15.0 -0.0927715919541
+v 3.0 -15.0 -0.0724677180965
+v 4.0 -15.0 0.014462641577
+v 5.0 -15.0 0.0880961152825
+v 6.0 -15.0 0.0807344268734
+v 7.0 -15.0 -0.000854121277292
+v 8.0 -15.0 -0.0816573942646
+v 9.0 -15.0 -0.0873852355474
+v 10.0 -15.0 -0.0127714942656
+v 11.0 -15.0 0.0735842999452
+v 12.0 -15.0 0.0922870281378
+v 13.0 -15.0 0.0261414882639
+v 14.0 -15.0 -0.0640384153622
+v -15.0 -14.0 0.0
+v -14.0 -14.0 0.025293990381
+v -13.0 -14.0 0.0273328026549
+v -12.0 -14.0 0.00424196221958
+v -11.0 -14.0 -0.0227489187176
+v -10.0 -14.0 -0.0288245486979
+v -9.0 -14.0 -0.0083990215365
+v -8.0 -14.0 0.0197485272914
+v -7.0 -14.0 0.0297393712026
+v -6.0 -14.0 0.0123879743803
+v -5.0 -14.0 -0.0163528689572
+v -4.0 -14.0 -0.0300589599906
+v -3.0 -14.0 -0.0161289818326
+v -2.0 -14.0 0.0126299078397
+v -1.0 -14.0 0.0297769184899
+v 0.0 -14.0 0.0195471676038
+v 1.0 -14.0 -0.00865415903086
+v 2.0 -14.0 -0.0288988917633
+v 3.0 -14.0 -0.0225741166826
+v 4.0 -14.0 0.00450519717018
+v 5.0 -14.0 0.0274424535215
+v 6.0 -14.0 0.0251492446625
+v 7.0 -14.0 -0.000266063757506
+v 8.0 -14.0 -0.0254367543859
+v 9.0 -14.0 -0.0272210103395
+v 10.0 -14.0 -0.00397839492308
+v 11.0 -14.0 0.0229219384383
+v 12.0 -14.0 0.0287479473094
+v 13.0 -14.0 0.00814322600223
+v 14.0 -14.0 -0.019948339737
+v -15.0 -13.0 -0.0
+v -14.0 -13.0 -0.0538662887752
+v -13.0 -13.0 -0.0582081600676
+v -12.0 -13.0 -0.00903371743454
+v -11.0 -13.0 0.0484462833468
+v -10.0 -13.0 0.0613849946405
+v -9.0 -13.0 0.0178866249532
+v -8.0 -13.0 -0.0420566252277
+v -7.0 -13.0 -0.0633332081283
+v -6.0 -13.0 -0.0263815315518
+v -5.0 -13.0 0.0348252034688
+v -4.0 -13.0 0.0640138070248
+v -3.0 -13.0 0.034348411617
+v -2.0 -13.0 -0.0268967550256
+v -1.0 -13.0 -0.0634131691385
+v 0.0 -13.0 -0.0416278079902
+v 1.0 -13.0 0.0184299678478
+v 2.0 -13.0 0.0615433162407
+v 3.0 -13.0 0.0480740235034
+v 4.0 -13.0 -0.00959430473812
+v 5.0 -13.0 -0.0584416734499
+v 6.0 -13.0 -0.0535580371094
+v 7.0 -13.0 0.000566611553914
+v 8.0 -13.0 0.0541703201676
+v 9.0 -13.0 0.0579700862384
+v 10.0 -13.0 0.00847242236439
+v 11.0 -13.0 -0.0488147475589
+v 12.0 -13.0 -0.0612218636973
+v 13.0 -13.0 -0.0173418806915
+v 14.0 -13.0 0.0424821474459
+v -15.0 -12.0 -0.0
+v -14.0 -12.0 -0.0835021504486
+v -13.0 -12.0 -0.0902328088647
+v -12.0 -12.0 -0.0140038389405
+v -11.0 -12.0 0.0751001959236
+v -10.0 -12.0 0.0951574569978
+v -9.0 -12.0 0.0277273909493
+v -8.0 -12.0 -0.0651951104665
+v -7.0 -12.0 -0.0981775279821
+v -6.0 -12.0 -0.0408959790398
+v -5.0 -12.0 0.0539851444302
+v -4.0 -12.0 0.0992325750764
+v -3.0 -12.0 0.0532460338318
+v -2.0 -12.0 -0.0416946653611
+v -1.0 -12.0 -0.0983014815058
+v 0.0 -12.0 -0.0645303688945
+v 1.0 -12.0 0.0285696672813
+v 2.0 -12.0 0.0954028831145
+v 3.0 -12.0 0.0745231281852
+v 4.0 -12.0 -0.0148728471166
+v 5.0 -12.0 -0.090594795369
+v 6.0 -12.0 -0.0830243065584
+v 7.0 -12.0 0.000878346815729
+v 8.0 -12.0 0.0839734521782
+v 9.0 -12.0 0.0898637528715
+v 10.0 -12.0 0.0131337336026
+v 11.0 -12.0 -0.0756713797712
+v 12.0 -12.0 -0.0949045755598
+v 13.0 -12.0 -0.0268829422536
+v 14.0 -12.0 0.0658547441835
+v -15.0 -11.0 -0.0
+v -14.0 -11.0 -0.0363665200894
+v -13.0 -11.0 -0.0392978293215
+v -12.0 -11.0 -0.00609889550654
+v -11.0 -11.0 0.0327073347106
+v -10.0 -11.0 0.0414425922324
+v -9.0 -11.0 0.0120757215781
+v -8.0 -11.0 -0.0283935118051
+v -7.0 -11.0 -0.0427578813781
+v -6.0 -11.0 -0.0178108520001
+v -5.0 -11.0 0.0235113925678
+v -4.0 -11.0 0.0432173712372
+v -3.0 -11.0 0.0231894980982
+v -2.0 -11.0 -0.0181586926484
+v -1.0 -11.0 -0.0428118651172
+v 0.0 -11.0 -0.0281040062343
+v 1.0 -11.0 0.0124425463722
+v 2.0 -11.0 0.0415494792258
+v 3.0 -11.0 0.0324560124944
+v 4.0 -11.0 -0.00647736244572
+v 5.0 -11.0 -0.0394554802252
+v 6.0 -11.0 -0.0361584114439
+v 7.0 -11.0 0.000382534065867
+v 8.0 -11.0 0.0365717795196
+v 9.0 -11.0 0.0391370995424
+v 10.0 -11.0 0.00571995073592
+v 11.0 -11.0 -0.0329560943983
+v 12.0 -11.0 -0.0413324583275
+v 13.0 -11.0 -0.0117079506848
+v 14.0 -11.0 0.0286807928235
+v -15.0 -10.0 0.0
+v -14.0 -10.0 0.0442043211272
+v -13.0 -10.0 0.0477673932687
+v -12.0 -10.0 0.0074133443296
+v -11.0 -10.0 -0.0397564991977
+v -10.0 -10.0 -0.0503744007092
+v -9.0 -10.0 -0.014678310522
+v -8.0 -10.0 0.0345129506666
+v -7.0 -10.0 0.0519731641769
+v -6.0 -10.0 0.0216494902296
+v -5.0 -10.0 -0.0285786251931
+v -4.0 -10.0 -0.0525316844103
+v -3.0 -10.0 -0.028187355243
+v -2.0 -10.0 0.0220722983421
+v -1.0 -10.0 0.0520387826231
+v 0.0 -10.0 0.0341610501495
+v 1.0 -10.0 -0.0151241942898
+v 2.0 -10.0 -0.0505043242479
+v 3.0 -10.0 -0.0394510114051
+v 4.0 -10.0 0.00787337938586
+v 5.0 -10.0 0.0479590214794
+v 6.0 -10.0 0.0439513603991
+v 7.0 -10.0 -0.000464978740008
+v 8.0 -10.0 -0.0444538185699
+v 9.0 -10.0 -0.047572022616
+v 10.0 -10.0 -0.00695272845849
+v 11.0 -10.0 0.0400588721796
+v 12.0 -10.0 0.0502405304767
+v 13.0 -10.0 0.0142312767496
+v 14.0 -10.0 -0.0348621471902
+v -15.0 -9.0 0.0
+v -14.0 -9.0 0.0841339133581
+v -13.0 -9.0 0.0909154947782
+v -12.0 -9.0 0.0141097895775
+v -11.0 -9.0 -0.0756683910902
+v -10.0 -9.0 -0.0958774019522
+v -9.0 -9.0 -0.0279371716207
+v -8.0 -9.0 0.06568836546
+v -7.0 -9.0 0.0989203222742
+v -6.0 -9.0 0.0412053909839
+v -5.0 -9.0 -0.0543935867486
+v -4.0 -9.0 -0.0999833516733
+v -3.0 -9.0 -0.0536488841664
+v -2.0 -9.0 0.0420101200285
+v -1.0 -9.0 0.0990452136089
+v 0.0 -9.0 0.0650185945676
+v 1.0 -9.0 -0.0287858204705
+v 2.0 -9.0 -0.0961246849206
+v 3.0 -9.0 -0.0750869573564
+v 4.0 -9.0 0.01498537252
+v 5.0 -9.0 0.0912802200102
+v 6.0 -9.0 0.0836524541833
+v 7.0 -9.0 -0.000884992236678
+v 8.0 -9.0 -0.0846087808756
+v 9.0 -9.0 -0.0905436465708
+v 10.0 -9.0 -0.0132331011723
+v 11.0 -9.0 0.0762438964165
+v 12.0 -9.0 0.0956226072567
+v 13.0 -9.0 0.0270863339714
+v 14.0 -9.0 -0.0663529898522
+v -15.0 -8.0 0.0
+v -14.0 -8.0 0.0467111736511
+v -13.0 -8.0 0.0504763096669
+v -12.0 -8.0 0.00783375935847
+v -11.0 -8.0 -0.0420111131769
+v -10.0 -8.0 -0.0532311620017
+v -9.0 -8.0 -0.0155107259701
+v -8.0 -8.0 0.036470199987
+v -7.0 -8.0 0.054920592267
+v -6.0 -8.0 0.022877245296
+v -5.0 -8.0 -0.0301993354963
+v -4.0 -8.0 -0.0555107865047
+v -3.0 -8.0 -0.0297858764018
+v -2.0 -8.0 0.0233240311003
+v -1.0 -8.0 0.054989931973
+v 0.0 -8.0 0.0360983429888
+v 1.0 -8.0 -0.0159818960633
+v 2.0 -8.0 -0.0533684535791
+v 3.0 -8.0 -0.0416883009955
+v 4.0 -8.0 0.00831988326787
+v 5.0 -8.0 0.0506788052239
+v 6.0 -8.0 0.0464438673744
+v 7.0 -8.0 -0.000491347952298
+v 8.0 -8.0 -0.0469748202376
+v 9.0 -8.0 -0.0502698594319
+v 10.0 -8.0 -0.00734702169588
+v 11.0 -8.0 0.0423306339048
+v 12.0 -8.0 0.0530896999111
+v 13.0 -8.0 0.0150383406549
+v 14.0 -8.0 -0.0368391996466
+v -15.0 -7.0 -0.0
+v -14.0 -7.0 -0.0336576036912
+v -13.0 -7.0 -0.0363705617687
+v -12.0 -7.0 -0.0056445930875
+v -11.0 -7.0 0.030270988447
+v -10.0 -7.0 0.0383555628051
+v -9.0 -7.0 0.011176209606
+v -8.0 -7.0 -0.0262784991632
+v -7.0 -7.0 -0.0395728769912
+v -6.0 -7.0 -0.0164841342132
+v -5.0 -7.0 0.0217600455399
+v -4.0 -7.0 0.0399981397752
+v -3.0 -7.0 0.0214621287621
+v -2.0 -7.0 -0.0168060644573
+v -1.0 -7.0 -0.0396228395197
+v 0.0 -7.0 -0.0260105586578
+v 1.0 -7.0 0.0115157098802
+v 2.0 -7.0 0.0384544878618
+v 3.0 -7.0 0.0300383870452
+v 4.0 -7.0 -0.00599486829166
+v 5.0 -7.0 -0.0365164693679
+v 6.0 -7.0 -0.0334649969117
+v 7.0 -7.0 0.000354039373458
+v 8.0 -7.0 0.0338475734914
+v 9.0 -7.0 0.0362218046374
+v 10.0 -7.0 0.00529387564521
+v 11.0 -7.0 -0.0305012182012
+v 12.0 -7.0 -0.038253632697
+v 13.0 -7.0 -0.0108358337068
+v 14.0 -7.0 0.0265443808214
+v -15.0 -6.0 -0.0
+v -14.0 -6.0 -0.0830817354197
+v -13.0 -6.0 -0.0897785064456
+v -12.0 -6.0 -0.0139333326802
+v -11.0 -6.0 0.0747220828946
+v -10.0 -6.0 0.0946783600546
+v -9.0 -6.0 0.0275877896121
+v -8.0 -6.0 -0.0648668673722
+v -7.0 -6.0 -0.0976832256433
+v -6.0 -6.0 -0.0406900767473
+v -5.0 -6.0 0.0537133410583
+v -4.0 -6.0 0.0987329608067
+v -3.0 -6.0 0.0529779517197
+v -2.0 -6.0 -0.041484741858
+v -1.0 -6.0 -0.0978065550882
+v 0.0 -6.0 -0.0642054726283
+v 1.0 -6.0 0.0284258252673
+v 2.0 -6.0 0.0949225505046
+v 3.0 -6.0 0.0741479205657
+v 4.0 -6.0 -0.0147979655906
+v 5.0 -6.0 -0.0901386704272
+v 6.0 -6.0 -0.0826062973688
+v 7.0 -6.0 0.000873924531993
+v 8.0 -6.0 0.0835506642484
+v 9.0 -6.0 0.0894113085685
+v 10.0 -6.0 0.0130676081321
+v 11.0 -6.0 -0.0752903909566
+v 12.0 -6.0 -0.0944267518192
+v 13.0 -6.0 -0.0267475925305
+v 14.0 -6.0 0.0655231799779
+v -15.0 -5.0 -0.0
+v -14.0 -5.0 -0.0561209027544
+v -13.0 -5.0 -0.0606445063313
+v -12.0 -5.0 -0.00941183046358
+v -11.0 -5.0 0.0504740389274
+v -10.0 -5.0 0.0639543097015
+v -9.0 -5.0 0.0186352830764
+v -8.0 -5.0 -0.0438169368681
+v -7.0 -5.0 -0.0659840671283
+v -6.0 -5.0 -0.0274857503718
+v -5.0 -5.0 0.0362828385194
+v -4.0 -5.0 0.0666931530028
+v -3.0 -5.0 0.0357860901866
+v -2.0 -5.0 -0.0280225389111
+v -1.0 -5.0 -0.0660673749666
+v 0.0 -5.0 -0.043370171163
+v 1.0 -5.0 0.019201367996
+v 2.0 -5.0 0.0641192579711
+v 3.0 -5.0 0.0500861978687
+v 4.0 -5.0 -0.00999588156985
+v 5.0 -5.0 -0.0608877935915
+v 6.0 -5.0 -0.0557997489836
+v 7.0 -5.0 0.000590327506123
+v 8.0 -5.0 0.0564376596091
+v 9.0 -5.0 0.0603964677431
+v 10.0 -5.0 0.00882704196665
+v 11.0 -5.0 -0.0508579254859
+v 12.0 -5.0 -0.0637843507901
+v 13.0 -5.0 -0.0180677381344
+v 14.0 -5.0 0.0442602696384
+v -15.0 -4.0 0.0
+v -14.0 -4.0 0.0224372290885
+v -13.0 -4.0 0.0242457732276
+v -12.0 -4.0 0.00376286527637
+v -11.0 -4.0 -0.0201796036566
+v -10.0 -4.0 -0.0255690380507
+v -9.0 -4.0 -0.00745041677866
+v -8.0 -4.0 0.0175180833204
+v -7.0 -4.0 0.0263805384034
+v -6.0 -4.0 0.0109888481385
+v -5.0 -4.0 -0.0145059384273
+v -4.0 -4.0 -0.0266640321006
+v -3.0 -4.0 -0.0143073376281
+v -2.0 -4.0 0.011203457078
+v -1.0 -4.0 0.026413845014
+v 0.0 -4.0 0.0173394656578
+v 1.0 -4.0 -0.00767673845917
+v 2.0 -4.0 -0.0256349846398
+v 3.0 -4.0 -0.0200245441644
+v 4.0 -4.0 0.00399636986785
+v 5.0 -4.0 0.0243430398738
+v 6.0 -4.0 0.0223088312835
+v 7.0 -4.0 -0.000236013906442
+v 8.0 -4.0 -0.0225638689992
+v 9.0 -4.0 -0.0241466069927
+v 10.0 -4.0 -0.0035290658749
+v 11.0 -4.0 0.0203330821332
+v 12.0 -4.0 0.0255010881988
+v 13.0 -4.0 0.00722351137877
+v 14.0 -4.0 -0.01769532849
+v -15.0 -3.0 0.0
+v -14.0 -3.0 0.0803666759821
+v -13.0 -3.0 0.0868446006961
+v -12.0 -3.0 0.0134780000346
+v -11.0 -3.0 -0.0722802117018
+v -10.0 -3.0 -0.0915843301368
+v -9.0 -3.0 -0.0266862378068
+v -8.0 -3.0 0.0627470584929
+v -7.0 -3.0 0.0944909985871
+v -6.0 -3.0 0.0393603503479
+v -5.0 -3.0 -0.0519580224816
+v -4.0 -3.0 -0.0955064290582
+v -3.0 -3.0 -0.0512466652092
+v -2.0 -3.0 0.0401290462971
+v -1.0 -3.0 0.0946102977024
+v 0.0 -3.0 0.0621072777179
+v 1.0 -3.0 -0.0274968869781
+v 2.0 -3.0 -0.0918205405947
+v 3.0 -3.0 -0.0717248126407
+v 4.0 -3.0 0.0143143772792
+v 5.0 -3.0 0.0871929947428
+v 6.0 -3.0 0.0799067749509
+v 7.0 -3.0 -0.000845365221858
+v 8.0 -3.0 -0.0808202805083
+v 9.0 -3.0 -0.0864894026172
+v 10.0 -3.0 -0.0126405668262
+v 11.0 -3.0 0.0728299478099
+v 12.0 -3.0 0.091340944302
+v 13.0 -3.0 0.0258734978433
+v 14.0 -3.0 -0.0633819232109
+v -15.0 -2.0 0.0
+v -14.0 -2.0 0.0644073716076
+v -13.0 -2.0 0.069598902789
+v -12.0 -2.0 0.010801523718
+v -11.0 -2.0 -0.0579267264456
+v -10.0 -2.0 -0.0733974114579
+v -9.0 -2.0 -0.0213868548653
+v -8.0 -2.0 0.0502866774599
+v -7.0 -2.0 0.0757268704374
+v -6.0 -2.0 0.031544127967
+v -5.0 -2.0 -0.041640140283
+v -4.0 -2.0 -0.0765406555902
+v -3.0 -2.0 -0.0410700451331
+v -2.0 -2.0 0.0321601754151
+v -1.0 -2.0 0.0758224790009
+v 0.0 -2.0 0.0497739450665
+v 1.0 -2.0 -0.0220365244177
+v 2.0 -2.0 -0.073586714979
+v 3.0 -2.0 -0.0574816191511
+v 4.0 -2.0 0.0114718122342
+v 5.0 -2.0 0.0698781123564
+v 6.0 -2.0 0.0640387982375
+v 7.0 -2.0 -0.000677491650899
+v 8.0 -2.0 -0.0647708988399
+v 9.0 -2.0 -0.0693142403418
+v 10.0 -2.0 -0.0101303889324
+v 11.0 -2.0 0.0583672953427
+v 12.0 -2.0 0.0732023574543
+v 13.0 -2.0 0.0207355097124
+v 14.0 -2.0 -0.0507954700324
+v -15.0 -1.0 -0.0
+v -14.0 -1.0 -0.010767773193
+v -13.0 -1.0 -0.0116357053705
+v -12.0 -1.0 -0.00180582369116
+v -11.0 -1.0 0.00968432396188
+v -10.0 -1.0 0.0122707488259
+v -9.0 -1.0 0.00357550380886
+v -8.0 -1.0 -0.00840704292078
+v -7.0 -1.0 -0.0126601931601
+v -6.0 -1.0 -0.00527362019352
+v -5.0 -1.0 0.00696149485846
+v -4.0 -1.0 0.0127962436421
+v -3.0 -1.0 0.00686618503415
+v -2.0 -1.0 -0.00537661242922
+v -1.0 -1.0 -0.0126761772207
+v 0.0 -1.0 -0.00832132313463
+v 1.0 -1.0 0.00368411706564
+v 2.0 -1.0 0.0123023970259
+v 3.0 -1.0 0.009609909896
+v 4.0 -1.0 -0.00191788407395
+v 5.0 -1.0 -0.0116823842711
+v 6.0 -1.0 -0.0107061542455
+v 7.0 -1.0 0.000113264619483
+v 8.0 -1.0 0.0108285485156
+v 9.0 -1.0 0.0115881148449
+v 10.0 -1.0 0.00169362182712
+v 11.0 -1.0 -0.00975797928799
+v 12.0 -1.0 -0.0122381392469
+v 13.0 -1.0 -0.00346661042133
+v 14.0 -1.0 0.00849210403857
+v -15.0 0.0 -0.0
+v -14.0 0.0 -0.0760430769782
+v -13.0 0.0 -0.0821724996732
+v -12.0 0.0 -0.0127529051266
+v -11.0 0.0 0.0683916515804
+v -10.0 0.0 0.0866572392286
+v -9.0 0.0 0.0252505607704
+v -8.0 0.0 -0.0593713668112
+v -7.0 0.0 -0.0894075335517
+v -6.0 0.0 -0.0372428262687
+v -5.0 0.0 0.0491627637316
+v -4.0 0.0 0.0903683354828
+v -3.0 0.0 0.048489676346
+v -2.0 0.0 -0.0379701676017
+v -1.0 0.0 -0.0895204145648
+v 0.0 0.0 -0.0587660052216
+v 1.0 0.0 0.026017598309
+v 2.0 0.0 0.0868807419406
+v 3.0 0.0 0.0678661321031
+v 4.0 0.0 -0.0135442866093
+v 5.0 0.0 -0.0825021506758
+v 6.0 0.0 -0.0756079178891
+v 7.0 0.0 0.000799885921059
+v 8.0 0.0 0.0764722783042
+v 9.0 0.0 0.0818364106845
+v 10.0 0.0 0.0119605244894
+v 11.0 0.0 -0.0689118127625
+v 12.0 0.0 -0.0864269471636
+v 13.0 0.0 -0.0244815449208
+v 14.0 0.0 0.0599720768198
+v -15.0 1.0 -0.0
+v -14.0 1.0 -0.0714047264802
+v -13.0 1.0 -0.0771602767342
+v -12.0 1.0 -0.0119750244017
+v -11.0 1.0 0.0642200101401
+v -10.0 1.0 0.0813714635249
+v -9.0 1.0 0.0237103686086
+v -8.0 1.0 -0.0557499298605
+v -7.0 1.0 -0.0839539999198
+v -6.0 1.0 -0.0349711496265
+v -5.0 1.0 0.0461640143556
+v -4.0 1.0 0.0848561964355
+v -3.0 1.0 0.045531982847
+v -2.0 1.0 -0.0356541257895
+v -1.0 1.0 -0.0840599956026
+v 0.0 1.0 -0.0551814931211
+v 1.0 1.0 0.0244306196534
+v 2.0 1.0 0.0815813333861
+v 3.0 1.0 0.0637265454353
+v 4.0 1.0 -0.0127181344988
+v 5.0 1.0 -0.0774698202273
+v 6.0 1.0 -0.0709961105093
+v 7.0 1.0 0.000751095795676
+v 8.0 1.0 0.0718077480899
+v 9.0 1.0 0.0768446879487
+v 10.0 1.0 0.0112309760949
+v 11.0 1.0 -0.0647084433863
+v 12.0 1.0 -0.0811552184364
+v 13.0 1.0 -0.0229882599225
+v 14.0 1.0 0.0563139987483
+v -15.0 2.0 -0.0
+v -14.0 2.0 -0.00111719975608
+v -13.0 2.0 -0.00120725120865
+v -12.0 2.0 -0.000187361467513
+v -11.0 2.0 0.0010047875428
+v -10.0 2.0 0.00127313952008
+v -9.0 2.0 0.000370972893979
+v -8.0 2.0 -0.000872264500013
+v -7.0 2.0 -0.00131354593535
+v -6.0 2.0 -0.00054715929545
+v -5.0 2.0 0.000722283077329
+v -4.0 2.0 0.00132766171979
+v -3.0 2.0 0.000712394299903
+v -2.0 2.0 -0.000557845153941
+v -1.0 2.0 -0.00131520434589
+v 0.0 2.0 -0.0008633707276
+v 1.0 2.0 0.000382241956004
+v 2.0 2.0 0.00127642314806
+v 3.0 2.0 0.000997066784313
+v 4.0 2.0 -0.000198988182719
+v 5.0 2.0 -0.00121209433224
+v 6.0 2.0 -0.00111080654256
+v 7.0 2.0 1.17516596042e-05
+v 8.0 2.0 0.00112350544012
+v 9.0 2.0 0.0012023135003
+v 10.0 2.0 0.000175720073058
+v 11.0 2.0 -0.00101242957898
+v 12.0 2.0 -0.00126975614516
+v 13.0 2.0 -0.000359674767263
+v 14.0 2.0 0.000881089932935
+v -15.0 3.0 0.0
+v -14.0 3.0 0.0701974752715
+v -13.0 3.0 0.0758557155106
+v -12.0 3.0 0.0117725607358
+v -11.0 3.0 -0.0631342320875
+v -10.0 3.0 -0.0799957030881
+v -9.0 3.0 -0.0233094935885
+v -8.0 3.0 0.0548073568191
+v -7.0 3.0 0.0825345761243
+v -6.0 3.0 0.0343798867685
+v -5.0 3.0 -0.0453835119313
+v -4.0 3.0 -0.0834215190582
+v -3.0 3.0 -0.0447621662811
+v -2.0 3.0 0.0350513157436
+v -1.0 3.0 0.082638779721
+v 0.0 3.0 0.0542485307312
+v 1.0 3.0 -0.0240175672329
+v 2.0 3.0 -0.0802020246458
+v 3.0 3.0 -0.0626491104699
+v 4.0 3.0 0.0125031069508
+v 5.0 3.0 0.076160025502
+v 6.0 3.0 0.0697957678366
+v 7.0 3.0 -0.000738396898112
+v 8.0 3.0 -0.07059368293
+v 9.0 3.0 -0.0755454624355
+v 10.0 3.0 -0.0110410921735
+v 11.0 3.0 0.0636144073142
+v 12.0 3.0 0.0797831140901
+v 13.0 3.0 0.0225995937103
+v 14.0 3.0 -0.0553618889034
+v -15.0 4.0 0.0
+v -14.0 4.0 0.0769729152667
+v -13.0 4.0 0.083177287216
+v -12.0 4.0 0.0129088448906
+v -11.0 4.0 -0.069227929895
+v -10.0 4.0 -0.0877168651961
+v -9.0 4.0 -0.025559319163
+v -8.0 4.0 0.0600973470358
+v -7.0 4.0 0.090500789523
+v -6.0 4.0 0.0376982234885
+v -5.0 4.0 -0.049763915367
+v -4.0 4.0 -0.0914733399322
+v -3.0 4.0 -0.0490825976146
+v -2.0 4.0 0.0384344585939
+v -1.0 4.0 0.0906150508207
+v 0.0 4.0 0.0594845832157
+v 1.0 4.0 -0.0263357358706
+v 2.0 4.0 -0.0879431008509
+v 3.0 4.0 -0.0686959844793
+v 4.0 4.0 0.0137099032148
+v 5.0 4.0 0.0835109691197
+v 6.0 4.0 0.0765324351465
+v 7.0 4.0 -0.000809666752996
+v 8.0 4.0 -0.0774073647737
+v 9.0 4.0 -0.0828370886039
+v 10.0 4.0 -0.0121067751944
+v 11.0 4.0 0.0697544514955
+v 12.0 4.0 0.0874837571696
+v 13.0 4.0 0.024780899954
+v 14.0 4.0 -0.0607054023964
+v -15.0 5.0 0.0
+v -14.0 5.0 0.0129798119445
+v -13.0 5.0 0.0140260446467
+v -12.0 5.0 0.00217679658514
+v -11.0 5.0 -0.011673788218
+v -10.0 5.0 -0.0147915459699
+v -9.0 5.0 -0.00431002457182
+v -8.0 5.0 0.0101341135409
+v -7.0 5.0 0.0152609944
+v -6.0 5.0 0.0063569873874
+v -5.0 5.0 -0.00839160451241
+v -4.0 5.0 -0.0154249939234
+v -3.0 5.0 -0.0082767150572
+v -2.0 5.0 0.00648113746255
+v -1.0 5.0 0.0152802620885
+v 0.0 5.0 0.0100307842189
+v 1.0 5.0 -0.0044409504023
+v 2.0 5.0 -0.0148296957041
+v 3.0 5.0 -0.0115840871662
+v 4.0 5.0 0.00231187768956
+v 5.0 5.0 0.0140823128593
+v 6.0 5.0 0.0129055345301
+v 7.0 5.0 -0.000136532729145
+v 8.0 5.0 -0.0130530724268
+v 9.0 5.0 -0.0139686775326
+v 10.0 5.0 -0.00204154493477
+v 11.0 5.0 0.0117625746611
+v 12.0 5.0 0.0147522373594
+v 13.0 5.0 0.00417876106294
+v 14.0 5.0 -0.0102366488834
+v -15.0 6.0 -0.0
+v -14.0 6.0 -0.06294687062
+v -13.0 6.0 -0.0680206786864
+v -12.0 6.0 -0.0105565884619
+v -11.0 6.0 0.0566131805103
+v -10.0 6.0 0.0717330524063
+v -9.0 6.0 0.020901886734
+v -8.0 6.0 -0.0491463772076
+v -7.0 6.0 -0.0740096885947
+v -6.0 6.0 -0.0308288336009
+v -5.0 6.0 0.0406959088311
+v -4.0 6.0 0.0748050203626
+v -3.0 6.0 0.0401387411537
+v -2.0 6.0 -0.0314309115625
+v -1.0 6.0 -0.0741031291393
+v 0.0 6.0 -0.0486452715295
+v 1.0 6.0 0.0215368243854
+v 2.0 6.0 0.0719180632825
+v 3.0 6.0 0.0561781664648
+v 4.0 6.0 -0.0112116775217
+v 5.0 6.0 -0.0682935569001
+v 6.0 6.0 -0.0625866550164
+v 7.0 6.0 0.000662128856229
+v 8.0 6.0 0.063302154512
+v 9.0 6.0 0.0677424712422
+v 10.0 6.0 0.00990067232282
+v 11.0 6.0 -0.0570437590709
+v 12.0 6.0 -0.0715424214456
+v 13.0 6.0 -0.020265311478
+v 14.0 6.0 0.0496436324042
+v -15.0 7.0 -0.0
+v -14.0 7.0 -0.0810004906309
+v -13.0 7.0 -0.0875295037286
+v -12.0 7.0 -0.0135842947613
+v -11.0 7.0 0.0728502521624
+v -10.0 7.0 0.0923066132141
+v -9.0 7.0 0.0268966997705
+v -8.0 7.0 -0.0632419154016
+v -7.0 7.0 -0.0952362052085
+v -6.0 7.0 -0.039670767151
+v -5.0 7.0 0.0523677912741
+v -4.0 7.0 0.0962596439082
+v -3.0 7.0 0.0516508238572
+v -2.0 7.0 -0.0404455254481
+v -1.0 7.0 -0.0953564451805
+v 0.0 7.0 -0.0625970889728
+v 1.0 7.0 0.0277137421553
+v 2.0 7.0 0.0925446865542
+v 3.0 7.0 0.0722904729269
+v 4.0 7.0 -0.0144272681248
+v 5.0 7.0 -0.0878806453974
+v 6.0 7.0 -0.0805369625739
+v 7.0 7.0 0.00085203222475
+v 8.0 7.0 0.0814576725253
+v 9.0 7.0 0.0871715043674
+v 10.0 7.0 0.0127402571061
+v 11.0 7.0 -0.0734043237839
+v 12.0 7.0 -0.0920613079083
+v 13.0 7.0 -0.0260775501043
+v 14.0 7.0 0.0638817870028
+v -15.0 8.0 -0.0
+v -14.0 8.0 -0.0245826331086
+v -13.0 8.0 -0.0265641067058
+v -12.0 8.0 -0.00412266310431
+v -11.0 8.0 0.0221091379426
+v -10.0 8.0 0.0280138995266
+v -9.0 8.0 0.00816281107856
+v -8.0 8.0 -0.0191931282304
+v -7.0 8.0 -0.028902993958
+v -6.0 8.0 -0.0120395803336
+v -5.0 8.0 0.0158929679261
+v -4.0 8.0 0.0292135947687
+v -3.0 8.0 0.0156753773063
+v -2.0 8.0 -0.0122747097608
+v -1.0 8.0 -0.0289394852816
+v 0.0 8.0 -0.0189974314958
+v 1.0 8.0 0.00841077319611
+v 2.0 8.0 0.0280861517997
+v 3.0 8.0 0.0219392519646
+v 4.0 8.0 -0.00437849494873
+v 5.0 8.0 -0.0266706737987
+v 6.0 8.0 -0.0244419581562
+v 7.0 8.0 0.000258581095183
+v 8.0 8.0 0.0247213820802
+v 9.0 8.0 0.0264554583892
+v 10.0 8.0 0.00386650826074
+v 11.0 8.0 -0.0222772917313
+v 12.0 8.0 -0.0279394524426
+v 13.0 8.0 -0.00791420942754
+v 14.0 8.0 0.0193873212369
+v -15.0 9.0 0.0
+v -14.0 9.0 0.0544363839251
+v -13.0 9.0 0.0588242075157
+v -12.0 9.0 0.00912932599811
+v -11.0 9.0 -0.0489590157401
+v -10.0 9.0 -0.062034664193
+v -9.0 9.0 -0.0180759284743
+v -8.0 9.0 0.0425017325222
+v -7.0 9.0 0.0640034966446
+v -6.0 9.0 0.0266607411192
+v -5.0 9.0 -0.0351937768389
+v -4.0 9.0 -0.0646912986757
+v -3.0 9.0 -0.0347119388493
+v -2.0 9.0 0.0271814174729
+v -1.0 9.0 0.064084303924
+v 0.0 9.0 0.0420683768873
+v 1.0 9.0 -0.0186250218513
+v 2.0 9.0 -0.0621946613935
+v 3.0 9.0 -0.0485828160759
+v 4.0 9.0 0.00969584629077
+v 5.0 9.0 0.0590601922924
+v 6.0 9.0 0.0541248698704
+v 7.0 9.0 -0.000572608300787
+v 8.0 9.0 -0.054743633041
+v 9.0 9.0 -0.0585836140265
+v 10.0 9.0 -0.00856209044823
+v 11.0 9.0 0.049331379602
+v 12.0 9.0 0.0618698067495
+v 13.0 9.0 0.0175254188987
+v 14.0 9.0 -0.0429317582649
+v -15.0 10.0 0.0
+v -14.0 10.0 0.0834068406243
+v -13.0 10.0 0.090129816629
+v -12.0 10.0 0.0139878548799
+v -11.0 10.0 -0.0750144761375
+v -10.0 10.0 -0.095048843741
+v -9.0 10.0 -0.0276957427493
+v -8.0 10.0 0.0651206964007
+v -7.0 10.0 0.0980654675993
+v -6.0 10.0 0.0408493001393
+v -5.0 10.0 -0.0539235254827
+v -4.0 10.0 -0.0991193104569
+v -3.0 10.0 -0.0531852585092
+v -2.0 10.0 0.0416470748355
+v -1.0 10.0 0.0981892796417
+v 0.0 10.0 0.0644567135684
+v 1.0 10.0 -0.0285370577023
+v 2.0 10.0 -0.095293989727
+v 3.0 10.0 -0.0744380670674
+v 4.0 10.0 0.0148558711652
+v 5.0 10.0 0.0904913899599
+v 6.0 10.0 0.0829295421478
+v 7.0 10.0 -0.000877344265732
+v 8.0 10.0 -0.0838776044075
+v 9.0 10.0 -0.0897611818784
+v 10.0 10.0 -0.0131187426852
+v 11.0 10.0 0.0755850080326
+v 12.0 10.0 0.0947962509433
+v 13.0 10.0 0.0268522579121
+v 14.0 10.0 -0.065779577208
+v -15.0 11.0 0.0
+v -14.0 11.0 0.0356934327039
+v -13.0 11.0 0.0385704879885
+v -12.0 11.0 0.00598601449342
+v -11.0 11.0 -0.032101973121
+v -10.0 11.0 -0.0406755546938
+v -9.0 11.0 -0.0118522188661
+v -8.0 11.0 0.0278679923278
+v -7.0 11.0 0.0419664998953
+v -6.0 11.0 0.0174812009975
+v -5.0 11.0 -0.0230762334787
+v -4.0 11.0 -0.0424174853161
+v -3.0 11.0 -0.0227602967721
+v -2.0 11.0 0.0178226036597
+v -1.0 11.0 0.0420194844799
+v 0.0 11.0 0.0275838450521
+v 1.0 11.0 -0.0122122543072
+v 2.0 11.0 -0.0407804633762
+v 3.0 11.0 -0.0318553024858
+v 4.0 11.0 0.00635747660173
+v 5.0 11.0 0.0387252210207
+v 6.0 11.0 0.0354891758237
+v 7.0 11.0 -0.000375453958844
+v 8.0 11.0 -0.0358948931031
+v 9.0 11.0 -0.0384127330662
+v 10.0 11.0 -0.00561408339758
+v 11.0 11.0 0.0323461286561
+v 12.0 11.0 0.0405674591952
+v 13.0 11.0 0.0114912548366
+v 14.0 11.0 -0.0281499562241
+v -15.0 12.0 -0.0
+v -14.0 12.0 -0.0448363526358
+v -13.0 12.0 -0.0484503694317
+v -12.0 12.0 -0.0075193400124
+v -11.0 12.0 0.0403249359371
+v -10.0 12.0 0.051094651754
+v -9.0 12.0 0.0148881803833
+v -8.0 12.0 -0.0350064153714
+v -7.0 12.0 -0.052716274274
+v -6.0 12.0 -0.0219590337227
+v -5.0 12.0 0.028987241164
+v -4.0 12.0 0.053282780206
+v -3.0 12.0 0.0285903768527
+v -2.0 12.0 -0.0223878871277
+v -1.0 12.0 -0.0527828309299
+v 0.0 12.0 -0.0346494833957
+v 1.0 12.0 0.0153404393783
+v 2.0 12.0 0.0512264329339
+v 3.0 12.0 0.0400150802929
+v 4.0 12.0 -0.00798595263039
+v 5.0 12.0 -0.0486447375344
+v 6.0 12.0 -0.044579775086
+v 7.0 12.0 0.000471626986311
+v 8.0 12.0 0.0450894173824
+v 9.0 12.0 0.0482522053777
+v 10.0 12.0 0.0070521382751
+v 11.0 12.0 -0.040631632235
+v 12.0 12.0 -0.0509588674506
+v 13.0 12.0 -0.014434754941
+v 14.0 12.0 0.0353606046921
+v -15.0 13.0 -0.0
+v -14.0 13.0 -0.0841438021356
+v -13.0 13.0 -0.0909261806367
+v -12.0 13.0 -0.014111447988
+v -11.0 13.0 0.0756772848626
+v -10.0 13.0 0.0958886710142
+v -9.0 13.0 0.0279404552486
+v -8.0 13.0 -0.0656960862185
+v -7.0 13.0 -0.0989319489894
+v -6.0 13.0 -0.0412102341075
+v -5.0 13.0 0.0543999799621
+v -4.0 13.0 0.0999951033329
+v -3.0 13.0 0.0536551898505
+v -2.0 13.0 -0.0420150577369
+v -1.0 13.0 -0.0990568550033
+v 0.0 13.0 -0.0650262366038
+v 1.0 13.0 0.0287892038454
+v 2.0 13.0 0.0961359830474
+v 3.0 13.0 0.0750957827894
+v 4.0 13.0 -0.0149871338432
+v 5.0 13.0 -0.0912909487371
+v 6.0 13.0 -0.0836622863718
+v 7.0 13.0 0.000885096255271
+v 8.0 13.0 0.0846187254671
+v 9.0 13.0 0.0905542887237
+v 10.0 13.0 0.0132346565403
+v 11.0 13.0 -0.0762528578316
+v 12.0 13.0 -0.0956338463711
+v 13.0 13.0 -0.0270895175951
+v 14.0 13.0 0.0663607887281
+v -15.0 14.0 -0.0
+v -14.0 14.0 -0.0460898280009
+v -13.0 14.0 -0.0498048806919
+v -12.0 14.0 -0.00772955576175
+v -11.0 14.0 0.0414522870891
+v -10.0 14.0 0.0525230883572
+v -9.0 14.0 0.0153044044124
+v -8.0 14.0 -0.0359850783694
+v -7.0 14.0 -0.054190046052
+v -6.0 14.0 -0.0225729353046
+v -5.0 14.0 0.0297976280614
+v -4.0 14.0 0.0547723896066
+v -3.0 14.0 0.0293896687433
+v -2.0 14.0 -0.0230137780252
+v -1.0 14.0 -0.0542584634107
+v 0.0 14.0 -0.0356181677622
+v 1.0 14.0 0.0157693070653
+v 2.0 14.0 0.0526585537009
+v 3.0 14.0 0.0411337689112
+v 4.0 14.0 -0.00820921331732
+v 5.0 14.0 -0.0500046826806
+v 6.0 14.0 -0.0458260773958
+v 7.0 14.0 0.000484812108965
+v 8.0 14.0 0.0463499675966
+v 9.0 14.0 0.0496011766297
+v 10.0 14.0 0.00724929261704
+v 11.0 14.0 -0.0417675575959
+v 12.0 14.0 -0.0523835079761
+v 13.0 14.0 -0.014838302702
+v 14.0 14.0 0.036349169646
+f 1 2 32
+f 1 32 31
+f 31 32 62
+f 31 62 61
+f 61 62 92
+f 61 92 91
+f 91 92 122
+f 91 122 121
+f 121 122 152
+f 121 152 151
+f 151 152 182
+f 151 182 181
+f 181 182 212
+f 181 212 211
+f 211 212 242
+f 211 242 241
+f 241 242 272
+f 241 272 271
+f 271 272 302
+f 271 302 301
+f 301 302 332
+f 301 332 331
+f 331 332 362
+f 331 362 361
+f 361 362 392
+f 361 392 391
+f 391 392 422
+f 391 422 421
+f 421 422 452
+f 421 452 451
+f 451 452 482
+f 451 482 481
+f 481 482 512
+f 481 512 511
+f 511 512 542
+f 511 542 541
+f 541 542 572
+f 541 572 571
+f 571 572 602
+f 571 602 601
+f 601 602 632
+f 601 632 631
+f 631 632 662
+f 631 662 661
+f 661 662 692
+f 661 692 691
+f 691 692 722
+f 691 722 721
+f 721 722 752
+f 721 752 751
+f 751 752 782
+f 751 782 781
+f 781 782 812
+f 781 812 811
+f 811 812 842
+f 811 842 841
+f 841 842 872
+f 841 872 871
+f 2 3 33
+f 2 33 32
+f 32 33 63
+f 32 63 62
+f 62 63 93
+f 62 93 92
+f 92 93 123
+f 92 123 122
+f 122 123 153
+f 122 153 152
+f 152 153 183
+f 152 183 182
+f 182 183 213
+f 182 213 212
+f 212 213 243
+f 212 243 242
+f 242 243 273
+f 242 273 272
+f 272 273 303
+f 272 303 302
+f 302 303 333
+f 302 333 332
+f 332 333 363
+f 332 363 362
+f 362 363 393
+f 362 393 392
+f 392 393 423
+f 392 423 422
+f 422 423 453
+f 422 453 452
+f 452 453 483
+f 452 483 482
+f 482 483 513
+f 482 513 512
+f 512 513 543
+f 512 543 542
+f 542 543 573
+f 542 573 572
+f 572 573 603
+f 572 603 602
+f 602 603 633
+f 602 633 632
+f 632 633 663
+f 632 663 662
+f 662 663 693
+f 662 693 692
+f 692 693 723
+f 692 723 722
+f 722 723 753
+f 722 753 752
+f 752 753 783
+f 752 783 782
+f 782 783 813
+f 782 813 812
+f 812 813 843
+f 812 843 842
+f 842 843 873
+f 842 873 872
+f 3 4 34
+f 3 34 33
+f 33 34 64
+f 33 64 63
+f 63 64 94
+f 63 94 93
+f 93 94 124
+f 93 124 123
+f 123 124 154
+f 123 154 153
+f 153 154 184
+f 153 184 183
+f 183 184 214
+f 183 214 213
+f 213 214 244
+f 213 244 243
+f 243 244 274
+f 243 274 273
+f 273 274 304
+f 273 304 303
+f 303 304 334
+f 303 334 333
+f 333 334 364
+f 333 364 363
+f 363 364 394
+f 363 394 393
+f 393 394 424
+f 393 424 423
+f 423 424 454
+f 423 454 453
+f 453 454 484
+f 453 484 483
+f 483 484 514
+f 483 514 513
+f 513 514 544
+f 513 544 543
+f 543 544 574
+f 543 574 573
+f 573 574 604
+f 573 604 603
+f 603 604 634
+f 603 634 633
+f 633 634 664
+f 633 664 663
+f 663 664 694
+f 663 694 693
+f 693 694 724
+f 693 724 723
+f 723 724 754
+f 723 754 753
+f 753 754 784
+f 753 784 783
+f 783 784 814
+f 783 814 813
+f 813 814 844
+f 813 844 843
+f 843 844 874
+f 843 874 873
+f 4 5 35
+f 4 35 34
+f 34 35 65
+f 34 65 64
+f 64 65 95
+f 64 95 94
+f 94 95 125
+f 94 125 124
+f 124 125 155
+f 124 155 154
+f 154 155 185
+f 154 185 184
+f 184 185 215
+f 184 215 214
+f 214 215 245
+f 214 245 244
+f 244 245 275
+f 244 275 274
+f 274 275 305
+f 274 305 304
+f 304 305 335
+f 304 335 334
+f 334 335 365
+f 334 365 364
+f 364 365 395
+f 364 395 394
+f 394 395 425
+f 394 425 424
+f 424 425 455
+f 424 455 454
+f 454 455 485
+f 454 485 484
+f 484 485 515
+f 484 515 514
+f 514 515 545
+f 514 545 544
+f 544 545 575
+f 544 575 574
+f 574 575 605
+f 574 605 604
+f 604 605 635
+f 604 635 634
+f 634 635 665
+f 634 665 664
+f 664 665 695
+f 664 695 694
+f 694 695 725
+f 694 725 724
+f 724 725 755
+f 724 755 754
+f 754 755 785
+f 754 785 784
+f 784 785 815
+f 784 815 814
+f 814 815 845
+f 814 845 844
+f 844 845 875
+f 844 875 874
+f 5 6 36
+f 5 36 35
+f 35 36 66
+f 35 66 65
+f 65 66 96
+f 65 96 95
+f 95 96 126
+f 95 126 125
+f 125 126 156
+f 125 156 155
+f 155 156 186
+f 155 186 185
+f 185 186 216
+f 185 216 215
+f 215 216 246
+f 215 246 245
+f 245 246 276
+f 245 276 275
+f 275 276 306
+f 275 306 305
+f 305 306 336
+f 305 336 335
+f 335 336 366
+f 335 366 365
+f 365 366 396
+f 365 396 395
+f 395 396 426
+f 395 426 425
+f 425 426 456
+f 425 456 455
+f 455 456 486
+f 455 486 485
+f 485 486 516
+f 485 516 515
+f 515 516 546
+f 515 546 545
+f 545 546 576
+f 545 576 575
+f 575 576 606
+f 575 606 605
+f 605 606 636
+f 605 636 635
+f 635 636 666
+f 635 666 665
+f 665 666 696
+f 665 696 695
+f 695 696 726
+f 695 726 725
+f 725 726 756
+f 725 756 755
+f 755 756 786
+f 755 786 785
+f 785 786 816
+f 785 816 815
+f 815 816 846
+f 815 846 845
+f 845 846 876
+f 845 876 875
+f 6 7 37
+f 6 37 36
+f 36 37 67
+f 36 67 66
+f 66 67 97
+f 66 97 96
+f 96 97 127
+f 96 127 126
+f 126 127 157
+f 126 157 156
+f 156 157 187
+f 156 187 186
+f 186 187 217
+f 186 217 216
+f 216 217 247
+f 216 247 246
+f 246 247 277
+f 246 277 276
+f 276 277 307
+f 276 307 306
+f 306 307 337
+f 306 337 336
+f 336 337 367
+f 336 367 366
+f 366 367 397
+f 366 397 396
+f 396 397 427
+f 396 427 426
+f 426 427 457
+f 426 457 456
+f 456 457 487
+f 456 487 486
+f 486 487 517
+f 486 517 516
+f 516 517 547
+f 516 547 546
+f 546 547 577
+f 546 577 576
+f 576 577 607
+f 576 607 606
+f 606 607 637
+f 606 637 636
+f 636 637 667
+f 636 667 666
+f 666 667 697
+f 666 697 696
+f 696 697 727
+f 696 727 726
+f 726 727 757
+f 726 757 756
+f 756 757 787
+f 756 787 786
+f 786 787 817
+f 786 817 816
+f 816 817 847
+f 816 847 846
+f 846 847 877
+f 846 877 876
+f 7 8 38
+f 7 38 37
+f 37 38 68
+f 37 68 67
+f 67 68 98
+f 67 98 97
+f 97 98 128
+f 97 128 127
+f 127 128 158
+f 127 158 157
+f 157 158 188
+f 157 188 187
+f 187 188 218
+f 187 218 217
+f 217 218 248
+f 217 248 247
+f 247 248 278
+f 247 278 277
+f 277 278 308
+f 277 308 307
+f 307 308 338
+f 307 338 337
+f 337 338 368
+f 337 368 367
+f 367 368 398
+f 367 398 397
+f 397 398 428
+f 397 428 427
+f 427 428 458
+f 427 458 457
+f 457 458 488
+f 457 488 487
+f 487 488 518
+f 487 518 517
+f 517 518 548
+f 517 548 547
+f 547 548 578
+f 547 578 577
+f 577 578 608
+f 577 608 607
+f 607 608 638
+f 607 638 637
+f 637 638 668
+f 637 668 667
+f 667 668 698
+f 667 698 697
+f 697 698 728
+f 697 728 727
+f 727 728 758
+f 727 758 757
+f 757 758 788
+f 757 788 787
+f 787 788 818
+f 787 818 817
+f 817 818 848
+f 817 848 847
+f 847 848 878
+f 847 878 877
+f 8 9 39
+f 8 39 38
+f 38 39 69
+f 38 69 68
+f 68 69 99
+f 68 99 98
+f 98 99 129
+f 98 129 128
+f 128 129 159
+f 128 159 158
+f 158 159 189
+f 158 189 188
+f 188 189 219
+f 188 219 218
+f 218 219 249
+f 218 249 248
+f 248 249 279
+f 248 279 278
+f 278 279 309
+f 278 309 308
+f 308 309 339
+f 308 339 338
+f 338 339 369
+f 338 369 368
+f 368 369 399
+f 368 399 398
+f 398 399 429
+f 398 429 428
+f 428 429 459
+f 428 459 458
+f 458 459 489
+f 458 489 488
+f 488 489 519
+f 488 519 518
+f 518 519 549
+f 518 549 548
+f 548 549 579
+f 548 579 578
+f 578 579 609
+f 578 609 608
+f 608 609 639
+f 608 639 638
+f 638 639 669
+f 638 669 668
+f 668 669 699
+f 668 699 698
+f 698 699 729
+f 698 729 728
+f 728 729 759
+f 728 759 758
+f 758 759 789
+f 758 789 788
+f 788 789 819
+f 788 819 818
+f 818 819 849
+f 818 849 848
+f 848 849 879
+f 848 879 878
+f 9 10 40
+f 9 40 39
+f 39 40 70
+f 39 70 69
+f 69 70 100
+f 69 100 99
+f 99 100 130
+f 99 130 129
+f 129 130 160
+f 129 160 159
+f 159 160 190
+f 159 190 189
+f 189 190 220
+f 189 220 219
+f 219 220 250
+f 219 250 249
+f 249 250 280
+f 249 280 279
+f 279 280 310
+f 279 310 309
+f 309 310 340
+f 309 340 339
+f 339 340 370
+f 339 370 369
+f 369 370 400
+f 369 400 399
+f 399 400 430
+f 399 430 429
+f 429 430 460
+f 429 460 459
+f 459 460 490
+f 459 490 489
+f 489 490 520
+f 489 520 519
+f 519 520 550
+f 519 550 549
+f 549 550 580
+f 549 580 579
+f 579 580 610
+f 579 610 609
+f 609 610 640
+f 609 640 639
+f 639 640 670
+f 639 670 669
+f 669 670 700
+f 669 700 699
+f 699 700 730
+f 699 730 729
+f 729 730 760
+f 729 760 759
+f 759 760 790
+f 759 790 789
+f 789 790 820
+f 789 820 819
+f 819 820 850
+f 819 850 849
+f 849 850 880
+f 849 880 879
+f 10 11 41
+f 10 41 40
+f 40 41 71
+f 40 71 70
+f 70 71 101
+f 70 101 100
+f 100 101 131
+f 100 131 130
+f 130 131 161
+f 130 161 160
+f 160 161 191
+f 160 191 190
+f 190 191 221
+f 190 221 220
+f 220 221 251
+f 220 251 250
+f 250 251 281
+f 250 281 280
+f 280 281 311
+f 280 311 310
+f 310 311 341
+f 310 341 340
+f 340 341 371
+f 340 371 370
+f 370 371 401
+f 370 401 400
+f 400 401 431
+f 400 431 430
+f 430 431 461
+f 430 461 460
+f 460 461 491
+f 460 491 490
+f 490 491 521
+f 490 521 520
+f 520 521 551
+f 520 551 550
+f 550 551 581
+f 550 581 580
+f 580 581 611
+f 580 611 610
+f 610 611 641
+f 610 641 640
+f 640 641 671
+f 640 671 670
+f 670 671 701
+f 670 701 700
+f 700 701 731
+f 700 731 730
+f 730 731 761
+f 730 761 760
+f 760 761 791
+f 760 791 790
+f 790 791 821
+f 790 821 820
+f 820 821 851
+f 820 851 850
+f 850 851 881
+f 850 881 880
+f 11 12 42
+f 11 42 41
+f 41 42 72
+f 41 72 71
+f 71 72 102
+f 71 102 101
+f 101 102 132
+f 101 132 131
+f 131 132 162
+f 131 162 161
+f 161 162 192
+f 161 192 191
+f 191 192 222
+f 191 222 221
+f 221 222 252
+f 221 252 251
+f 251 252 282
+f 251 282 281
+f 281 282 312
+f 281 312 311
+f 311 312 342
+f 311 342 341
+f 341 342 372
+f 341 372 371
+f 371 372 402
+f 371 402 401
+f 401 402 432
+f 401 432 431
+f 431 432 462
+f 431 462 461
+f 461 462 492
+f 461 492 491
+f 491 492 522
+f 491 522 521
+f 521 522 552
+f 521 552 551
+f 551 552 582
+f 551 582 581
+f 581 582 612
+f 581 612 611
+f 611 612 642
+f 611 642 641
+f 641 642 672
+f 641 672 671
+f 671 672 702
+f 671 702 701
+f 701 702 732
+f 701 732 731
+f 731 732 762
+f 731 762 761
+f 761 762 792
+f 761 792 791
+f 791 792 822
+f 791 822 821
+f 821 822 852
+f 821 852 851
+f 851 852 882
+f 851 882 881
+f 12 13 43
+f 12 43 42
+f 42 43 73
+f 42 73 72
+f 72 73 103
+f 72 103 102
+f 102 103 133
+f 102 133 132
+f 132 133 163
+f 132 163 162
+f 162 163 193
+f 162 193 192
+f 192 193 223
+f 192 223 222
+f 222 223 253
+f 222 253 252
+f 252 253 283
+f 252 283 282
+f 282 283 313
+f 282 313 312
+f 312 313 343
+f 312 343 342
+f 342 343 373
+f 342 373 372
+f 372 373 403
+f 372 403 402
+f 402 403 433
+f 402 433 432
+f 432 433 463
+f 432 463 462
+f 462 463 493
+f 462 493 492
+f 492 493 523
+f 492 523 522
+f 522 523 553
+f 522 553 552
+f 552 553 583
+f 552 583 582
+f 582 583 613
+f 582 613 612
+f 612 613 643
+f 612 643 642
+f 642 643 673
+f 642 673 672
+f 672 673 703
+f 672 703 702
+f 702 703 733
+f 702 733 732
+f 732 733 763
+f 732 763 762
+f 762 763 793
+f 762 793 792
+f 792 793 823
+f 792 823 822
+f 822 823 853
+f 822 853 852
+f 852 853 883
+f 852 883 882
+f 13 14 44
+f 13 44 43
+f 43 44 74
+f 43 74 73
+f 73 74 104
+f 73 104 103
+f 103 104 134
+f 103 134 133
+f 133 134 164
+f 133 164 163
+f 163 164 194
+f 163 194 193
+f 193 194 224
+f 193 224 223
+f 223 224 254
+f 223 254 253
+f 253 254 284
+f 253 284 283
+f 283 284 314
+f 283 314 313
+f 313 314 344
+f 313 344 343
+f 343 344 374
+f 343 374 373
+f 373 374 404
+f 373 404 403
+f 403 404 434
+f 403 434 433
+f 433 434 464
+f 433 464 463
+f 463 464 494
+f 463 494 493
+f 493 494 524
+f 493 524 523
+f 523 524 554
+f 523 554 553
+f 553 554 584
+f 553 584 583
+f 583 584 614
+f 583 614 613
+f 613 614 644
+f 613 644 643
+f 643 644 674
+f 643 674 673
+f 673 674 704
+f 673 704 703
+f 703 704 734
+f 703 734 733
+f 733 734 764
+f 733 764 763
+f 763 764 794
+f 763 794 793
+f 793 794 824
+f 793 824 823
+f 823 824 854
+f 823 854 853
+f 853 854 884
+f 853 884 883
+f 14 15 45
+f 14 45 44
+f 44 45 75
+f 44 75 74
+f 74 75 105
+f 74 105 104
+f 104 105 135
+f 104 135 134
+f 134 135 165
+f 134 165 164
+f 164 165 195
+f 164 195 194
+f 194 195 225
+f 194 225 224
+f 224 225 255
+f 224 255 254
+f 254 255 285
+f 254 285 284
+f 284 285 315
+f 284 315 314
+f 314 315 345
+f 314 345 344
+f 344 345 375
+f 344 375 374
+f 374 375 405
+f 374 405 404
+f 404 405 435
+f 404 435 434
+f 434 435 465
+f 434 465 464
+f 464 465 495
+f 464 495 494
+f 494 495 525
+f 494 525 524
+f 524 525 555
+f 524 555 554
+f 554 555 585
+f 554 585 584
+f 584 585 615
+f 584 615 614
+f 614 615 645
+f 614 645 644
+f 644 645 675
+f 644 675 674
+f 674 675 705
+f 674 705 704
+f 704 705 735
+f 704 735 734
+f 734 735 765
+f 734 765 764
+f 764 765 795
+f 764 795 794
+f 794 795 825
+f 794 825 824
+f 824 825 855
+f 824 855 854
+f 854 855 885
+f 854 885 884
+f 15 16 46
+f 15 46 45
+f 45 46 76
+f 45 76 75
+f 75 76 106
+f 75 106 105
+f 105 106 136
+f 105 136 135
+f 135 136 166
+f 135 166 165
+f 165 166 196
+f 165 196 195
+f 195 196 226
+f 195 226 225
+f 225 226 256
+f 225 256 255
+f 255 256 286
+f 255 286 285
+f 285 286 316
+f 285 316 315
+f 315 316 346
+f 315 346 345
+f 345 346 376
+f 345 376 375
+f 375 376 406
+f 375 406 405
+f 405 406 436
+f 405 436 435
+f 435 436 466
+f 435 466 465
+f 465 466 496
+f 465 496 495
+f 495 496 526
+f 495 526 525
+f 525 526 556
+f 525 556 555
+f 555 556 586
+f 555 586 585
+f 585 586 616
+f 585 616 615
+f 615 616 646
+f 615 646 645
+f 645 646 676
+f 645 676 675
+f 675 676 706
+f 675 706 705
+f 705 706 736
+f 705 736 735
+f 735 736 766
+f 735 766 765
+f 765 766 796
+f 765 796 795
+f 795 796 826
+f 795 826 825
+f 825 826 856
+f 825 856 855
+f 855 856 886
+f 855 886 885
+f 16 17 47
+f 16 47 46
+f 46 47 77
+f 46 77 76
+f 76 77 107
+f 76 107 106
+f 106 107 137
+f 106 137 136
+f 136 137 167
+f 136 167 166
+f 166 167 197
+f 166 197 196
+f 196 197 227
+f 196 227 226
+f 226 227 257
+f 226 257 256
+f 256 257 287
+f 256 287 286
+f 286 287 317
+f 286 317 316
+f 316 317 347
+f 316 347 346
+f 346 347 377
+f 346 377 376
+f 376 377 407
+f 376 407 406
+f 406 407 437
+f 406 437 436
+f 436 437 467
+f 436 467 466
+f 466 467 497
+f 466 497 496
+f 496 497 527
+f 496 527 526
+f 526 527 557
+f 526 557 556
+f 556 557 587
+f 556 587 586
+f 586 587 617
+f 586 617 616
+f 616 617 647
+f 616 647 646
+f 646 647 677
+f 646 677 676
+f 676 677 707
+f 676 707 706
+f 706 707 737
+f 706 737 736
+f 736 737 767
+f 736 767 766
+f 766 767 797
+f 766 797 796
+f 796 797 827
+f 796 827 826
+f 826 827 857
+f 826 857 856
+f 856 857 887
+f 856 887 886
+f 17 18 48
+f 17 48 47
+f 47 48 78
+f 47 78 77
+f 77 78 108
+f 77 108 107
+f 107 108 138
+f 107 138 137
+f 137 138 168
+f 137 168 167
+f 167 168 198
+f 167 198 197
+f 197 198 228
+f 197 228 227
+f 227 228 258
+f 227 258 257
+f 257 258 288
+f 257 288 287
+f 287 288 318
+f 287 318 317
+f 317 318 348
+f 317 348 347
+f 347 348 378
+f 347 378 377
+f 377 378 408
+f 377 408 407
+f 407 408 438
+f 407 438 437
+f 437 438 468
+f 437 468 467
+f 467 468 498
+f 467 498 497
+f 497 498 528
+f 497 528 527
+f 527 528 558
+f 527 558 557
+f 557 558 588
+f 557 588 587
+f 587 588 618
+f 587 618 617
+f 617 618 648
+f 617 648 647
+f 647 648 678
+f 647 678 677
+f 677 678 708
+f 677 708 707
+f 707 708 738
+f 707 738 737
+f 737 738 768
+f 737 768 767
+f 767 768 798
+f 767 798 797
+f 797 798 828
+f 797 828 827
+f 827 828 858
+f 827 858 857
+f 857 858 888
+f 857 888 887
+f 18 19 49
+f 18 49 48
+f 48 49 79
+f 48 79 78
+f 78 79 109
+f 78 109 108
+f 108 109 139
+f 108 139 138
+f 138 139 169
+f 138 169 168
+f 168 169 199
+f 168 199 198
+f 198 199 229
+f 198 229 228
+f 228 229 259
+f 228 259 258
+f 258 259 289
+f 258 289 288
+f 288 289 319
+f 288 319 318
+f 318 319 349
+f 318 349 348
+f 348 349 379
+f 348 379 378
+f 378 379 409
+f 378 409 408
+f 408 409 439
+f 408 439 438
+f 438 439 469
+f 438 469 468
+f 468 469 499
+f 468 499 498
+f 498 499 529
+f 498 529 528
+f 528 529 559
+f 528 559 558
+f 558 559 589
+f 558 589 588
+f 588 589 619
+f 588 619 618
+f 618 619 649
+f 618 649 648
+f 648 649 679
+f 648 679 678
+f 678 679 709
+f 678 709 708
+f 708 709 739
+f 708 739 738
+f 738 739 769
+f 738 769 768
+f 768 769 799
+f 768 799 798
+f 798 799 829
+f 798 829 828
+f 828 829 859
+f 828 859 858
+f 858 859 889
+f 858 889 888
+f 19 20 50
+f 19 50 49
+f 49 50 80
+f 49 80 79
+f 79 80 110
+f 79 110 109
+f 109 110 140
+f 109 140 139
+f 139 140 170
+f 139 170 169
+f 169 170 200
+f 169 200 199
+f 199 200 230
+f 199 230 229
+f 229 230 260
+f 229 260 259
+f 259 260 290
+f 259 290 289
+f 289 290 320
+f 289 320 319
+f 319 320 350
+f 319 350 349
+f 349 350 380
+f 349 380 379
+f 379 380 410
+f 379 410 409
+f 409 410 440
+f 409 440 439
+f 439 440 470
+f 439 470 469
+f 469 470 500
+f 469 500 499
+f 499 500 530
+f 499 530 529
+f 529 530 560
+f 529 560 559
+f 559 560 590
+f 559 590 589
+f 589 590 620
+f 589 620 619
+f 619 620 650
+f 619 650 649
+f 649 650 680
+f 649 680 679
+f 679 680 710
+f 679 710 709
+f 709 710 740
+f 709 740 739
+f 739 740 770
+f 739 770 769
+f 769 770 800
+f 769 800 799
+f 799 800 830
+f 799 830 829
+f 829 830 860
+f 829 860 859
+f 859 860 890
+f 859 890 889
+f 20 21 51
+f 20 51 50
+f 50 51 81
+f 50 81 80
+f 80 81 111
+f 80 111 110
+f 110 111 141
+f 110 141 140
+f 140 141 171
+f 140 171 170
+f 170 171 201
+f 170 201 200
+f 200 201 231
+f 200 231 230
+f 230 231 261
+f 230 261 260
+f 260 261 291
+f 260 291 290
+f 290 291 321
+f 290 321 320
+f 320 321 351
+f 320 351 350
+f 350 351 381
+f 350 381 380
+f 380 381 411
+f 380 411 410
+f 410 411 441
+f 410 441 440
+f 440 441 471
+f 440 471 470
+f 470 471 501
+f 470 501 500
+f 500 501 531
+f 500 531 530
+f 530 531 561
+f 530 561 560
+f 560 561 591
+f 560 591 590
+f 590 591 621
+f 590 621 620
+f 620 621 651
+f 620 651 650
+f 650 651 681
+f 650 681 680
+f 680 681 711
+f 680 711 710
+f 710 711 741
+f 710 741 740
+f 740 741 771
+f 740 771 770
+f 770 771 801
+f 770 801 800
+f 800 801 831
+f 800 831 830
+f 830 831 861
+f 830 861 860
+f 860 861 891
+f 860 891 890
+f 21 22 52
+f 21 52 51
+f 51 52 82
+f 51 82 81
+f 81 82 112
+f 81 112 111
+f 111 112 142
+f 111 142 141
+f 141 142 172
+f 141 172 171
+f 171 172 202
+f 171 202 201
+f 201 202 232
+f 201 232 231
+f 231 232 262
+f 231 262 261
+f 261 262 292
+f 261 292 291
+f 291 292 322
+f 291 322 321
+f 321 322 352
+f 321 352 351
+f 351 352 382
+f 351 382 381
+f 381 382 412
+f 381 412 411
+f 411 412 442
+f 411 442 441
+f 441 442 472
+f 441 472 471
+f 471 472 502
+f 471 502 501
+f 501 502 532
+f 501 532 531
+f 531 532 562
+f 531 562 561
+f 561 562 592
+f 561 592 591
+f 591 592 622
+f 591 622 621
+f 621 622 652
+f 621 652 651
+f 651 652 682
+f 651 682 681
+f 681 682 712
+f 681 712 711
+f 711 712 742
+f 711 742 741
+f 741 742 772
+f 741 772 771
+f 771 772 802
+f 771 802 801
+f 801 802 832
+f 801 832 831
+f 831 832 862
+f 831 862 861
+f 861 862 892
+f 861 892 891
+f 22 23 53
+f 22 53 52
+f 52 53 83
+f 52 83 82
+f 82 83 113
+f 82 113 112
+f 112 113 143
+f 112 143 142
+f 142 143 173
+f 142 173 172
+f 172 173 203
+f 172 203 202
+f 202 203 233
+f 202 233 232
+f 232 233 263
+f 232 263 262
+f 262 263 293
+f 262 293 292
+f 292 293 323
+f 292 323 322
+f 322 323 353
+f 322 353 352
+f 352 353 383
+f 352 383 382
+f 382 383 413
+f 382 413 412
+f 412 413 443
+f 412 443 442
+f 442 443 473
+f 442 473 472
+f 472 473 503
+f 472 503 502
+f 502 503 533
+f 502 533 532
+f 532 533 563
+f 532 563 562
+f 562 563 593
+f 562 593 592
+f 592 593 623
+f 592 623 622
+f 622 623 653
+f 622 653 652
+f 652 653 683
+f 652 683 682
+f 682 683 713
+f 682 713 712
+f 712 713 743
+f 712 743 742
+f 742 743 773
+f 742 773 772
+f 772 773 803
+f 772 803 802
+f 802 803 833
+f 802 833 832
+f 832 833 863
+f 832 863 862
+f 862 863 893
+f 862 893 892
+f 23 24 54
+f 23 54 53
+f 53 54 84
+f 53 84 83
+f 83 84 114
+f 83 114 113
+f 113 114 144
+f 113 144 143
+f 143 144 174
+f 143 174 173
+f 173 174 204
+f 173 204 203
+f 203 204 234
+f 203 234 233
+f 233 234 264
+f 233 264 263
+f 263 264 294
+f 263 294 293
+f 293 294 324
+f 293 324 323
+f 323 324 354
+f 323 354 353
+f 353 354 384
+f 353 384 383
+f 383 384 414
+f 383 414 413
+f 413 414 444
+f 413 444 443
+f 443 444 474
+f 443 474 473
+f 473 474 504
+f 473 504 503
+f 503 504 534
+f 503 534 533
+f 533 534 564
+f 533 564 563
+f 563 564 594
+f 563 594 593
+f 593 594 624
+f 593 624 623
+f 623 624 654
+f 623 654 653
+f 653 654 684
+f 653 684 683
+f 683 684 714
+f 683 714 713
+f 713 714 744
+f 713 744 743
+f 743 744 774
+f 743 774 773
+f 773 774 804
+f 773 804 803
+f 803 804 834
+f 803 834 833
+f 833 834 864
+f 833 864 863
+f 863 864 894
+f 863 894 893
+f 24 25 55
+f 24 55 54
+f 54 55 85
+f 54 85 84
+f 84 85 115
+f 84 115 114
+f 114 115 145
+f 114 145 144
+f 144 145 175
+f 144 175 174
+f 174 175 205
+f 174 205 204
+f 204 205 235
+f 204 235 234
+f 234 235 265
+f 234 265 264
+f 264 265 295
+f 264 295 294
+f 294 295 325
+f 294 325 324
+f 324 325 355
+f 324 355 354
+f 354 355 385
+f 354 385 384
+f 384 385 415
+f 384 415 414
+f 414 415 445
+f 414 445 444
+f 444 445 475
+f 444 475 474
+f 474 475 505
+f 474 505 504
+f 504 505 535
+f 504 535 534
+f 534 535 565
+f 534 565 564
+f 564 565 595
+f 564 595 594
+f 594 595 625
+f 594 625 624
+f 624 625 655
+f 624 655 654
+f 654 655 685
+f 654 685 684
+f 684 685 715
+f 684 715 714
+f 714 715 745
+f 714 745 744
+f 744 745 775
+f 744 775 774
+f 774 775 805
+f 774 805 804
+f 804 805 835
+f 804 835 834
+f 834 835 865
+f 834 865 864
+f 864 865 895
+f 864 895 894
+f 25 26 56
+f 25 56 55
+f 55 56 86
+f 55 86 85
+f 85 86 116
+f 85 116 115
+f 115 116 146
+f 115 146 145
+f 145 146 176
+f 145 176 175
+f 175 176 206
+f 175 206 205
+f 205 206 236
+f 205 236 235
+f 235 236 266
+f 235 266 265
+f 265 266 296
+f 265 296 295
+f 295 296 326
+f 295 326 325
+f 325 326 356
+f 325 356 355
+f 355 356 386
+f 355 386 385
+f 385 386 416
+f 385 416 415
+f 415 416 446
+f 415 446 445
+f 445 446 476
+f 445 476 475
+f 475 476 506
+f 475 506 505
+f 505 506 536
+f 505 536 535
+f 535 536 566
+f 535 566 565
+f 565 566 596
+f 565 596 595
+f 595 596 626
+f 595 626 625
+f 625 626 656
+f 625 656 655
+f 655 656 686
+f 655 686 685
+f 685 686 716
+f 685 716 715
+f 715 716 746
+f 715 746 745
+f 745 746 776
+f 745 776 775
+f 775 776 806
+f 775 806 805
+f 805 806 836
+f 805 836 835
+f 835 836 866
+f 835 866 865
+f 865 866 896
+f 865 896 895
+f 26 27 57
+f 26 57 56
+f 56 57 87
+f 56 87 86
+f 86 87 117
+f 86 117 116
+f 116 117 147
+f 116 147 146
+f 146 147 177
+f 146 177 176
+f 176 177 207
+f 176 207 206
+f 206 207 237
+f 206 237 236
+f 236 237 267
+f 236 267 266
+f 266 267 297
+f 266 297 296
+f 296 297 327
+f 296 327 326
+f 326 327 357
+f 326 357 356
+f 356 357 387
+f 356 387 386
+f 386 387 417
+f 386 417 416
+f 416 417 447
+f 416 447 446
+f 446 447 477
+f 446 477 476
+f 476 477 507
+f 476 507 506
+f 506 507 537
+f 506 537 536
+f 536 537 567
+f 536 567 566
+f 566 567 597
+f 566 597 596
+f 596 597 627
+f 596 627 626
+f 626 627 657
+f 626 657 656
+f 656 657 687
+f 656 687 686
+f 686 687 717
+f 686 717 716
+f 716 717 747
+f 716 747 746
+f 746 747 777
+f 746 777 776
+f 776 777 807
+f 776 807 806
+f 806 807 837
+f 806 837 836
+f 836 837 867
+f 836 867 866
+f 866 867 897
+f 866 897 896
+f 27 28 58
+f 27 58 57
+f 57 58 88
+f 57 88 87
+f 87 88 118
+f 87 118 117
+f 117 118 148
+f 117 148 147
+f 147 148 178
+f 147 178 177
+f 177 178 208
+f 177 208 207
+f 207 208 238
+f 207 238 237
+f 237 238 268
+f 237 268 267
+f 267 268 298
+f 267 298 297
+f 297 298 328
+f 297 328 327
+f 327 328 358
+f 327 358 357
+f 357 358 388
+f 357 388 387
+f 387 388 418
+f 387 418 417
+f 417 418 448
+f 417 448 447
+f 447 448 478
+f 447 478 477
+f 477 478 508
+f 477 508 507
+f 507 508 538
+f 507 538 537
+f 537 538 568
+f 537 568 567
+f 567 568 598
+f 567 598 597
+f 597 598 628
+f 597 628 627
+f 627 628 658
+f 627 658 657
+f 657 658 688
+f 657 688 687
+f 687 688 718
+f 687 718 717
+f 717 718 748
+f 717 748 747
+f 747 748 778
+f 747 778 777
+f 777 778 808
+f 777 808 807
+f 807 808 838
+f 807 838 837
+f 837 838 868
+f 837 868 867
+f 867 868 898
+f 867 898 897
+f 28 29 59
+f 28 59 58
+f 58 59 89
+f 58 89 88
+f 88 89 119
+f 88 119 118
+f 118 119 149
+f 118 149 148
+f 148 149 179
+f 148 179 178
+f 178 179 209
+f 178 209 208
+f 208 209 239
+f 208 239 238
+f 238 239 269
+f 238 269 268
+f 268 269 299
+f 268 299 298
+f 298 299 329
+f 298 329 328
+f 328 329 359
+f 328 359 358
+f 358 359 389
+f 358 389 388
+f 388 389 419
+f 388 419 418
+f 418 419 449
+f 418 449 448
+f 448 449 479
+f 448 479 478
+f 478 479 509
+f 478 509 508
+f 508 509 539
+f 508 539 538
+f 538 539 569
+f 538 569 568
+f 568 569 599
+f 568 599 598
+f 598 599 629
+f 598 629 628
+f 628 629 659
+f 628 659 658
+f 658 659 689
+f 658 689 688
+f 688 689 719
+f 688 719 718
+f 718 719 749
+f 718 749 748
+f 748 749 779
+f 748 779 778
+f 778 779 809
+f 778 809 808
+f 808 809 839
+f 808 839 838
+f 838 839 869
+f 838 869 868
+f 868 869 899
+f 868 899 898
+f 29 30 60
+f 29 60 59
+f 59 60 90
+f 59 90 89
+f 89 90 120
+f 89 120 119
+f 119 120 150
+f 119 150 149
+f 149 150 180
+f 149 180 179
+f 179 180 210
+f 179 210 209
+f 209 210 240
+f 209 240 239
+f 239 240 270
+f 239 270 269
+f 269 270 300
+f 269 300 299
+f 299 300 330
+f 299 330 329
+f 329 330 360
+f 329 360 359
+f 359 360 390
+f 359 390 389
+f 389 390 420
+f 389 420 419
+f 419 420 450
+f 419 450 449
+f 449 450 480
+f 449 480 479
+f 479 480 510
+f 479 510 509
+f 509 510 540
+f 509 540 539
+f 539 540 570
+f 539 570 569
+f 569 570 600
+f 569 600 599
+f 599 600 630
+f 599 630 629
+f 629 630 660
+f 629 660 659
+f 659 660 690
+f 659 690 689
+f 689 690 720
+f 689 720 719
+f 719 720 750
+f 719 750 749
+f 749 750 780
+f 749 780 779
+f 779 780 810
+f 779 810 809
+f 809 810 840
+f 809 840 839
+f 839 840 870
+f 839 870 869
+f 869 870 900
+f 869 900 899
diff --git a/examples/pybullet/gym/pybullet_data/testdata/__init__.py b/examples/pybullet/gym/pybullet_data/testdata/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/__init__.py b/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/accelerometer.npy b/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/accelerometer.npy
new file mode 100644
index 000000000..5201d49f1
Binary files /dev/null and b/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/accelerometer.npy differ
diff --git a/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/estimated_velocities.npy b/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/estimated_velocities.npy
new file mode 100644
index 000000000..cb988fe3d
Binary files /dev/null and b/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/estimated_velocities.npy differ
diff --git a/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/feet_contact_forces.npy b/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/feet_contact_forces.npy
new file mode 100644
index 000000000..b235bdc36
Binary files /dev/null and b/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/feet_contact_forces.npy differ
diff --git a/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/gyroscope.npy b/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/gyroscope.npy
new file mode 100644
index 000000000..a446a2ba7
Binary files /dev/null and b/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/gyroscope.npy differ
diff --git a/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/jacobians.npy b/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/jacobians.npy
new file mode 100644
index 000000000..5f7bd7a14
Binary files /dev/null and b/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/jacobians.npy differ
diff --git a/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/motor_velocities.npy b/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/motor_velocities.npy
new file mode 100644
index 000000000..1cb98d24a
Binary files /dev/null and b/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/motor_velocities.npy differ
diff --git a/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/timestamp.npy b/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/timestamp.npy
new file mode 100644
index 000000000..79384090a
Binary files /dev/null and b/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/timestamp.npy differ
diff --git a/examples/pybullet/gym/pybullet_data/tex256.png b/examples/pybullet/gym/pybullet_data/tex256.png
new file mode 100644
index 000000000..130d4aade
Binary files /dev/null and b/examples/pybullet/gym/pybullet_data/tex256.png differ
diff --git a/examples/pybullet/gym/pybullet_data/torus.vtk b/examples/pybullet/gym/pybullet_data/torus.vtk
new file mode 100644
index 000000000..a06ef2061
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/torus.vtk
@@ -0,0 +1,9917 @@
+# vtk DataFile Version 2.0
+torus_, Created by Gmsh
+ASCII
+DATASET UNSTRUCTURED_GRID
+POINTS 1036 double
+-0.75 -2.18556941e-08 1.13246855e-07
+-0.71031338 -0.135160238 1.07254337e-07
+-0.710313141 0.135160565 1.07254301e-07
+-0.692909718 -2.18556941e-08 -0.287012398
+-0.692909598 -2.18556941e-08 0.287012607
+-0.65624404 -0.135160238 -0.271825016
+-0.6562439799999999 -0.135160238 0.271825194
+-0.656243861 0.135160565 -0.271824926
+-0.656243742 0.135160565 0.271825105
+-0.603853762 -0.227407992 9.11793805e-08
+-0.603853405 0.227408156 9.11793308e-08
+-0.558141589621361 -0.2272469457790709 -0.2307452454467901
+-0.557888091 -0.227407992 0.231084868
+-0.557887852 0.227408156 -0.23108457
+-0.557887793 0.227408156 0.231084719
+-0.530330241 -2.18556941e-08 -0.530329943
+-0.50226754 -0.135160238 -0.5022673010000001
+-0.50226742 -0.135160238 0.50226742
+-0.50226742 0.135160565 -0.502267122
+-0.502267241 0.135160565 0.502267241
+-0.464421362 -0.247455373 7.01256795e-08
+-0.464420974 0.247455314 7.01256155e-08
+-0.42906943 -0.247455373 -0.177726254
+-0.4296712671318722 -0.247455373 0.1747004492923097
+-0.429069072 0.247455314 -0.17772612
+-0.429069012 0.247455314 0.177726239
+-0.426989228 -0.227407992 -0.426988989
+-0.426989079 -0.227407992 0.426989079
+-0.426988959 0.227408156 -0.426988721
+-0.42698884 0.227408156 0.42698884
+-0.336284906 -0.18893747 5.07776079e-08
+-0.336284608 0.188937217 5.07775653e-08
+-0.328395605 -0.247455373 -0.328395396
+-0.328395486 -0.247455373 0.328395486
+-0.328395337 0.247455314 -0.328395128
+-0.328395218 0.247455314 0.328395218
+-0.310686767 -0.18893747 -0.128690585
+-0.310686737 -0.18893747 0.128690675
+-0.310686499 0.188937217 -0.128690481
+-0.310686469 0.188937217 0.128690571
+-0.287012696 -2.18556941e-08 -0.692909598
+-0.287012637 -2.18556941e-08 0.692909598
+-0.271825254 -0.135160238 -0.6562439799999999
+-0.271825224 -0.135160238 0.6562439799999999
+-0.271825165 0.135160565 -0.656243742
+-0.271825135 0.135160565 0.656243742
+-0.260126799 -0.07043330370000001 3.92780564e-08
+-0.26012671 0.07043293120000001 3.92780422e-08
+-0.240325853 -0.07043330370000001 -0.09954615679999999
+-0.240325823 -0.07043330370000001 0.0995462313
+-0.240325764 0.07043293120000001 -0.099546127
+-0.240325734 0.07043293120000001 0.0995461941
+-0.237789407 -0.18893747 -0.237789273
+-0.237789333 -0.18893747 0.237789333
+-0.237789199 0.188937217 -0.237789065
+-0.237789124 0.188937217 0.237789124
+-0.231084913 -0.227407992 -0.557888091
+-0.231084883 -0.227407992 0.557888091
+-0.231084779 0.227408156 -0.557887793
+-0.231084749 0.227408156 0.557887793
+-0.18393749 -0.07043330370000001 -0.183937371
+-0.183937415 -0.07043330370000001 0.183937415
+-0.183937415 0.07043293120000001 -0.183937311
+-0.1826027894390168 0.07043797995747503 0.1848329123509294
+-0.177726433 -0.247455373 -0.42906937
+-0.177726403 -0.247455373 0.42906937
+-0.177726284 0.247455314 -0.429069012
+-0.177726254 0.247455314 0.429069012
+-0.128690705 -0.18893747 -0.310686737
+-0.12869069 -0.18893747 0.310686737
+-0.1286906 0.188937217 -0.310686469
+-0.128690571 0.188937217 0.310686469
+-0.0995462537 -0.07043330370000001 -0.240325823
+-0.09954623880000001 -0.07043330370000001 0.240325823
+-0.0995462164 0.07043293120000001 -0.240325734
+-0.0995462015 0.07043293120000001 0.240325734
+-3.27835394e-08 -2.18556941e-08 0.75
+-3.10487849e-08 -0.135160238 0.71031338
+-0.002247339125087955 0.135160565 0.7098661234313948
+-2.6395286e-08 -0.227407992 0.603853762
+-2.639527e-08 0.227408156 0.603853405
+0.005294134150853763 -0.247455373 0.4633682886880875
+-2.03004848e-08 0.247455314 0.464420974
+-1.46994799e-08 -0.18893747 0.336284906
+-1.46994674e-08 0.188937217 0.336284608
+-1.13705036e-08 -0.07043330370000001 0.260126799
+-1.13704992e-08 0.07043293120000001 0.26012671
+3.10198112e-09 -0.07043330370000001 -0.260126799
+3.10198001e-09 0.07043293120000001 -0.26012671
+4.01015754e-09 -0.18893747 -0.336284906
+4.01015399e-09 0.188937217 -0.336284608
+5.53816948e-09 -0.247455373 -0.464421362
+5.53816459e-09 0.247455314 -0.464420974
+7.20088389e-09 -0.227407992 -0.603853762
+7.20087989e-09 0.227408156 -0.603853405
+8.470402160000001e-09 -0.135160238 -0.71031338
+8.470399489999999e-09 0.135160565 -0.710313141
+8.943660029999999e-09 -2.18556941e-08 -0.75
+0.0995461792 0.07043293120000001 0.240325734
+0.0995462164 -0.07043330370000001 0.240325823
+0.09954622389999999 0.07043293120000001 -0.240325719
+0.09954626110000001 -0.07043330370000001 -0.240325809
+0.128690541 0.188937217 0.310686469
+0.1286906 0.188937217 -0.310686439
+0.12869066 -0.18893747 0.310686737
+0.12869072 -0.18893747 -0.310686707
+0.177726209 0.247455314 0.429069012
+0.177726299 0.247455314 -0.429068983
+0.177726358 -0.247455373 0.42906937
+0.177726448 -0.247455373 -0.42906934
+0.183937356 0.07043293120000001 0.183937356
+0.183937415 -0.07043330370000001 0.183937415
+0.183937415 0.07043293120000001 -0.183937296
+0.18393749 -0.07043330370000001 -0.183937356
+0.231084689 0.227408156 0.557887793
+0.231084794 0.227408156 -0.5578877330000001
+0.231084824 -0.227407992 0.557888091
+0.231084928 -0.227407992 -0.557888091
+0.237789124 0.188937217 0.237789124
+0.237789199 0.188937217 -0.237789035
+0.237789333 -0.18893747 0.237789333
+0.237789407 -0.18893747 -0.237789258
+0.240325734 0.07043293120000001 0.0995461866
+0.240325794 0.07043293120000001 -0.09954606739999999
+0.2405965857521089 -0.07043330370000001 0.09818501000602463
+0.240325883 -0.07043330370000001 -0.0995460972
+0.26012671 0.07043293120000001 1.69520092e-07
+0.260126799 -0.07043330370000001 1.69520149e-07
+0.271825075 0.135160565 0.656243742
+0.271825165 -0.135160238 0.6562439799999999
+0.271825194 0.135160565 -0.6562436820000001
+0.271825284 -0.135160238 -0.65624392
+0.287012577 -2.18556941e-08 0.692909598
+0.287012696 -2.18556941e-08 -0.692909598
+0.310686469 0.188937217 0.128690556
+0.310686529 0.188937217 -0.128690392
+0.310686737 -0.18893747 0.128690675
+0.310686827 -0.18893747 -0.128690511
+0.328395218 0.247455314 0.328395218
+0.328395337 0.247455314 -0.328395098
+0.328395486 -0.247455373 0.328395486
+0.328395605 -0.247455373 -0.328395367
+0.336284608 0.188937217 2.19150877e-07
+0.336284906 -0.18893747 2.19151062e-07
+0.42698884 0.227408156 0.42698884
+0.426988959 0.227408156 -0.426988691
+0.426989079 -0.227407992 0.426989079
+0.426989228 -0.227407992 -0.42698893
+0.429069012 0.247455314 0.177726224
+0.429069132 0.247455314 -0.177726001
+0.4285832038493428 -0.2473905309719522 0.1782033175787506
+0.429069489 -0.247455373 -0.17772615
+0.464420974 0.247455314 3.02655138e-07
+0.464421362 -0.247455373 3.02655394e-07
+0.502267241 0.135160565 0.502267241
+0.50226742 -0.135160238 0.50226742
+0.50226742 0.135160565 -0.502267063
+0.50226754 -0.135160238 -0.502267241
+0.530330062 -2.18556941e-08 0.530330062
+0.530330241 -2.18556941e-08 -0.5303298829999999
+0.557887793 0.227408156 0.231084704
+0.557887912 0.227408156 -0.231084421
+0.557888091 -0.227407992 0.231084839
+0.557888269 -0.227407992 -0.231084555
+0.603853405 0.227408156 3.9352085e-07
+0.603853762 -0.227407992 3.93521077e-07
+0.656243742 0.135160565 0.271825075
+0.65624392 0.135160565 -0.271824747
+0.6562439799999999 -0.135160238 0.271825165
+0.656244159 -0.135160238 -0.271824837
+0.692909598 -2.18556941e-08 0.287012577
+0.692909837 -2.18556941e-08 -0.287012219
+0.710313141 0.135160565 4.62898811e-07
+0.71031338 -0.135160238 4.62898981e-07
+0.75 -2.18556941e-08 4.88762055e-07
+0.2148047599579061 0.05862636186789426 0.502491616306411
+0.08708371449759361 -0.07904678396405236 0.4544761890310727
+-0.146602316861926 -0.009632696391538147 -0.315119087525655
+0.1692244930490057 0.1812843605 0.623422415988969
+0.2079651687174781 0.1331645320914401 -0.5836410988931576
+0.1536632925775345 0.1239510807482383 0.4572738201945878
+-0.5671834958762344 -0.01939125278810325 -0.1175294783630399
+0.5569435355 0.06758027157215296 0.45546928025
+0.45546936975 -0.06758012992784704 0.5569436249999999
+-0.6446421632553362 -0.05976163801653798 -0.1092778761071423
+-0.6214703736046712 -0.0666511357727524 0.1250423258651282
+0.300758051598111 0.07172693423651585 0.4583027905022347
+-0.654305795925001 0.03357102801595683 0.001848341005699126
+-0.6258175216303261 0.05432506588159775 0.1162604558790866
+-0.442057131510203 0.03095741689814975 -0.2765487156973705
+0.30043105825 -0.181284115 0.5743412825
+0.6159713774510746 0.1837769934522046 -0.1922216808197105
+0.5322994338378045 0.008502274921264123 -0.3744977122371063
+0.04265738067802943 -0.1046145807176613 -0.4450745330079127
+0.1246996850352893 0.06574632593164867 -0.7058907283422399
+0.414704820039705 0.05850602124603817 -0.5873244043967869
+-0.5997874394191635 -0.06652994583433987 -0.3918934603056267
+0.4105207575547945 -0.02280618123261142 -0.4112567807811419
+0.1154383286484265 -0.0670131696695989 0.3444306649723132
+-0.5457887659485642 -0.1555429921777584 -0.2112258584708629
+-0.556036820477818 -0.1322847934326201 -0.08914487821102607
+-0.5287910658346734 -0.145964002948216 0.09942815883918801
+-0.5488808869220985 -0.1230845936541704 0.1994070919703603
+-0.5514166647614607 -0.06021660203561584 0.006754432547757371
+-0.5410083912396699 -0.04992438886868006 0.1097211985421525
+-0.5282477366216665 -0.0457190959344033 0.2285741534938628
+-0.5471074779992053 -0.03863338164943837 0.3383833878444414
+-0.5379197870483288 0.04712173579130802 -0.311524103556143
+-0.5444049150912931 0.04459180613937631 0.004718340344478199
+-0.5221095018638044 0.0369594492107734 0.119071275868263
+-0.5308799781628447 0.04747798722078263 0.3117821923412912
+0.4475426814502823 0.1489278561181345 -0.2688886257995359
+0.2996792148585254 0.1654024873724193 -0.4496521940991762
+0.0006848522370035207 -0.1023848032902419 0.5037054741521728
+-0.5278138632473582 0.1468044649733942 0.09245296480095991
+-0.5412677375001846 0.1432254272463678 0.2064531176041771
+-0.4231612307707837 -0.1037401749254241 -0.1337060640227804
+0.3305945547851408 0.01949853570690828 0.5514300148461841
+-0.4037618125828935 -0.1605445022397436 -0.336392455313646
+-0.4247760234915319 -0.1530323475734049 0.2024351829109144
+-0.4495954867690495 -0.1502396951148145 0.3301820862519624
+-0.4408269404029921 -0.07227179543752946 -0.0169300934674944
+-0.4374801290473587 -0.07174959795745348 0.1003803329881969
+-0.4416965121862891 -0.04176342978079188 0.3173556488626441
+-0.4210349730051253 -0.04504186570420775 0.4288698757126937
+-0.4469474911194326 0.05062181151367538 -0.4380625839577525
+-0.4208545034966771 0.02789523773719308 -0.124023188409767
+-0.4426622831200132 0.05234302201265242 -0.01514508992852398
+-0.4508707370426806 0.07401740893526983 0.235274710670023
+-0.4193989654820721 0.05283121747753344 0.3293365305174706
+-0.429259712191232 0.04147422157022783 0.4546009980040832
+-0.4365211327554736 0.1347201315161209 -0.3271173605683869
+-0.4285450353537538 0.1540513903482495 0.1047628151789162
+-0.4403871896259152 0.1709419521191819 0.210772629951794
+-0.4174665753576615 0.1439620252099086 0.3300534389646202
+0.2079223709471942 -0.1155629893027928 -0.3256443507170392
+0.4773776328763831 0.08791904828105823 -0.3315091410248636
+0.6109661671423812 -0.1312233317339073 -0.04826091956918894
+-0.3210912808149326 0.09886036457796581 -0.3443362945660071
+-0.5107203347875002 -0.0511037324690895 -0.2162246520966066
+-0.4274951252036663 -0.03507233851382638 -0.1978104046406515
+-0.3221922211749222 -0.1445975170440534 -0.4425145553301829
+-0.3286954227264652 -0.1434492302514711 0.2222906885168947
+-0.3285017090814557 -0.1265089069691785 0.308151421041821
+-0.3395649970840241 -0.1554064149526791 0.4225686128741589
+-0.3290190794293953 -0.05254108065203134 -0.5467141964176405
+-0.3159309151850979 -0.04120775039923903 -0.3224716358271946
+-0.3258398369406388 -0.03510117790117903 -0.1134249561622734
+-0.3376001506561476 -0.05717751503157612 0.102342810283574
+-0.3114520801316878 -0.03409065073118795 0.2177680683076604
+-0.328474165477487 -0.02648855206584399 0.3310258350020063
+-0.3225401600591715 0.05796838860563992 -0.5319520165267214
+0.2015107240799966 -0.0348512362201389 -0.4020904760206556
+-0.3346454802644096 0.05207433096846966 -0.01301303916511887
+-0.324625531231217 0.04835716156963638 0.1075824478319873
+-0.3479391062280647 -0.126665390258168 -0.2287931817071033
+-0.3339391802062537 0.08638104506876045 0.4153871716356871
+-0.3437931748730649 0.06799499691212332 0.5252200011202918
+0.2457413693967162 0.1472628478350848 0.429295858344446
+0.5295492770417918 0.1093862086104987 0.2733651267765829
+-0.3160302043757477 0.1364302962912125 -0.2386521499676249
+-0.3325980279608176 0.1444494011689514 0.2364387692234707
+-0.2930387724981217 0.1411993925395293 0.34719695896367
+-0.3437518688936977 0.1722947186730353 0.4268002410906342
+-0.3002781246773413 -0.05656847936871779 0.5773421353060838
+0.6116200389999999 -2.18556941e-08 -0.408671051
+0.4086714685 -2.18556941e-08 -0.6116197404999999
+-0.6116199795 -2.18556941e-08 -0.4086711705
+0.4086713195 -2.18556941e-08 0.61161983
+0.61161983 -2.18556941e-08 0.4086713195
+-0.3961672556480173 -6.155555294249769e-05 0.6199534874067836
+0.2898606410459698 0.09953935377376469 -0.5721343919035593
+0.1274258562189954 0.1820950256176694 -0.6308011075224357
+-0.2983307807445332 0.1419806263712661 -0.4269982341363224
+-0.01753325163574215 -0.1046126048589783 0.5737683073312927
+-0.1065471808270385 0.08839229482936621 -0.6047472533910651
+-0.5078357084441076 -0.1134888177294395 -0.3438187367651265
+0.07138585320850009 -0.05840339156309945 0.6482034465556364
+0.1875194341956316 -0.06385973718928681 0.6174557333286409
+-0.1940775370390703 0.1189988285552581 -0.5653164243045676
+0.210606074927093 -0.07955350966472875 -0.5157443637988349
+-0.2329323291013624 -0.1248761065775685 -0.5354600680477379
+0.4529133510639263 0.1027731533397032 0.4209756521472898
+-0.4260979781625683 -0.08344441861394807 -0.4140970192187416
+-0.2169229250232756 -0.139728824653224 0.3022348443465113
+-0.2198768006712144 -0.149968159355379 0.4321842579547452
+-0.2214114204232291 -0.1225220799957973 0.5409621791502991
+-0.1929736857355174 -0.03818610480938463 -0.5031431915557217
+0.2447988857322374 0.00381362611841892 0.5899237674964953
+-0.2253454435756435 -0.0510535612519891 0.3044597114244738
+-0.2040544704023095 -0.03176029619634726 0.427052550607548
+-0.2092215002784332 -0.02565337133532111 0.5411861284909295
+0.480989766979571 -0.08319996631774527 -0.4210647206063204
+0.6832787695 -0.135160238 -0.1359121870505095
+-0.1359126227647989 -0.135160238 -0.6832786799999999
+-0.2289056667134217 0.04663271431507766 -0.4255814967903377
+0.1359126012351997 0.135160565 -0.6832784115
+-0.2066628191114914 0.05480046826812753 0.3198049648384878
+-0.2243058615160185 0.07964437541474738 0.4247297184543852
+-0.208113090690223 0.06445928225980971 0.5231662485997532
+0.1359125669756076 -0.135160238 0.6832786799999999
+-0.1359126275243924 -0.135160238 0.6832786799999999
+0.387046307 0.135160565 -0.5792553725
+0.3870462925 -0.135160238 0.5792557
+-0.6832786799999999 -0.135160238 0.1359126506271685
+0.6832786799999999 -0.135160238 0.1359128139494905
+-0.2194004785644676 0.1475847545349244 -0.3469514787709857
+-0.5956393160156814 0.09490531950004515 -0.2042934565935917
+-0.2178745302422234 0.1453261132084583 0.3328986335035305
+-0.2418619683830102 0.1723327817400896 0.4269176136807822
+-0.1831818339704835 0.1484536604414157 0.5221446291367574
+0.03663870813279047 0.05005509956049407 -0.6414864284917617
+0.1942350135205994 0.05851852115451094 -0.6161295752759666
+-0.5420417635302861 0.1012002134691774 -0.1193827443122811
+-0.5756313767461709 0.1636057234662092 -0.06013303154197015
+-0.08651201916266667 -0.06865067701275893 -0.3321620682941319
+0.1027626229516373 -0.1314567021304453 0.3834698204860338
+-0.362428029410273 -0.1070244315800844 0.4886937738212236
+-0.1011488098433493 -0.1488108851409737 -0.5298702748774109
+0.1020647599765114 -0.07371707566437634 -0.366754583133816
+0.3903368429134106 0.101699745956457 -0.433818578133064
+-0.1074126412370914 -0.1526189193003277 0.4088007775076468
+-0.1151641662391919 -0.1438442203091108 0.5312796308049904
+0.2921648111109675 -0.07492673849593907 -0.2773684319721069
+-0.1285905486543048 -0.05647037586924304 -0.6133127505128496
+-0.1216163521409942 -0.03813129009229205 -0.4360208940675083
+-0.09905003496170785 -0.06196894632151512 0.3368756483460959
+-0.0990668004357604 -0.05903067457631835 0.4461241622524353
+-0.4776363609860341 0.1465133577617411 -0.2274197198236627
+-0.08773698924304667 -0.05184598555492647 0.6268363586823165
+-0.2267429148408685 0.1249132199309427 -0.4913909097447509
+-0.1121832809510704 0.04646092772441284 -0.5238551018576121
+0.2073757465014909 -0.0005490189447118193 -0.2818296401738018
+0.3574044012045842 0.1547702511617189 -0.2277470630249493
+-0.2354812082527474 -0.150851567101606 -0.3768350049973243
+-0.08743158532283671 0.05697845190875393 0.3357206925042919
+-0.1277896175209473 0.0578934535227305 0.440598521116173
+-0.1033125957436848 0.03928284369367165 0.5233789300229542
+-0.09304793425364484 0.05084289554029058 0.6380521403377361
+0.2386457396262456 0.1715816661461778 -0.3828757503815334
+0.423909910978836 0.1317032982272586 0.06356530576229823
+0.4097262710096638 0.1454523582378435 0.1438076249432514
+-0.0911470481544097 0.1452334358097956 -0.4158156824845399
+-0.1016133527545378 0.136860667934726 0.4330388861157719
+-0.4292389925744527 -0.1654781027874459 -0.07721888439167002
+0.2059596359472754 -0.0381533441888445 0.2457693811615254
+0.2680266045401745 -0.0409369721715605 -0.1942463155450726
+-0.5684009439817774 -0.181284115 0.3093214345483903
+0.156376008505201 -0.03237108451866057 0.2901862018655822
+0.4036041502130215 0.07115527728618469 0.4897612657744972
+0.4145351070767421 0.04784708682520229 -0.4968945110667345
+0.5250185200556235 0.04036754828805152 0.3708378603636437
+-0.3939857586234845 -0.0354399188984108 0.5159839140024425
+-0.5136737465592929 -0.02096928334744927 -0.3939635237507689
+-0.6094018716296354 0.002003007624447968 -0.202297769504363
+-0.3663963947652291 0.008372259341285189 -0.228274073560665
+0.3184424023319196 -0.02249886458617977 0.3579632423854461
+0.006561687502009625 0.05724662934245741 -0.5358999954437873
+-0.02114722495813348 0.04872463253946129 -0.3390579201943842
+0.3048686051070211 0.1840817211670154 -0.5675677732621035
+-0.01038008191495194 0.05798805100217008 0.4357427118599579
+0.0008467820919496345 0.03331469483369875 0.5521021583643705
+0.407017633 0.1812843605 -0.50312203175
+0.05776611603134262 0.1376535741227746 -0.2918362873837625
+0.0497730822502663 0.1296850741 0.288305171
+0.2457107185925527 0.135139000195845 0.1648979522399937
+0.1686676516 0.1296850741 0.239057429
+-0.5000568779581299 -0.1795990531282913 0.4150386052352156
+0.5014528792180527 -0.1816753454552545 -0.408719237110252
+-0.5029512339302212 0.1812843605 -0.4072733907773048
+0.5029512152035382 0.1812843605 -0.4072733664139633
+-0.3764123707272886 0.09879670488745546 -0.4095454500084148
+-0.4725332676819864 0.005642352703116546 0.2020216355148748
+-0.1496650072925399 -0.235647257655321 0.5167782537253931
+-0.5302451570783469 0.2365965943548609 -0.04876828145860426
+-0.002591613671415457 -0.1026208303124244 0.3928908007681053
+-0.0523451173707068 0.1302623922386563 0.4927621326423263
+0.05860755375353332 0.1257279774771141 0.4794596148821863
+0.08802430931468684 -0.1225325715056166 0.5603036431373815
+0.1032241182142573 -0.0340591879952308 -0.5533129910255919
+0.00745278544232821 -0.06582211103224113 0.630052478596229
+-0.01082466888663697 -0.03801533319731521 -0.6156480556982972
+0.4726120106138331 0.12484313881195 0.3448375643845454
+0.4814544006975456 -0.05513774069059533 -0.2986534077813469
+0.4732694108739305 -0.05522915948677016 -0.1778180843447857
+0.4909415760273616 -0.03837679976116375 -0.06661192117720928
+0.100095691090951 0.05093218150821434 -0.4304550833388545
+0.1239385805155771 0.044544506316063 -0.3352807243241612
+-0.2802087049206544 -0.08144478342526922 0.4785192195300021
+0.09580812999656925 0.05650133103318054 0.5018653908681395
+0.1112308163202565 0.150059329766388 -0.5249317956161899
+0.6459383261078916 0.06758027157215296 0.3222791452973603
+-0.1565518800075456 0.04876135164867082 -0.3722630768346345
+0.5560387821509727 -0.04441632956197366 -0.1490117451147615
+0.581792943649261 -0.04102538424861867 -0.06392366822802879
+-0.2582127270014075 0.02913406223927319 0.2684441064386298
+0.2927387924644004 -0.0149633208788464 -0.4491729904496716
+0.2013878265835575 -0.1266401577586878 0.3266978710424491
+0.443141937 -0.2374316825 -0.279739961
+0.5157281007889414 -0.2374270833779515 -0.09271138174935373
+0.2090559597555721 -0.04395774488034123 0.4278392112683149
+-0.4944336897194478 -0.09005668250886321 0.3978113830106517
+-0.432532593947051 -0.2375984594420146 -0.2935702535581112
+0.2277292536980891 0.0428303539450235 -0.5320631122757074
+0.2865164264133694 -0.2374651039703354 0.4383398348635704
+0.2056064164492523 0.04985412590180793 0.3127630284553709
+-0.2820232971018125 0.2372968610762593 -0.4427223751947126
+0.4395616904484677 -0.2384988760845237 0.2719947621906035
+-0.5081604390019669 -0.2378210470469928 0.1169813244873835
+0.4327316470501742 0.2384381322497851 -0.2829623286064294
+0.2877680748853014 0.2378403864535273 -0.4344245922410273
+0.2797399535 0.237431735 0.4431415055
+-0.4340564080289104 0.2378165773830367 0.2886114810717543
+-0.1828011762689302 0.05364457025045295 -0.6355453293052928
+0.6375438199914241 -0.02935637925234931 -0.152892417653076
+0.2602975085172395 -0.09745952344616611 0.5477608240078249
+0.2775263424353848 -0.1228377492099226 0.384791984454249
+0.3596151937197254 -0.141597768176489 -0.4231164447026335
+0.3208415581685442 -0.1349795480412619 -0.2171832530745286
+0.3109056466661628 -0.1388776022384641 0.1987929501681949
+0.3335612390022412 -0.04552068754656854 -0.5281548867660718
+-0.4531333533695962 0.07692855650042288 -0.555266519638251
+-0.4585715218401867 -0.07760329815977847 -0.5513992488579733
+0.3396141930387401 -0.05279514643595638 -0.0921131507041876
+0.3204727004155962 -0.06529806721664831 0.1046288254150648
+0.31306525603795 -0.06567965594681423 0.2046364937964013
+0.3159038841314221 0.02599107147295122 -0.3415755577703607
+0.3351976816345975 0.06128680549085182 -0.1479704239571715
+0.3388606878224936 0.0394365795661426 -0.03384902201879005
+0.3437417281098993 0.05703090942030965 0.107072764716187
+0.3220169919082694 0.05095046011482658 0.2177406215607413
+0.0689665525288998 -0.01163147790780546 -0.665821750833281
+0.4247853193968322 0.138055140745489 0.2498450206885172
+0.5666235297019543 -0.1721782523580675 -0.1155446617485881
+0.2372398230317664 -0.03167176195468971 -0.6080155472507058
+0.3357930029995391 0.1567136729231967 0.4253357575546892
+0.3540986515356748 -0.1424058629083627 0.3969252358031086
+-0.3414012713464936 -0.0621048952609243 0.002901790927666791
+-0.006352275712959725 -0.06508036243897163 -0.3374551632923645
+0.4077025720526192 0.04826923006195883 -0.2421012815042307
+0.1654412905940297 0.04562478981903653 0.4212167249231981
+0.4197084439871971 0.05192469954844516 0.1814469788058192
+-0.2208049830591295 0.06251985565641553 -0.6877215877450071
+0.426338398304634 -0.1434752602320325 -0.3367315691514332
+0.4326170457437952 -0.1411491565218985 -0.2188400560397645
+0.4237305177426459 -0.141661095432702 -0.08709124284118519
+0.404215024391447 -0.1399533434716075 0.2156067827615666
+0.4328630722823912 -0.1328013499737094 0.3308716072960298
+0.2248175293217677 -0.06758012992784704 0.6854376795667962
+0.4161155946855566 -0.05584439860466436 -0.002441013836284896
+0.4313140076389336 -0.05978578617486206 0.1088070022131595
+0.450918256819464 -0.03847381900589373 0.2359583877933817
+0.4457060890403761 -0.02620527055913056 0.413482502816888
+0.09835692228621801 0.06442805687023616 0.3643175222483206
+0.4369762376240091 0.04583458804269828 -0.1157588923316206
+0.4259958964602643 0.06108205071788198 -0.01539096574106599
+0.4162805245750874 0.04502838616233909 0.3078736604558807
+-0.2079118128565157 -0.04963516806072479 -0.3889561128873697
+-0.4299064880738119 0.135160565 0.5506171529961174
+0.5281180042296583 -0.1378503329927562 -0.2334815132298401
+0.5223484067072454 -0.1458065956801332 -0.02071680486975328
+0.5425252574054265 -0.1373263383845702 0.1072054410899196
+0.5350977120239054 -0.05103806950915329 0.3245688546285393
+0.5333863472702021 0.05046379975240864 -0.1975902518114613
+0.532455096746828 0.04560488532328932 -0.005699071116475381
+0.5419193981501835 0.0486765864019862 0.1239391687217768
+0.5355481056583311 0.1414417937687622 -0.09261594688985184
+-0.4967510892678877 0.04316175247488441 -0.1928494949928883
+-0.02348498660855512 -0.05726929540916417 -0.4929404584818559
+0.3389491987958484 0.1263167279134933 0.5054477140885989
+0.6274560120419712 -0.05198890175872962 0.1099921569861211
+0.4878419111093683 0.05875103888030383 0.2200223268103224
+0.6240885685723052 0.04492717659879997 -0.1054570120072385
+-0.6018963927443802 0.05107175258897927 0.2237810152843244
+0.641493437015596 0.04698768983416271 0.1038913003334417
+-0.2638401636369636 -0.05493290412767491 -0.4459854414188743
+-0.4048759839159633 -0.06931554420171929 0.2011181734596874
+-0.6013782642278841 -0.008670811585460226 0.05953811030783254
+0.236902116816932 0.07230934141622231 -0.4053013921960443
+0.1152841078827855 0.0709724499571117 -0.6207345739323095
+-0.2104418741655573 -0.06950722531256681 -0.2601247953068311
+-0.2245041950568541 -0.1051655974782383 -0.3165675198391922
+0.2110263114297359 0.1100623185224954 -0.31596710503771
+-0.3303083506116173 -0.02760598174876897 0.4317847611411088
+-0.3969790557273846 0.08278970153870198 -0.223719143864455
+0.3181074745640283 0.001246966633677706 -0.2511003853726549
+-0.001060707671683717 -0.1065111122750068 -0.5541061858275393
+0.650853451752721 0.009920080387569559 0.011665510543036
+-0.3574599974014963 0.09734137741098607 -0.09965203923949038
+-0.1436064464398353 0.08831529589285453 -0.3151052839264627
+-0.007506786969251721 -0.004105502730003744 -0.4243101107139098
+0.1065931068429387 -0.0001085366789827557 0.4135578084754778
+-0.3329912408681484 0.06421172716095264 0.3029882505886588
+-0.3375421951262143 0.006229202633078193 -0.4349069127969654
+-0.6985554476013591 -0.1024719621527603 0.1073642970980115
+0.5452928391102743 0.01608060722268402 0.2319397301056004
+0.1155424676004419 -0.227407992 -0.5808709265000001
+0.4924387485 -0.227407992 -0.3290367425
+0.5805699011497513 -0.227407992 -0.1170558901122047
+0.3290369515 -0.227407992 0.492438585
+0.492438585 -0.227407992 0.329036959
+0.11554240060044 0.227408156 -0.580870569
+0.4924384355 0.227408156 -0.329036556
+0.5808706585 0.227408156 -0.115542013739575
+-0.329036869 0.227408156 -0.492438257
+-0.5770880277546183 -0.227407992 -0.1345605514496855
+-0.492438585 -0.227407992 0.3290369735
+0.3290368765 0.227408156 -0.492438212
+0.3290367645 0.227408156 0.4924383165
+-0.4846991967808343 0.2275824300242007 0.3384793634329527
+-0.4924384055 0.227408156 -0.3290366455
+-0.5808706285 0.227408156 -0.1155422394103346
+0.5263242134816848 0.1427634180929979 0.04084918483631195
+-0.1523500837504407 0.1543677763815984 -0.5036055611948859
+0.502190705004582 0.1453440444158882 0.1545363581935308
+0.1381249001229966 -0.1411164872603262 -0.4731299872355259
+-0.6070921062993293 0.07694558145276738 0.3755620352998136
+0.1406031141985266 0.1429805795156574 -0.4029942841166447
+-0.5103359034007181 0.2364667903916487 0.1533976511473124
+0.1425452530490057 0.237431735 0.5057831616462883
+-0.3731252291502142 0.1294891353399314 -0.4883120817928434
+0.3723381797994571 -0.04828163511992769 -0.2021711263110909
+0.3764661100207322 -0.04326195962319036 -0.3110841143459703
+-0.5149001436269404 0.1817831010578217 0.3883744154118514
+-0.4075790725065548 -0.07011207976826006 -0.5880659568874272
+-0.5303300891969429 -5.833858670062555e-08 0.5303300379042957
+0.3692070733341348 0.05887650239374177 0.3977133352200331
+-0.4079152010050289 -0.1850652088177073 -0.4973754617889673
+0.3329196737418877 0.1396696471672832 -0.5128027077454528
+-0.06310352592163579 -0.05063790301029709 0.5320612101990535
+-0.4016297809554171 0.07665157112216678 -0.5897760485424426
+-0.3792981735569309 0.2069810069713509 0.4866624706549424
+0.3021500544248019 -0.07243349234632183 -0.3680829388445839
+-0.3032017640904151 0.0466666413092313 -0.2590452631690672
+-0.2390805197549287 0.05934859095666144 -0.3203443511140383
+-0.3843687658895464 0.0597781261690172 -0.320510805268692
+-0.297150850631037 0.04789404968614406 -0.1751198455174624
+-0.491150875371558 -0.02431634563507673 -0.1034359022115692
+-0.3426425664506795 0.06567166007506782 -0.6329930304230099
+0.622054807140332 -0.06919338799472372 0.2517012930533061
+-0.1703098257617832 -0.1826550824458945 -0.6216246471829776
+-0.2174347219811193 0.01272135947138664 -0.245695297556412
+0.4439378877973604 0.06758027157215296 0.5646485581078916
+0.04763992432268208 -0.01224661208412182 0.5024657892071374
+-0.1708675632617437 -0.1854258582415338 0.6183160665788725
+-0.1716332338339093 -0.07729741666714569 0.6931634498852716
+0.4859623908309793 -0.1751256506962191 -0.1293025462109802
+-0.1895947160490057 0.135160565 0.6726003965825593
+0.5863066585133995 -0.05325755980125289 -0.2416842667002071
+-0.6726003999896268 0.135160565 0.1895947010490057
+0.1895946860490058 0.135160565 0.6726003904889689
+-0.6705388748474567 0.135160565 -0.1999589308460862
+0.6726004757712991 0.135160565 -0.1895945220490057
+-0.5959268895394246 -0.08763387546571111 -0.193621137504829
+0.2024962576489122 0.1321155658524761 -0.4660263074064109
+0.4354731070692088 0.1354985673547656 -0.1637449946554581
+-0.1637739673863473 0.08397379746190908 0.6927663054735529
+0.3265732257357251 -0.1509614322112238 0.2884515197475907
+0.2732793647740832 0.1546584917298763 0.3314554491719456
+0.3352013350452636 0.1560501476469682 0.2713065059666275
+0.1855214459446696 0.1402342518748386 0.377939225620567
+0.5952374280764472 0.05149610372169009 0.2848800021155177
+0.309976557537971 0.02941156572264442 -0.5861860739822928
+-0.5819583326726456 -0.05848943025699382 -0.2922827175629795
+0.4478022466495745 0.1487306241748066 -0.05278736440785489
+-0.06061965019220951 -0.1546782628110873 -0.4451638315554993
+-0.4739719375085265 0.1595241468053785 -0.07235625997418475
+-0.4389376217620313 -0.1571890255726863 0.05923078313502133
+0.03523894539594524 0.1322226216130693 -0.4287150964234601
+0.4406472348063602 -0.1457053958490441 0.05768599293734747
+-0.6888037686428277 -0.07036105566768017 -0.2037901556721537
+-0.06229448528435831 0.03133867538934622 -0.6561603563492422
+0.6623351609753214 -0.05696093651650429 -0.04555963134792926
+0.7142163401685105 -0.06509681597650166 -0.08380314178483944
+0.08210689966282012 0.05937132490740349 0.7162348457707524
+0.08333145252853005 -0.06595830440988804 -0.7140572377641591
+0.06608137356851983 0.1491054590402661 0.4009851807085034
+-0.3611274408840614 -0.1077702581600939 -0.06242896573356903
+0.3796094865805174 0.1486590723480749 0.3447589131660058
+0.04712138799494611 -0.09589795714366683 -0.6301910061738919
+0.0659446078747277 0.09224666523738477 0.621360874667907
+0.181154043491014 -0.08994650355324102 -0.6200918491966262
+0.5771762656224043 0.09484020020585483 0.1912421960319913
+0.0226914655921382 0.02659887758775728 0.6516329293572531
+0.3075185328989046 0.07299906362394958 -0.4985846099332604
+0.07577543452462263 -0.1433929495342712 -0.5746623127656189
+0.1596925585545952 -0.1348942439547208 -0.561311843514139
+0.4452187262509794 0.04287656012943429 0.1108911178589169
+-0.646405235051362 0.1812843605 -0.0536821029593403
+-0.2904552754354776 -0.04991148299524247 -0.2135005964108463
+-0.2027902505473725 0.04060653507155906 -0.5494798040191847
+0.312554733939242 -0.1507221313137781 -0.3192189438369041
+0.532405183857342 0.02947761865663639 -0.102715572874153
+0.5275086031262067 -0.1055867242650335 -0.1088883244036733
+-0.4264568237336529 0.03087572834513268 0.1118487827067294
+0.5386311781670815 -0.1739261713797737 0.3688656825855084
+0.318987690826632 0.08094470550083935 0.3082873048685877
+-0.5247960083693182 0.05760442579045268 0.2134851504654077
+0.6158593372512614 -0.1874400435104712 0.1715324352895766
+0.4315624334526753 -0.02798427575426788 -0.5103779956811643
+-0.4123433473060267 -0.04764382775535243 -0.5032775626162018
+0.7173603882156612 0.07561188915947202 -0.05247448317129177
+-0.2713328395803039 0.1161625451622493 0.5697169244581305
+-0.1741215833851504 -0.1867567049684828 0.4804577250082678
+0.6976598577362255 -0.06827042213568386 0.1623536943023283
+-0.04610012519939551 0.1448809950000666 0.5674450728739088
+-0.06820624315971494 0.1391422404993094 -0.5422448272563861
+0.04976206459513634 0.1459319893446545 0.5648956198046813
+-0.4405659160829608 -0.05935747034583092 -0.2987452142975358
+0.7101340715369242 0.06931532101520638 0.09809893503159287
+-0.5233272589156744 0.1371867973524671 -0.006070351955850449
+0.003411548988928413 0.1576623979034729 0.5143549750367112
+-0.007709896184820183 0.1617475887412644 -0.4979439886996553
+-0.5015913141449097 -0.148026251126252 -0.0007504796998853022
+0.3156046534344432 -0.02657801295699586 0.456855152278661
+-0.7146314176639721 -2.18556941e-08 -0.1778099321739148
+-0.714631391435433 -2.18556941e-08 0.1778099059453759
+-0.6889572874750536 -0.135160238 -0.1073642970980114
+-0.0800325114130878 -0.2144714877278948 -0.376277204125747
+-0.2045620552658866 -0.2184651849984501 -0.3362589268787387
+-0.6889570651027241 0.135160565 -0.1073642970980114
+-0.6889570579792537 0.135160565 0.1073642970980115
+0.07462547832493387 -0.2197393122455355 0.3888876602281924
+0.3226232787263165 -0.2162282417004459 0.2163220611664897
+-0.382677138 -0.2181964215 0.08886321938880395
+0.3260948967187077 0.2168535096272338 -0.2135434742241258
+-0.08047208479996108 0.2175162374763322 -0.3828568457923386
+-0.5709528055146412 -0.1788981212378028 -0.3103633892284121
+0.2294042978034777 0.2168584666811915 -0.3155098213988843
+-0.5989670613063136 -0.135160238 -0.3575461347945523
+0.38267687 0.2181962655 -0.0888628909245615
+0.091470285862678 0.217038063599734 -0.379622106314556
+-0.07140032689038553 0.2154323042171401 0.3800981761532723
+0.221030943918663 0.2171582416863385 0.3218790327721636
+-0.3857783053634699 0.2178326861474558 -0.06926851353584285
+-0.2269041027428502 0.2188124539906703 0.322227403264663
+-0.3317374084906508 0.2173518025519454 0.207025087688183
+-0.5835519514615468 -0.2268300252085159 0.105417351210259
+-0.3827407025570413 0.217881574844037 0.08507772784439768
+-0.5824973265862685 0.227408156 0.1073642970980115
+0.3812711629175531 -0.2165997755367423 0.0783551961348893
+-0.1521653160164021 -0.1287108372045364 -0.4187192410115158
+-0.4859042057485281 -0.227407992 -0.3388164423898491
+-0.4257266172768689 0.1457644899011558 -0.1483052091300335
+0.02270542428141074 -0.01774936420632196 0.3414903994776349
+-0.3716315400264734 0.1944972016295138 -0.508779491398935
+-0.3048382051472016 -0.1745592467809746 0.5805511771905953
+-0.3135551595490744 -0.1851134063945669 -0.5603592340360726
+-0.4356442171131527 -2.18556941e-08 -0.5935971483996163
+-0.07717212950074337 -0.1785652289067969 -0.6448708338027288
+-0.06507398806140052 -0.1761046441822036 0.6501169839026913
+0.6453716043734744 -0.1791687299460072 0.07115349393080496
+0.3897301333485112 0.1893134174198639 0.5037431591401457
+-0.6887090665053673 -0.07379341498840072 0.1991994430700723
+-0.5935971383366712 -2.18556941e-08 0.4356441969872622
+-0.6117749063923518 0.01644354433148199 -0.2978603801681667
+-0.387558001269515 -0.135160238 -0.5789137973090235
+-0.5789137969635547 -0.135160238 0.3875580010967806
+-0.3875580048364737 -0.135160238 0.5789138044429412
+-0.5789135088604425 0.135160565 -0.3875580605546097
+-0.3875580495343058 0.135160565 -0.5789134868198349
+0.5644643984827786 -0.06495018187921164 -0.4455770106383233
+-0.578913471633707 0.135160565 0.3875580419412419
+0.23709719603894 0.1014150416760966 0.2680955908015779
+0.04018451750921416 -0.1349437665400113 0.6245923834942313
+-0.6051502767477215 -0.1339196929326989 0.04824931533981232
+0.06106294228067399 0.1185684120684981 -0.6226282579399706
+-0.6062220019205854 0.1293499032270441 0.03599799757209045
+-0.6078126614727571 0.08914747057635643 -0.04573977024164903
+0.5754370663803664 0.1269034291391263 -0.2020234705263327
+-0.3709021398994805 -0.00134424386124089 -0.6363908604690129
+-0.1778099073798559 -2.18556941e-08 -0.7146313928699131
+-0.1778099039496093 -2.18556941e-08 0.7146313894396665
+0.4347152300153192 -0.03845077592829239 0.3242724391810436
+-0.3575457461164083 0.135160565 -0.598967070476627
+0.5212411926782172 -0.05775566056478709 0.01756777546771002
+0.5259509822148767 -0.03592485772890536 0.09957469724955148
+-0.1073642970980114 0.135160565 -0.6889570487660757
+-0.3575457351476239 0.135160565 0.5989670649922347
+-0.1073642970980114 0.135160565 0.6889570511651186
+-0.3462750200225362 -0.227407992 -0.4809205082873176
+-0.1073642970980114 -0.227407992 -0.5824976636260482
+-0.33153474006408 -0.227407992 0.4907696355829997
+-0.1073642970980114 -0.227407992 0.5824976644313991
+-0.1073642970980114 0.227408156 -0.5824973216541982
+-0.3318778745421381 0.227663389878087 0.4884459660073816
+-0.1073642970980114 0.227408156 0.5824973224595431
+-0.09142014233927777 -0.04957767833824132 -0.5334365820416362
+-0.1411060818092554 -0.05316603507842676 0.5382820154556561
+0.07839309595428723 -0.03606997118258356 0.5627924560811802
+0.2228311347285496 -0.04566485180113804 0.3316338944249109
+0.1778098936435355 -2.18556941e-08 0.7146313791335925
+0.1119377679059749 0.135160565 0.688047318674596
+0.1073642970980115 -0.227407992 0.5824976533568385
+0.1107310237709541 0.2263410543301188 0.5830591323502694
+0.1073642970980115 -0.135160238 -0.6889572750606456
+0.1778099085090079 -2.18556941e-08 -0.7146313939990649
+-0.2867197080342601 0.06935592017993335 0.2082907636499682
+-0.1040286901134598 0.04955933054357604 -0.428733056122008
+0.1087667398617572 0.05680384432486649 -0.5469337818933063
+0.3462749938603848 -0.227407992 -0.4809204952062419
+0.3575457135947208 0.135160565 0.5989670542157832
+0.3575461221925426 -0.135160238 -0.5989670550053088
+0.1812223911135857 -0.1736821308172768 -0.6298093261526846
+-0.6193708249721932 -0.181284115 0.1895947455490057
+0.1905191600437536 -0.1743820496510929 0.62715234095378
+-0.1895947310490057 0.1812843605 -0.6193705318270991
+-0.6237485270845429 0.1807169479230611 0.1708771116222541
+0.6193705496990755 0.1812843605 0.1895946860490058
+-0.6285656566370914 0.1761324746024394 -0.1732584543457864
+0.394701284275205 -0.1042605866668516 -0.4997370861425395
+0.4699929441616674 0.227408156 0.3626286470636558
+-0.5084742133742602 0.06489101169978406 0.3985956981616894
+0.5085238562446177 -0.09862272073648808 0.3981796479379747
+-0.252784993881451 0.08889640338330405 -0.2133526125177653
+-0.053682158699257 -0.2374316825 0.5234595132156995
+0.5789137897334826 -0.135160238 0.3875579974817446
+0.5789134714070765 0.135160565 -0.3875580418279266
+-0.5234595159721932 -0.2374316825 0.05368218361184548
+0.5789137748970357 -0.135160238 -0.387557990063521
+0.4119679658371732 0.171320395173378 -0.3619113785125
+0.5824973573981509 0.227408156 0.1073642970980115
+0.5824976994624926 -0.227407992 0.1073642970980115
+0.5989670542157832 0.135160565 0.3575457135947208
+-0.1851208683566414 0.124208603344928 -0.4239884656902839
+0.6889570984230848 0.135160565 0.1073642970980115
+0.173734377190098 -0.1592113573896186 0.4569211512536634
+-0.28606352997408 0.1545826835379782 -0.5211144100507749
+0.6889570315425984 0.135160565 -0.1073642970980114
+0.5456779809973004 -0.1215914160305011 0.2584213678038438
+-0.558393685464368 -0.1146177080545493 0.2834043871933823
+0.6190017582884673 -0.07274211245430767 -0.3599170327319804
+0.0411829889215951 -0.01367151147718778 0.4358118977414294
+-0.426429427626163 0.143843426080977 0.002542584081843323
+0.7146314120566302 -2.18556941e-08 0.1778099265665732
+0.7143730345430807 -0.00140176429054186 -0.1770398293756113
+0.3739725842088806 0.1215769468470212 -0.08292337630397037
+-0.05485930368709809 -0.1053031201782086 -0.3871347894732904
+-0.3714078393416314 -0.1180642596737125 0.05878671147875928
+0.3598360547299361 -0.1098711723824991 0.05711255573169563
+-0.2229579979104957 -0.01042827856541734 -0.3327072361197876
+-0.6422801729477911 -0.03894346107858589 -0.3425982442479343
+0.6608410879144642 -0.08766114178271175 0.05095820244838246
+0.1426328224684481 -0.01999499921288682 -0.6425278445708872
+0.1451338818735528 0.04608510717745574 0.6124456115996282
+0.01809742601197574 -0.1721964576789065 -0.4967334593128879
+-0.246127712574023 0.04135798095259782 0.5983282486609528
+-0.3138521513056136 -0.1556622707844267 -0.3161040462964457
+0.323493049226149 0.1290001483902456 -0.3253386948631966
+0.2728508941316176 0.07549347174791185 -0.2372820664953083
+0.5562842649028102 -0.0485295044900356 0.1953580131414835
+-0.3673911643395928 -0.09103973029133546 0.3705530982467184
+0.4355860589542803 -0.06553550803975287 -0.5709373490885763
+0.5632559126388801 -0.06387772482078147 0.4479415235371896
+-0.5623988653662189 -0.05767263513801224 0.4524406607131942
+-0.5561151775794455 0.06810076880820351 -0.4564393132378073
+0.5660902677743312 0.06010819246444307 -0.4456534833601336
+-0.2548913804596937 0.06258836563347484 -0.609930088042597
+0.6241570345002212 0.002226028570825955 0.2002465762605789
+-0.649776531707054 -0.0725164334290863 0.001715429544915426
+-0.004389374735116239 0.1180139820763546 0.6205176565062831
+-0.02316628772584089 0.1114646632479806 -0.6235726113015037
+-0.1543271382898399 -0.09275660230181169 -0.3310827964762754
+-0.6484132865773928 0.03340105755634293 -0.112249268939819
+0.08134865638375682 -0.0668181207163879 0.7141991738988013
+-0.08165379287038785 -0.05609759458137611 -0.7172863175211743
+-0.08318554957996099 -0.0696514590344952 0.7130018514806905
+-0.0739077083046028 0.05868607434484126 0.7180669935902767
+0.05266965457730004 -0.1751399486361745 0.6536976595518434
+0.05368213302461863 0.1812843605 0.6464052261462883
+-0.05328394604293371 0.1784463116146165 -0.6497597270761811
+-0.0536821640733928 0.1812843605 0.6464052317297715
+-0.6464055249721932 -0.181284115 0.05368220217617423
+0.6467232799244399 0.1809402739390439 0.05407987922160953
+0.234169056917011 0.05670640816227374 0.4156758517070452
+-0.5089134388924373 -0.1923102973818645 0.05068212359475061
+0.1676413756067232 -0.06781041643326767 0.531558722267077
+0.5865201611484786 0.1316140442522556 0.1131982866086703
+-0.04196278628353433 0.08189709568967014 -0.4641796369398359
+-0.2423508878776404 0.001876770083158745 0.1933769569910977
+0.2800534084650199 -0.001419473771088816 0.1515067266544594
+-0.1560211292574079 -0.0006481472840781907 0.2851380005602722
+0.7123616063516282 -0.08049241694007397 0.07040161526726069
+-0.6278805340950337 0.105766568169776 0.1909343800546901
+0.6415084338809066 0.0798150248930703 0.1822983339745222
+-0.193204923899704 -0.05572412844052755 0.6224482061031797
+-0.6304617438564843 -0.1035122462338923 0.198036709796241
+0.6156512441366255 -0.1032623802693025 0.1753596888902071
+0.6334392450885363 0.06696755323160408 -0.1838983985512123
+0.3782062441392018 -0.12204988584297 0.133183390216391
+-0.3828751918786762 -0.1285998340989022 0.1330862319117084
+0.5072024770378601 0.02634700497318234 0.4506209427980654
+-0.7191544659780258 -0.06197279360122413 -0.06358919318901379
+0.5728575186449349 0.07727439263062541 -0.2980850278327078
+-0.7044764983498082 0.06248231541307675 0.1366278139779501
+0.2566443629179671 -0.1170930898586065 -0.57977294954992
+-0.2765169719865896 0.01562580963005574 0.4859752387090623
+-0.1753814023370256 -0.124284950943586 0.367624994491605
+0.4898723741986995 -0.09038045200736297 0.1732492806675991
+0.3766615163973158 -0.123978037291708 -0.1575263032116939
+0.4881935107812874 -0.0085710500253026 0.1662459875931393
+-0.56444933379114 -0.002316203731541758 0.1710273760706686
+-0.5614368059721748 0.0973977351253799 0.1566922547789323
+-0.3759252044357331 0.0156099603600098 0.3811072818299944
+0.05383705383083659 0.2145299440226167 0.3816158270208281
+-0.1619732726120931 0.1330765460643622 0.3981378604129159
+0.3740296575279978 0.2111397287358895 0.05465499176783607
+0.382604670169172 0.2201042797991723 0.1102298907106016
+-0.05959478828599387 0.005930859013459864 -0.5663805162004295
+-0.01717673474360292 -0.03153853481555432 0.4675011682325878
+0.1364068407452007 -0.007277509535040018 0.4931110900239976
+0.385242784587982 0.00493529360549544 0.0469820988995634
+-0.6153711266794439 -0.1812035918895354 -0.2101699509978979
+-0.4726629202983678 -0.003327897542008701 0.3806000630414419
+-0.2761035454032154 0.007953612206295075 -0.3797424360944924
+-0.3888636207135883 -0.007938706908226399 0.04178998606509782
+-0.383278176165127 0.002973667381423688 -0.3690494541521624
+-0.3951191553982251 -0.008625205350272521 0.167258353233485
+-0.3778274958314471 0.097823632437333 0.06631071941621698
+-0.3790763659663187 0.1397429760054126 0.166309023532448
+-0.07173407266451474 -0.01249665451441885 -0.3804837643958644
+0.2877288837036373 0.09196999960831206 0.3768515569196144
+-0.2638412931493026 -0.1036766116893414 0.2391878653767805
+0.5991487400768469 -0.08920454298047388 0.02674751036901173
+0.607540993752861 0.1001621139512669 -0.03389076552791264
+0.5998837187108649 0.08338905575378201 0.04540625595553115
+-0.4743685905329021 0.09628227912602481 0.1566870273403166
+-0.4899423282037244 -0.08777947848176004 0.0496655557590468
+-0.4872238388481392 -0.008111734031781198 0.05133027066828713
+-0.2690792112134239 0.1005271315784323 0.2846467055547287
+-0.1622708114648139 0.01555625283271768 0.5870885011921256
+-0.1696039443719094 0.08373059661137741 0.606094424742615
+-0.4727827962661386 0.08448025899041636 0.06102069089748234
+-0.0605344123561698 0.008905324522109764 -0.4866242428196116
+-0.3815748147572086 0.001616454738300511 0.2542510822105668
+-0.3829317236935794 0.09050305714296698 0.249640564453501
+0.2616995180087382 -0.0944119383604532 0.2673071635684744
+0.2822514647509506 -0.005850493949298635 0.2691987114193257
+0.5935218637257337 0.002482111530269636 -0.3213259738383724
+-0.505464412258112 -0.1833568021761748 -0.3992898990550236
+-0.280967206550657 -0.1069018532045351 -0.2712158017948815
+-0.1161557276237301 0.1330592507396156 0.5699473656025384
+0.1369649530693391 0.1316506868926868 0.5592349600599259
+-0.1563140975 -0.12968538685 -0.247312054
+0.2471092834451507 -0.1351991643392156 -0.1628731266204674
+0.05673806489688069 -0.1305484283378361 -0.2874745836492385
+0.1628260980373148 -0.1376422746858275 -0.2489927473513118
+0.285898666730311 -0.1265875515597322 -0.05186365307744811
+-0.05510728922311799 -0.1288880401778301 -0.2867319091516609
+-0.05504061217906892 -0.1306847878369744 0.287899872612026
+0.05682477422099689 -0.1333662801572196 0.2892682689746576
+0.1581282015648371 -0.1267552300700679 0.2438786222378669
+-0.247312076 -0.12968538685 0.156314045
+0.2422066962597056 -0.1302997933916133 0.164651855644622
+-0.1563140525 -0.12968538685 0.247312076
+-0.1630249122458465 0.1419671064563262 -0.2521384231743916
+-0.289625310045959 -0.1340672575029545 -0.05729459453291589
+-0.2879229197619732 -0.1301596644734891 0.05322815267895153
+0.2896188458856734 0.1357133639383724 -0.06264541548807114
+-0.0627561069358414 0.1340128003260848 -0.2885039532850485
+-0.06003912720029972 0.1284922867886495 0.2854965770306342
+-0.2526884136545073 0.1362608592798777 0.1557278784104276
+-0.1651630450343733 0.1414745578076895 0.2503363810445914
+-0.247311957 0.1296850741 -0.156313896
+-0.2854066045 0.1296850741 -0.06434522086097889
+-0.2821769618407176 0.1225299473665231 0.05746447519469901
+0.2874402478974803 -0.131064917060987 0.05857953496711188
+0.2854065895 0.1296850741 0.064345362760046
+0.09956721556483739 0.008900787700942986 0.3040684694373372
+0.141227164390001 -0.03915237319037788 -0.4606711437015398
+0.7323157185262316 -2.18556941e-08 -0.08890473140017549
+-0.732315708831986 -2.18556941e-08 -0.08890490946352991
+0.08890493042999804 -2.18556941e-08 0.7323156895667963
+-0.08890494921809791 -2.18556941e-08 -0.7323156964349565
+0.08890495872633397 -2.18556941e-08 -0.7323156969995325
+-0.08890496836657437 -2.18556941e-08 0.7323156947198333
+-0.7323156957177165 -2.18556941e-08 0.08890500959611544
+0.7323157060283151 -2.18556941e-08 0.08890520766431409
+0.08886322676908474 -0.247455373 -0.446745351
+-0.08886321373091526 -0.247455373 -0.446745366
+-0.253061019 -0.247455373 -0.378732383
+0.4467454255 -0.247455373 -0.088862923672303
+0.378732547 -0.247455373 -0.2530607585
+0.2530610265 -0.247455373 -0.3787323535
+-0.08886321165025131 -0.247455373 0.446745366
+0.08886316884974869 -0.247455373 0.446745366
+0.253060922 -0.247455373 0.378732428
+0.378732428 -0.247455373 0.2530609295
+-0.3787325175 -0.247455373 -0.253060825
+-0.378732428 -0.247455373 0.253060937
+-0.446745366 -0.247455373 0.08886322906283976
+-0.446745396 -0.247455373 -0.08886309193716024
+0.0888631522690823 0.247455314 -0.4467449785
+-0.08886313923091771 0.247455314 -0.446744993
+-0.2530608105 0.247455314 -0.37873207
+0.446745053 0.247455314 -0.08886284917243099
+0.2610196563250436 0.247455314 -0.3734141119799477
+0.3787322345 0.247455314 -0.2530605495
+-0.0888631371502424 0.247455314 0.446744993
+0.0888630943497576 0.247455314 0.446744993
+0.2530607135 0.247455314 0.378732115
+0.378732115 0.247455314 0.253060721
+-0.446745023 0.247455314 -0.08886302493719224
+-0.378732115 0.247455314 0.2530607285
+-0.446744993 0.247455314 0.08886315456280776
+-0.3787322045 0.247455314 -0.253060624
+-0.253060736 0.247455314 0.378732115
+0.446745366 -0.247455373 0.08886333782769699
+0.446744993 0.247455314 0.088863263327569
+0.6200791714077696 0.03064473749778887 -0.2440277489762376
+-0.2626299684804731 -0.01779926795500601 -0.599686805331662
+-0.6123164101680636 -0.04121299126851069 0.2479495684496193
+-0.6378744981290908 -0.08641631458216771 -0.3245838369211331
+0.3222791452973603 0.06758027157215296 0.6459383261078916
+-0.6432533681683356 -2.18556941e-08 0.3613284019936311
+-0.2395605239587259 0.1861935899383363 -0.4408444524343927
+-0.01815759320534445 -0.1340331295755921 -0.618553481301664
+-0.416173620953785 0.1334507751933589 0.4501201060061978
+-0.4299310368060829 -0.1311833513982786 0.4365280856573984
+0.4154314094009814 -0.1274728968155736 0.4504484112711975
+0.3741153953545832 -0.1242355469020759 -0.0241854340486779
+-0.5578965797509108 0.07758988814764095 0.4488543656622259
+0.066887528377056 0.1134054046445166 -0.3500728402253111
+-0.05292311315347678 0.1260431071072435 0.3580833601168914
+0.3551564114844889 0.1039168763322923 0.03884608765805646
+0.2530163933438245 0.1514527965299385 0.5046020668611034
+0.2170199306452358 0.07035957400422863 -0.6861725016313338
+-0.6886531840337877 0.06831837761274545 0.2075619354184494
+0.2146550888955935 0.06925477135835592 0.6869673050358971
+0.6856670468292471 0.07175132639315042 0.2175067344931936
+-0.6855026363122276 0.06738785333983141 -0.224774707991908
+0.210947856979893 -0.08000473670797401 -0.6845483310126054
+0.6855619969327065 0.07218350698739592 -0.2173972238496296
+0.6854377980262316 -0.06758012992784704 -0.224817394281203
+0.2474381341489587 0.1061938412000234 0.5766180351873803
+0.489503016304909 -0.1785221406910227 0.263169924743284
+0.3037402902914204 -0.1480370568980555 -0.5174410365166965
+-0.4864719094610887 0.1752779821252042 0.2794938371976077
+0.4313217303001755 0.135160565 0.5496715059782117
+-0.5506173006531567 -0.135160238 -0.4299067178972762
+0.1092678841352424 -0.08993497320759923 -0.6208877979904343
+0.3315483361717592 -0.1186488286544302 -0.101273559173753
+-0.05332356464008531 -0.003220860878376586 -0.2495200389000687
+-0.04525869337901792 -0.0002733267622190005 0.2511242429814381
+0.1406911262572085 0.00458300473952665 0.2128336142806865
+-0.212131627 -1.862499999991107e-07 -0.141741749
+0.2502262665 -1.862499999991107e-07 0.049773196710046
+-0.3038750919021984 -0.00420447504443615 0.04991694629414084
+0.3060295077376545 0.01098691065300924 0.05504694712444477
+0.1170500682120619 0.1810802133846133 0.4998806125319112
+0.4856608853215326 0.07434546177884246 -0.4322363107862517
+-0.458826067180756 -0.1539076895358543 -0.2411783655387011
+0.2414409659261295 -0.1395981122085382 -0.4286050346936315
+0.4102733319937368 -0.03911917457333562 0.5089220698446709
+0.6436677085590493 0.05276964120720886 -0.3333546024610379
+-0.6328982967916651 0.06338561222163799 -0.3439693233029654
+0.6393865205766276 -0.06783917936103923 0.3319505522899195
+-0.6254841434299707 -0.07086294435058846 0.3511895693749568
+-0.3416269174305976 -0.06356443637116133 -0.6344016122869763
+0.3434770270001724 -0.06692247982426226 -0.6320022943216725
+-0.3433089855389704 0.06847439970572868 0.631576980383792
+0.01187510501014911 0.09071444418817076 0.3552258624282731
+-0.09777969011320647 0.1858866393441107 0.4930423179985466
+-0.09000200204206606 0.1904619032906356 -0.4874776471335208
+-0.1425452905490057 0.237431735 -0.5057831668270991
+-0.1960150597271896 0.1899289804033522 0.3766604375387921
+0.6036653880207526 -0.1001926936403398 -0.1444334652759952
+-0.5402624007491028 0.1237898832495177 0.2844891380745601
+-0.05006475582362858 -0.01280714490476897 0.3921732275300897
+0.1790506689861273 0.09320743189656971 -0.5309763578678544
+0.6040451972022797 -0.01620572259224715 0.2810845165686729
+0.1895947905490057 -0.135160238 -0.6726005975303229
+0.6726004202115424 0.135160565 0.1895946860490058
+-0.6726006637375268 -0.135160238 -0.1895946565490057
+-0.02211429515907533 0.1594037691029022 0.4252731110919391
+-0.2178001310488206 -0.2187878848473082 0.3282470318331274
+-0.5592332994570895 0.1902521000391926 -0.3047708475982757
+-0.5620171585001306 0.1836874142745606 0.3139790672500916
+0.5719226547333054 -0.184839971894952 0.2968062977771854
+-0.6502893879669944 -0.1778044424086221 -0.05434518036839411
+0.05368215214944767 -0.181284115 -0.6464055185303228
+-0.2158743658231015 0.09526841972257911 -0.2647745309902759
+-0.2114200026182485 0.1010409781318316 0.2608651734868205
+-0.1982647250058633 -0.09896759786522454 0.2555716819455021
+-0.3129920331155432 0.1084669587386941 0.1627058173366869
+-0.1551685730867522 0.1080878068838509 0.3164997837178271
+0.3107971034315091 -0.09707599796195117 -0.154823053830057
+-0.1583120171750342 -0.1054586665971662 0.303083453438425
+0.1842856487044458 0.1328246243631194 0.3071171654625506
+0.3405021210770199 0.1414651450899855 0.1906460150110045
+0.1222497898060191 -0.01459653842951159 0.6716419314541555
+0.6187727551000032 -0.1822425903685795 -0.1870407695448417
+-0.5140658381072688 -0.1017583303142997 -0.1591315148082528
+0.1506872594387387 0.107011086778144 -0.5785905499054989
+-0.4543830012629956 0.02070776700615354 -0.3534233502059753
+0.05208701996476486 0.2369451377659843 0.5271608231354115
+-0.05035418058057255 0.236541723503403 -0.5303113484986198
+-0.0536821586992481 0.237431735 0.5234591482297716
+0.652264938 -2.18556941e-08 -0.347841635
+-0.65226484875 -2.18556941e-08 -0.34784178425
+0.34784194825 -2.18556941e-08 0.652264714
+0.570974946 -2.18556941e-08 0.46950069075
+0.05288088412692972 -0.1696738401445088 0.4812655001975786
+-0.03458365933052948 -0.1696053900123612 0.4469547003571025
+0.5253901567920133 0.01334285814744612 -0.2715949297219139
+0.5008552018403365 0.0008968449184086402 -0.4476622468363466
+-0.4679785893234675 -0.1720388775723341 0.1441529655658877
+0.4608469644229725 -0.1715736804020539 0.1526932380534281
+0.4712710999359984 0.1910190031111572 -0.1336326997259548
+-0.4922261458666373 -0.1795631286234275 -0.1535029704981328
+-0.04003071875929925 -0.01735235223485209 0.6844048178359888
+0.05786895306571511 0.04951594845714467 -0.7239498885560181
+0.2127057635028024 -0.0833398813849658 -0.2538144673578778
+-0.5133981546337892 0.03912537763871422 0.4644387356716861
+0.3773758525022843 -0.1145068351912803 -0.2694092044220085
+0.1576327673835248 -0.00174623569951705 0.3567090232373112
+0.06760794136056919 -0.03105061583133411 -0.4613535665477629
+-0.7234264959800163 -0.05601355432624411 0.0509091710031585
+0.3307809421322569 -0.06714567745126444 0.6404082325370969
+-0.2708684508911888 -0.1157463251588329 0.3750957971723319
+-0.2789744190406261 0.03582508947728972 0.3637404139816443
+0.4699139694816132 0.01791589074177289 0.04241381727681382
+-0.1542487939470152 0.03758487944378665 0.3777753383082115
+0.370206414436605 -0.02919216874927832 0.1646086345337974
+-0.3843560426382542 -0.01459984482335379 -0.05331358427969477
+-0.3756036815077443 0.0655473812595731 0.1739739322222879
+0.3624013269252122 -0.0545436899147785 0.2752232163131648
+-0.4804420812630442 -0.05407407237313248 0.1595268585124839
+-0.1649969073003941 0.07391696382954899 -0.4781800483207995
+0.172943408404652 0.03270492546648051 -0.4770234506938139
+-0.5131791762068501 0.2357481102122256 -0.1642331900472321
+0.4967968321015735 0.01720857935661327 0.2988126091935098
+-0.3058843387849141 -0.09545112865391527 0.1671091894972072
+-0.6105761183708176 -0.01727895628303047 -0.05109497939867216
+-0.1020149918114597 0.1002042812521527 -0.4793742608257536
+0.34784208225 -2.18556941e-08 -0.6522646692499999
+
+CELLS 4436 22180
+4 416 356 836 690
+4 0 759 1017 1
+4 0 1 792 759
+4 0 1017 187 876
+4 899 180 560 106
+4 1 665 759 494
+4 667 314 610 668
+4 543 176 732 810
+4 145 370 320 156
+4 10 314 667 588
+4 2 187 668 667
+4 3 563 570 5
+4 812 200 199 553
+4 136 419 424 853
+4 414 573 870 572
+4 193 585 486 379
+4 314 511 10 374
+4 204 477 209 828
+4 492 803 250 1020
+4 6 12 202 730
+4 473 210 597 967
+4 312 296 479 272
+4 6 202 704 786
+4 6 911 202 786
+4 6 202 911 730
+4 22 216 951 1009
+4 10 214 610 21
+4 418 986 799 521
+4 774 400 405 356
+4 808 468 357 833
+4 11 22 951 1009
+4 1025 232 819 818
+4 228 826 819 1025
+4 207 994 608 353
+4 499 146 436 919
+4 953 155 303 919
+4 84 974 632 923
+4 328 1030 643 24
+4 554 212 179 115
+4 735 932 171 909
+4 15 18 755 225
+4 15 18 225 421
+4 525 17 224 352
+4 712 206 210 813
+4 525 224 230 352
+4 352 257 264 796
+4 561 166 929 784
+4 295 392 698 724
+4 617 979 184 1
+4 1027 222 594 817
+4 17 224 401 754
+4 17 352 918 224
+4 483 352 388 796
+4 18 28 225 520
+4 697 829 261 492
+4 1033 668 208 181
+4 752 420 599 266
+4 966 293 991 933
+4 548 991 459 169
+4 19 917 230 712
+4 63 697 395 982
+4 19 917 458 230
+4 478 332 252 426
+4 258 821 558 435
+4 623 557 446 419
+4 21 904 214 232
+4 21 374 610 10
+4 21 733 214 610
+4 21 610 566 733
+4 21 214 733 232
+4 240 589 608 355
+4 23 37 624 219
+4 23 219 1006 12
+4 226 484 536 355
+4 231 34 260 238
+4 25 903 233 636
+4 25 819 232 39
+4 25 636 819 39
+4 826 232 233 819
+4 27 33 220 244
+4 28 371 231 225
+4 28 231 238 34
+4 30 577 856 36
+4 30 857 738 37
+4 30 46 856 577
+4 30 46 577 437
+4 30 46 437 857
+4 30 857 437 738
+4 31 865 253 47
+4 920 445 423 449
+4 127 423 449 920
+4 178 550 744 128
+4 178 694 842 692
+4 178 842 744 692
+4 33 243 889 220
+4 272 115 390 179
+4 33 243 244 1019
+4 179 969 993 312
+4 272 312 179 993
+4 35 262 261 55
+4 723 351 269 182
+4 35 308 262 55
+4 1001 791 158 182
+4 35 262 308 906
+4 953 183 268 303
+4 36 22 344 216
+4 657 401 206 754
+4 37 248 49 857
+4 37 248 1032 49
+4 499 919 303 146
+4 37 790 219 242
+4 37 790 242 1032
+4 303 499 190 415
+4 38 905 643 24
+4 499 116 190 415
+4 728 191 669 503
+4 503 191 669 161
+4 39 818 254 819
+4 40 324 910 42
+4 40 44 538 757
+4 41 43 264 785
+4 41 556 785 746
+4 42 281 647 56
+4 42 245 281 910
+4 42 324 910 281
+4 44 251 757 279
+4 45 310 602 59
+4 46 947 49 47
+4 46 47 253 947
+4 47 49 51 947
+4 47 254 253 947
+4 50 62 945 536
+4 51 697 779 63
+4 68 315 762 72
+4 68 315 641 762
+4 52 480 589 60
+4 72 942 177 74
+4 52 32 888 747
+4 53 61 822 983
+4 53 242 243 822
+4 119 135 427 333
+4 119 333 427 749
+4 584 271 562 403
+4 55 829 697 982
+4 55 829 262 261
+4 558 405 596 821
+4 558 578 138 435
+4 56 281 647 241
+4 57 65 285 603
+4 58 66 330 513
+4 59 310 309 67
+4 59 686 310 67
+4 60 541 945 62
+4 61 781 75 63
+4 61 63 779 781
+4 61 289 983 73
+4 288 132 1000 913
+4 62 541 74 60
+4 62 74 541 981
+4 114 694 949 842
+4 554 212 584 179
+4 179 584 403 271
+4 528 584 271 302
+4 63 297 781 75
+4 63 982 297 75
+4 179 212 584 528
+4 63 395 697 779
+4 55 982 697 63
+4 554 584 403 179
+4 313 511 314 566
+4 314 313 668 620
+4 313 511 620 314
+4 314 313 610 668
+4 566 313 610 314
+4 184 1033 181 763
+4 65 69 975 797
+4 187 763 668 1033
+4 65 69 797 321
+4 537 227 313 208
+4 354 207 307 467
+4 65 884 321 603
+4 960 257 264 270
+4 66 342 963 893
+4 960 264 257 746
+4 960 746 602 45
+4 67 805 965 71
+4 67 343 805 71
+4 67 962 343 898
+4 68 72 848 315
+4 68 64 334 641
+4 69 326 849 73
+4 69 73 987 326
+4 69 284 797 987
+4 404 726 116 415
+4 70 342 306 66
+4 70 342 489 306
+4 404 726 415 416
+4 732 176 644 491
+4 71 75 860 335
+4 71 335 985 75
+4 375 176 644 732
+4 71 923 860 84
+4 360 453 491 644
+4 71 308 985 805
+4 72 177 541 74
+4 72 87 848 438
+4 73 75 781 943
+4 73 849 85 326
+4 73 85 943 326
+4 73 326 289 987
+4 74 358 859 88
+4 74 541 489 177
+4 85 644 375 850
+4 850 198 644 375
+4 75 335 943 86
+4 75 335 985 297
+4 75 297 781 335
+4 76 77 1010 380
+4 76 77 380 277
+4 76 583 767 78
+4 76 1010 767 583
+4 6 347 730 657
+4 77 79 650 664
+4 77 650 329 380
+4 78 760 771 80
+4 78 769 760 80
+4 78 769 580 760
+4 81 83 884 1003
+4 81 83 1003 622
+4 82 995 949 899
+4 82 376 974 611
+4 82 611 974 377
+4 83 375 850 85
+4 83 85 849 375
+4 83 375 849 321
+4 84 86 364 961
+4 84 961 923 86
+4 207 231 328 189
+4 85 86 943 644
+4 86 453 644 868
+4 86 644 335 943
+4 86 335 644 961
+4 87 89 438 845
+4 88 90 363 922
+4 88 922 358 90
+4 88 100 101 387
+4 89 91 565 745
+4 359 528 271 302
+4 359 212 179 528
+4 302 130 271 359
+4 359 212 528 507
+4 302 320 156 362
+4 507 528 302 362
+4 528 320 302 362
+4 176 375 316 1002
+4 316 176 198 375
+4 88 363 100 922
+4 100 363 103 922
+4 112 119 482 103
+4 749 119 482 112
+4 364 86 98 453
+4 364 98 102 453
+4 145 720 320 370
+4 94 666 761 96
+4 365 118 989 134
+4 366 110 118 663
+4 954 171 909 838
+4 95 97 765 381
+4 95 97 381 431
+4 838 909 548 171
+4 102 366 118 988
+4 988 366 118 663
+4 95 324 916 381
+4 869 379 386 1029
+4 506 367 220 657
+4 869 252 1029 386
+4 368 157 292 719
+4 96 311 571 97
+4 368 292 443 719
+4 497 719 368 443
+4 737 490 468 325
+4 18 659 225 369
+4 96 761 311 666
+4 194 479 666 311
+4 950 717 156 370
+4 86 453 868 98
+4 98 405 944 110
+4 129 303 190 415
+4 99 345 111 944
+4 452 155 158 953
+4 100 332 113 101
+4 884 603 65 373
+4 100 101 387 332
+4 683 373 322 884
+4 101 113 1012 332
+4 566 902 374 21
+4 511 902 374 566
+4 98 348 868 99
+4 98 868 348 405
+4 102 576 560 899
+4 178 744 934 128
+4 178 842 934 744
+4 27 220 918 244
+4 105 109 515 952
+4 751 220 243 244
+4 105 319 101 235
+4 107 212 554 115
+4 107 103 339 628
+4 711 382 578 432
+4 711 259 382 432
+4 723 351 382 259
+4 382 282 351 723
+4 110 780 345 111
+4 110 780 111 122
+4 161 555 149 211
+4 110 837 405 345
+4 110 405 837 430
+4 111 836 851 120
+4 111 425 853 124
+4 107 339 554 212
+4 339 410 212 107
+4 112 113 332 346
+4 100 112 482 103
+4 302 320 350 156
+4 362 320 156 145
+4 1012 323 332 346
+4 113 1012 332 346
+4 113 112 123 346
+4 969 179 390 554
+4 213 529 327 322
+4 809 176 732 543
+4 1016 869 379 386
+4 278 691 744 990
+4 278 744 689 277
+4 278 990 744 277
+4 992 200 216 537
+4 166 723 561 391
+4 391 723 561 269
+4 200 537 221 216
+4 568 517 390 892
+4 517 568 631 892
+4 118 558 559 138
+4 119 482 103 628
+4 959 133 434 562
+4 959 562 434 420
+4 122 124 946 780
+4 122 126 867 429
+4 122 429 948 126
+4 436 452 614 919
+4 919 452 614 953
+4 123 428 858 126
+4 46 47 50 253
+4 124 424 127 946
+4 124 424 866 127
+4 124 136 866 424
+4 424 789 425 419
+4 125 127 847 423
+4 125 941 847 137
+4 125 137 986 941
+4 126 924 142 867
+4 126 428 858 142
+4 126 924 428 142
+4 126 948 428 429
+4 206 754 401 813
+4 754 224 401 813
+4 129 1018 278 132
+4 276 218 642 839
+4 614 953 452 349
+4 129 415 116 705
+4 129 303 415 1018
+4 839 26 218 642
+4 130 562 312 133
+4 452 953 158 349
+4 131 133 434 959
+4 547 679 831 841
+4 134 341 989 429
+4 134 989 341 148
+4 547 679 841 686
+4 135 142 858 736
+4 15 421 225 600
+4 135 119 625 333
+4 136 150 1007 446
+4 524 600 648 422
+4 648 600 15 422
+4 458 917 257 230
+4 136 789 424 419
+4 136 446 789 419
+4 678 458 263 257
+4 263 917 257 458
+4 137 151 882 799
+4 137 151 799 445
+4 137 986 941 799
+4 793 1004 463 236
+4 237 293 572 173
+4 141 147 417 443
+4 141 417 591 443
+4 731 548 459 169
+4 142 736 428 858
+4 142 455 924 428
+4 142 428 736 455
+4 719 731 459 169
+4 143 153 445 920
+4 156 320 350 950
+4 144 154 652 282
+4 144 578 435 138
+4 144 578 282 435
+4 231 484 535 238
+4 231 535 371 238
+4 966 459 593 393
+4 433 593 966 459
+4 85 644 850 99
+4 99 198 644 850
+4 40 442 413 671
+4 184 553 354 181
+4 239 207 354 467
+4 677 671 413 442
+4 448 132 278 691
+4 150 162 1007 935
+4 448 691 278 300
+4 150 935 1007 446
+4 151 546 445 881
+4 207 189 467 239
+4 240 355 484 226
+4 152 721 512 164
+4 61 779 63 51
+4 153 445 920 460
+4 155 753 452 158
+4 155 919 452 713
+4 156 950 350 159
+4 159 292 599 157
+4 159 599 292 1005
+4 280 396 403 1029
+4 113 123 125 346
+4 1029 396 403 478
+4 123 428 126 125
+4 52 255 589 840
+4 22 216 36 951
+4 255 52 48 36
+4 125 126 127 428
+4 36 216 255 951
+4 164 773 824 172
+4 165 173 237 651
+4 106 258 180 560
+4 774 821 405 560
+4 170 561 929 758
+4 140 416 404 436
+4 404 146 140 436
+4 166 582 561 259
+4 112 482 749 332
+4 735 414 788 909
+4 351 1031 456 259
+4 167 788 669 909
+4 878 515 193 319
+4 168 170 539 956
+4 878 319 193 89
+4 302 130 562 271
+4 508 701 469 652
+4 235 121 591 323
+4 701 349 469 652
+4 925 701 469 508
+4 105 235 121 883
+4 170 539 970 758
+4 621 876 2 188
+4 876 188 187 2
+4 354 763 930 615
+4 172 825 773 824
+4 538 44 251 757
+4 538 670 910 251
+4 173 237 651 823
+4 173 823 572 237
+4 170 561 970 269
+4 170 956 269 970
+4 270 458 960 257
+4 185 204 203 477
+4 187 477 203 208
+4 203 828 208 477
+4 296 272 96 666
+4 207 307 467 328
+4 154 791 282 182
+4 282 791 452 351
+4 349 282 791 452
+4 349 154 791 282
+4 28 231 371 238
+4 330 915 724 66
+4 330 915 66 58
+4 28 520 504 273
+4 77 650 380 664
+4 274 213 322 529
+4 933 991 548 169
+4 275 357 606 331
+4 966 991 548 933
+4 677 770 761 96
+4 939 283 839 16
+4 277 77 380 664
+4 378 300 278 277
+4 56 64 281 241
+4 241 334 64 281
+4 129 415 705 278
+4 281 318 641 64
+4 129 278 705 300
+4 58 66 513 964
+4 44 279 413 706
+4 279 606 331 275
+4 44 279 706 58
+4 795 131 581 434
+4 737 325 820 490
+4 869 386 1016 319
+4 722 150 162 1007
+4 282 578 526 435
+4 907 150 722 1007
+4 880 241 334 64
+4 64 680 880 241
+4 129 415 278 1018
+4 288 132 1018 1000
+4 288 1018 217 1000
+4 217 1018 268 1000
+4 573 173 572 293
+4 104 108 886 726
+4 649 294 324 95
+4 926 312 130 296
+4 681 879 318 64
+4 148 514 721 160
+4 148 721 514 908
+4 650 77 329 301
+4 639 904 518 214
+4 639 215 518 14
+4 639 214 518 215
+4 519 114 694 949
+4 899 694 949 519
+4 304 704 786 6
+4 651 305 173 742
+4 173 782 742 305
+4 184 181 354 763
+4 537 467 313 226
+4 467 313 226 643
+4 307 551 7 930
+4 314 511 374 566
+4 21 566 610 374
+4 311 357 379 699
+4 566 314 610 374
+4 479 311 379 699
+4 97 431 1011 311
+4 163 719 497 459
+4 743 479 379 312
+4 312 562 434 133
+4 312 403 699 379
+4 163 719 459 169
+4 312 479 379 699
+4 696 133 312 434
+4 459 719 497 443
+4 427 555 454 439
+4 736 427 555 454
+4 181 313 668 208
+4 763 307 620 313
+4 610 313 208 668
+4 315 942 177 72
+4 315 820 325 177
+4 315 72 848 438
+4 848 89 737 438
+4 596 578 526 456
+4 596 821 526 578
+4 478 426 252 396
+4 396 426 252 532
+4 179 528 584 271
+4 359 528 179 271
+4 359 528 302 507
+4 528 320 584 302
+4 353 196 276 563
+4 563 276 353 608
+4 682 317 27 658
+4 27 682 244 317
+4 319 869 386 252
+4 145 720 212 320
+4 720 236 748 320
+4 701 217 469 349
+4 701 934 925 217
+4 925 217 175 186
+4 925 934 175 217
+4 469 217 186 349
+4 346 121 1012 323
+4 113 121 1012 346
+4 123 427 428 125
+4 125 428 127 423
+4 13 976 510 328
+4 58 330 727 279
+4 320 950 236 197
+4 252 386 387 478
+4 72 177 480 541
+4 541 246 534 740
+4 139 720 212 145
+4 68 334 481 762
+4 32 241 747 334
+4 241 246 334 475
+4 52 334 481 68
+4 334 32 52 747
+4 339 103 482 628
+4 564 340 512 152
+4 340 429 811 587
+4 148 807 341 134
+4 989 441 430 429
+4 341 587 429 340
+4 148 908 514 341
+4 809 543 360 361
+4 361 689 543 389
+4 361 543 360 389
+4 213 689 543 361
+4 213 543 809 361
+4 200 216 221 344
+4 677 442 413 44
+4 677 44 413 706
+4 679 556 547 831
+4 621 927 783 549
+4 549 707 783 621
+4 178 692 744 550
+4 30 344 577 36
+4 692 744 550 928
+4 307 620 551 930
+4 82 377 974 576
+4 551 307 709 620
+4 360 389 453 377
+4 185 204 477 188
+4 185 801 204 188
+4 82 576 974 84
+4 552 932 728 788
+4 185 201 203 204
+4 728 552 669 191
+4 728 788 669 552
+4 5 973 812 553
+4 98 99 944 348
+4 98 405 348 944
+4 99 851 111 345
+4 345 836 851 111
+4 345 836 690 397
+4 184 200 553 181
+4 187 188 477 208
+4 112 123 346 749
+4 1033 763 668 181
+4 346 125 986 423
+4 203 828 477 204
+4 188 209 801 204
+4 188 208 209 477
+4 188 801 209 473
+4 137 418 986 799
+4 188 214 802 209
+4 188 209 208 214
+4 365 118 663 989
+4 348 851 198 99
+4 348 99 345 851
+4 348 690 397 345
+4 288 400 175 614
+4 415 400 776 288
+4 415 614 400 288
+4 288 776 810 400
+4 288 175 400 810
+4 212 720 748 320
+4 139 720 748 212
+4 349 953 158 542
+4 938 349 652 154
+4 542 154 158 349
+4 777 708 514 582
+4 584 396 420 350
+4 777 721 514 708
+4 350 562 420 584
+4 791 158 452 753
+4 1001 753 158 791
+4 270 352 257 264
+4 353 196 563 207
+4 5 553 199 563
+4 41 264 658 270
+4 41 960 264 270
+4 7 655 930 3
+4 850 316 198 375
+4 104 316 198 850
+4 40 44 757 413
+4 201 203 204 827
+4 201 222 827 204
+4 202 219 1027 205
+4 279 275 590 413
+4 188 209 204 477
+4 203 200 537 221
+4 204 1027 801 205
+4 477 208 209 828
+4 204 222 828 209
+4 205 801 597 372
+4 205 219 1027 476
+4 206 813 223 210
+4 206 220 223 401
+4 200 992 181 537
+4 209 208 214 832
+4 209 222 828 594
+4 8 215 967 473
+4 210 228 223 372
+4 210 229 813 223
+4 210 223 228 229
+4 210 813 229 712
+4 210 234 712 229
+4 92 390 612 94
+4 92 390 568 612
+4 92 568 390 892
+4 92 501 390 94
+4 501 390 892 92
+4 328 313 1030 13
+4 207 231 994 225
+4 826 215 228 233
+4 353 15 196 267
+4 353 15 939 196
+4 608 246 189 355
+4 189 231 484 535
+4 355 589 246 533
+4 22 216 1009 344
+4 175 400 774 614
+4 821 435 526 578
+4 186 526 821 435
+4 506 27 889 220
+4 218 888 32 747
+4 500 447 140 146
+4 407 447 140 500
+4 115 390 107 501
+4 501 390 107 892
+4 221 815 828 222
+4 221 226 227 1024
+4 221 815 1024 227
+4 895 152 466 503
+4 503 152 466 164
+4 223 243 476 834
+4 223 250 803 224
+4 223 224 751 250
+4 223 250 834 229
+4 224 813 230 803
+4 224 483 803 230
+4 224 244 751 483
+4 161 211 409 502
+4 643 328 24 905
+4 226 247 355 536
+4 508 144 435 411
+4 226 247 488 1024
+4 228 826 233 819
+4 228 817 594 1025
+4 228 229 834 835
+4 826 1025 594 232
+4 228 835 819 233
+4 228 1025 834 817
+4 534 306 238 260
+4 238 306 34 260
+4 279 413 590 757
+4 229 492 250 834
+4 230 256 483 803
+4 728 503 466 164
+4 230 917 257 256
+4 728 466 824 164
+4 728 824 172 164
+4 2 668 620 588
+4 588 314 668 620
+4 314 511 620 588
+4 444 459 384 546
+4 546 593 459 384
+4 364 86 453 961
+4 364 453 102 576
+4 32 241 334 880
+4 242 822 249 243
+4 243 1019 751 244
+4 244 285 388 682
+4 249 395 492 250
+4 249 289 243 822
+4 886 726 404 416
+4 726 397 416 886
+4 28 520 371 225
+4 28 371 520 273
+4 371 493 251 273
+4 493 246 475 814
+4 238 273 34 306
+4 256 492 262 1020
+4 256 796 1020 298
+4 251 475 287 295
+4 287 251 295 590
+4 287 590 910 251
+4 245 287 251 475
+4 910 287 251 245
+4 273 34 306 894
+4 205 801 372 1027
+4 205 476 1027 372
+4 209 1027 594 372
+4 378 278 689 277
+4 601 824 472 487
+4 870 728 472 601
+4 728 601 824 472
+4 870 472 487 601
+4 809 968 327 360
+4 82 376 962 974
+4 337 343 841 376
+4 337 361 376 605
+4 389 377 842 180
+4 82 995 377 949
+4 361 389 377 607
+4 870 487 174 601
+4 96 97 571 873
+4 96 873 571 677
+4 0 876 187 2
+4 284 289 797 987
+4 286 688 290 291
+4 602 310 309 59
+4 602 309 257 678
+4 602 310 299 309
+4 602 299 257 309
+4 57 603 285 286
+4 57 603 286 322
+4 475 457 814 246
+4 603 285 286 322
+4 287 325 1028 295
+4 287 687 325 318
+4 246 814 534 740
+4 257 309 299 796
+4 290 297 298 1022
+4 290 327 1022 336
+4 257 299 746 796
+4 291 337 290 299
+4 291 830 337 299
+4 722 153 460 165
+4 153 461 460 569
+4 907 153 569 461
+4 153 907 722 461
+4 724 342 66 306
+4 724 306 392 342
+4 724 330 66 513
+4 724 342 513 66
+4 295 325 1028 698
+4 306 342 489 392
+4 76 380 1010 583
+4 297 1022 308 298
+4 298 308 309 805
+4 299 336 337 290
+4 76 380 583 277
+4 329 274 322 529
+4 380 274 361 689
+4 381 97 765 571
+4 381 97 311 431
+4 324 486 687 318
+4 66 963 342 513
+4 656 281 245 241
+4 236 426 522 439
+4 439 384 383 1004
+4 384 393 593 459
+4 445 460 385 449
+4 445 460 593 385
+4 385 394 460 593
+4 449 460 385 675
+4 460 385 675 394
+4 184 1033 200 181
+4 203 181 537 200
+4 244 751 483 1019
+4 388 290 286 285
+4 810 732 389 491
+4 378 176 689 776
+4 936 795 420 702
+4 689 744 361 583
+4 583 580 744 361
+4 322 688 329 529
+4 322 529 327 688
+4 447 462 729 451
+4 215 937 233 14
+4 233 215 214 826
+4 325 331 698 833
+4 518 215 233 14
+4 518 214 233 215
+4 280 795 434 420
+4 177 820 325 392
+4 246 814 740 457
+4 795 959 434 420
+4 295 392 325 698
+4 392 698 820 325
+4 420 795 959 702
+4 324 808 275 331
+4 331 778 606 1034
+4 572 394 472 414
+4 700 952 936 117
+4 173 742 572 823
+4 414 393 394 472
+4 394 675 487 464
+4 335 360 343 336
+4 968 336 327 360
+4 336 360 343 337
+4 336 337 327 360
+4 337 361 605 338
+4 337 343 376 360
+4 337 361 360 376
+4 337 360 809 327
+4 698 490 820 325
+4 698 325 833 490
+4 63 781 395 779
+4 63 395 781 297
+4 347 12 730 220
+4 12 506 220 347
+4 952 417 396 532
+4 43 264 286 646
+4 646 286 682 264
+4 42 281 656 647
+4 647 281 656 241
+4 527 656 241 647
+4 117 131 795 702
+4 117 795 936 702
+4 236 211 439 463
+4 370 717 236 950
+4 236 463 439 1004
+4 8 662 967 977
+4 602 59 309 678
+4 45 59 602 678
+4 263 59 678 309
+4 263 59 531 678
+4 504 674 58 727
+4 44 674 727 58
+4 7 207 307 655
+4 655 7 207 955
+4 655 207 354 563
+4 196 563 207 655
+4 207 267 196 655
+4 267 207 955 655
+4 307 207 354 655
+4 399 153 165 460
+4 715 884 1003 81
+4 318 468 687 325
+4 888 402 22 951
+4 886 108 404 726
+4 915 406 66 58
+4 407 935 162 150
+4 64 879 318 565
+4 889 27 33 220
+4 149 503 1008 161
+4 64 565 318 641
+4 161 211 149 409
+4 366 110 663 405
+4 988 366 663 405
+4 339 896 410 107
+4 410 145 139 212
+4 900 258 411 106
+4 411 144 435 138
+4 110 663 405 430
+4 988 663 558 405
+4 663 405 430 596
+4 412 29 234 35
+4 28 510 231 34
+4 328 1030 24 13
+4 458 525 230 352
+4 325 833 490 468
+4 334 641 64 281
+4 334 475 641 281
+4 40 413 910 324
+4 176 732 810 491
+4 40 413 324 671
+4 571 671 324 413
+4 571 413 324 275
+4 354 763 615 184
+4 930 354 615 3
+4 3 354 615 570
+4 735 933 909 171
+4 167 788 909 932
+4 238 273 306 295
+4 915 306 724 66
+4 361 389 360 377
+4 86 868 644 99
+4 98 868 86 99
+4 583 580 760 78
+4 583 78 574 580
+4 689 744 389 361
+4 95 431 381 579
+4 431 379 479 311
+4 381 311 357 379
+4 575 579 431 95
+4 743 431 379 479
+4 138 711 578 901
+4 559 138 578 901
+4 387 101 319 332
+4 165 399 460 433
+4 386 252 1029 478
+4 399 151 546 163
+4 312 403 379 434
+4 743 434 312 379
+4 931 131 434 581
+4 170 166 929 561
+4 689 543 389 810
+4 693 726 116 108
+4 758 474 470 465
+4 784 561 582 758
+4 379 1029 699 386
+4 517 339 554 107
+4 517 103 339 107
+4 991 433 966 459
+4 146 436 919 447
+4 415 499 404 436
+4 436 447 356 452
+4 186 217 175 614
+4 186 175 774 614
+4 102 899 560 106
+4 437 46 947 248
+4 54 714 260 981
+4 714 981 533 260
+4 738 221 222 815
+4 437 815 1024 221
+4 738 248 815 222
+4 72 87 438 942
+4 714 536 260 533
+4 54 863 260 714
+4 863 714 536 260
+4 315 72 438 942
+4 315 737 325 820
+4 774 405 400 440
+4 405 560 440 774
+4 360 389 491 453
+4 365 663 430 989
+4 989 663 430 559
+4 341 587 441 429
+4 110 663 430 365
+4 429 441 450 587
+4 441 800 451 450
+4 663 596 430 559
+4 341 432 514 148
+4 89 845 319 438
+4 89 105 319 845
+4 89 193 438 319
+4 693 81 79 1002
+4 681 91 318 879
+4 683 884 322 715
+4 637 775 201 408
+4 637 9 201 775
+4 553 239 199 563
+4 84 961 364 576
+4 364 961 453 576
+4 218 241 283 246
+4 886 416 140 557
+4 218 32 241 747
+4 886 557 140 120
+4 416 557 436 140
+4 415 303 953 217
+4 415 953 614 217
+4 98 453 868 405
+4 1015 440 400 405
+4 440 453 560 405
+4 56 64 241 680
+4 7 655 307 930
+4 560 106 258 900
+4 558 578 559 138
+4 307 655 354 930
+4 405 821 356 596
+4 68 334 762 641
+4 287 325 295 457
+4 457 295 392 325
+4 457 177 325 392
+4 360 732 644 491
+4 968 732 644 360
+4 809 732 360 543
+4 543 732 360 389
+4 732 360 389 491
+4 968 360 809 732
+4 232 233 214 826
+4 518 904 232 214
+4 518 214 232 233
+4 152 512 466 164
+4 152 564 466 512
+4 895 152 564 466
+4 417 443 532 591
+4 419 446 789 425
+4 222 828 594 815
+4 222 248 815 594
+4 594 227 832 818
+4 370 950 320 156
+4 39 232 818 819
+4 594 248 815 254
+4 346 986 521 423
+4 418 521 1014 323
+4 418 444 882 1014
+4 423 384 445 799
+4 423 445 384 385
+4 423 385 449 445
+4 424 1023 780 425
+4 424 948 429 811
+4 424 450 811 429
+4 425 780 430 1023
+4 425 446 1023 451
+4 356 837 456 1026
+4 356 447 1026 673
+4 356 452 447 673
+4 302 195 562 1035
+4 960 746 257 602
+4 602 678 257 960
+4 746 299 257 602
+4 425 451 1023 430
+4 430 441 451 1023
+4 430 451 441 456
+4 425 451 430 1026
+4 1026 430 456 451
+4 428 449 423 455
+4 429 430 1023 441
+4 429 450 811 587
+4 603 884 321 322
+4 884 322 603 373
+4 373 322 603 57
+4 603 285 322 321
+4 20 30 613 567
+4 344 613 20 30
+4 613 30 221 567
+4 344 221 613 30
+4 17 224 918 401
+4 657 367 401 17
+4 989 432 341 148
+4 240 467 537 226
+4 467 484 226 240
+4 467 643 226 484
+4 505 200 1009 199
+4 5 812 199 553
+4 202 1027 204 205
+4 185 204 205 202
+4 318 486 687 468
+4 446 729 451 447
+4 446 450 451 798
+4 446 729 798 451
+4 196 563 655 741
+4 912 563 196 741
+4 1031 561 462 495
+4 655 267 196 741
+4 441 471 465 514
+4 441 451 800 471
+4 651 305 742 470
+4 1031 495 462 451
+4 742 782 470 305
+4 742 470 487 823
+4 439 454 384 463
+4 742 823 651 470
+4 742 487 470 782
+4 4 914 911 473
+4 473 205 801 597
+4 8 967 210 473
+4 454 592 385 384
+4 8 4 473 914
+4 449 1021 811 450
+4 967 215 228 597
+4 449 385 423 455
+4 450 451 798 800
+4 285 797 290 327
+4 285 327 322 321
+4 322 285 290 327
+4 285 797 327 321
+4 45 556 746 831
+4 451 750 729 798
+4 831 556 746 830
+4 447 462 451 673
+4 476 242 1032 249
+4 455 1021 464 512
+4 223 224 918 751
+4 224 751 244 918
+4 194 479 296 666
+4 117 936 280 952
+4 272 666 479 390
+4 194 479 312 296
+4 143 127 739 920
+4 847 127 143 920
+4 127 449 428 811
+4 739 127 811 449
+4 423 449 428 127
+4 507 212 528 362
+4 212 320 528 362
+4 346 521 485 427
+4 387 100 482 103
+4 480 740 246 541
+4 481 740 457 246
+4 52 334 747 481
+4 481 740 246 480
+4 119 748 628 139
+4 339 628 748 139
+4 40 757 538 910
+4 384 463 454 592
+4 460 823 675 461
+4 385 454 464 592
+4 460 823 394 675
+4 538 757 251 910
+4 40 757 910 413
+4 387 332 478 482
+4 224 352 483 230
+4 224 244 483 317
+4 230 256 257 483
+4 244 483 317 388
+4 707 639 667 621
+4 483 796 256 257
+4 707 667 188 621
+4 264 291 388 796
+4 454 592 466 464
+4 454 463 466 592
+4 464 465 487 825
+4 91 318 565 745
+4 879 91 318 565
+4 328 231 484 189
+4 723 259 382 711
+4 723 259 711 160
+4 346 323 521 418
+4 787 461 729 750
+4 598 787 461 729
+4 798 750 729 461
+4 598 729 461 162
+4 1007 162 461 729
+4 1007 729 461 798
+4 95 579 381 916
+4 381 379 357 468
+4 658 17 918 27
+4 658 17 352 918
+4 394 487 824 464
+4 658 918 317 27
+4 658 918 352 317
+4 38 260 488 643
+4 226 247 536 488
+4 50 863 488 38
+4 70 74 489 859
+4 70 342 859 489
+4 319 490 358 386
+4 319 1016 490 386
+4 568 386 358 490
+4 90 922 358 568
+4 491 810 176 400
+4 491 176 198 400
+4 689 176 543 810
+4 491 389 810 440
+4 491 1015 400 198
+4 491 440 400 1015
+4 701 217 925 469
+4 925 217 186 469
+4 925 469 186 435
+4 137 882 418 799
+4 799 882 418 444
+4 229 492 835 234
+4 492 395 829 1020
+4 250 395 492 1020
+4 492 261 262 829
+4 367 918 401 17
+4 367 220 401 918
+4 731 383 719 459
+4 292 383 443 719
+4 719 383 443 459
+4 292 383 719 731
+4 385 464 454 455
+4 385 464 455 1021
+4 385 1021 675 464
+4 746 299 310 831
+4 746 310 299 602
+4 225 493 251 371
+4 836 1026 425 837
+4 836 557 425 1026
+4 837 430 1026 425
+4 517 390 107 554
+4 892 517 390 107
+4 554 115 390 107
+4 718 775 637 408
+4 718 9 637 775
+4 9 775 613 201
+4 471 582 495 465
+4 471 465 495 800
+4 209 826 214 802
+4 802 214 215 826
+4 802 826 597 209
+4 802 597 826 215
+4 775 567 201 890
+4 775 567 613 201
+4 775 408 890 201
+4 537 226 313 227
+4 991 163 433 459
+4 566 226 227 313
+4 537 227 221 226
+4 572 394 414 966
+4 456 432 259 471
+4 496 703 586 117
+4 163 497 398 459
+4 165 399 433 498
+4 3 7 655 955
+4 4 653 911 6
+4 4 6 911 957
+4 293 498 165 433
+4 5 11 199 812
+4 499 146 404 436
+4 162 935 407 500
+4 162 978 935 500
+4 501 115 390 272
+4 10 639 214 21
+4 504 727 58 406
+4 493 251 475 245
+4 251 493 475 295
+4 506 27 220 367
+4 507 212 362 145
+4 507 145 410 212
+4 16 527 283 26
+4 17 367 918 27
+4 508 652 435 144
+4 18 369 225 28
+4 234 509 523 29
+4 19 531 917 29
+4 509 29 234 412
+4 510 28 231 369
+4 511 13 313 709
+4 511 13 1030 313
+4 239 354 181 467
+4 745 486 468 193
+4 745 93 486 585
+4 193 486 468 379
+4 878 109 496 515
+4 93 496 585 745
+4 455 340 1021 512
+4 279 513 331 606
+4 510 24 328 905
+4 295 306 392 724
+4 891 22 344 36
+4 1028 513 331 279
+4 513 606 1034 331
+4 514 340 587 341
+4 341 441 587 514
+4 35 906 308 635
+4 37 889 219 23
+4 514 512 465 340
+4 40 42 910 958
+4 40 294 324 42
+4 41 43 785 545
+4 42 540 281 56
+4 745 515 585 193
+4 585 515 379 193
+4 43 646 286 57
+4 43 544 57 286
+4 52 619 334 68
+4 759 203 187 477
+4 53 69 284 975
+4 54 38 260 863
+4 1017 187 477 759
+4 187 1017 477 876
+4 876 477 187 188
+4 59 67 309 906
+4 41 785 264 746
+4 746 785 264 291
+4 36 216 577 247
+4 76 766 1010 77
+4 36 247 255 216
+4 76 766 875 1010
+4 386 478 517 387
+4 387 103 482 517
+4 85 644 326 375
+4 82 804 576 84
+4 966 991 293 433
+4 991 498 293 433
+4 212 320 362 145
+4 93 95 916 980
+4 102 84 576 804
+4 127 811 948 424
+4 739 127 424 811
+4 127 811 428 948
+4 106 633 560 102
+4 18 520 225 421
+4 520 251 727 273
+4 520 251 371 225
+4 520 371 251 273
+4 660 421 520 18
+4 520 674 727 251
+4 465 777 582 474
+4 582 784 708 166
+4 117 280 936 795
+4 510 369 231 659
+4 225 659 231 369
+4 659 231 207 225
+4 659 976 207 231
+4 659 510 976 231
+4 120 136 419 623
+4 715 81 1003 1002
+4 715 1003 322 213
+4 129 132 278 448
+4 715 1002 1003 213
+4 300 277 77 764
+4 138 144 578 711
+4 44 279 757 413
+4 139 409 720 145
+4 279 275 331 590
+4 146 595 919 155
+4 147 417 292 157
+4 147 157 292 368
+4 195 350 159 156
+4 149 895 1008 503
+4 748 236 426 320
+4 130 1035 562 133
+4 130 302 562 1035
+4 157 159 292 661
+4 478 426 396 320
+4 478 426 320 748
+4 478 396 584 320
+4 212 478 584 320
+4 212 478 320 748
+4 384 459 383 548
+4 444 443 383 459
+4 166 391 561 170
+4 925 949 180 842
+4 106 925 949 180
+4 539 170 970 956
+4 168 604 539 170
+4 168 598 162 729
+4 269 351 791 182
+4 173 174 572 782
+4 351 791 452 753
+4 269 753 791 351
+4 174 487 877 609
+4 224 317 483 352
+4 483 257 352 796
+4 182 351 791 282
+4 1014 444 443 383
+4 726 316 622 1002
+4 1014 383 443 522
+4 30 221 738 437
+4 30 437 577 221
+4 521 444 383 384
+4 567 30 221 738
+4 521 439 383 522
+4 8 967 215 14
+4 30 344 221 577
+4 937 14 977 509
+4 43 264 785 286
+4 388 286 290 291
+4 2 187 620 668
+4 187 620 668 763
+4 323 485 522 426
+4 349 452 614 526
+4 282 456 452 526
+4 356 526 452 456
+4 614 452 356 526
+4 578 711 144 382
+4 423 521 384 799
+4 288 928 128 744
+4 128 132 288 913
+4 450 676 1021 587
+4 676 464 465 487
+4 132 928 128 288
+4 758 750 465 470
+4 451 800 750 798
+4 461 470 676 750
+4 800 465 495 750
+4 128 288 744 934
+4 758 750 495 465
+4 128 288 934 913
+4 203 537 208 221
+4 203 181 208 537
+4 283 816 225 994
+4 600 493 241 245
+4 283 493 246 241
+4 493 475 241 245
+4 381 324 687 808
+4 381 468 808 687
+4 324 331 687 808
+4 331 325 687 833
+4 687 468 833 325
+4 493 475 246 241
+4 213 809 327 529
+4 329 274 529 361
+4 529 809 327 337
+4 274 213 689 378
+4 380 274 689 378
+4 277 380 689 378
+4 213 378 176 689
+4 359 130 271 179
+4 302 350 195 156
+4 532 522 426 323
+4 62 945 536 541
+4 484 238 260 533
+4 534 306 295 238
+4 534 295 306 392
+4 534 489 392 306
+4 621 794 188 927
+4 876 188 621 794
+4 656 600 241 245
+4 656 527 241 600
+4 355 535 533 246
+4 225 816 535 994
+4 488 50 536 863
+4 484 536 533 260
+4 151 444 546 459
+4 398 151 163 459
+4 151 163 459 546
+4 151 459 398 444
+4 563 276 608 951
+4 608 218 276 283
+4 276 218 608 951
+4 280 795 581 434
+4 117 795 586 280
+4 586 795 581 280
+4 15 421 600 648
+4 648 421 600 251
+4 382 711 144 723
+4 382 144 282 723
+4 825 465 487 474
+4 229 835 492 834
+4 249 395 697 492
+4 168 539 729 956
+4 168 787 729 539
+4 112 332 749 346
+4 332 323 532 426
+4 485 332 426 323
+4 60 480 589 541
+4 541 589 533 246
+4 541 534 246 533
+4 217 268 542 701
+4 349 217 542 701
+4 938 154 542 349
+4 883 235 121 591
+4 328 307 467 313
+4 43 658 264 646
+4 646 264 682 658
+4 223 243 834 250
+4 834 243 249 250
+4 283 493 241 600
+4 546 460 445 153
+4 546 433 459 593
+4 399 546 460 433
+4 546 153 399 460
+4 548 171 909 933
+4 280 434 379 403
+4 723 561 259 166
+4 785 264 291 286
+4 56 318 281 64
+4 56 681 318 64
+4 540 318 281 56
+4 681 56 318 540
+4 115 212 179 359
+4 554 584 478 403
+4 386 390 554 1029
+4 386 478 1029 554
+4 179 115 390 554
+4 211 463 555 439
+4 555 454 463 466
+4 439 463 555 454
+4 281 318 287 641
+4 641 325 287 457
+4 641 325 318 287
+4 397 836 557 120
+4 419 446 425 557
+4 430 596 456 432
+4 989 901 432 148
+4 41 658 264 43
+4 560 405 558 821
+4 560 633 988 102
+4 8 215 473 783
+4 215 707 783 8
+4 430 432 441 989
+4 430 559 432 989
+4 561 970 758 170
+4 121 141 591 882
+4 561 462 495 970
+4 121 418 137 882
+4 121 882 591 418
+4 878 109 515 105
+4 878 515 319 105
+4 878 105 319 89
+4 3 655 354 563
+4 563 354 239 207
+4 741 563 655 3
+4 264 746 796 257
+4 142 455 736 564
+4 564 464 455 454
+4 630 736 564 142
+4 565 318 325 468
+4 565 737 468 325
+4 1003 83 321 375
+4 1003 321 322 327
+4 1003 213 327 322
+4 1003 327 213 375
+4 226 643 313 566
+4 24 643 634 902
+4 567 222 827 201
+4 922 568 90 631
+4 143 569 920 739
+4 569 449 450 675
+4 640 739 569 143
+4 319 387 386 358
+4 319 386 387 252
+4 571 808 275 324
+4 735 573 414 293
+4 572 487 472 394
+4 870 572 174 487
+4 82 899 576 804
+4 102 804 576 899
+4 901 118 559 138
+4 989 901 118 559
+4 989 134 118 901
+4 148 134 989 901
+4 735 728 472 870
+4 414 573 572 293
+4 870 573 174 572
+4 908 152 340 806
+4 76 574 872 583
+4 148 908 341 807
+4 743 379 431 940
+4 874 431 97 575
+4 908 806 340 807
+4 908 340 341 807
+4 577 344 221 216
+4 902 1030 24 643
+4 46 253 50 247
+4 437 46 247 253
+4 437 253 247 1024
+4 1024 247 488 253
+4 596 578 456 432
+4 579 431 381 379
+4 940 379 431 579
+4 93 916 486 585
+4 579 379 381 486
+4 93 916 585 980
+4 580 607 389 361
+4 361 580 744 389
+4 744 842 389 580
+4 105 515 319 952
+4 105 319 235 952
+4 319 515 252 952
+4 319 252 235 952
+4 796 299 291 290
+4 796 298 299 290
+4 943 75 781 335
+4 280 581 379 434
+4 943 335 781 326
+4 743 581 434 379
+4 940 743 379 581
+4 582 758 465 474
+4 939 283 16 15
+4 582 495 465 758
+4 283 16 15 600
+4 563 276 627 629
+4 292 197 443 383
+4 583 380 361 689
+4 197 522 443 383
+4 96 311 761 571
+4 381 571 324 808
+4 381 571 357 311
+4 197 236 383 192
+4 292 197 383 192
+4 197 236 522 383
+4 950 192 236 197
+4 705 776 726 378
+4 212 584 478 554
+4 590 275 331 324
+4 910 413 590 324
+4 287 324 910 590
+4 287 331 324 590
+4 413 275 590 324
+4 745 585 486 193
+4 585 579 486 379
+4 585 579 379 940
+4 238 493 273 295
+4 371 238 493 273
+4 181 354 313 467
+4 471 514 582 465
+4 465 512 514 777
+4 585 940 379 586
+4 515 586 379 280
+4 585 515 586 379
+4 586 581 379 280
+4 586 379 581 940
+4 838 548 731 171
+4 200 973 812 505
+4 572 742 487 823
+4 572 174 487 782
+4 572 487 742 782
+4 570 563 553 5
+4 514 465 587 340
+4 514 465 441 587
+4 587 450 811 1021
+4 52 589 48 60
+4 52 255 48 589
+4 240 589 247 255
+4 355 247 589 536
+4 590 1028 331 279
+4 287 590 295 1028
+4 121 591 323 418
+4 591 1014 323 418
+4 591 882 1014 418
+4 616 911 185 801
+4 616 473 911 801
+4 394 592 464 824
+4 592 463 466 472
+4 237 593 460 394
+4 433 460 237 593
+4 546 460 593 445
+4 546 433 593 460
+4 733 21 232 31
+4 172 824 487 825
+4 464 825 824 512
+4 427 423 428 125
+4 427 125 346 423
+4 465 777 514 582
+4 412 25 903 233
+4 209 826 832 214
+4 209 222 594 1027
+4 214 826 832 232
+4 217 303 953 268
+4 1018 303 415 217
+4 217 303 268 1018
+4 149 555 630 135
+4 135 427 555 736
+4 555 736 630 135
+4 135 333 149 555
+4 35 29 234 263
+4 126 428 948 127
+4 31 253 733 488
+4 436 919 614 415
+4 415 919 614 953
+4 614 356 452 436
+4 558 821 578 435
+4 596 356 837 456
+4 597 210 372 228
+4 372 834 223 476
+4 478 332 426 482
+4 35 906 263 262
+4 86 961 644 453
+4 252 319 235 332
+4 420 959 562 266
+4 752 157 599 710
+4 225 251 493 600
+4 648 600 524 245
+4 648 600 245 251
+4 525 458 230 19
+4 155 713 452 753
+4 269 351 462 753
+4 269 716 753 462
+4 525 230 1013 19
+4 525 230 224 754
+4 654 754 206 712
+4 921 1013 19 525
+4 525 17 754 224
+4 525 754 1013 230
+4 654 525 754 1013
+4 207 353 196 267
+4 353 207 659 267
+4 51 39 254 984
+4 45 41 746 556
+4 239 216 951 240
+4 114 178 934 128
+4 349 452 526 282
+4 349 791 158 452
+4 353 225 283 15
+4 225 283 15 600
+4 225 283 600 493
+4 952 591 417 532
+4 578 144 282 382
+4 80 611 997 82
+4 82 997 376 611
+4 337 605 376 841
+4 337 605 841 338
+4 92 612 996 94
+4 94 761 770 96
+4 80 995 611 82
+4 82 611 377 995
+4 607 389 377 842
+4 580 842 389 607
+4 584 320 396 350
+4 350 320 396 197
+4 227 313 208 610
+4 610 832 214 208
+4 610 313 566 227
+4 80 607 605 611
+4 611 376 360 361
+4 611 361 360 377
+4 611 605 376 361
+4 611 361 377 607
+4 612 778 342 606
+4 612 342 778 568
+4 612 390 568 357
+4 9 613 200 665
+4 9 613 665 201
+4 613 203 201 827
+4 613 200 203 221
+4 618 89 565 737
+4 45 746 310 831
+4 618 89 91 565
+4 151 137 881 445
+4 619 64 334 68
+4 880 32 619 334
+4 235 952 591 883
+4 310 45 831 547
+4 591 121 883 141
+4 235 105 952 883
+4 622 104 726 108
+4 885 1002 622 81
+4 888 52 255 36
+4 36 255 888 951
+4 104 120 397 886
+4 120 623 419 557
+4 37 53 242 889
+4 624 738 30 567
+4 624 567 30 20
+4 624 738 37 30
+4 254 47 51 947
+4 30 891 344 36
+4 891 344 20 30
+4 107 628 339 896
+4 894 70 306 66
+4 70 306 54 894
+4 149 555 1008 630
+4 149 1008 895 630
+4 568 92 631 892
+4 631 103 517 107
+4 106 900 560 633
+4 633 118 138 558
+4 899 949 576 180
+4 634 488 31 733
+4 634 733 31 21
+4 634 31 488 38
+4 905 54 260 34
+4 905 38 260 54
+4 635 67 965 71
+4 71 635 55 308
+4 636 35 261 55
+4 636 55 261 39
+4 638 31 232 21
+4 150 1007 640 136
+4 150 1007 907 640
+4 641 565 318 325
+4 624 37 790 219
+4 1006 201 202 1027
+4 1006 219 1027 202
+4 567 222 201 1006
+4 1006 222 790 567
+4 1006 790 219 624
+4 136 1007 789 446
+4 1007 450 446 798
+4 1007 450 569 789
+4 1007 789 640 136
+4 386 390 517 554
+4 386 478 554 517
+4 568 386 390 517
+4 386 568 922 517
+4 226 484 488 536
+4 333 555 427 439
+4 555 564 454 466
+4 555 454 564 736
+4 660 520 645 18
+4 645 674 504 520
+4 255 48 247 36
+4 48 255 247 589
+4 123 135 427 119
+4 123 119 427 749
+4 674 251 520 660
+4 674 520 645 660
+4 544 650 329 301
+4 785 544 329 301
+4 650 683 322 79
+4 787 305 651 470
+4 651 722 460 165
+4 787 461 470 651
+4 653 304 786 6
+4 616 653 494 185
+4 743 194 479 312
+4 431 479 194 311
+4 502 161 211 236
+4 717 161 502 236
+4 349 614 186 526
+4 186 614 356 526
+4 157 661 292 719
+4 661 192 731 265
+4 759 184 1033 200
+4 979 200 184 759
+4 979 9 200 665
+4 664 274 380 378
+4 277 664 380 378
+4 300 378 664 277
+4 664 693 378 300
+4 164 512 466 824
+4 728 824 466 472
+4 916 486 324 318
+4 649 916 324 318
+4 650 79 322 274
+4 650 274 322 329
+4 665 201 203 185
+4 665 9 201 637
+4 94 390 612 666
+4 296 666 479 272
+4 272 94 666 390
+4 667 214 610 10
+4 667 188 208 214
+4 667 639 214 10
+4 10 314 610 667
+4 763 313 620 668
+4 2 667 668 588
+4 324 281 318 287
+4 540 324 281 318
+4 785 688 322 286
+4 544 322 286 785
+4 12 201 637 704
+4 704 12 201 202
+4 705 278 378 300
+4 300 693 378 705
+4 973 200 553 184
+4 617 973 184 200
+4 272 115 179 130
+4 14 707 639 215
+4 802 214 188 707
+4 669 472 466 463
+4 728 472 466 669
+4 503 669 466 161
+4 441 471 514 432
+4 521 384 383 439
+4 423 385 384 454
+4 356 673 1026 456
+4 356 452 673 456
+4 423 454 521 427
+4 673 1031 451 456
+4 423 384 521 454
+4 521 384 439 454
+4 384 548 383 1004
+4 675 823 676 461
+4 675 385 464 394
+4 449 450 675 1021
+4 450 1021 676 675
+4 675 487 464 676
+4 676 470 465 750
+4 416 415 400 726
+4 149 1008 555 161
+4 555 466 669 161
+4 161 669 555 211
+4 1008 466 555 161
+4 920 127 739 449
+4 920 569 449 739
+4 979 665 200 759
+4 287 331 325 687
+4 979 665 759 1
+4 291 688 290 337
+4 688 529 327 337
+4 744 689 389 810
+4 543 810 732 389
+4 1015 405 400 690
+4 345 837 405 690
+4 348 1015 690 405
+4 348 405 690 345
+4 304 665 185 704
+4 304 704 637 665
+4 496 109 117 515
+4 496 515 117 586
+4 515 117 586 280
+4 218 241 246 747
+4 172 474 773 825
+4 172 825 487 474
+4 609 474 725 172
+4 725 172 474 773
+4 609 172 487 474
+4 975 53 243 284
+4 975 243 1019 284
+4 678 458 531 263
+4 531 917 263 458
+4 228 834 1025 835
+4 254 984 697 51
+4 698 342 1028 724
+4 698 331 1034 778
+4 295 698 1028 724
+4 57 682 285 65
+4 666 357 699 390
+4 479 666 699 390
+4 699 1029 390 386
+4 974 84 632 82
+4 847 423 127 920
+4 568 92 90 631
+4 90 103 922 631
+4 123 427 858 428
+4 966 393 394 414
+4 663 405 596 558
+4 663 558 596 559
+4 271 403 179 312
+4 271 403 312 562
+4 703 581 131 971
+4 677 706 275 684
+4 706 279 275 684
+4 706 279 684 58
+4 310 547 831 841
+4 547 59 686 310
+4 679 841 338 831
+4 114 694 842 178
+4 725 777 474 582
+4 7 307 709 551
+4 728 669 466 503
+4 731 383 459 548
+4 192 383 731 838
+4 838 548 383 731
+4 292 192 383 731
+4 421 520 225 251
+4 660 251 520 421
+4 600 421 225 251
+4 648 421 251 530
+4 660 530 251 421
+4 656 600 245 524
+4 234 523 210 712
+4 654 210 516 712
+4 713 716 462 753
+4 595 462 447 713
+4 657 220 206 401
+4 353 659 207 225
+4 483 1019 250 290
+4 1020 483 250 290
+4 658 352 264 317
+4 584 302 562 271
+4 302 562 195 350
+4 125 423 847 941
+4 125 941 986 423
+4 941 986 423 799
+4 941 445 423 920
+4 847 423 920 941
+4 941 799 423 445
+4 375 968 326 327
+4 634 488 733 566
+4 634 566 733 21
+4 110 430 780 122
+4 122 780 429 430
+4 122 365 430 989
+4 122 430 429 989
+4 122 110 430 365
+4 322 650 329 544
+4 785 322 329 544
+4 211 333 409 139
+4 211 748 333 139
+4 211 139 409 720
+4 211 748 139 720
+4 837 430 456 1026
+4 596 837 430 456
+4 405 837 430 596
+4 330 273 295 724
+4 724 392 698 342
+4 1028 330 724 513
+4 1028 342 513 724
+4 273 306 295 724
+4 726 397 400 416
+4 176 378 726 776
+4 207 307 328 976
+4 976 207 231 328
+4 7 207 976 307
+4 659 7 207 976
+4 251 273 330 727
+4 44 674 251 727
+4 44 251 279 727
+4 595 729 447 462
+4 716 168 729 956
+4 716 729 595 462
+4 730 202 205 220
+4 657 730 206 220
+4 42 281 245 656
+4 643 902 1030 566
+4 511 902 566 1030
+4 936 280 420 795
+4 920 445 449 460
+4 613 567 827 201
+4 221 200 344 613
+4 564 455 464 512
+4 455 340 512 564
+4 455 142 340 564
+4 733 832 214 610
+4 733 610 566 227
+4 733 214 832 232
+4 974 360 376 343
+4 974 376 360 611
+4 974 611 360 377
+4 974 377 360 576
+4 320 370 236 950
+4 502 370 236 720
+4 196 276 563 629
+4 912 563 741 3
+4 680 527 241 647
+4 117 700 702 936
+4 60 589 48 945
+4 945 247 536 589
+4 651 460 461 823
+4 165 237 460 651
+4 237 460 651 823
+4 736 454 428 427
+4 736 428 454 455
+4 736 455 454 564
+4 227 566 488 226
+4 488 1024 227 226
+4 643 226 488 566
+4 634 488 643 38
+4 922 358 386 387
+4 922 386 358 568
+4 922 517 387 386
+4 143 153 920 569
+4 153 920 569 460
+4 641 737 565 325
+4 375 327 326 321
+4 1003 375 321 327
+4 569 920 449 460
+4 738 248 790 37
+4 567 738 221 222
+4 790 222 738 567
+4 624 790 37 738
+4 136 739 424 789
+4 739 450 811 424
+4 739 569 449 450
+4 789 450 569 739
+4 640 789 739 136
+4 236 522 383 439
+4 445 593 384 385
+4 385 393 593 384
+4 740 295 534 392
+4 740 177 392 534
+4 740 295 392 457
+4 740 177 457 392
+4 656 422 600 524
+4 966 593 394 393
+4 140 407 557 447
+4 146 140 436 447
+4 140 447 557 436
+4 385 393 592 394
+4 385 592 464 394
+4 471 800 495 451
+4 451 800 495 750
+4 493 251 273 295
+4 251 273 295 330
+4 295 251 330 590
+4 696 194 743 312
+4 696 434 312 743
+4 695 743 696 575
+4 695 696 743 581
+4 300 691 278 990
+4 990 574 691 744
+4 928 744 691 574
+4 404 416 415 436
+4 0 187 1017 759
+4 0 759 871 187
+4 759 203 477 185
+4 745 318 565 468
+4 745 193 468 565
+4 745 318 468 486
+4 252 332 235 532
+4 441 432 514 341
+4 989 432 441 341
+4 52 747 255 840
+4 747 241 246 334
+4 481 747 52 840
+4 416 356 436 557
+4 557 447 356 436
+4 557 447 1026 356
+4 426 748 478 482
+4 119 333 749 748
+4 339 482 478 748
+4 288 614 175 217
+4 415 217 614 288
+4 570 553 973 5
+4 749 119 748 482
+4 482 426 749 332
+4 749 332 426 485
+4 52 840 589 480
+4 570 553 184 973
+4 481 52 480 840
+4 480 246 589 541
+4 199 200 812 505
+4 970 750 462 495
+4 539 750 462 970
+4 283 527 241 26
+4 527 283 241 600
+4 147 417 443 292
+4 147 700 417 157
+4 700 710 417 157
+4 919 447 452 713
+4 146 595 447 919
+4 595 713 447 919
+4 918 367 220 27
+4 657 220 401 367
+4 918 317 244 224
+4 27 317 244 918
+4 276 218 839 283
+4 283 26 218 839
+4 147 292 443 368
+4 917 234 230 712
+4 234 523 712 917
+4 234 523 917 29
+4 144 652 435 282
+4 539 604 758 170
+4 917 234 256 230
+4 917 531 263 29
+4 539 604 787 758
+4 225 369 231 28
+4 745 878 515 193
+4 702 710 752 420
+4 270 352 658 525
+4 485 749 427 439
+4 749 439 333 427
+4 521 454 439 427
+4 521 485 427 439
+4 723 351 182 282
+4 682 285 65 33
+4 755 353 659 267
+4 717 156 756 950
+4 717 265 192 756
+4 245 656 524 958
+4 211 555 149 333
+4 211 333 149 409
+4 355 189 484 535
+4 371 816 493 238
+4 225 816 493 371
+4 535 816 371 238
+4 225 816 371 535
+4 467 313 643 328
+4 89 193 737 438
+4 438 193 737 490
+4 672 338 329 785
+4 672 338 785 556
+4 911 653 786 6
+4 616 801 185 188
+4 616 911 653 185
+4 616 473 801 188
+4 473 8 783 927
+4 168 604 787 539
+4 604 474 470 758
+4 734 474 604 758
+4 166 561 582 784
+4 615 354 184 570
+4 735 414 472 788
+4 181 537 313 208
+4 181 467 313 537
+4 239 467 181 537
+4 759 665 203 185
+4 2 667 588 10
+4 667 208 188 187
+4 667 187 668 208
+4 760 607 605 80
+4 760 338 605 361
+4 760 607 580 361
+4 583 580 361 760
+4 583 361 338 760
+4 94 612 606 761
+4 94 666 612 761
+4 761 357 275 571
+4 761 357 311 666
+4 761 357 606 275
+4 761 311 357 571
+4 614 356 436 416
+4 132 691 928 288
+4 691 278 744 288
+4 132 278 691 288
+4 288 691 928 744
+4 240 355 247 589
+4 216 247 255 240
+4 240 226 247 355
+4 216 240 226 247
+4 22 36 888 951
+4 432 514 259 471
+4 139 119 333 625
+4 453 180 560 576
+4 139 119 748 333
+4 139 897 333 409
+4 139 625 333 897
+4 735 870 472 414
+4 554 339 478 212
+4 211 439 555 333
+4 762 315 177 72
+4 762 315 325 177
+4 762 177 480 72
+4 762 177 457 481
+4 762 177 325 457
+4 762 177 481 480
+4 538 530 251 674
+4 307 620 930 763
+4 620 763 615 930
+4 677 671 571 413
+4 677 413 571 275
+4 83 321 69 884
+4 1003 83 884 321
+4 990 764 300 691
+4 95 294 324 765
+4 765 324 671 294
+4 766 301 329 77
+4 766 545 672 329
+4 287 457 295 475
+4 641 457 287 475
+4 556 767 672 338
+4 768 79 664 693
+4 768 693 664 300
+4 769 694 607 80
+4 769 694 842 607
+4 692 694 842 769
+4 677 684 275 770
+4 94 606 684 770
+4 770 606 684 275
+4 771 605 686 80
+4 771 338 841 605
+4 679 771 338 841
+4 772 9 665 637
+4 772 304 637 665
+4 621 639 667 10
+4 164 721 512 773
+4 773 825 777 512
+4 725 773 474 777
+4 332 252 387 478
+4 175 774 400 440
+4 965 805 308 71
+4 635 965 308 71
+4 906 309 308 965
+4 355 536 589 533
+4 541 536 533 589
+4 906 965 308 635
+4 965 309 308 805
+4 532 522 323 591
+4 582 495 561 259
+4 582 561 495 758
+4 859 74 489 358
+4 47 31 864 253
+4 47 864 50 253
+4 667 214 208 610
+4 667 208 668 610
+4 605 607 361 611
+4 760 607 361 605
+4 612 390 357 666
+4 612 357 606 761
+4 612 666 357 761
+4 665 613 200 203
+4 665 613 203 201
+4 759 665 200 203
+4 745 515 496 585
+4 496 515 586 585
+4 564 142 340 152
+4 278 378 689 776
+4 705 776 378 278
+4 142 924 455 340
+4 415 776 278 288
+4 216 951 240 255
+4 515 952 280 252
+4 1033 203 200 181
+4 759 1033 203 200
+4 81 622 1003 1002
+4 316 375 622 1002
+4 1002 622 1003 375
+4 497 147 398 443
+4 398 147 141 443
+4 28 273 504 406
+4 28 273 406 34
+4 310 547 841 686
+4 771 841 686 605
+4 679 771 841 686
+4 465 825 512 777
+4 160 514 721 708
+4 725 721 777 708
+4 773 777 721 512
+4 725 773 777 721
+4 778 568 358 490
+4 778 490 357 568
+4 778 833 357 490
+4 681 93 318 91
+4 91 93 318 745
+4 649 93 318 681
+4 93 916 318 486
+4 916 93 318 649
+4 745 93 318 486
+4 49 254 779 51
+4 51 697 254 779
+4 779 395 697 249
+4 61 289 73 781
+4 61 781 779 289
+4 781 289 395 779
+4 781 395 289 297
+4 780 111 124 425
+4 780 425 345 111
+4 780 837 110 345
+4 780 110 837 430
+4 224 751 250 483
+4 243 1019 250 751
+4 751 250 483 1019
+4 780 124 424 425
+4 254 697 249 779
+4 73 781 289 326
+4 934 288 175 217
+4 934 288 744 175
+4 558 596 559 578
+4 782 604 470 305
+4 782 174 487 877
+4 644 968 335 326
+4 644 335 968 360
+4 375 644 326 968
+4 179 403 969 312
+4 403 179 969 554
+4 312 403 969 699
+4 1029 969 554 403
+4 699 403 969 1029
+4 831 746 299 830
+4 679 556 831 338
+4 783 802 473 188
+4 802 707 188 783
+4 473 927 188 616
+4 794 616 188 927
+4 783 707 188 621
+4 784 758 582 474
+4 758 784 734 474
+4 609 725 474 784
+4 725 582 474 784
+4 785 286 291 688
+4 545 672 329 785
+4 301 545 329 785
+4 304 704 185 786
+4 653 494 185 304
+4 911 653 185 786
+4 787 750 729 539
+4 787 470 750 758
+4 787 604 470 758
+4 384 459 548 393
+4 305 604 470 787
+4 384 393 548 463
+4 617 184 973 570
+4 570 354 184 553
+4 788 472 669 463
+4 788 735 728 472
+4 594 227 818 815
+4 594 254 815 818
+4 728 472 669 788
+4 389 180 453 377
+4 541 981 533 714
+4 440 389 180 453
+4 541 536 714 533
+4 584 320 350 302
+4 641 315 325 762
+4 641 762 325 457
+4 641 737 325 315
+4 789 1007 450 446
+4 789 739 424 450
+4 790 222 219 476
+4 790 222 476 248
+4 1006 790 222 219
+4 738 248 222 790
+4 135 333 555 427
+4 871 615 620 763
+4 620 763 187 871
+4 620 187 2 871
+4 677 571 671 873
+4 876 188 794 616
+4 717 236 192 793
+4 282 456 526 578
+4 596 526 356 456
+4 220 223 751 243
+4 223 751 243 250
+4 160 582 708 166
+4 1019 285 797 290
+4 244 1019 388 285
+4 1019 388 285 290
+4 1020 297 290 289
+4 250 395 1020 289
+4 395 1020 289 297
+4 796 388 290 291
+4 483 796 388 290
+4 256 298 1020 262
+4 262 308 298 297
+4 309 310 299 298
+4 286 285 290 322
+4 810 776 176 400
+4 415 400 726 776
+4 176 776 726 400
+4 731 169 171 548
+4 569 449 675 460
+4 569 461 460 675
+4 798 676 750 461
+4 553 992 200 199
+4 973 200 812 553
+4 704 202 201 185
+4 786 202 704 185
+4 789 424 425 1023
+4 789 446 1023 425
+4 418 799 444 521
+4 799 384 445 444
+4 521 799 444 384
+4 449 385 455 1021
+4 1021 676 464 465
+4 449 1021 675 385
+4 1021 464 676 675
+4 441 587 800 450
+4 441 800 465 471
+4 450 800 676 587
+4 800 465 750 676
+4 564 466 464 454
+4 564 464 466 512
+4 911 473 205 801
+4 215 214 802 707
+4 783 215 473 802
+4 215 707 802 783
+4 458 352 230 257
+4 352 257 483 230
+4 352 317 388 264
+4 352 317 483 388
+4 224 813 803 223
+4 224 483 250 803
+4 803 1020 483 250
+4 234 256 492 262
+4 234 256 262 263
+4 297 1022 781 335
+4 1022 968 327 326
+4 290 336 337 327
+4 298 308 805 1022
+4 298 336 1022 805
+4 805 343 336 335
+4 310 343 337 336
+4 808 357 275 331
+4 381 468 357 808
+4 808 331 687 833
+4 571 357 275 808
+4 381 571 808 357
+4 337 360 361 809
+4 529 361 809 337
+4 175 440 400 810
+4 491 810 400 440
+4 428 811 449 455
+4 429 1023 450 441
+4 1023 441 451 450
+4 712 206 813 754
+4 754 230 224 813
+4 206 401 223 813
+4 790 476 219 242
+4 790 476 242 1032
+4 814 493 238 295
+4 814 534 295 238
+4 493 814 475 295
+4 475 457 295 814
+4 814 295 534 740
+4 814 295 740 457
+4 437 248 947 815
+4 815 248 947 254
+4 536 260 484 488
+4 316 176 726 400
+4 316 726 397 400
+4 222 248 594 817
+4 817 248 594 254
+4 817 254 249 248
+4 222 248 817 476
+4 476 249 248 817
+4 815 227 818 253
+4 815 254 253 818
+4 31 253 818 733
+4 31 818 253 865
+4 826 228 594 1025
+4 228 835 1025 819
+4 319 438 358 490
+4 438 820 358 490
+4 315 438 820 358
+4 342 568 358 778
+4 90 568 358 342
+4 859 342 358 489
+4 90 342 358 859
+4 918 220 751 244
+4 220 223 918 751
+4 425 1026 446 451
+4 1026 451 447 446
+4 557 425 1026 446
+4 417 532 443 197
+4 417 396 532 197
+4 417 710 197 292
+4 417 197 443 292
+4 417 420 396 197
+4 417 420 197 710
+4 205 223 210 206
+4 205 223 372 210
+4 205 597 210 372
+4 473 205 597 210
+4 911 210 206 205
+4 911 210 205 473
+4 225 535 231 994
+4 225 535 371 231
+4 428 423 454 455
+4 423 385 454 455
+4 117 586 795 703
+4 586 581 795 703
+4 475 287 641 281
+4 891 200 1009 505
+4 344 200 1009 891
+4 186 435 821 258
+4 560 558 900 258
+4 258 558 900 435
+4 239 467 537 240
+4 53 822 284 983
+4 785 329 322 688
+4 722 153 461 460
+4 651 722 461 460
+4 68 89 737 848
+4 618 68 89 737
+4 68 737 641 315
+4 618 68 737 641
+4 173 823 651 742
+4 823 470 676 461
+4 651 823 461 470
+4 787 461 750 470
+4 487 465 470 474
+4 676 487 465 470
+4 823 470 487 676
+4 251 330 590 279
+4 251 279 590 757
+4 465 825 777 474
+4 727 330 251 279
+4 773 474 777 825
+4 209 372 826 597
+4 597 826 215 228
+4 209 372 594 826
+4 826 232 819 1025
+4 827 203 204 828
+4 827 222 828 204
+4 567 222 221 827
+4 613 203 827 221
+4 613 567 221 827
+4 201 222 204 1027
+4 1006 201 1027 222
+4 1006 219 222 1027
+4 208 828 221 227
+4 219 889 243 220
+4 243 242 476 249
+4 219 476 243 242
+4 493 251 245 600
+4 239 189 467 240
+4 981 306 534 260
+4 982 297 829 395
+4 55 829 982 308
+4 982 297 395 63
+4 291 688 337 830
+4 785 291 830 688
+4 785 338 329 830
+4 299 830 337 831
+4 831 841 338 337
+4 209 208 832 828
+4 209 594 828 832
+4 610 227 832 208
+4 733 227 832 610
+4 833 357 490 468
+4 808 331 833 357
+4 687 468 808 833
+4 329 337 338 361
+4 380 274 329 361
+4 529 337 329 361
+4 583 329 338 361
+4 583 380 329 361
+4 331 357 606 778
+4 612 357 778 606
+4 612 778 357 568
+4 833 331 778 357
+4 834 492 250 249
+4 834 697 492 249
+4 835 261 492 697
+4 179 969 390 993
+4 272 993 179 390
+4 993 272 479 390
+4 993 969 390 699
+4 993 479 699 390
+4 64 618 565 641
+4 64 618 879 565
+4 809 375 327 968
+4 375 732 644 968
+4 375 968 809 732
+4 226 484 643 488
+4 488 260 484 643
+4 280 403 396 420
+4 403 584 396 420
+4 871 187 2 0
+4 280 434 403 420
+4 584 420 403 562
+4 434 562 403 420
+4 735 414 909 933
+4 548 933 909 414
+4 616 4 911 473
+4 627 5 11 199
+4 967 597 228 210
+4 4 8 473 927
+4 664 79 274 378
+4 664 79 378 693
+4 79 213 274 378
+4 693 1002 79 378
+4 79 1002 213 378
+4 627 951 199 11
+4 627 642 951 11
+4 408 12 23 1006
+4 6 12 704 202
+4 14 412 233 25
+4 967 215 14 937
+4 473 967 597 215
+4 202 205 220 219
+4 210 234 229 228
+4 234 835 229 228
+4 655 999 741 3
+4 422 600 15 16
+4 563 608 207 239
+4 111 853 836 120
+4 120 836 419 853
+4 345 425 836 111
+4 345 425 837 836
+4 836 356 1026 837
+4 836 425 557 419
+4 836 557 1026 356
+4 211 236 439 748
+4 439 236 426 748
+4 780 425 837 345
+4 780 837 425 430
+4 333 439 749 748
+4 654 712 206 210
+4 211 439 333 748
+4 426 439 748 749
+4 16 26 283 839
+4 793 909 463 1004
+4 1004 463 548 909
+4 527 16 283 600
+4 717 838 192 265
+4 265 192 731 838
+4 717 838 793 192
+4 839 629 627 276
+4 645 520 28 18
+4 353 283 939 15
+4 749 748 426 482
+4 840 255 589 246
+4 917 523 712 19
+4 840 747 255 246
+4 917 523 19 29
+4 481 747 840 246
+4 840 246 589 480
+4 481 840 480 246
+4 160 582 514 708
+4 512 721 514 777
+4 841 343 962 376
+4 841 605 376 962
+4 279 684 606 275
+4 279 513 606 684
+4 513 963 342 606
+4 842 377 607 949
+4 890 567 624 20
+4 607 842 949 694
+4 307 763 930 354
+4 520 727 504 273
+4 520 674 504 727
+4 902 566 634 21
+4 68 843 762 481
+4 68 52 843 481
+4 68 843 72 762
+4 904 638 232 21
+4 137 418 121 844
+4 137 844 125 986
+4 87 845 438 101
+4 846 101 113 1012
+4 846 113 121 1012
+4 847 920 143 137
+4 848 87 89 438
+4 852 1032 61 49
+4 852 53 61 822
+4 136 853 120 419
+4 124 424 425 853
+4 69 284 854 53
+4 69 987 73 854
+4 70 306 855 54
+4 70 489 74 855
+4 856 247 48 36
+4 906 29 35 263
+4 856 46 48 247
+4 857 248 49 46
+4 23 1006 219 624
+4 954 998 838 265
+4 265 838 731 998
+4 864 31 38 488
+4 363 90 103 922
+4 864 38 50 488
+4 24 634 643 38
+4 123 427 135 858
+4 865 39 254 51
+4 25 39 232 638
+4 655 267 741 999
+4 267 655 955 999
+4 859 358 90 88
+4 119 749 123 112
+4 860 75 86 335
+4 860 923 86 84
+4 15 353 755 267
+4 84 364 102 576
+4 861 51 63 697
+4 861 697 63 55
+4 71 55 862 308
+4 71 862 75 985
+4 269 791 1001 182
+4 269 753 1001 791
+4 253 31 864 488
+4 253 864 50 488
+4 47 865 254 51
+4 953 183 158 268
+4 953 268 158 542
+4 866 739 143 127
+4 866 136 143 739
+4 122 867 134 429
+4 867 924 142 134
+4 496 695 585 703
+4 868 198 644 99
+4 868 348 198 99
+4 868 1015 348 405
+4 868 198 453 644
+4 868 453 1015 405
+4 280 379 1029 403
+4 280 1029 379 869
+4 515 379 869 280
+4 635 35 55 308
+4 515 252 280 869
+4 478 482 517 387
+4 478 339 554 517
+4 334 641 475 457
+4 295 330 724 1028
+4 330 513 1028 279
+4 295 590 330 1028
+4 590 330 1028 279
+4 870 572 472 414
+4 735 573 870 414
+4 871 615 763 184
+4 792 871 184 615
+4 872 574 691 990
+4 990 872 764 691
+4 571 765 671 873
+4 875 766 672 329
+4 338 672 329 875
+4 767 875 672 338
+4 188 876 185 616
+4 494 616 185 876
+4 877 474 734 609
+4 470 877 474 604
+4 474 734 604 877
+4 974 376 962 343
+4 898 962 343 974
+4 898 974 343 632
+4 482 103 339 517
+4 670 40 910 958
+4 207 189 239 608
+4 478 339 517 482
+4 467 643 484 328
+4 608 189 239 240
+4 879 618 91 565
+4 64 880 619 334
+4 881 546 445 153
+4 672 556 785 41
+4 881 153 399 546
+4 540 42 281 324
+4 882 151 398 444
+4 544 785 286 43
+4 417 591 883 141
+4 122 365 989 134
+4 122 989 429 134
+4 700 883 141 417
+4 885 108 726 693
+4 108 726 622 885
+4 148 432 514 160
+4 140 886 404 416
+4 432 711 160 148
+4 432 160 259 514
+4 711 259 432 160
+4 887 150 446 407
+4 623 887 150 446
+4 49 249 1032 61
+4 49 249 248 1032
+4 49 61 779 249
+4 49 254 248 249
+4 49 249 779 254
+4 32 402 888 218
+4 890 408 23 1006
+4 37 1032 852 49
+4 23 1006 624 890
+4 952 396 417 420
+4 936 952 417 420
+4 891 1009 22 505
+4 344 1009 22 891
+4 33 65 975 285
+4 37 53 852 242
+4 854 983 61 53
+4 915 894 306 66
+4 915 894 66 406
+4 895 564 152 630
+4 680 647 241 56
+4 896 628 339 139
+4 897 149 333 409
+4 897 625 333 149
+4 898 962 974 82
+4 373 603 65 57
+4 898 82 974 632
+4 138 435 411 900
+4 900 558 138 435
+4 273 34 894 406
+4 900 138 558 633
+4 901 711 432 148
+4 78 338 771 760
+4 583 760 338 78
+4 767 78 338 679
+4 679 78 338 771
+4 767 583 338 78
+4 903 35 234 261
+4 412 35 234 903
+4 903 35 261 636
+4 854 983 73 61
+4 25 638 232 904
+4 905 34 260 231
+4 510 905 231 34
+4 906 309 965 67
+4 906 67 965 635
+4 907 569 153 640
+4 908 721 512 152
+4 908 152 512 340
+4 64 68 618 641
+4 65 321 884 69
+4 66 70 342 626
+4 67 632 343 71
+4 76 78 574 583
+4 767 875 1010 76
+4 277 872 583 76
+4 277 76 764 872
+4 381 431 311 379
+4 381 571 311 97
+4 67 898 343 632
+4 314 511 588 10
+4 322 884 1003 715
+4 66 626 342 893
+4 272 94 96 666
+4 194 96 311 666
+4 134 341 429 340
+4 807 806 340 134
+4 134 340 924 142
+4 782 877 470 604
+4 807 340 341 134
+4 793 909 1004 838
+4 838 1004 548 909
+4 806 142 340 134
+4 134 340 429 924
+4 105 101 846 235
+4 312 403 434 562
+4 746 291 299 830
+4 315 848 737 438
+4 68 848 737 315
+4 212 410 115 107
+4 81 1002 693 885
+4 404 108 116 726
+4 1019 285 244 33
+4 1019 975 797 285
+4 140 887 623 557
+4 557 407 446 447
+4 262 309 308 906
+4 262 906 263 309
+4 903 234 233 261
+4 700 109 883 952
+4 883 105 952 109
+4 104 397 120 851
+4 629 563 196 912
+4 629 5 563 912
+4 128 913 934 701
+4 217 913 268 701
+4 105 846 121 235
+4 914 210 516 654
+4 844 346 121 113
+4 654 206 957 914
+4 210 914 206 654
+4 844 113 125 346
+4 774 356 405 821
+4 349 282 652 154
+4 555 466 463 669
+4 669 463 555 211
+4 236 669 211 463
+4 793 669 909 167
+4 793 669 236 463
+4 439 384 1004 463
+4 384 548 1004 463
+4 908 340 514 341
+4 908 512 514 340
+4 33 53 242 243
+4 53 889 33 242
+4 33 243 242 889
+4 743 940 431 575
+4 695 940 743 575
+4 575 940 431 579
+4 695 940 575 579
+4 583 580 574 744
+4 692 744 574 580
+4 330 273 724 915
+4 330 727 915 58
+4 727 406 915 58
+4 273 306 724 915
+4 273 894 306 915
+4 273 894 915 406
+4 757 413 590 910
+4 332 323 235 532
+4 251 757 590 910
+4 218 246 608 255
+4 218 246 283 608
+4 246 747 255 218
+4 335 336 968 360
+4 1022 335 968 326
+4 1022 968 335 336
+4 1002 726 176 378
+4 726 176 316 1002
+4 237 966 572 293
+4 966 293 414 572
+4 360 377 453 576
+4 377 180 453 576
+4 717 669 793 167
+4 717 669 236 793
+4 334 457 475 246
+4 481 334 246 457
+4 87 101 358 88
+4 88 387 101 358
+4 319 101 387 358
+4 87 438 358 101
+4 438 101 319 358
+4 942 74 88 358
+4 942 358 88 87
+4 942 87 438 358
+4 315 942 438 358
+4 747 334 246 481
+4 381 486 687 324
+4 916 486 381 324
+4 650 274 329 380
+4 217 268 953 542
+4 349 217 953 542
+4 916 579 381 486
+4 195 599 420 266
+4 195 420 599 350
+4 630 736 142 135
+4 919 436 452 447
+4 235 591 532 323
+4 640 739 143 136
+4 270 264 658 352
+4 170 391 561 269
+4 269 723 561 351
+4 293 433 165 237
+4 677 413 275 706
+4 413 279 275 706
+4 654 1013 921 525
+4 654 712 516 921
+4 137 445 941 920
+4 847 941 920 137
+4 577 247 856 36
+4 577 46 856 247
+4 577 46 247 437
+4 577 437 1024 221
+4 69 326 321 849
+4 849 85 326 375
+4 849 375 326 321
+4 136 623 150 446
+4 857 248 738 37
+4 857 46 437 248
+4 857 248 437 738
+4 135 858 427 736
+4 858 736 428 427
+4 866 424 739 127
+4 866 136 739 424
+4 88 922 100 387
+4 88 387 358 922
+4 922 103 387 517
+4 100 922 103 387
+4 71 335 860 923
+4 923 961 335 86
+4 71 343 335 923
+4 923 360 343 335
+4 860 335 86 923
+4 865 254 253 47
+4 700 141 147 417
+4 594 254 818 1025
+4 865 39 818 254
+4 865 818 253 254
+4 126 429 924 867
+4 126 429 428 924
+4 924 811 429 428
+4 924 429 811 340
+4 867 429 924 134
+4 762 334 481 457
+4 762 334 457 641
+4 504 273 727 406
+4 330 273 915 727
+4 273 406 915 727
+4 484 231 260 238
+4 905 231 260 484
+4 630 564 152 142
+4 806 152 340 142
+4 881 143 153 445
+4 1013 230 712 19
+4 921 712 19 1013
+4 1013 754 712 230
+4 640 569 153 143
+4 654 1013 754 712
+4 654 712 921 1013
+4 502 211 409 720
+4 502 717 236 370
+4 925 175 258 186
+4 186 435 258 925
+4 696 194 312 926
+4 696 133 926 312
+4 40 44 413 442
+4 473 927 783 188
+4 4 927 473 616
+4 621 188 783 927
+4 758 929 734 784
+4 170 929 734 758
+4 609 725 784 929
+4 140 557 407 887
+4 696 931 133 434
+4 695 696 581 931
+4 932 735 728 788
+4 167 932 909 171
+4 40 671 324 294
+4 41 545 785 672
+4 735 293 414 933
+4 555 736 564 630
+4 141 882 398 443
+4 634 643 488 566
+4 922 568 631 517
+4 618 737 565 641
+4 1006 790 624 567
+4 624 790 738 567
+4 1007 789 569 640
+4 640 789 569 739
+4 151 881 399 546
+4 258 180 774 175
+4 258 175 774 186
+4 774 180 440 175
+4 114 842 925 934
+4 175 842 744 934
+4 276 283 839 939
+4 939 629 839 276
+4 353 283 276 939
+4 939 629 276 196
+4 282 154 182 723
+4 277 744 689 583
+4 282 144 154 723
+4 583 380 689 277
+4 237 823 394 460
+4 164 773 512 824
+4 756 156 159 950
+4 824 825 773 512
+4 608 239 951 240
+4 199 1009 992 200
+4 992 200 1009 216
+4 288 278 744 776
+4 278 689 744 776
+4 459 497 398 443
+4 276 642 218 951
+4 642 951 402 218
+4 510 976 231 328
+4 13 510 24 328
+4 150 935 446 407
+4 935 446 447 729
+4 935 447 407 500
+4 935 595 447 500
+4 595 935 447 729
+4 12 23 219 889
+4 12 202 730 220
+4 12 220 219 202
+4 657 347 730 220
+4 12 506 889 220
+4 347 506 220 657
+4 411 258 925 106
+4 925 508 435 411
+4 109 117 515 952
+4 700 952 117 109
+4 234 937 523 509
+4 233 412 14 937
+4 215 228 233 937
+4 967 215 937 228
+4 937 509 234 412
+4 643 260 484 905
+4 535 484 533 238
+4 355 535 484 533
+4 508 469 435 652
+4 469 349 435 652
+4 925 469 435 508
+4 701 349 652 938
+4 701 938 542 349
+4 353 939 276 196
+4 198 176 316 400
+4 104 886 397 726
+4 991 459 169 163
+4 695 743 940 581
+4 585 579 940 695
+4 585 695 940 703
+4 703 940 581 695
+4 692 842 744 580
+4 745 878 496 515
+4 908 721 514 512
+4 758 561 929 784
+4 929 972 784 166
+4 954 909 171 167
+4 644 176 198 491
+4 375 176 198 644
+4 58 513 330 279
+4 58 513 279 684
+4 598 168 787 729
+4 991 163 498 433
+4 936 420 417 710
+4 702 710 420 936
+4 936 700 702 710
+4 700 936 417 710
+4 933 293 991 169
+4 237 433 165 460
+4 169 171 548 933
+4 60 541 74 72
+4 60 480 541 72
+4 734 758 604 170
+4 756 265 1005 159
+4 100 332 112 113
+4 85 644 99 86
+4 609 172 174 487
+4 944 345 111 110
+4 111 780 124 122
+4 49 779 61 51
+4 61 781 73 75
+4 122 946 126 948
+4 946 948 127 126
+4 471 259 495 582
+4 471 514 259 582
+4 46 50 48 247
+4 85 943 326 644
+4 943 644 335 326
+4 46 248 49 947
+4 947 49 51 254
+4 122 429 780 948
+4 122 946 948 780
+4 946 424 127 948
+4 549 927 783 8
+4 899 949 180 106
+4 82 949 576 899
+4 377 949 180 576
+4 82 949 377 576
+4 576 180 560 899
+4 653 185 786 304
+4 129 116 415 190
+4 717 950 192 236
+4 350 1005 599 159
+4 717 950 756 192
+4 756 950 159 1005
+4 484 231 328 905
+4 510 328 231 905
+4 882 398 443 444
+4 398 459 443 444
+4 900 435 411 258
+4 411 435 925 258
+4 234 412 903 233
+4 233 234 412 937
+4 888 402 951 218
+4 218 747 255 888
+4 887 407 446 557
+4 407 935 446 447
+4 12 889 219 220
+4 682 244 285 33
+4 417 591 952 883
+4 700 952 883 417
+4 700 417 936 952
+4 953 155 158 183
+4 599 292 710 157
+4 415 499 436 919
+4 953 155 919 452
+4 499 919 415 303
+4 60 541 589 945
+4 945 589 536 541
+4 598 305 651 787
+4 598 461 787 651
+4 540 294 324 649
+4 540 649 324 318
+4 717 954 838 265
+4 717 954 793 838
+4 717 793 954 167
+4 955 7 207 659
+4 267 207 659 955
+4 956 716 269 462
+4 729 956 462 716
+4 914 516 210 8
+4 670 958 245 524
+4 538 674 251 44
+4 966 433 293 237
+4 560 900 558 633
+4 989 559 432 901
+4 84 974 923 961
+4 961 335 644 360
+4 84 961 576 974
+4 961 360 644 453
+4 961 453 576 360
+4 923 360 335 961
+4 382 456 578 432
+4 382 259 456 432
+4 382 351 456 259
+4 456 282 351 382
+4 456 578 282 382
+4 759 792 0 871
+4 67 686 962 898
+4 67 310 962 686
+4 82 376 997 962
+4 841 962 686 605
+4 310 841 962 686
+4 898 997 962 82
+4 66 963 964 893
+4 66 964 963 513
+4 92 612 963 996
+4 513 684 963 606
+4 893 963 996 92
+4 350 396 420 197
+4 350 197 420 599
+4 1 9 665 772
+4 58 964 513 684
+4 617 200 184 979
+4 964 963 684 893
+4 964 684 963 513
+4 979 9 665 1
+4 518 904 25 232
+4 518 233 25 14
+4 106 114 519 949
+4 899 519 949 106
+4 244 483 388 1019
+4 483 388 1019 290
+4 621 10 667 2
+4 395 829 1020 297
+4 616 4 653 911
+4 776 176 689 810
+4 912 5 563 3
+4 200 505 9 979
+4 1 665 494 304
+4 1 304 772 665
+4 657 957 730 6
+4 8 707 783 549
+4 662 8 210 516
+4 177 358 392 489
+4 177 358 820 392
+4 392 698 358 820
+4 489 342 358 392
+4 392 342 358 698
+4 394 823 487 675
+4 505 1009 22 11
+4 11 22 402 951
+4 627 563 629 5
+4 637 408 201 12
+4 8 977 967 14
+4 662 967 210 8
+4 215 14 707 8
+4 977 967 14 937
+4 523 967 234 210
+4 523 967 937 234
+4 15 755 353 225
+4 999 655 955 3
+4 525 17 352 658
+4 1018 217 415 288
+4 754 206 957 654
+4 74 177 489 358
+4 315 358 820 177
+4 942 358 177 74
+4 315 358 177 942
+4 656 16 600 422
+4 656 527 600 16
+4 754 657 957 206
+4 657 401 754 17
+4 225 755 659 18
+4 712 210 516 662
+4 712 523 210 662
+4 712 662 921 19
+4 712 19 523 662
+4 531 917 458 19
+4 639 21 904 214
+4 880 26 32 241
+4 682 244 33 27
+4 14 509 937 412
+4 541 740 534 177
+4 685 263 531 29
+4 685 906 263 29
+4 480 177 740 541
+4 481 177 457 740
+4 619 32 52 334
+4 481 177 740 480
+4 245 656 958 42
+4 649 95 324 916
+4 540 42 324 294
+4 544 301 785 43
+4 301 545 785 43
+4 44 727 279 58
+4 45 678 602 960
+4 45 59 547 310
+4 193 490 468 737
+4 319 193 438 490
+4 565 193 468 737
+4 353 225 994 283
+4 918 352 317 224
+4 547 556 45 831
+4 39 51 861 984
+4 843 52 60 480
+4 438 737 820 490
+4 855 981 62 54
+4 39 261 861 55
+4 862 55 63 982
+4 28 645 504 520
+4 263 59 309 906
+4 263 59 906 685
+4 843 60 72 480
+4 855 981 74 62
+4 349 435 652 282
+4 862 63 75 982
+4 560 821 558 258
+4 264 317 682 658
+4 682 264 388 317
+4 57 286 682 646
+4 682 57 285 286
+4 346 332 485 323
+4 749 332 485 346
+4 764 277 77 76
+4 77 79 664 768
+4 729 956 539 462
+4 970 561 462 269
+4 970 956 269 462
+4 539 970 462 956
+4 911 6 730 957
+4 914 206 957 911
+4 210 911 206 914
+4 914 210 911 473
+4 692 580 574 78
+4 692 769 580 78
+4 8 914 473 210
+4 41 264 960 746
+4 910 42 245 958
+4 670 910 245 958
+4 910 670 245 251
+4 131 959 434 795
+4 752 710 599 420
+4 878 745 91 89
+4 703 581 971 695
+4 695 931 581 971
+4 95 93 916 649
+4 929 725 784 972
+4 725 708 784 972
+4 575 431 97 95
+4 167 932 552 788
+4 552 167 669 191
+4 552 788 669 167
+4 695 579 575 95
+4 426 332 252 532
+4 97 311 1011 96
+4 677 761 571 96
+4 874 97 431 1011
+4 280 396 952 420
+4 936 280 952 420
+4 411 925 114 106
+4 523 967 210 662
+4 91 878 496 745
+4 227 566 733 488
+4 733 253 227 488
+4 733 818 832 227
+4 733 253 818 227
+4 974 360 343 923
+4 974 360 923 961
+4 974 961 576 360
+4 94 501 390 272
+4 186 774 356 614
+4 934 913 217 701
+4 934 288 217 913
+4 93 496 745 91
+4 93 980 585 496
+4 550 744 128 928
+4 258 180 560 774
+4 560 180 440 774
+4 440 453 180 560
+4 114 949 925 842
+4 842 377 949 180
+4 106 114 949 925
+4 77 768 664 300
+4 220 33 243 244
+4 35 234 262 263
+4 931 131 133 434
+4 971 931 581 131
+4 561 970 495 758
+4 758 750 970 495
+4 539 750 970 758
+4 195 266 562 1035
+4 953 155 183 303
+4 117 795 131 703
+4 795 131 959 702
+4 926 133 130 312
+4 309 678 263 257
+4 263 685 531 59
+4 114 508 925 411
+4 115 507 410 212
+4 79 81 715 1002
+4 79 213 322 274
+4 375 327 213 809
+4 375 176 809 213
+4 937 977 523 509
+4 523 967 977 937
+4 662 523 967 977
+4 978 595 935 500
+4 595 978 935 729
+4 716 978 595 729
+4 200 505 979 617
+4 349 154 158 791
+4 182 791 158 154
+4 980 95 579 695
+4 980 579 585 695
+4 980 695 585 496
+4 595 713 919 155
+4 713 462 716 595
+4 713 155 595 716
+4 752 157 159 599
+4 288 776 744 810
+4 288 175 810 744
+4 417 710 292 157
+4 119 748 482 628
+4 339 482 748 628
+4 702 752 710 157
+4 702 157 710 700
+4 748 139 212 339
+4 339 139 212 410
+4 339 139 410 896
+4 981 74 541 489
+4 981 489 534 306
+4 855 306 981 54
+4 855 489 74 981
+4 55 829 261 697
+4 697 395 829 492
+4 861 51 697 984
+4 861 261 697 55
+4 982 985 297 75
+4 862 55 982 308
+4 862 982 75 985
+4 54 62 714 981
+4 50 62 536 863
+4 54 863 714 62
+4 61 249 1032 822
+4 852 1032 822 61
+4 852 53 822 242
+4 851 836 397 120
+4 345 836 397 851
+4 236 161 211 669
+4 983 289 987 73
+4 854 284 983 53
+4 854 987 73 983
+4 346 121 323 418
+4 844 418 121 346
+4 844 346 125 986
+4 863 260 488 38
+4 488 863 536 260
+4 863 62 536 714
+4 978 162 935 729
+4 39 984 861 261
+4 861 984 697 261
+4 71 862 985 308
+4 982 308 297 985
+4 862 982 985 308
+4 722 162 598 461
+4 70 306 489 855
+4 855 306 489 981
+4 346 986 418 521
+4 986 423 799 521
+4 137 844 986 418
+4 844 346 986 418
+4 348 851 345 397
+4 124 424 853 136
+4 37 242 852 1032
+4 1032 249 242 822
+4 852 242 822 1032
+4 69 284 987 854
+4 983 289 284 987
+4 854 284 987 983
+4 723 351 259 561
+4 129 448 278 300
+4 983 61 822 289
+4 983 822 284 289
+4 111 425 836 853
+4 853 836 419 425
+4 972 708 784 166
+4 982 697 395 829
+4 982 829 297 308
+4 735 932 909 788
+4 102 118 633 988
+4 98 110 366 405
+4 102 98 366 988
+4 989 118 663 559
+4 717 161 669 167
+4 191 167 669 161
+4 598 168 305 787
+4 978 168 162 729
+4 305 604 787 168
+4 541 981 534 533
+4 933 293 414 966
+4 541 981 714 62
+4 541 536 62 714
+4 159 1005 661 265
+4 159 1005 292 661
+4 98 988 453 405
+4 988 453 405 560
+4 560 558 405 988
+4 988 118 558 663
+4 988 98 366 405
+4 110 118 663 365
+4 601 487 174 172
+4 181 354 763 313
+4 1033 187 203 208
+4 181 313 763 668
+4 573 173 174 572
+4 146 919 303 155
+4 953 919 303 415
+4 951 218 255 888
+4 914 911 957 4
+4 300 990 278 277
+4 277 990 744 583
+4 277 764 300 990
+4 277 990 583 872
+4 277 872 764 990
+4 642 11 402 951
+4 402 218 32 26
+4 670 524 245 648
+4 648 538 530 251
+4 186 526 356 821
+4 596 821 356 526
+4 26 241 880 680
+4 26 527 241 680
+4 660 251 530 674
+4 571 765 873 97
+4 541 534 489 177
+4 534 177 392 489
+4 355 536 533 484
+4 716 168 978 729
+4 6 12 730 347
+4 116 499 404 415
+4 114 128 934 701
+4 114 934 925 701
+4 114 701 925 508
+4 199 951 239 992
+4 239 992 951 216
+4 199 1009 951 992
+4 951 992 1009 216
+4 627 276 563 951
+4 627 642 276 951
+4 469 349 186 435
+4 359 130 179 115
+4 186 821 774 258
+4 186 356 774 821
+4 901 711 578 432
+4 559 901 578 432
+4 497 368 147 443
+4 500 595 447 146
+4 502 720 409 145
+4 502 370 720 145
+4 240 355 189 484
+4 467 189 484 240
+4 717 161 236 669
+4 207 231 189 994
+4 994 535 231 189
+4 608 816 994 189
+4 994 816 535 189
+4 389 440 491 453
+4 40 670 910 538
+4 1035 959 562 133
+4 430 456 441 432
+4 441 456 471 432
+4 441 451 471 456
+4 995 694 949 899
+4 995 607 377 949
+4 80 694 607 995
+4 995 607 949 694
+4 996 606 684 94
+4 996 606 963 684
+4 893 963 684 996
+4 80 605 686 997
+4 997 962 376 605
+4 997 605 686 962
+4 898 686 962 997
+4 648 251 245 670
+4 648 670 538 251
+4 718 20 775 408
+4 718 9 775 20
+4 930 655 354 3
+4 3 354 570 563
+4 570 354 553 563
+4 563 553 239 354
+4 349 217 186 614
+4 300 664 77 277
+4 767 679 338 556
+4 766 545 329 301
+4 9 20 200 613
+4 613 200 344 20
+4 20 200 891 505
+4 344 200 891 20
+4 1002 378 176 213
+4 884 1003 321 322
+4 683 322 79 715
+4 79 715 322 213
+4 9 20 613 775
+4 20 567 775 890
+4 20 567 613 775
+4 20 408 890 775
+4 769 607 760 80
+4 769 607 580 760
+4 94 606 770 761
+4 761 606 770 275
+4 771 760 605 80
+4 771 338 605 760
+4 793 236 192 1004
+4 838 1004 383 548
+4 192 236 383 1004
+4 793 838 1004 192
+4 192 1004 383 838
+4 1005 197 292 192
+4 756 265 192 1005
+4 950 192 197 1005
+4 756 950 1005 192
+4 1005 192 661 265
+4 1005 192 292 661
+4 12 1006 201 202
+4 408 12 1006 201
+4 890 567 201 1006
+4 890 408 1006 201
+4 1007 907 569 461
+4 1007 729 798 446
+4 722 1007 162 461
+4 907 1007 722 461
+4 1008 895 564 466
+4 1008 503 466 161
+4 1008 895 466 503
+4 1008 564 555 466
+4 705 726 116 693
+4 1002 693 726 378
+4 200 216 344 1009
+4 505 199 1009 11
+4 515 117 280 952
+4 416 436 614 415
+4 193 515 379 1016
+4 515 1016 193 319
+4 193 1016 379 468
+4 193 1016 468 490
+4 319 193 490 1016
+4 135 625 149 333
+4 1006 567 624 890
+4 517 892 631 107
+4 1007 569 907 640
+4 643 1030 313 566
+4 902 643 634 566
+4 1008 555 564 630
+4 1008 564 895 630
+4 48 589 247 945
+4 870 572 487 472
+4 871 1033 184 763
+4 763 1033 187 871
+4 871 759 184 1033
+4 95 765 324 381
+4 381 765 324 571
+4 571 324 671 765
+4 1010 77 329 380
+4 1010 329 338 583
+4 1010 380 329 583
+4 1010 766 329 77
+4 1010 766 875 329
+4 338 875 329 1010
+4 767 1010 338 583
+4 338 875 1010 767
+4 1017 759 494 1
+4 1017 477 185 759
+4 477 1017 185 876
+4 494 876 185 1017
+4 876 185 477 188
+4 173 572 742 782
+4 782 487 470 877
+4 470 487 474 877
+4 487 474 877 609
+4 416 415 614 400
+4 1011 431 194 311
+4 1011 311 194 96
+4 874 1011 431 194
+4 194 312 926 296
+4 195 159 599 266
+4 195 599 159 350
+4 101 1012 235 332
+4 846 101 1012 235
+4 649 681 318 540
+4 650 544 322 683
+4 651 598 461 722
+4 532 426 522 197
+4 532 396 426 197
+4 599 292 197 710
+4 123 346 749 427
+4 749 346 485 427
+4 521 439 522 485
+4 323 521 522 485
+4 444 383 384 459
+4 244 682 388 317
+4 1014 521 522 323
+4 591 522 323 1014
+4 591 443 522 1014
+4 418 444 1014 521
+4 521 444 1014 383
+4 521 383 1014 522
+4 443 532 522 197
+4 443 522 532 591
+4 444 546 384 445
+4 198 316 397 400
+4 198 690 400 397
+4 348 198 397 690
+4 348 397 198 851
+4 198 1015 400 690
+4 348 198 690 1015
+4 868 198 348 1015
+4 868 453 198 1015
+4 533 534 238 260
+4 981 534 533 260
+4 644 453 491 198
+4 453 440 1015 405
+4 491 453 1015 198
+4 491 440 1015 453
+4 240 608 189 355
+4 189 246 535 355
+4 608 255 246 589
+4 608 246 816 189
+4 816 246 535 189
+4 207 328 467 189
+4 467 328 484 189
+4 456 259 1031 471
+4 471 259 1031 495
+4 471 495 1031 451
+4 456 471 1031 451
+4 468 357 1016 379
+4 490 357 1016 468
+4 188 802 473 209
+4 473 802 597 209
+4 473 597 802 215
+4 403 478 396 584
+4 563 951 239 199
+4 563 951 608 239
+4 537 208 221 227
+4 225 493 816 283
+4 207 225 994 353
+4 608 276 353 283
+4 207 563 353 608
+4 207 994 189 608
+4 316 375 83 622
+4 910 324 287 281
+4 911 185 202 786
+4 909 788 669 463
+4 793 669 463 909
+4 130 271 179 312
+4 130 271 312 562
+4 349 526 186 435
+4 349 526 435 282
+4 341 441 989 429
+4 114 842 934 178
+4 118 558 663 559
+4 752 959 420 266
+4 702 959 420 752
+4 712 662 516 921
+4 678 458 257 960
+4 155 716 713 753
+4 353 659 225 755
+4 725 777 582 708
+4 725 708 582 784
+4 572 823 394 237
+4 572 823 487 394
+4 667 314 668 588
+4 374 314 610 10
+4 762 843 480 481
+4 762 843 72 480
+4 387 332 482 100
+4 642 218 402 26
+4 496 703 585 586
+4 585 703 940 586
+4 586 940 581 703
+4 734 609 474 784
+4 734 609 784 929
+4 692 744 928 574
+4 615 570 184 617
+4 617 792 184 615
+4 41 746 960 45
+4 730 202 911 205
+4 657 957 206 730
+4 911 730 206 957
+4 104 316 397 198
+4 622 104 316 726
+4 104 726 397 316
+4 850 198 104 99
+4 83 104 316 622
+4 211 236 748 720
+4 478 748 212 339
+4 83 316 850 375
+4 99 851 198 104
+4 851 397 198 104
+4 98 102 453 988
+4 102 453 988 560
+4 102 453 560 576
+4 599 420 710 197
+4 548 393 966 414
+4 548 459 966 393
+4 80 995 607 611
+4 611 607 377 995
+4 612 606 996 94
+4 612 606 342 963
+4 612 606 963 996
+4 80 605 997 611
+4 611 997 376 605
+4 511 1030 566 313
+4 950 320 350 197
+4 950 1005 350 159
+4 274 213 361 689
+4 213 689 176 543
+4 274 213 529 361
+4 213 361 809 529
+4 213 176 809 543
+4 1002 1003 213 375
+4 79 1002 715 213
+4 1016 515 379 869
+4 515 869 1016 319
+4 319 515 869 252
+4 216 1024 226 221
+4 240 537 216 226
+4 537 226 221 216
+4 556 338 785 830
+4 746 291 830 785
+4 746 556 785 830
+4 195 420 562 266
+4 195 562 420 350
+4 707 639 214 667
+4 707 214 188 667
+4 922 103 517 631
+4 89 565 737 193
+4 89 745 565 193
+4 878 193 745 89
+4 451 495 462 750
+4 346 427 423 521
+4 346 323 485 521
+4 381 379 468 486
+4 381 486 468 687
+4 283 816 246 493
+4 677 770 275 761
+4 677 275 571 761
+4 296 96 194 666
+4 557 447 446 1026
+4 28 238 273 34
+4 28 238 371 273
+4 288 1000 217 913
+4 217 1000 268 913
+4 614 217 953 349
+4 774 400 356 614
+4 430 559 596 432
+4 559 578 596 432
+4 776 689 744 810
+4 351 452 456 1031
+4 447 462 673 452
+4 351 753 452 462
+4 673 1031 456 452
+4 713 447 452 462
+4 713 462 452 753
+4 243 289 250 1019
+4 553 239 354 181
+4 553 200 992 181
+4 682 286 388 264
+4 1020 796 256 483
+4 250 1019 289 290
+4 1020 250 289 290
+4 1020 492 262 829
+4 1020 290 297 298
+4 1020 298 297 262
+4 256 257 796 309
+4 796 299 298 309
+4 262 298 308 309
+4 882 444 443 1014
+4 141 591 882 443
+4 591 443 1014 882
+4 485 426 749 439
+4 485 439 522 426
+4 151 444 445 546
+4 445 593 546 384
+4 512 464 465 1021
+4 1021 340 465 512
+4 340 587 1021 465
+4 911 202 185 205
+4 185 205 204 801
+4 911 185 801 205
+4 229 230 803 256
+4 229 250 492 803
+4 229 256 234 230
+4 229 256 492 234
+4 29 917 234 263
+4 69 797 321 326
+4 65 285 603 321
+4 797 321 326 327
+4 287 318 324 687
+4 287 687 324 331
+4 289 781 297 290
+4 289 290 797 326
+4 290 797 326 327
+4 290 297 1022 781
+4 290 327 326 1022
+4 298 336 299 290
+4 290 322 327 688
+4 290 688 327 337
+4 297 985 308 1022
+4 1022 327 968 336
+4 1022 308 805 985
+4 1022 336 335 805
+4 298 805 309 310
+4 298 310 299 336
+4 310 336 337 299
+4 71 805 335 343
+4 67 343 310 805
+4 67 310 343 962
+4 310 337 343 841
+4 310 841 343 962
+4 137 799 941 445
+4 739 450 449 811
+4 424 450 429 1023
+4 1023 446 450 451
+4 789 424 1023 450
+4 789 446 450 1023
+4 340 811 1021 587
+4 924 455 811 428
+4 924 811 455 340
+4 450 800 798 676
+4 800 676 750 798
+4 981 306 260 54
+4 218 255 608 951
+4 608 255 589 240
+4 608 589 246 355
+4 577 247 216 1024
+4 577 437 247 1024
+4 216 247 226 1024
+4 401 220 223 918
+4 712 754 813 230
+4 401 224 223 813
+4 488 253 227 1024
+4 712 813 229 230
+4 712 234 230 229
+4 39 819 254 984
+4 594 1025 818 232
+4 1025 817 594 254
+4 1025 254 249 817
+4 1025 249 254 697
+4 1025 697 254 984
+4 1025 818 819 254
+4 1025 984 254 819
+4 778 490 358 698
+4 698 490 358 820
+4 342 778 358 698
+4 26 241 283 218
+4 26 32 241 218
+4 400 356 614 416
+4 1026 447 451 673
+4 241 475 334 281
+4 394 487 472 824
+4 394 592 824 472
+4 592 472 466 824
+4 675 823 487 676
+4 203 221 208 828
+4 827 203 828 221
+4 827 222 221 828
+4 204 222 209 1027
+4 1027 219 222 476
+4 1027 476 817 372
+4 1027 476 222 817
+4 778 698 833 490
+4 698 331 778 833
+4 372 1027 594 817
+4 372 228 826 597
+4 372 228 594 826
+4 53 822 243 284
+4 249 250 243 289
+4 249 289 395 250
+4 61 779 249 289
+4 779 289 395 249
+4 822 61 249 289
+4 55 308 262 829
+4 785 688 830 329
+4 287 325 331 1028
+4 1028 325 331 698
+4 1028 1034 331 513
+4 287 590 1028 331
+4 1028 698 331 1034
+4 831 556 830 338
+4 830 338 337 831
+4 357 386 379 699
+4 1016 357 386 379
+4 490 357 386 1016
+4 568 490 357 386
+4 568 390 386 357
+4 357 386 699 390
+4 828 208 832 227
+4 828 594 227 832
+4 379 403 699 1029
+4 1029 478 403 554
+4 234 492 261 262
+4 262 261 234 35
+4 835 233 234 261
+4 835 492 261 234
+4 1025 835 834 697
+4 835 697 492 834
+4 834 243 476 249
+4 219 242 243 889
+4 967 228 234 210
+4 228 234 233 937
+4 967 228 937 234
+4 205 206 220 223
+4 205 219 476 220
+4 205 223 476 372
+4 476 220 219 243
+4 228 834 223 372
+4 228 223 834 229
+4 345 837 690 836
+4 836 356 837 690
+4 120 557 140 623
+4 120 836 557 419
+4 270 458 352 525
+4 270 257 352 458
+4 269 351 561 462
+4 252 235 952 532
+4 172 824 601 487
+4 728 172 824 601
+4 1 617 792 184
+4 979 759 184 1
+4 792 1 184 759
+4 759 792 871 184
+4 621 2 667 188
+4 2 188 187 667
+4 494 665 759 185
+4 494 665 185 304
+4 1017 759 185 494
+4 925 180 258 175
+4 925 842 180 175
+4 925 842 175 934
+4 25 233 232 819
+4 25 636 233 819
+4 67 309 965 805
+4 67 310 309 805
+4 799 151 882 444
+4 799 151 444 445
+4 65 797 975 285
+4 65 797 285 321
+4 696 434 743 581
+4 696 434 581 931
+4 165 293 237 173
+4 311 357 699 666
+4 479 311 699 666
+4 769 842 580 607
+4 692 842 580 769
+4 643 484 328 905
+4 665 201 185 704
+4 665 704 637 201
+4 839 627 642 276
+4 1002 176 375 213
+4 944 405 345 110
+4 944 99 345 348
+4 944 405 348 345
+4 239 537 216 240
+4 239 992 216 537
+4 1002 885 726 693
+4 885 726 622 1002
+4 622 83 1003 375
+4 375 732 809 176
+4 272 312 130 179
+4 272 296 130 312
+4 312 272 479 993
+4 312 969 993 699
+4 312 479 699 993
+4 440 180 389 175
+4 180 842 389 175
+4 175 842 389 744
+4 92 342 612 568
+4 793 954 909 838
+4 793 909 954 167
+4 845 101 319 438
+4 845 105 319 101
+4 202 1006 219 12
+4 236 439 383 1004
+4 197 426 522 236
+4 320 236 426 197
+4 320 426 396 197
+4 599 197 292 1005
+4 350 1005 197 599
+4 950 197 350 1005
+4 246 534 238 533
+4 246 493 238 814
+4 246 534 814 238
+4 816 246 493 238
+4 535 533 246 238
+4 535 246 816 238
+4 560 558 988 633
+4 633 118 558 988
+4 414 472 463 393
+4 548 414 463 393
+4 414 788 463 472
+4 414 909 463 788
+4 548 909 463 414
+4 981 541 534 489
+4 933 548 966 414
+4 966 991 459 548
+4 966 593 237 394
+4 433 237 966 593
+4 175 440 810 389
+4 744 175 810 389
+4 787 750 539 758
+4 282 452 456 351
+4 415 726 116 705
+4 415 776 726 705
+4 415 776 705 278
+4 104 316 850 83
+4 498 399 433 163
+4 9 505 200 20
+4 328 313 643 1030
+4 935 162 1007 729
+4 935 729 1007 446
+4 11 951 199 1009
+4 720 236 320 370
+4 33 53 243 975
+4 33 243 1019 975
+4 397 416 886 557
+4 397 557 886 120
+4 894 306 54 34
+4 260 306 34 54
+4 315 438 737 820
+4 437 221 738 815
+4 437 248 815 738
+4 401 918 223 224
+4 450 676 461 675
+4 569 461 675 450
+4 1007 569 450 461
+4 450 798 461 676
+4 1007 450 798 461
+4 592 393 385 384
+4 384 393 463 592
+4 1019 289 797 284
+4 1019 797 289 290
+4 223 250 229 803
+4 813 229 230 803
+4 813 229 803 223
+4 587 676 1021 465
+4 441 465 800 587
+4 587 800 676 465
+4 351 462 1031 561
+4 673 462 451 1031
+4 351 452 1031 462
+4 673 462 1031 452
+4 803 256 483 1020
+4 229 803 492 256
+4 492 803 1020 256
+4 558 596 578 821
+4 822 289 243 284
+4 243 284 289 1019
+4 245 475 281 287
+4 910 245 281 287
+4 245 241 281 475
+4 354 307 313 467
+4 354 307 763 313
+4 992 239 181 537
+4 553 239 992 199
+4 553 992 239 181
+4 69 987 797 326
+4 987 289 797 326
+4 424 419 425 853
+4 201 1027 204 202
+4 185 204 202 201
+4 352 264 388 796
+4 746 299 291 796
+4 746 264 796 291
+4 286 322 290 688
+4 829 262 297 308
+4 1020 829 262 297
+4 45 310 746 602
+4 37 790 1032 248
+4 476 1032 248 249
+4 790 476 1032 248
+4 256 298 262 309
+4 263 256 262 309
+4 256 298 309 796
+4 256 309 263 257
+4 796 290 1020 298
+4 1020 290 796 483
+4 951 255 608 240
+4 393 463 592 472
+4 385 393 394 593
+4 283 816 994 608
+4 283 246 816 608
+4 353 994 608 283
+4 805 310 336 343
+4 298 336 805 310
+4 781 1022 326 335
+4 289 326 781 290
+4 290 781 1022 326
+4 424 1023 429 780
+4 780 429 430 1023
+4 423 427 428 454
+4 1033 208 203 181
+4 187 668 208 1033
+4 759 1033 187 203
+4 871 759 1033 187
+4 437 46 253 947
+4 437 947 253 815
+4 815 947 253 254
+4 71 985 335 805
+4 297 335 985 1022
+4 1022 985 805 335
+4 815 253 1024 227
+4 437 253 1024 815
+4 948 428 429 811
+4 465 825 464 512
+4 372 228 834 817
+4 372 817 594 228
+4 464 824 466 512
+4 464 592 466 824
+4 834 1025 249 817
+4 834 249 1025 697
+4 476 249 817 834
+4 204 1027 209 801
+4 801 209 597 372
+4 801 209 372 1027
+4 473 801 209 597
+4 221 815 227 828
+4 828 227 594 815
+4 835 261 819 233
+4 835 697 1025 261
+4 835 261 1025 819
+4 1029 390 554 969
+4 699 969 390 1029
+4 290 336 1022 298
+4 39 819 984 261
+4 1025 697 984 261
+4 1025 261 984 819
+4 299 337 310 831
+4 831 337 310 841
+4 594 832 232 818
+4 733 818 232 832
+4 449 1021 455 811
+4 340 811 455 1021
+4 917 263 257 256
+4 917 263 256 234
+4 280 952 396 252
+4 252 952 396 532
+4 252 387 319 332
+4 101 235 319 332
+4 1034 778 606 342
+4 513 606 342 1034
+4 698 1034 342 778
+4 1028 342 1034 513
+4 1028 698 1034 342
+4 209 826 594 832
+4 832 826 594 232
+4 393 472 592 394
+4 280 396 1029 252
+4 252 396 1029 478
+4 280 252 1029 869
+4 688 337 329 529
+4 830 329 337 338
+4 688 329 337 830
+4 464 825 487 824
+4 397 557 836 416
+4 416 557 836 356
+4 397 400 416 690
+4 690 400 416 356
+4 397 416 836 690
+4 730 205 206 220
+4 730 205 911 206
+4 205 220 476 223
+4 476 220 243 223
+4 451 750 462 729
+4 539 729 462 750
+4 234 233 835 228
+4 372 817 834 476
+4 1026 673 451 456
+4 405 356 837 596
+4 690 356 837 405
+4 690 356 405 400
+4 52 888 255 747
+4 650 79 274 664
+4 650 274 380 664
+4 623 887 446 557
+4 136 623 446 419
+4 37 889 242 219
+4 975 69 284 797
+4 33 975 1019 285
+4 975 797 284 1019
+4 38 260 643 905
+4 636 903 233 261
+4 636 261 819 39
+4 636 261 233 819
+4 774 821 560 258
+4 258 106 180 925
+4 200 973 505 617
+4 199 812 11 505
+4 92 612 342 963
+4 893 342 963 92
+4 893 626 342 92
+4 100 482 112 332
+4 843 52 480 481
+4 502 236 211 720
+4 237 394 572 966
+4 166 582 259 160
+4 582 514 259 160
+4 723 259 160 166
+4 1031 259 561 495
+4 351 561 1031 259
+4 627 5 199 563
+4 627 951 563 199
+4 682 286 285 388
+4 235 591 952 532
+4 1012 121 235 323
+4 1012 323 235 332
+4 846 1012 121 235
+4 137 445 920 143
+4 881 137 143 445
+4 626 70 342 90
+4 90 342 859 70
+4 568 92 342 90
+4 626 90 342 92
+4 632 923 343 71
+4 632 84 923 71
+4 632 974 343 923
+4 31 39 232 818
+4 31 818 232 733
+4 31 39 818 865
+4 638 39 232 31
+4 69 849 321 83
+4 1018 288 278 132
+4 1018 415 278 288
+4 36 216 344 577
+4 221 577 216 1024
+4 780 124 946 424
+4 780 429 424 948
+4 780 946 948 424
+4 947 49 254 248
+4 73 943 781 326
+4 48 50 945 247
+4 488 50 247 536
+4 253 50 247 488
+4 50 945 247 536
+4 123 125 346 427
+4 954 171 838 998
+4 998 838 731 171
+4 266 959 562 1035
+4 159 752 599 266
+4 661 192 292 731
+4 719 661 292 731
+4 291 264 388 286
+4 976 307 328 13
+4 307 709 313 13
+4 7 307 13 709
+4 7 976 13 307
+4 328 307 313 13
+4 544 57 286 322
+4 683 57 322 373
+4 683 57 544 322
+4 378 726 705 693
+4 215 707 639 214
+4 518 232 25 233
+4 916 95 579 980
+4 916 579 486 585
+4 916 579 585 980
+4 115 212 359 507
+4 212 320 584 528
+4 350 302 562 584
+4 307 620 313 709
+4 511 709 313 620
+4 795 581 131 703
+4 163 433 459 546
+4 399 163 546 433
+4 743 431 479 194
+4 696 431 743 194
+4 696 743 431 575
+4 696 431 874 575
+4 696 874 431 194
+4 583 574 990 744
+4 872 574 990 583
+
+CELL_TYPES 4436
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
+10
diff --git a/examples/pybullet/gym/pybullet_data/torus/torus_textured.mtl b/examples/pybullet/gym/pybullet_data/torus/torus_textured.mtl
new file mode 100644
index 000000000..36228e66e
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/torus/torus_textured.mtl
@@ -0,0 +1,11 @@
+# Blender MTL File: 'None'
+# Material Count: 1
+
+newmtl None
+Ns 0
+Ka 0.000000 0.000000 0.000000
+Kd 0.8 0.8 0.8
+Ks 0.8 0.8 0.8
+d 1
+illum 2
+map_Kd ../cube.png
diff --git a/examples/pybullet/gym/pybullet_data/torus/torus_textured.obj b/examples/pybullet/gym/pybullet_data/torus/torus_textured.obj
new file mode 100644
index 000000000..ede99fd1d
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/torus/torus_textured.obj
@@ -0,0 +1,2270 @@
+# Blender v2.79 (sub 0) OBJ File: ''
+# www.blender.org
+mtllib torus_textured.mtl
+o torus
+v -0.710313 -0.135160 -0.000000
+v -0.750000 -0.000000 -0.000000
+v -0.719154 -0.061973 -0.063589
+v -0.723426 -0.056014 0.050909
+v -0.710313 0.135161 0.000000
+v -0.732316 0.000000 -0.088905
+v -0.732316 -0.000000 0.088905
+v -0.646406 -0.181284 0.053682
+v -0.603854 -0.227408 -0.000000
+v -0.650289 -0.177804 -0.054345
+v -0.698555 -0.102472 0.107364
+v -0.683279 -0.135160 0.135913
+v -0.688957 -0.135160 -0.107364
+v -0.646405 0.181284 -0.053682
+v -0.603853 0.227408 0.000000
+v -0.688957 0.135161 0.107364
+v -0.688957 0.135161 -0.107364
+v -0.688804 -0.070361 -0.203790
+v -0.692910 0.000000 -0.287012
+v -0.656244 -0.135160 -0.271825
+v -0.637874 -0.086416 -0.324584
+v -0.685503 0.067388 -0.224775
+v -0.656244 0.135161 -0.271825
+v -0.632898 0.063386 -0.343969
+v -0.714631 0.000000 -0.177810
+v -0.642280 -0.038943 -0.342598
+v -0.652265 0.000000 -0.347842
+v -0.656244 -0.135160 0.271825
+v -0.692910 -0.000000 0.287013
+v -0.688709 -0.073793 0.199199
+v -0.625484 -0.070863 0.351190
+v -0.643253 -0.000000 0.361328
+v -0.656244 0.135161 0.271825
+v -0.688653 0.068318 0.207562
+v -0.714631 -0.000000 0.177810
+v -0.570953 -0.178898 -0.310363
+v -0.558142 -0.227247 -0.230745
+v -0.615371 -0.181204 -0.210170
+v -0.672601 -0.135160 -0.189595
+v -0.598967 -0.135160 -0.357546
+v -0.568401 -0.181284 0.309321
+v -0.557888 -0.227408 0.231085
+v -0.619371 -0.181284 0.189595
+v -0.578914 -0.135160 0.387558
+v -0.557888 0.227408 -0.231085
+v -0.628566 0.176132 -0.173258
+v -0.559233 0.190252 -0.304771
+v -0.670539 0.135161 -0.199959
+v -0.578914 0.135161 -0.387558
+v -0.557888 0.227408 0.231085
+v -0.623749 0.180717 0.170877
+v -0.562017 0.183687 0.313979
+v -0.607092 0.076946 0.375562
+v -0.578913 0.135161 0.387558
+v -0.672600 0.135161 0.189595
+v -0.464421 -0.247455 -0.000000
+v -0.577088 -0.227408 -0.134561
+v -0.523460 -0.237432 0.053682
+v -0.583552 -0.226830 0.105417
+v -0.464421 0.247455 0.000000
+v -0.530245 0.236597 -0.048768
+v -0.582497 0.227408 0.107364
+v -0.580871 0.227408 -0.115542
+v -0.429069 -0.247455 -0.177726
+v -0.432533 -0.237598 -0.293570
+v -0.485904 -0.227408 -0.338816
+v -0.508160 -0.237821 0.116981
+v -0.429671 -0.247455 0.174700
+v -0.378732 -0.247455 0.253061
+v -0.492439 -0.227408 0.329037
+v -0.492438 0.227408 -0.329037
+v -0.429069 0.247455 -0.177726
+v -0.513179 0.235748 -0.164233
+v -0.429069 0.247455 0.177726
+v -0.434056 0.237817 0.288611
+v -0.510336 0.236467 0.153398
+v -0.484699 0.227582 0.338479
+v -0.530330 0.000000 -0.530330
+v -0.458572 -0.077603 -0.551399
+v -0.502268 -0.135160 -0.502267
+v -0.550617 -0.135160 -0.429907
+v -0.453133 0.076929 -0.555267
+v -0.502267 0.135161 -0.502267
+v -0.556115 0.068101 -0.456439
+v -0.611620 0.000000 -0.408671
+v -0.599787 -0.066530 -0.391893
+v -0.435644 0.000000 -0.593597
+v -0.426989 -0.227408 -0.426989
+v -0.407915 -0.185065 -0.497375
+v -0.505464 -0.183357 -0.399290
+v -0.387558 -0.135160 -0.578914
+v -0.426989 -0.227408 0.426989
+v -0.502267 -0.135160 0.502267
+v -0.500057 -0.179599 0.415039
+v -0.387558 -0.135160 0.578914
+v -0.530330 -0.000000 0.530330
+v -0.562399 -0.057673 0.452441
+v -0.426989 0.227408 -0.426989
+v -0.502951 0.181284 -0.407273
+v -0.371632 0.194497 -0.508779
+v -0.387558 0.135161 -0.578913
+v -0.426989 0.227408 0.426989
+v -0.514900 0.181783 0.388374
+v -0.502267 0.135161 0.502267
+v -0.379298 0.206981 0.486662
+v -0.429906 0.135161 0.550617
+v -0.557897 0.077590 0.448854
+v -0.336285 -0.188937 -0.000000
+v -0.382677 -0.218196 0.088863
+v -0.446745 -0.247455 -0.088863
+v -0.446745 -0.247455 0.088863
+v -0.336285 0.188937 0.000000
+v -0.385778 0.217833 -0.069269
+v -0.382741 0.217882 0.085078
+v -0.446745 0.247455 -0.088863
+v -0.446745 0.247455 0.088863
+v -0.310687 -0.188937 -0.128691
+v -0.378733 -0.247455 -0.253061
+v -0.310687 -0.188937 0.128691
+v -0.310686 0.188937 -0.128690
+v -0.378732 0.247455 -0.253061
+v -0.310686 0.188937 0.128691
+v -0.331737 0.217352 0.207025
+v -0.378732 0.247455 0.253061
+v -0.328396 -0.247455 -0.328395
+v -0.253061 -0.247455 -0.378732
+v -0.346275 -0.227408 -0.480921
+v -0.328395 -0.247455 0.328395
+v -0.331535 -0.227408 0.490770
+v -0.282023 0.237297 -0.442722
+v -0.328395 0.247455 -0.328395
+v -0.329037 0.227408 -0.492438
+v -0.328395 0.247455 0.328395
+v -0.253061 0.247455 0.378732
+v -0.331878 0.227663 0.488446
+v -0.289625 -0.134067 -0.057295
+v -0.287923 -0.130160 0.053228
+v -0.260127 -0.070433 -0.000000
+v -0.285407 0.129685 -0.064345
+v -0.282177 0.122530 0.057464
+v -0.260127 0.070433 0.000000
+v -0.204562 -0.218465 -0.336259
+v -0.237789 -0.188937 -0.237789
+v -0.237789 -0.188937 0.237789
+v -0.217800 -0.218788 0.328247
+v -0.177726 -0.247455 0.429069
+v -0.237789 0.188937 -0.237789
+v -0.253061 0.247455 -0.378732
+v -0.226904 0.218812 0.322227
+v -0.237789 0.188937 0.237789
+v -0.240326 -0.070433 -0.099546
+v -0.247312 -0.129685 0.156314
+v -0.240326 -0.070433 0.099546
+v -0.240326 0.070433 -0.099546
+v -0.247312 0.129685 -0.156314
+v -0.240326 0.070433 0.099546
+v -0.252688 0.136261 0.155728
+v -0.271825 -0.135160 -0.656244
+v -0.287013 0.000000 -0.692910
+v -0.135913 -0.135160 -0.683279
+v -0.341627 -0.063564 -0.634402
+v -0.220805 0.062520 -0.687722
+v -0.271825 0.135161 -0.656244
+v -0.342643 0.065672 -0.632993
+v -0.177810 0.000000 -0.714631
+v -0.370902 -0.001344 -0.636391
+v -0.171633 -0.077297 0.693163
+v -0.287013 -0.000000 0.692910
+v -0.271825 -0.135160 0.656244
+v -0.163774 0.083974 0.692766
+v -0.271825 0.135161 0.656244
+v -0.343309 0.068474 0.631577
+v -0.396167 -0.000062 0.619953
+v -0.177810 -0.000000 0.714631
+v -0.231085 -0.227408 -0.557888
+v -0.170310 -0.182655 -0.621625
+v -0.313555 -0.185113 -0.560359
+v -0.170868 -0.185426 0.618316
+v -0.231085 -0.227408 0.557888
+v -0.304838 -0.174559 0.580551
+v -0.135913 -0.135160 0.683279
+v -0.231085 0.227408 -0.557888
+v -0.357546 0.135161 -0.598967
+v -0.189595 0.181284 -0.619371
+v -0.107364 0.135161 -0.688957
+v -0.231085 0.227408 0.557888
+v -0.189595 0.135161 0.672600
+v -0.357546 0.135161 0.598967
+v -0.212132 0.000000 -0.141742
+v -0.183937 -0.070433 -0.183937
+v -0.183937 -0.070433 0.183937
+v -0.183937 0.070433 -0.183937
+v -0.182603 0.070438 0.184833
+v -0.156314 -0.129685 -0.247312
+v -0.128691 -0.188937 -0.310687
+v -0.156314 -0.129685 0.247312
+v -0.128691 -0.188937 0.310687
+v -0.163025 0.141967 -0.252138
+v -0.128691 0.188937 -0.310686
+v -0.165163 0.141475 0.250336
+v -0.128691 0.188937 0.310686
+v -0.177726 -0.247455 -0.429069
+v -0.107364 -0.227408 -0.582498
+v -0.149665 -0.235647 0.516778
+v -0.107364 -0.227408 0.582498
+v -0.177726 0.247455 -0.429069
+v -0.142545 0.237432 -0.505783
+v -0.107364 0.227408 -0.582497
+v -0.177726 0.247455 0.429069
+v -0.107364 0.227408 0.582497
+v -0.099546 0.070433 -0.240326
+v -0.099546 -0.070433 -0.240326
+v -0.099546 0.070433 0.240326
+v -0.099546 -0.070433 0.240326
+v -0.080033 -0.214471 -0.376277
+v -0.088863 -0.247455 -0.446745
+v -0.088863 -0.247455 0.446745
+v -0.080472 0.217516 -0.382857
+v -0.088863 0.247455 -0.446745
+v -0.071400 0.215432 0.380098
+v -0.088863 0.247455 0.446745
+v -0.055107 -0.128888 -0.286732
+v 0.000000 -0.188937 -0.336285
+v -0.055041 -0.130685 0.287900
+v -0.000000 -0.188937 0.336285
+v -0.062756 0.134013 -0.288504
+v 0.000000 0.188937 -0.336285
+v -0.060039 0.128492 0.285497
+v -0.000000 0.188937 0.336285
+v -0.053324 -0.003221 -0.249520
+v 0.000000 -0.070433 -0.260127
+v -0.045259 -0.000273 0.251124
+v -0.000000 -0.070433 0.260127
+v 0.000000 0.070433 -0.260127
+v -0.000000 0.070433 0.260127
+v -0.000000 -0.135160 0.710313
+v 0.081349 -0.066818 0.714199
+v -0.000000 -0.000000 0.750000
+v -0.083186 -0.069651 0.713002
+v -0.002247 0.135161 0.709866
+v 0.082107 0.059371 0.716235
+v -0.073908 0.058686 0.718067
+v 0.088905 -0.000000 0.732316
+v -0.088905 -0.000000 0.732316
+v -0.000000 -0.227408 0.603854
+v -0.065074 -0.176105 0.650117
+v 0.052670 -0.175140 0.653698
+v 0.135913 -0.135160 0.683279
+v -0.000000 0.227408 0.603853
+v 0.053682 0.181284 0.646405
+v -0.053682 0.181284 0.646405
+v 0.111938 0.135161 0.688047
+v -0.107364 0.135161 0.688957
+v 0.005294 -0.247455 0.463368
+v 0.107364 -0.227408 0.582498
+v -0.053682 -0.237432 0.523460
+v -0.000000 0.247455 0.464421
+v 0.052087 0.236945 0.527161
+v -0.053682 0.237432 0.523459
+v 0.110731 0.226341 0.583059
+v 0.074625 -0.219739 0.388888
+v 0.088863 -0.247455 0.446745
+v 0.053837 0.214530 0.381616
+v 0.088863 0.247455 0.446745
+v 0.056825 -0.133366 0.289268
+v 0.128691 -0.188937 0.310687
+v 0.049773 0.129685 0.288305
+v 0.128691 0.188937 0.310686
+v 0.099546 -0.070433 0.240326
+v 0.099546 0.070433 0.240326
+v 0.099546 -0.070433 -0.240326
+v 0.056738 -0.130548 -0.287475
+v 0.057766 0.137654 -0.291836
+v 0.099546 0.070433 -0.240326
+v 0.000000 -0.247455 -0.464421
+v 0.088863 -0.247455 -0.446745
+v 0.128691 -0.188937 -0.310687
+v 0.000000 0.247455 -0.464421
+v 0.091470 0.217038 -0.379622
+v 0.128691 0.188937 -0.310686
+v 0.000000 -0.227408 -0.603854
+v 0.115542 -0.227408 -0.580871
+v 0.000000 0.227408 -0.603853
+v 0.115542 0.227408 -0.580871
+v -0.050354 0.236542 -0.530311
+v 0.088863 0.247455 -0.446745
+v -0.077172 -0.178565 -0.644871
+v 0.000000 -0.135160 -0.710313
+v 0.053682 -0.181284 -0.646406
+v 0.127426 0.182095 -0.630801
+v 0.000000 0.135161 -0.710313
+v -0.053284 0.178446 -0.649760
+v 0.000000 0.000000 -0.750000
+v 0.083331 -0.065958 -0.714057
+v -0.081654 -0.056098 -0.717286
+v 0.107364 -0.135160 -0.688957
+v -0.088905 0.000000 -0.732316
+v 0.057869 0.049516 -0.723950
+v 0.135913 0.135161 -0.683278
+v 0.124700 0.065746 -0.705891
+v 0.088905 0.000000 -0.732316
+v 0.140691 0.004583 0.212834
+v 0.168668 0.129685 0.239057
+v 0.183937 0.070433 0.183937
+v 0.158128 -0.126755 0.243879
+v 0.183937 -0.070433 0.183937
+v 0.183937 -0.070433 -0.183937
+v 0.183937 0.070433 -0.183937
+v 0.162826 -0.137642 -0.248993
+v 0.177726 0.247455 0.429069
+v 0.221031 0.217158 0.321879
+v 0.237789 0.188937 0.237789
+v 0.229404 0.216858 -0.315510
+v 0.177726 0.247455 -0.429069
+v 0.237789 0.188937 -0.237789
+v 0.177726 -0.247455 0.429069
+v 0.253061 -0.247455 0.378732
+v 0.237789 -0.188937 0.237789
+v 0.177726 -0.247455 -0.429069
+v 0.253061 -0.247455 -0.378732
+v 0.237789 -0.188937 -0.237789
+v 0.231085 0.227408 0.557888
+v 0.279740 0.237432 0.443142
+v 0.142545 0.237432 0.505783
+v 0.253061 0.247455 0.378732
+v 0.287768 0.237840 -0.434425
+v 0.231085 0.227408 -0.557888
+v 0.261020 0.247455 -0.373414
+v 0.286516 -0.237465 0.438340
+v 0.231085 -0.227408 0.557888
+v 0.231085 -0.227408 -0.557888
+v 0.346275 -0.227408 -0.480920
+v 0.240326 0.070433 0.099546
+v 0.245711 0.135139 0.164898
+v 0.242207 -0.130300 0.164652
+v 0.240597 -0.070433 0.098185
+v 0.240326 0.070433 -0.099546
+v 0.247109 -0.135199 -0.162873
+v 0.240326 -0.070433 -0.099546
+v 0.271825 0.135161 0.656244
+v 0.169224 0.181284 0.623422
+v 0.357546 0.135161 0.598967
+v 0.329037 0.227408 0.492438
+v 0.271825 0.135161 -0.656244
+v 0.304869 0.184082 -0.567568
+v 0.329037 0.227408 -0.492438
+v 0.300431 -0.181284 0.574341
+v 0.271825 -0.135160 0.656244
+v 0.190519 -0.174382 0.627152
+v 0.329037 -0.227408 0.492439
+v 0.357546 -0.135160 -0.598967
+v 0.271825 -0.135160 -0.656244
+v 0.181222 -0.173682 -0.629809
+v 0.310686 0.188937 0.128691
+v 0.378732 0.247455 0.253061
+v 0.328395 0.247455 0.328395
+v 0.310687 0.188937 -0.128690
+v 0.326095 0.216854 -0.213543
+v 0.328395 0.247455 -0.328395
+v 0.322623 -0.216228 0.216322
+v 0.310687 -0.188937 0.128691
+v 0.328395 -0.247455 0.328395
+v 0.310687 -0.188937 -0.128691
+v 0.378733 -0.247455 -0.253061
+v 0.328396 -0.247455 -0.328395
+v 0.250226 -0.000000 0.049773
+v 0.260127 0.070433 0.000000
+v 0.285407 0.129685 0.064345
+v 0.289619 0.135713 -0.062645
+v 0.287440 -0.131065 0.058580
+v 0.260127 -0.070433 -0.000000
+v 0.285899 -0.126588 -0.051864
+v 0.336285 0.188937 0.000000
+v 0.336285 -0.188937 -0.000000
+v 0.322279 0.067580 0.645938
+v 0.287013 -0.000000 0.692910
+v 0.214655 0.069255 0.686967
+v 0.189595 0.135161 0.672600
+v 0.224818 -0.067580 0.685438
+v 0.330781 -0.067146 0.640408
+v 0.387046 -0.135160 0.579256
+v 0.287013 0.000000 -0.692910
+v 0.217020 0.070360 -0.686173
+v 0.347842 0.000000 -0.652265
+v 0.387046 0.135161 -0.579255
+v 0.210948 -0.080005 -0.684548
+v 0.343477 -0.066922 -0.632002
+v 0.189595 -0.135160 -0.672601
+v 0.177810 -0.000000 0.714631
+v 0.347842 -0.000000 0.652265
+v 0.177810 0.000000 -0.714631
+v 0.374030 0.211140 0.054655
+v 0.429069 0.247455 0.177726
+v 0.382605 0.220104 0.110230
+v 0.382677 0.218196 -0.088863
+v 0.429069 0.247455 -0.177726
+v 0.381271 -0.216600 0.078355
+v 0.428583 -0.247391 0.178203
+v 0.446745 -0.247455 -0.088863
+v 0.429069 -0.247455 -0.177726
+v 0.426989 0.227408 0.426989
+v 0.469993 0.227408 0.362629
+v 0.426989 0.227408 -0.426989
+v 0.432732 0.238438 -0.282962
+v 0.378732 0.247455 -0.253061
+v 0.426989 -0.227408 0.426989
+v 0.492439 -0.227408 0.329037
+v 0.439562 -0.238499 0.271995
+v 0.378732 -0.247455 0.253061
+v 0.426989 -0.227408 -0.426989
+v 0.443142 -0.237432 -0.279740
+v 0.464421 0.247455 0.000000
+v 0.464421 -0.247455 -0.000000
+v 0.502267 0.135161 0.502267
+v 0.389730 0.189313 0.503743
+v 0.598967 0.135161 0.357546
+v 0.502267 0.135161 -0.502267
+v 0.407018 0.181284 -0.503122
+v 0.502951 0.181284 -0.407273
+v 0.492438 0.227408 -0.329037
+v 0.502267 -0.135160 0.502267
+v 0.538631 -0.173926 0.368866
+v 0.501453 -0.181675 -0.408719
+v 0.502268 -0.135160 -0.502267
+v 0.492439 -0.227408 -0.329037
+v 0.557888 0.227408 0.231085
+v 0.582497 0.227408 0.107364
+v 0.446745 0.247455 0.088863
+v 0.557888 0.227408 -0.231084
+v 0.580871 0.227408 -0.115542
+v 0.446745 0.247455 -0.088863
+v 0.557888 -0.227408 0.231085
+v 0.582498 -0.227408 0.107364
+v 0.446745 -0.247455 0.088863
+v 0.557888 -0.227408 -0.231085
+v 0.515728 -0.237427 -0.092711
+v 0.603853 0.227408 0.000000
+v 0.603854 -0.227408 -0.000000
+v 0.530330 -0.000000 0.530330
+v 0.556944 0.067580 0.455469
+v 0.443938 0.067580 0.564649
+v 0.431322 0.135161 0.549672
+v 0.455469 -0.067580 0.556944
+v 0.563256 -0.063878 0.447942
+v 0.578914 -0.135160 0.387558
+v 0.530330 0.000000 -0.530330
+v 0.414705 0.058506 -0.587324
+v 0.566090 0.060108 -0.445653
+v 0.578913 0.135161 -0.387558
+v 0.564464 -0.064950 -0.445577
+v 0.435586 -0.065536 -0.570937
+v 0.578914 -0.135160 -0.387558
+v 0.570975 -0.000000 0.469501
+v 0.408671 -0.000000 0.611620
+v 0.408671 0.000000 -0.611620
+v 0.611620 0.000000 -0.408671
+v 0.619371 0.181284 0.189595
+v 0.656244 0.135161 0.271825
+v 0.615971 0.183777 -0.192222
+v 0.656244 0.135161 -0.271825
+v 0.615859 -0.187440 0.171532
+v 0.656244 -0.135160 0.271825
+v 0.571923 -0.184840 0.296806
+v 0.656244 -0.135160 -0.271825
+v 0.618773 -0.182243 -0.187041
+v 0.580570 -0.227408 -0.117056
+v 0.710313 0.135161 0.000000
+v 0.688957 0.135161 -0.107364
+v 0.646723 0.180940 0.054080
+v 0.710313 -0.135160 -0.000000
+v 0.683279 -0.135160 -0.135912
+v 0.645372 -0.179169 0.071153
+v 0.692910 -0.000000 0.287013
+v 0.645938 0.067580 0.322279
+v 0.685667 0.071751 0.217507
+v 0.672600 0.135161 0.189595
+v 0.692910 0.000000 -0.287012
+v 0.685562 0.072184 -0.217397
+v 0.643668 0.052770 -0.333355
+v 0.672600 0.135161 -0.189595
+v 0.697660 -0.068270 0.162354
+v 0.639387 -0.067839 0.331951
+v 0.683279 -0.135160 0.135913
+v 0.619002 -0.072742 -0.359917
+v 0.685438 -0.067580 -0.224817
+v 0.611620 -0.000000 0.408671
+v 0.714631 -0.000000 0.177810
+v 0.652265 0.000000 -0.347842
+v 0.714373 -0.001402 -0.177040
+v 0.750000 0.000000 0.000000
+v 0.717360 0.075612 -0.052474
+v 0.710134 0.069315 0.098099
+v 0.688957 0.135161 0.107364
+v 0.714216 -0.065097 -0.083803
+v 0.712362 -0.080492 0.070402
+v 0.732316 0.000000 -0.088905
+v 0.732316 -0.000000 0.088905
+v -0.401630 0.076652 -0.589776
+v -0.407579 -0.070112 -0.588066
+v -0.593597 -0.000000 0.435644
+v -0.704476 0.062482 0.136628
+vt 0.732166 0.155461
+vt 0.714953 0.202717
+vt 0.708046 0.168831
+vt 0.732209 0.193512
+vt 0.701021 0.251467
+vt 0.695974 0.187245
+vt 0.732616 0.220277
+vt 0.754661 0.148648
+vt 0.752805 0.114521
+vt 0.726540 0.123737
+vt 0.750726 0.190465
+vt 0.761929 0.186795
+vt 0.705871 0.132777
+vt 0.684170 0.264421
+vt 0.689922 0.296828
+vt 0.721587 0.273032
+vt 0.678525 0.235129
+vt 0.673377 0.143118
+vt 0.648399 0.162273
+vt 0.657651 0.109185
+vt 0.637606 0.123659
+vt 0.657819 0.193767
+vt 0.641663 0.215784
+vt 0.626501 0.182337
+vt 0.675241 0.174073
+vt 0.631458 0.141944
+vt 0.630026 0.157329
+vt 0.787143 0.223027
+vt 0.768265 0.265466
+vt 0.764144 0.222215
+vt 0.792111 0.265020
+vt 0.782493 0.287007
+vt 0.751329 0.309317
+vt 0.747242 0.270599
+vt 0.749382 0.239903
+vt 0.637385 0.079834
+vt 0.670821 0.061503
+vt 0.677846 0.091493
+vt 0.682660 0.118885
+vt 0.624903 0.100431
+vt 0.809123 0.224691
+vt 0.807142 0.186241
+vt 0.784644 0.186679
+vt 0.810694 0.259651
+vt 0.636653 0.264964
+vt 0.657977 0.245210
+vt 0.622512 0.240774
+vt 0.657917 0.223189
+vt 0.609208 0.210333
+vt 0.736728 0.350100
+vt 0.729357 0.309709
+vt 0.756807 0.349975
+vt 0.777270 0.318929
+vt 0.773538 0.344441
+vt 0.736840 0.290959
+vt 0.773212 0.086238
+vt 0.710285 0.078135
+vt 0.778034 0.118471
+vt 0.780520 0.144897
+vt 0.682155 0.331179
+vt 0.674913 0.302893
+vt 0.711978 0.320993
+vt 0.663737 0.279519
+vt 0.686618 0.026488
+vt 0.615441 0.030872
+vt 0.610289 0.048084
+vt 0.795710 0.140692
+vt 0.823961 0.159192
+vt 0.846338 0.197017
+vt 0.830037 0.222870
+vt 0.608258 0.261143
+vt 0.633294 0.302000
+vt 0.646561 0.284317
+vt 0.725937 0.380887
+vt 0.751537 0.397237
+vt 0.720810 0.348908
+vt 0.759506 0.387496
+vt 0.572717 0.157060
+vt 0.550689 0.130920
+vt 0.567080 0.102975
+vt 0.595570 0.098918
+vt 0.557603 0.192476
+vt 0.576749 0.211301
+vt 0.592411 0.182042
+vt 0.610858 0.154722
+vt 0.613963 0.128021
+vt 0.544142 0.165540
+vt 0.558895 0.053757
+vt 0.538934 0.082993
+vt 0.593524 0.073932
+vt 0.524143 0.116033
+vt 0.849912 0.262118
+vt 0.831806 0.298238
+vt 0.830327 0.263014
+vt 0.853031 0.335173
+vt 0.813224 0.338775
+vt 0.809127 0.300664
+vt 0.579717 0.261025
+vt 0.594540 0.233878
+vt 0.555612 0.250329
+vt 0.545339 0.222559
+vt 0.778401 0.417103
+vt 0.772003 0.373190
+vt 0.795140 0.379753
+vt 0.794726 0.432209
+vt 0.807905 0.403747
+vt 0.791039 0.341872
+vt 0.777303 0.081668
+vt 0.808131 0.125476
+vt 0.738283 0.051098
+vt 0.802615 0.121174
+vt 0.680844 0.337119
+vt 0.659824 0.319965
+vt 0.705886 0.361169
+vt 0.657941 0.315887
+vt 0.704116 0.355911
+vt 0.690307 0.020639
+vt 0.620443 0.010475
+vt 0.827880 0.156348
+vt 0.632737 0.308394
+vt 0.607532 0.299189
+vt 0.724071 0.386196
+vt 0.741288 0.410397
+vt 0.745291 0.413243
+vt 0.548336 0.017192
+vt 0.487740 0.035724
+vt 0.515689 0.065301
+vt 0.864626 0.235997
+vt 0.870093 0.296989
+vt 0.551111 0.289679
+vt 0.581695 0.298427
+vt 0.551969 0.271928
+vt 0.765346 0.444757
+vt 0.783325 0.481829
+vt 0.796791 0.453835
+vt 0.726111 0.068061
+vt 0.788546 0.134500
+vt 0.740594 0.136936
+vt 0.658119 0.291607
+vt 0.711562 0.332488
+vt 0.695953 0.271534
+vt 0.475413 0.039567
+vt 0.545788 0.010956
+vt 0.867288 0.231608
+vt 0.885709 0.269207
+vt 0.902567 0.306617
+vt 0.582023 0.304881
+vt 0.556526 0.308969
+vt 0.779949 0.479179
+vt 0.763030 0.449472
+vt 0.662670 0.087831
+vt 0.833443 0.207774
+vt 0.795630 0.206399
+vt 0.639341 0.237593
+vt 0.611510 0.276146
+vt 0.744783 0.327369
+vt 0.747761 0.392049
+vt 0.486801 0.140337
+vt 0.502736 0.189221
+vt 0.454509 0.169171
+vt 0.513471 0.154072
+vt 0.496408 0.222087
+vt 0.515249 0.239396
+vt 0.525421 0.202483
+vt 0.479772 0.208969
+vt 0.525237 0.174009
+vt 0.883124 0.418480
+vt 0.855998 0.414593
+vt 0.873856 0.373097
+vt 0.862673 0.470842
+vt 0.837274 0.455346
+vt 0.835143 0.414988
+vt 0.836735 0.380732
+vt 0.872868 0.442582
+vt 0.466878 0.097675
+vt 0.455985 0.137528
+vt 0.499622 0.101112
+vt 0.895900 0.374250
+vt 0.890087 0.335009
+vt 0.870984 0.340420
+vt 0.896040 0.407532
+vt 0.525010 0.285953
+vt 0.537275 0.226482
+vt 0.507137 0.272752
+vt 0.482511 0.270077
+vt 0.818664 0.491352
+vt 0.850143 0.478738
+vt 0.821176 0.427867
+vt 0.610858 0.154722
+vt 0.563964 0.081082
+vt 0.839672 0.282054
+vt 0.578128 0.233361
+vt 0.788378 0.397577
+vt 0.494584 0.059202
+vt 0.441925 0.063170
+vt 0.878372 0.291927
+vt 0.904765 0.301757
+vt 0.552629 0.294068
+vt 0.532881 0.327257
+vt 0.790271 0.471398
+vt 0.800484 0.521446
+vt 0.446194 0.067982
+vt 0.431086 0.131496
+vt 0.906513 0.342316
+vt 0.912981 0.369336
+vt 0.531749 0.321170
+vt 0.513678 0.315340
+vt 0.498610 0.311098
+vt 0.803287 0.517141
+vt 0.840770 0.533423
+vt 0.519727 0.260004
+vt 0.478779 0.121076
+vt 0.829233 0.471520
+vt 0.881037 0.356176
+vt 0.412798 0.096853
+vt 0.412829 0.101481
+vt 0.922967 0.336681
+vt 0.510893 0.344643
+vt 0.509021 0.343328
+vt 0.821724 0.560558
+vt 0.822089 0.556499
+vt 0.425124 0.119531
+vt 0.385244 0.136150
+vt 0.916869 0.355988
+vt 0.944665 0.361840
+vt 0.506450 0.322646
+vt 0.488308 0.371526
+vt 0.831928 0.538303
+vt 0.841600 0.597216
+vt 0.473819 0.210666
+vt 0.418854 0.186932
+vt 0.880657 0.454366
+vt 0.925216 0.422444
+vt 0.468860 0.310922
+vt 0.875211 0.546434
+vt 0.918970 0.440998
+vt 0.925575 0.481920
+vt 0.902561 0.486000
+vt 0.896941 0.443427
+vt 0.883383 0.528908
+vt 0.910084 0.524586
+vt 0.881846 0.486746
+vt 0.918615 0.505815
+vt 0.887461 0.464965
+vt 0.932833 0.399055
+vt 0.913384 0.407004
+vt 0.934728 0.435260
+vt 0.944408 0.469025
+vt 0.863381 0.566968
+vt 0.885323 0.563297
+vt 0.863798 0.532753
+vt 0.905684 0.558359
+vt 0.864124 0.501681
+vt 0.944240 0.368654
+vt 0.954966 0.422268
+vt 0.926555 0.368431
+vt 0.845075 0.592979
+vt 0.867898 0.598015
+vt 0.842547 0.560430
+vt 0.888553 0.602345
+vt 0.965193 0.386940
+vt 0.965377 0.390038
+vt 0.859677 0.623326
+vt 0.869185 0.631885
+vt 0.960307 0.408648
+vt 0.989236 0.406110
+vt 0.878491 0.610715
+vt 0.898281 0.670197
+vt 0.976083 0.473363
+vt 0.935136 0.612290
+vt 0.372877 0.261846
+vt 0.376389 0.191811
+vt 0.461048 0.381657
+vt 0.424681 0.377190
+vt 0.388719 0.140387
+vt 0.365733 0.176867
+vt 0.344128 0.211920
+vt 0.486574 0.366047
+vt 0.463411 0.405150
+vt 0.448408 0.431610
+vt 0.407025 0.166398
+vt 0.382820 0.202659
+vt 0.476477 0.334311
+vt 0.455101 0.366330
+vt 0.491794 0.335460
+vt 0.466611 0.396685
+vt 0.434533 0.164564
+vt 0.427452 0.203914
+vt 0.407442 0.200613
+vt 0.446802 0.345775
+vt 0.462614 0.292369
+vt 0.479025 0.299446
+vt 0.446205 0.247366
+vt 0.422783 0.246750
+vt 0.454062 0.210015
+vt 0.407768 0.231685
+vt 0.462258 0.227551
+vt 0.442411 0.276979
+vt 0.439684 0.325834
+vt 0.433137 0.299007
+vt 0.431105 0.268402
+vt 0.990655 0.568941
+vt 0.963373 0.680418
+vt 1.020321 0.652284
+vt 1.008054 0.447965
+vt 1.034484 0.500005
+vt 0.331412 0.336988
+vt 0.383315 0.451313
+vt 0.335139 0.262725
+vt 0.902550 0.665384
+vt 0.945561 0.701796
+vt 1.002144 0.722411
+vt 0.425317 0.471964
+vt 0.446211 0.426749
+vt 0.410931 0.501758
+vt 0.988105 0.412197
+vt 1.012882 0.424397
+vt 1.038379 0.428486
+vt 0.346931 0.216225
+vt 0.326969 0.251537
+vt 0.306674 0.283894
+vt 0.923234 0.635692
+vt 0.949612 0.676160
+vt 0.893228 0.630835
+vt 0.944096 0.697642
+vt 0.421030 0.447656
+vt 0.433731 0.398357
+vt 0.426013 0.466591
+vt 1.009071 0.444110
+vt 0.981366 0.447413
+vt 0.362307 0.242015
+vt 0.337833 0.284999
+vt 1.119026 0.645535
+vt 1.073562 0.702905
+vt 1.063205 0.457200
+vt 1.096489 0.495430
+vt 0.339273 0.526967
+vt 0.294721 0.336695
+vt 0.288427 0.405997
+vt 0.943157 0.593030
+vt 0.912268 0.594851
+vt 0.970053 0.611910
+vt 0.963612 0.664209
+vt 0.417500 0.360270
+vt 0.415391 0.398637
+vt 0.414252 0.435434
+vt 0.989174 0.479580
+vt 0.971605 0.493970
+vt 0.962383 0.463761
+vt 1.008325 0.461439
+vt 0.364820 0.305499
+vt 0.380918 0.278021
+vt 0.385853 0.241185
+vt 1.146663 0.712727
+vt 1.076798 0.722892
+vt 1.004691 0.716174
+vt 0.371523 0.577019
+vt 0.390379 0.540042
+vt 0.408270 0.497369
+vt 1.063624 0.431204
+vt 1.089093 0.424973
+vt 1.038052 0.434940
+vt 0.267715 0.347171
+vt 0.288935 0.320124
+vt 0.308989 0.288609
+vt 0.183777 0.495430
+vt 0.206314 0.645535
+vt 0.227056 0.554546
+vt 0.284238 0.596430
+vt 0.262651 0.666920
+vt 0.233951 0.712727
+vt 0.160849 0.702905
+vt 0.337858 0.595497
+vt 0.204354 0.439361
+vt 0.239596 0.461832
+vt 0.150493 0.457200
+vt 0.176381 0.424973
+vt 0.252082 0.401558
+vt 0.320946 0.651698
+vt 0.224488 0.396248
+vt 0.963855 0.577209
+vt 0.959093 0.544145
+vt 0.936817 0.557082
+vt 0.923067 0.576550
+vt 0.954202 0.510228
+vt 0.978773 0.528469
+vt 1.001556 0.510744
+vt 0.399642 0.318773
+vt 0.418583 0.324153
+vt 0.388934 0.337410
+vt 0.396769 0.398025
+vt 0.399253 0.276549
+vt 0.379012 0.318837
+vt 0.393787 0.254629
+vt 0.936128 0.524397
+vt 0.975207 0.555084
+vt 0.416511 0.290785
+vt 0.294810 0.676578
+vt 0.230262 0.706879
+vt 0.263792 0.696624
+vt 0.164086 0.722892
+vt 0.351775 0.607890
+vt 0.368251 0.572809
+vt 0.200368 0.415126
+vt 0.150912 0.431204
+vt 0.176759 0.431387
+vt 0.247759 0.377455
+vt 0.269581 0.352479
+vt 1.015251 0.679609
+vt 1.052308 0.685232
+vt 0.393555 0.471248
+vt 0.380569 0.525541
+vt 0.389982 0.536349
+vt 1.036073 0.472342
+vt 1.064614 0.472224
+vt 1.069055 0.452709
+vt 1.063888 0.434178
+vt 0.322045 0.316264
+vt 0.292974 0.340665
+vt 0.316856 0.647128
+vt 0.225799 0.402187
+vt 1.023436 0.630392
+vt 0.987854 0.650405
+vt 1.081260 0.632936
+vt 0.375450 0.435129
+vt 0.395287 0.432891
+vt 0.373041 0.473426
+vt 0.373681 0.510497
+vt 1.033105 0.522065
+vt 1.063461 0.503179
+vt 0.319761 0.353604
+vt 0.338784 0.353613
+vt 0.301066 0.349590
+vt 1.142974 0.706879
+vt 1.127029 0.672004
+vt 0.263805 0.648829
+vt 0.214317 0.672004
+vt 0.281927 0.682268
+vt 0.350785 0.547125
+vt 0.326759 0.585500
+vt 0.346259 0.612192
+vt 0.180297 0.468403
+vt 0.156343 0.452709
+vt 0.209285 0.452692
+vt 0.151176 0.434178
+vt 0.201585 0.417479
+vt 0.280371 0.383266
+vt 0.250851 0.398078
+vt 0.296449 0.618845
+vt 0.233566 0.436538
+vt 1.029073 0.576306
+vt 1.047618 0.605928
+vt 1.002193 0.596620
+vt 0.996477 0.623833
+vt 1.013677 0.544731
+vt 1.051511 0.553164
+vt 1.065564 0.523033
+vt 0.356868 0.394591
+vt 0.383243 0.379132
+vt 0.351943 0.435491
+vt 0.354338 0.473716
+vt 0.335349 0.397184
+vt 0.362542 0.349044
+vt 0.317182 0.388926
+vt 1.047889 0.578710
+vt 0.992528 0.564366
+vt 0.378158 0.356573
+vt 0.335038 0.432001
+vt 0.228691 0.638896
+vt 0.201295 0.624182
+vt 0.168547 0.632936
+vt 0.329528 0.546675
+vt 0.330787 0.510340
+vt 0.200336 0.482368
+vt 0.185307 0.517582
+vt 0.169454 0.495114
+vt 0.151902 0.472224
+vt 0.294973 0.424049
+vt 0.275947 0.419133
+vt 0.257603 0.410106
+vt 0.275810 0.577905
+vt 0.299668 0.553712
+vt 0.270851 0.611088
+vt 0.244665 0.481900
+vt 0.270596 0.454206
+vt 0.224259 0.472619
+vt 0.192043 0.571094
+vt 0.181929 0.601642
+vt 0.213389 0.592512
+vt 0.226304 0.614482
+vt 0.311909 0.467900
+vt 0.310849 0.506078
+vt 0.329207 0.468913
+vt 0.316114 0.532588
+vt 0.215993 0.531738
+vt 0.173374 0.548576
+vt 0.215898 0.502176
+vt 0.152852 0.523033
+vt 0.318356 0.420778
+vt 0.294029 0.459078
+vt 0.154502 0.578644
+vt 0.218885 0.559294
+vt 0.323573 0.450375
+vt 0.292730 0.493163
+vt 0.258597 0.530649
+vt 0.278864 0.546234
+vt 0.244265 0.573574
+vt 0.249515 0.600590
+vt 0.268136 0.491207
+vt 0.235424 0.513720
+vt 0.276260 0.513089
+vt 0.239618 0.546121
+vt 1.067214 0.578644
+vt 0.542951 0.198214
+vt 0.533776 0.140417
+vt 0.150749 0.503179
+vt 0.796380 0.309665
+vt 0.734856 0.252555
+vn -0.9425 -0.2767 -0.1875
+vn -0.9425 -0.2767 0.1875
+vn -0.9425 0.2767 -0.1875
+vn -0.9425 0.2767 0.1875
+vn -0.6494 -0.7494 0.1292
+vn -0.6494 -0.7494 -0.1292
+vn -0.6494 0.7494 -0.1292
+vn -0.6494 0.7494 0.1292
+vn -0.7990 -0.2767 -0.5339
+vn -0.7990 0.2767 -0.5339
+vn -0.7990 -0.2768 -0.5339
+vn -0.7990 -0.2767 0.5339
+vn -0.7990 0.2767 0.5339
+vn -0.5504 -0.7500 -0.3669
+vn -0.5505 -0.7494 -0.3678
+vn -0.5505 -0.7494 0.3678
+vn -0.5505 0.7494 -0.3678
+vn -0.5505 0.7494 0.3678
+vn -0.1423 -0.9894 -0.0283
+vn -0.1423 -0.9894 0.0283
+vn -0.1456 -0.9888 0.0335
+vn -0.1423 0.9894 -0.0283
+vn -0.1423 0.9894 0.0283
+vn -0.1218 -0.9893 -0.0805
+vn -0.1427 -0.9893 -0.0298
+vn -0.1214 -0.9894 -0.0797
+vn -0.5505 -0.7499 -0.3668
+vn -0.1203 -0.9896 0.0782
+vn -0.1481 -0.9886 0.0257
+vn -0.1206 -0.9894 0.0806
+vn -0.1206 0.9894 -0.0806
+vn -0.1206 0.9894 0.0806
+vn -0.5446 0.7527 0.3699
+vn -0.5339 -0.2767 -0.7990
+vn -0.5339 0.2767 -0.7990
+vn -0.3678 -0.7494 -0.5505
+vn -0.3678 -0.7494 0.5505
+vn -0.5339 -0.2767 0.7990
+vn -0.3678 0.7494 -0.5505
+vn -0.3678 0.7494 0.5505
+vn -0.5339 0.2767 0.7990
+vn 0.4140 -0.9065 -0.0824
+vn 0.4140 -0.9065 0.0824
+vn -0.1422 -0.9894 0.0283
+vn 0.4140 0.9065 0.0824
+vn 0.4140 0.9065 -0.0824
+vn -0.1422 0.9894 -0.0283
+vn 0.4140 0.9065 0.0823
+vn 0.3510 -0.9065 0.2345
+vn -0.1206 -0.9894 -0.0806
+vn 0.4140 -0.9065 -0.0823
+vn 0.3559 -0.9055 -0.2313
+vn 0.3510 0.9065 0.2345
+vn 0.3510 0.9065 -0.2345
+vn 0.4140 0.9065 -0.0823
+vn -0.1422 0.9894 0.0283
+vn -0.0806 -0.9894 -0.1206
+vn -0.0806 -0.9894 0.1206
+vn -0.0806 0.9894 -0.1206
+vn -0.0806 0.9894 0.1206
+vn -0.5494 0.7542 0.3597
+vn -0.3528 0.7621 0.5429
+vn 0.8297 -0.5332 0.1650
+vn 0.4140 -0.9065 0.0823
+vn 0.8297 -0.5332 -0.1650
+vn 0.8297 0.5332 0.1650
+vn 0.8297 0.5332 -0.1650
+vn 0.2345 -0.9065 0.3510
+vn 0.3510 -0.9065 -0.2345
+vn 0.2345 -0.9065 -0.3510
+vn 0.2345 0.9065 0.3510
+vn 0.2345 0.9065 -0.3510
+vn 0.7034 -0.5332 0.4700
+vn 0.7034 -0.5332 -0.4700
+vn 0.7034 0.5332 0.4700
+vn 0.7034 0.5332 -0.4700
+vn -0.1875 -0.2767 -0.9425
+vn -0.1875 0.2767 -0.9425
+vn -0.5412 0.2686 -0.7969
+vn -0.1875 -0.2767 0.9425
+vn -0.1875 0.2767 0.9425
+vn -0.5341 0.2765 0.7989
+vn -0.1292 -0.7494 -0.6494
+vn -0.1292 -0.7494 0.6494
+vn -0.1292 0.7494 -0.6494
+vn -0.1292 0.7494 0.6494
+vn 0.9808 0.0000 -0.1951
+vn 0.9808 -0.0000 0.1951
+vn 0.8315 -0.0000 0.5556
+vn 0.8315 0.0000 -0.5556
+vn 0.8281 -0.0043 -0.5605
+vn 0.6994 0.5354 -0.4734
+vn 0.4700 -0.5332 0.7034
+vn 0.4700 -0.5332 -0.7034
+vn 0.4700 0.5332 0.7034
+vn 0.6998 0.5366 -0.4715
+vn 0.4700 0.5332 -0.7034
+vn -0.0283 -0.9894 -0.1423
+vn -0.0283 -0.9894 0.1423
+vn -0.0283 0.9894 -0.1423
+vn -0.0283 0.9894 0.1423
+vn -0.3575 0.7746 0.5217
+vn 0.5556 -0.0000 0.8315
+vn 0.5555 0.0000 -0.8315
+vn 0.5556 0.0000 -0.8315
+vn 0.0824 -0.9065 0.4140
+vn 0.0824 -0.9065 -0.4140
+vn 0.0824 0.9065 0.4140
+vn -0.0283 0.9894 -0.1422
+vn 0.0824 0.9065 -0.4140
+vn 0.1650 -0.5332 0.8297
+vn 0.0823 -0.9065 0.4140
+vn 0.1650 -0.5332 -0.8297
+vn 0.1650 0.5332 0.8297
+vn 0.1651 0.5332 -0.8297
+vn 0.1650 0.5332 -0.8297
+vn 0.1951 -0.0000 0.9808
+vn 0.1951 0.0000 -0.9808
+vn 0.1875 -0.2767 0.9425
+vn 0.1829 0.2826 0.9416
+vn 0.1875 0.2767 0.9425
+vn 0.1292 -0.7494 0.6494
+vn 0.1245 0.7472 0.6528
+vn 0.1802 0.2798 0.9430
+vn 0.1248 0.7471 0.6529
+vn 0.0283 -0.9894 0.1423
+vn -0.0253 -0.9898 0.1403
+vn 0.0283 0.9894 0.1423
+vn 0.1292 0.7494 0.6494
+vn 0.0374 0.9882 0.1483
+vn -0.0824 -0.9065 -0.4140
+vn 0.0741 -0.9047 -0.4197
+vn -0.0248 -0.9897 0.1407
+vn -0.0824 0.9065 -0.4140
+vn -0.0283 0.9894 0.1422
+vn -0.1650 -0.5332 -0.8297
+vn -0.1650 0.5332 -0.8297
+vn -0.1951 0.0000 -0.9808
+vn -0.1951 -0.0000 0.9808
+vn -0.1650 -0.5332 0.8297
+vn -0.1651 0.5332 0.8297
+vn -0.1650 0.5332 0.8297
+vn -0.0824 -0.9065 0.4140
+vn -0.0824 0.9065 0.4140
+vn 0.0283 -0.9894 -0.1423
+vn 0.0283 0.9894 -0.1423
+vn 0.1292 -0.7494 -0.6494
+vn 0.1292 0.7494 -0.6494
+vn 0.1875 -0.2767 -0.9425
+vn 0.1875 0.2767 -0.9425
+vn -0.5556 0.0000 -0.8315
+vn -0.4700 0.5332 -0.7034
+vn -0.4700 -0.5332 -0.7034
+vn -0.5556 -0.0000 0.8315
+vn -0.4700 0.5332 0.7034
+vn -0.4700 -0.5332 0.7034
+vn -0.2345 0.9065 -0.3510
+vn -0.0823 0.9065 -0.4140
+vn -0.2345 0.9065 0.3510
+vn -0.0823 0.9065 0.4140
+vn -0.2345 -0.9065 -0.3510
+vn -0.2345 -0.9065 0.3510
+vn 0.0806 0.9894 0.1206
+vn 0.0283 0.9894 0.1422
+vn 0.0806 0.9894 -0.1206
+vn 0.0806 -0.9894 0.1206
+vn 0.0806 -0.9894 -0.1206
+vn -0.8315 0.0000 -0.5556
+vn -0.7034 0.5332 -0.4700
+vn -0.7034 -0.5332 -0.4700
+vn -0.8343 0.0037 -0.5513
+vn -0.7045 -0.5357 -0.4655
+vn -0.8315 -0.0000 0.5556
+vn -0.7034 0.5332 0.4700
+vn -0.7034 -0.5332 0.4700
+vn 0.3678 0.7494 0.5505
+vn 0.0229 0.9882 0.1513
+vn 0.3678 0.7494 -0.5505
+vn 0.3678 -0.7494 0.5505
+vn 0.3678 -0.7494 -0.5505
+vn -0.3510 0.9065 -0.2345
+vn -0.3510 0.9065 0.2345
+vn -0.3510 -0.9065 -0.2345
+vn -0.3510 -0.9065 0.2345
+vn -0.9808 -0.0000 -0.1951
+vn -0.8297 0.5332 -0.1650
+vn -0.9808 -0.0000 0.1951
+vn -0.8297 0.5332 0.1650
+vn -0.8297 -0.5332 -0.1650
+vn -0.7038 -0.5362 -0.4659
+vn -0.8297 -0.5332 0.1650
+vn 0.5339 0.2767 0.7990
+vn 0.5339 -0.2767 0.7990
+vn 0.5339 0.2767 -0.7990
+vn 0.5339 -0.2767 -0.7990
+vn -0.4140 0.9065 -0.0824
+vn -0.4140 0.9065 -0.0823
+vn -0.4140 0.9065 0.0824
+vn -0.4140 -0.9065 -0.0824
+vn -0.4144 -0.9063 -0.0831
+vn -0.4140 -0.9065 0.0823
+vn -0.4140 -0.9065 0.0824
+vn 0.1206 0.9894 0.0806
+vn 0.1206 0.9894 -0.0806
+vn 0.1206 -0.9894 0.0806
+vn 0.1206 -0.9894 -0.0806
+vn 0.5505 0.7494 0.3678
+vn 0.5505 0.7494 -0.3678
+vn 0.5505 -0.7494 0.3678
+vn 0.5505 -0.7494 -0.3678
+vn 0.1423 0.9894 0.0283
+vn 0.1423 0.9894 -0.0283
+vn 0.1203 -0.9895 0.0797
+vn 0.1414 -0.9895 0.0281
+vn 0.1209 -0.9895 0.0797
+vn -0.4138 -0.9065 -0.0835
+vn 0.1421 -0.9894 0.0296
+vn 0.1423 -0.9894 -0.0283
+vn 0.1423 -0.9894 0.0283
+vn 0.7990 0.2767 0.5339
+vn 0.7990 -0.2767 0.5339
+vn 0.7990 0.2767 -0.5339
+vn 0.7990 -0.2767 -0.5339
+vn 0.6494 0.7494 0.1292
+vn 0.6494 0.7494 -0.1292
+vn 0.6494 -0.7494 0.1292
+vn 0.6494 -0.7494 -0.1292
+vn 0.9425 0.2767 0.1875
+vn 0.9425 0.2767 -0.1875
+vn 0.9425 -0.2767 0.1875
+vn 0.9425 -0.2767 -0.1875
+vn 0.9454 0.2709 -0.1811
+vn -0.5337 0.2766 0.7991
+vn -0.5341 0.2764 0.7990
+vn -0.1487 -0.9884 0.0299
+vn -0.5444 0.7557 0.3641
+vn 0.0196 0.9885 0.1500
+vn -0.5281 0.2636 -0.8072
+vn -0.9425 -0.2768 -0.1875
+vn -0.3679 0.7494 -0.5505
+vn 0.0387 0.9884 0.1469
+vn 0.9412 0.2752 -0.1960
+vn 0.9454 0.2695 -0.1834
+usemtl None
+s off
+f 1/1/1 2/2/1 3/3/1
+f 4/4/2 2/2/2 1/1/2
+f 5/5/3 6/6/3 2/2/3
+f 5/5/4 2/2/4 7/7/4
+f 6/6/1 3/3/1 2/2/1
+f 7/7/2 2/2/2 4/4/2
+f 8/8/5 1/1/5 9/9/5
+f 1/1/6 10/10/6 9/9/6
+f 11/11/2 1/1/2 12/12/2
+f 12/12/5 1/1/5 8/8/5
+f 11/11/2 4/4/2 1/1/2
+f 13/13/1 1/1/1 3/3/1
+f 1/1/6 13/13/6 10/10/6
+f 14/14/7 5/5/7 15/15/7
+f 5/5/8 16/16/8 15/15/8
+f 17/17/7 5/5/7 14/14/7
+f 5/5/3 17/17/3 6/6/3
+f 7/7/4 16/16/4 5/5/4
+f 18/18/1 19/19/1 20/20/1
+f 19/19/9 21/21/9 20/20/9
+f 22/22/3 23/23/3 19/19/3
+f 24/24/10 19/19/10 23/23/10
+f 25/25/1 19/19/1 18/18/1
+f 25/25/3 22/22/3 19/19/3
+f 26/26/9 21/21/9 19/19/9
+f 19/19/11 27/27/11 26/26/11
+f 24/24/10 27/27/10 19/19/10
+f 28/28/2 29/29/2 30/30/2
+f 31/31/12 29/29/12 28/28/12
+f 32/32/13 33/33/13 29/29/13
+f 34/34/4 29/29/4 33/33/4
+f 29/29/2 35/35/2 30/30/2
+f 35/35/4 29/29/4 34/34/4
+f 31/31/12 32/32/12 29/29/12
+f 20/20/14 36/36/14 37/37/14
+f 38/38/6 20/20/6 37/37/6
+f 39/39/1 18/18/1 20/20/1
+f 40/40/15 36/36/15 20/20/15
+f 21/21/9 40/40/9 20/20/9
+f 39/39/6 20/20/6 38/38/6
+f 41/41/16 28/28/16 42/42/16
+f 42/42/5 28/28/5 43/43/5
+f 28/28/2 30/30/2 12/12/2
+f 28/28/5 12/12/5 43/43/5
+f 44/44/16 28/28/16 41/41/16
+f 28/28/12 44/44/12 31/31/12
+f 45/45/7 23/23/7 46/46/7
+f 47/47/17 23/23/17 45/45/17
+f 46/46/7 23/23/7 48/48/7
+f 22/22/3 48/48/3 23/23/3
+f 49/49/10 24/24/10 23/23/10
+f 47/47/17 49/49/17 23/23/17
+f 33/33/8 50/50/8 51/51/8
+f 50/50/18 33/33/18 52/52/18
+f 53/53/13 54/54/13 33/33/13
+f 33/33/13 32/32/13 53/53/13
+f 55/55/8 33/33/8 51/51/8
+f 33/33/4 55/55/4 34/34/4
+f 52/52/18 33/33/18 54/54/18
+f 56/56/19 9/9/19 57/57/19
+f 56/56/20 58/58/20 9/9/20
+f 10/10/6 57/57/6 9/9/6
+f 9/9/21 58/58/21 59/59/21
+f 59/59/5 8/8/5 9/9/5
+f 15/15/22 60/60/22 61/61/22
+f 60/60/23 15/15/23 62/62/23
+f 61/61/22 63/63/22 15/15/22
+f 15/15/7 63/63/7 14/14/7
+f 15/15/8 16/16/8 62/62/8
+f 64/64/24 37/37/24 65/65/24
+f 64/64/25 57/57/25 37/37/25
+f 37/37/26 66/66/26 65/65/26
+f 57/57/6 38/38/6 37/37/6
+f 37/37/27 36/36/27 66/66/27
+f 42/42/20 67/67/20 68/68/20
+f 69/69/28 42/42/28 68/68/28
+f 41/41/16 42/42/16 70/70/16
+f 42/42/29 59/59/29 67/67/29
+f 70/70/30 42/42/30 69/69/30
+f 59/59/5 42/42/5 43/43/5
+f 71/71/31 45/45/31 72/72/31
+f 45/45/22 73/73/22 72/72/22
+f 47/47/17 45/45/17 71/71/17
+f 46/46/7 63/63/7 45/45/7
+f 45/45/22 63/63/22 73/73/22
+f 74/74/32 50/50/32 75/75/32
+f 74/74/23 76/76/23 50/50/23
+f 75/75/32 50/50/32 77/77/32
+f 77/77/33 50/50/33 52/52/33
+f 76/76/23 62/62/23 50/50/23
+f 51/51/8 50/50/8 62/62/8
+f 78/78/34 79/79/34 80/80/34
+f 80/80/9 81/81/9 78/78/9
+f 82/82/35 78/78/35 83/83/35
+f 83/83/10 78/78/10 84/84/10
+f 85/85/9 78/78/9 86/86/9
+f 86/86/9 78/78/9 81/81/9
+f 84/84/10 78/78/10 85/85/10
+f 87/87/35 78/78/35 82/82/35
+f 78/78/34 87/87/34 79/79/34
+f 88/88/36 80/80/36 89/89/36
+f 90/90/15 80/80/15 88/88/15
+f 79/79/34 91/91/34 80/80/34
+f 80/80/36 91/91/36 89/89/36
+f 90/90/15 81/81/15 80/80/15
+f 92/92/16 93/93/16 94/94/16
+f 92/92/37 95/95/37 93/93/37
+f 93/93/16 44/44/16 94/94/16
+f 95/95/38 96/96/38 93/93/38
+f 93/93/12 96/96/12 97/97/12
+f 97/97/12 44/44/12 93/93/12
+f 98/98/17 83/83/17 99/99/17
+f 98/98/39 100/100/39 83/83/39
+f 99/99/17 83/83/17 49/49/17
+f 83/83/35 101/101/35 82/82/35
+f 100/100/39 101/101/39 83/83/39
+f 83/83/10 84/84/10 49/49/10
+f 102/102/18 103/103/18 104/104/18
+f 102/102/40 104/104/40 105/105/40
+f 104/104/41 96/96/41 106/106/41
+f 106/106/40 105/105/40 104/104/40
+f 54/54/18 104/104/18 103/103/18
+f 104/104/13 107/107/13 96/96/13
+f 104/104/13 54/54/13 107/107/13
+f 108/108/42 109/109/42 56/56/42
+f 56/56/43 110/110/43 108/108/43
+f 67/67/44 58/58/44 56/56/44
+f 67/67/20 56/56/20 111/111/20
+f 110/110/19 56/56/19 57/57/19
+f 109/109/42 111/111/42 56/56/42
+f 112/112/45 113/113/45 60/60/45
+f 60/60/46 114/114/46 112/112/46
+f 60/60/47 115/115/47 61/61/47
+f 113/113/48 115/115/48 60/60/48
+f 60/60/46 116/116/46 114/114/46
+f 60/60/23 62/62/23 116/116/23
+f 117/117/49 64/64/49 118/118/49
+f 117/117/43 110/110/43 64/64/43
+f 65/65/50 118/118/50 64/64/50
+f 64/64/19 110/110/19 57/57/19
+f 119/119/51 68/68/51 109/109/51
+f 68/68/52 119/119/52 69/69/52
+f 67/67/20 111/111/20 68/68/20
+f 109/109/42 68/68/42 111/111/42
+f 120/120/48 72/72/48 113/113/48
+f 72/72/53 120/120/53 121/121/53
+f 121/121/31 71/71/31 72/72/31
+f 113/113/48 72/72/48 115/115/48
+f 73/73/22 115/115/22 72/72/22
+f 122/122/54 74/74/54 123/123/54
+f 114/114/55 74/74/55 122/122/55
+f 74/74/32 75/75/32 124/124/32
+f 116/116/56 76/76/56 74/74/56
+f 123/123/54 74/74/54 124/124/54
+f 116/116/55 74/74/55 114/114/55
+f 125/125/50 65/65/50 88/88/50
+f 88/88/57 126/126/57 125/125/57
+f 65/65/50 66/66/50 88/88/50
+f 127/127/36 88/88/36 89/89/36
+f 66/66/15 90/90/15 88/88/15
+f 126/126/57 88/88/57 127/127/57
+f 128/128/58 129/129/58 92/92/58
+f 92/92/30 69/69/30 128/128/30
+f 94/94/16 70/70/16 92/92/16
+f 92/92/30 70/70/30 69/69/30
+f 92/92/37 129/129/37 95/95/37
+f 130/130/59 98/98/59 131/131/59
+f 131/131/31 98/98/31 71/71/31
+f 99/99/17 71/71/17 98/98/17
+f 132/132/59 98/98/59 130/130/59
+f 100/100/39 98/98/39 132/132/39
+f 133/133/32 75/75/32 102/102/32
+f 102/102/60 134/134/60 133/133/60
+f 75/75/32 77/77/32 102/102/32
+f 102/102/61 77/77/61 103/103/61
+f 105/105/62 135/135/62 102/102/62
+f 102/102/60 135/135/60 134/134/60
+f 136/136/63 108/108/63 117/117/63
+f 117/117/64 108/108/64 110/110/64
+f 119/119/42 109/109/42 108/108/42
+f 119/119/65 108/108/65 137/137/65
+f 138/138/63 108/108/63 136/136/63
+f 137/137/65 108/108/65 138/138/65
+f 120/120/45 113/113/45 112/112/45
+f 112/112/66 139/139/66 120/120/66
+f 112/112/46 114/114/46 122/122/46
+f 140/140/67 112/112/67 122/122/67
+f 112/112/66 141/141/66 139/139/66
+f 141/141/67 112/112/67 140/140/67
+f 125/125/68 142/142/68 143/143/68
+f 125/125/49 143/143/49 118/118/49
+f 65/65/50 125/125/50 118/118/50
+f 125/125/68 126/126/68 142/142/68
+f 69/69/69 144/144/69 128/128/69
+f 145/145/70 128/128/70 144/144/70
+f 146/146/58 129/129/58 128/128/58
+f 146/146/70 128/128/70 145/145/70
+f 147/147/71 148/148/71 131/131/71
+f 131/131/53 121/121/53 147/147/53
+f 130/130/59 131/131/59 148/148/59
+f 131/131/31 71/71/31 121/121/31
+f 133/133/72 149/149/72 150/150/72
+f 150/150/54 123/123/54 133/133/54
+f 124/124/32 75/75/32 133/133/32
+f 149/149/72 133/133/72 134/134/72
+f 123/123/54 124/124/54 133/133/54
+f 117/117/73 143/143/73 151/151/73
+f 151/151/63 136/136/63 117/117/63
+f 117/117/49 118/118/49 143/143/49
+f 152/152/74 119/119/74 153/153/74
+f 153/153/65 119/119/65 137/137/65
+f 144/144/74 119/119/74 152/152/74
+f 69/69/69 119/119/69 144/144/69
+f 120/120/75 154/154/75 155/155/75
+f 120/120/66 139/139/66 154/154/66
+f 155/155/75 147/147/75 120/120/75
+f 147/147/53 121/121/53 120/120/53
+f 156/156/76 122/122/76 157/157/76
+f 156/156/67 140/140/67 122/122/67
+f 122/122/54 123/123/54 150/150/54
+f 157/157/76 122/122/76 150/150/76
+f 158/158/77 159/159/77 160/160/77
+f 161/161/34 159/159/34 158/158/34
+f 162/162/78 159/159/78 163/163/78
+f 163/163/35 159/159/35 164/164/35
+f 160/160/77 159/159/77 165/165/77
+f 165/165/78 159/159/78 162/162/78
+f 164/164/79 159/159/79 166/166/79
+f 161/161/34 166/166/34 159/159/34
+f 167/167/80 168/168/80 169/169/80
+f 169/169/38 168/168/38 95/95/38
+f 170/170/81 171/171/81 168/168/81
+f 172/172/41 168/168/41 171/171/41
+f 95/95/38 168/168/38 173/173/38
+f 173/173/82 168/168/82 172/172/82
+f 174/174/80 168/168/80 167/167/80
+f 168/168/81 174/174/81 170/170/81
+f 175/175/83 158/158/83 176/176/83
+f 177/177/36 158/158/36 175/175/36
+f 160/160/83 176/176/83 158/158/83
+f 91/91/36 158/158/36 177/177/36
+f 158/158/34 91/91/34 161/161/34
+f 178/178/84 169/169/84 179/179/84
+f 179/179/37 169/169/37 180/180/37
+f 169/169/84 178/178/84 181/181/84
+f 169/169/80 181/181/80 167/167/80
+f 180/180/37 169/169/37 95/95/37
+f 182/182/39 163/163/39 183/183/39
+f 184/184/85 163/163/85 182/182/85
+f 163/163/78 185/185/78 162/162/78
+f 163/163/35 164/164/35 183/183/35
+f 184/184/85 185/185/85 163/163/85
+f 186/186/86 171/171/86 187/187/86
+f 188/188/40 171/171/40 186/186/40
+f 170/170/81 187/187/81 171/171/81
+f 172/172/41 171/171/41 188/188/41
+f 153/153/87 138/138/87 141/141/87
+f 141/141/88 138/138/88 154/154/88
+f 154/154/88 138/138/88 151/151/88
+f 138/138/63 136/136/63 151/151/63
+f 153/153/65 137/137/65 138/138/65
+f 153/153/87 141/141/87 156/156/87
+f 139/139/66 141/141/66 154/154/66
+f 156/156/67 141/141/67 140/140/67
+f 154/154/89 151/151/89 189/189/89
+f 151/151/73 143/143/73 190/190/73
+f 151/151/89 190/190/89 189/189/89
+f 191/191/90 153/153/90 156/156/90
+f 191/191/74 152/152/74 153/153/74
+f 155/155/75 154/154/75 192/192/75
+f 192/192/89 154/154/89 189/189/89
+f 193/193/91 191/191/91 156/156/91
+f 156/156/92 157/157/92 193/193/92
+f 143/143/93 194/194/93 190/190/93
+f 195/195/68 143/143/68 142/142/68
+f 143/143/93 195/195/93 194/194/93
+f 144/144/74 152/152/74 191/191/74
+f 191/191/94 196/196/94 144/144/94
+f 196/196/94 197/197/94 144/144/94
+f 145/145/70 144/144/70 197/197/70
+f 192/192/95 198/198/95 147/147/95
+f 192/192/75 147/147/75 155/155/75
+f 198/198/95 199/199/95 147/147/95
+f 147/147/71 199/199/71 148/148/71
+f 193/193/96 157/157/96 150/150/96
+f 150/150/97 200/200/97 193/193/97
+f 149/149/72 201/201/72 150/150/72
+f 150/150/97 201/201/97 200/200/97
+f 127/127/57 175/175/57 202/202/57
+f 202/202/98 175/175/98 203/203/98
+f 176/176/83 203/203/83 175/175/83
+f 175/175/36 127/127/36 177/177/36
+f 146/146/99 204/204/99 179/179/99
+f 146/146/58 179/179/58 129/129/58
+f 204/204/99 205/205/99 179/179/99
+f 179/179/84 205/205/84 178/178/84
+f 129/129/37 179/179/37 180/180/37
+f 182/182/59 130/130/59 206/206/59
+f 207/207/100 182/182/100 206/206/100
+f 182/182/59 132/132/59 130/130/59
+f 183/183/39 132/132/39 182/182/39
+f 208/208/85 184/184/85 182/182/85
+f 208/208/100 182/182/100 207/207/100
+f 209/209/101 186/186/101 210/210/101
+f 134/134/60 186/186/60 209/209/60
+f 188/188/40 186/186/40 105/105/40
+f 186/186/102 135/135/102 105/105/102
+f 186/186/86 187/187/86 210/210/86
+f 135/135/60 186/186/60 134/134/60
+f 211/211/103 192/192/103 190/190/103
+f 189/189/89 190/190/89 192/192/89
+f 211/211/103 190/190/103 212/212/103
+f 190/190/93 194/194/93 212/212/93
+f 213/213/104 191/191/104 193/193/104
+f 214/214/105 191/191/105 213/213/105
+f 214/214/94 196/196/94 191/191/94
+f 211/211/95 198/198/95 192/192/95
+f 193/193/97 200/200/97 213/213/97
+f 195/195/106 202/202/106 215/215/106
+f 195/195/68 142/142/68 202/202/68
+f 215/215/106 202/202/106 216/216/106
+f 126/126/68 202/202/68 142/142/68
+f 127/127/57 202/202/57 126/126/57
+f 202/202/98 203/203/98 216/216/98
+f 217/217/107 146/146/107 197/197/107
+f 197/197/70 146/146/70 145/145/70
+f 146/146/99 217/217/99 204/204/99
+f 218/218/108 206/206/108 199/199/108
+f 206/206/71 148/148/71 199/199/71
+f 130/130/59 148/148/59 206/206/59
+f 219/219/108 206/206/108 218/218/108
+f 207/207/109 206/206/109 219/219/109
+f 201/201/110 209/209/110 220/220/110
+f 201/201/72 149/149/72 209/209/72
+f 220/220/110 209/209/110 221/221/110
+f 149/149/72 134/134/72 209/209/72
+f 221/221/101 209/209/101 210/210/101
+f 194/194/93 195/195/93 212/212/93
+f 212/212/111 195/195/111 222/222/111
+f 195/195/112 215/215/112 223/223/112
+f 222/222/111 195/195/111 223/223/111
+f 224/224/113 197/197/113 214/214/113
+f 214/214/94 197/197/94 196/196/94
+f 225/225/113 197/197/113 224/224/113
+f 197/197/107 225/225/107 217/217/107
+f 211/211/95 199/199/95 198/198/95
+f 226/226/114 199/199/114 211/211/114
+f 227/227/108 218/218/108 199/199/108
+f 226/226/114 227/227/114 199/199/114
+f 213/213/115 201/201/115 228/228/115
+f 200/200/97 201/201/97 213/213/97
+f 201/201/110 220/220/110 229/229/110
+f 228/228/116 201/201/116 229/229/116
+f 211/211/117 212/212/117 230/230/117
+f 231/231/111 212/212/111 222/222/111
+f 230/230/117 212/212/117 231/231/117
+f 232/232/118 214/214/118 213/213/118
+f 224/224/113 214/214/113 233/233/113
+f 233/233/118 214/214/118 232/232/118
+f 226/226/114 211/211/114 234/234/114
+f 211/211/117 230/230/117 234/234/117
+f 213/213/116 228/228/116 235/235/116
+f 232/232/118 213/213/118 235/235/118
+f 236/236/119 237/237/119 238/238/119
+f 236/236/80 238/238/80 239/239/80
+f 240/240/120 238/238/120 241/241/120
+f 242/242/81 238/238/81 240/240/81
+f 241/241/121 238/238/121 243/243/121
+f 243/243/119 238/238/119 237/237/119
+f 239/239/80 238/238/80 244/244/80
+f 238/238/81 242/242/81 244/244/81
+f 245/245/84 236/236/84 246/246/84
+f 247/247/122 236/236/122 245/245/122
+f 236/236/119 248/248/119 237/237/119
+f 248/248/122 236/236/122 247/247/122
+f 181/181/84 246/246/84 236/236/84
+f 236/236/80 239/239/80 181/181/80
+f 249/249/123 240/240/123 250/250/123
+f 251/251/86 240/240/86 249/249/86
+f 241/241/124 252/252/124 240/240/124
+f 253/253/81 242/242/81 240/240/81
+f 251/251/86 253/253/86 240/240/86
+f 240/240/125 252/252/125 250/250/125
+f 254/254/126 255/255/126 245/245/126
+f 254/254/127 245/245/127 256/256/127
+f 245/245/84 246/246/84 205/205/84
+f 245/245/99 205/205/99 256/256/99
+f 255/255/122 247/247/122 245/245/122
+f 257/257/128 249/249/128 258/258/128
+f 259/259/101 249/249/101 257/257/101
+f 210/210/86 251/251/86 249/249/86
+f 210/210/101 249/249/101 259/259/101
+f 249/249/129 250/250/129 260/260/129
+f 258/258/130 249/249/130 260/260/130
+f 261/261/131 254/254/131 225/225/131
+f 225/225/132 254/254/132 217/217/132
+f 261/261/131 262/262/131 254/254/131
+f 255/255/126 254/254/126 262/262/126
+f 254/254/133 256/256/133 217/217/133
+f 257/257/110 229/229/110 220/220/110
+f 229/229/134 257/257/134 263/263/134
+f 220/220/110 221/221/110 257/257/110
+f 263/263/134 257/257/134 264/264/134
+f 257/257/135 221/221/135 259/259/135
+f 264/264/128 257/257/128 258/258/128
+f 233/233/113 225/225/113 224/224/113
+f 265/265/136 225/225/136 233/233/136
+f 261/261/131 225/225/131 266/266/131
+f 265/265/136 266/266/136 225/225/136
+f 235/235/137 229/229/137 267/267/137
+f 235/235/116 228/228/116 229/229/116
+f 267/267/137 229/229/137 268/268/137
+f 263/263/134 268/268/134 229/229/134
+f 269/269/138 233/233/138 235/235/138
+f 235/235/118 233/233/118 232/232/118
+f 265/265/136 233/233/136 269/269/136
+f 235/235/138 270/270/138 269/269/138
+f 235/235/137 267/267/137 270/270/137
+f 234/234/139 231/231/139 271/271/139
+f 234/234/117 230/230/117 231/231/117
+f 272/272/140 231/231/140 223/223/140
+f 231/231/111 222/222/111 223/223/111
+f 271/271/140 231/231/140 272/272/140
+f 227/227/141 234/234/141 273/273/141
+f 227/227/114 226/226/114 234/234/114
+f 274/274/139 234/234/139 271/271/139
+f 273/273/142 234/234/142 274/274/142
+f 223/223/106 215/215/106 275/275/106
+f 275/275/143 276/276/143 223/223/143
+f 272/272/140 223/223/140 277/277/140
+f 223/223/143 276/276/143 277/277/143
+f 278/278/108 218/218/108 227/227/108
+f 279/279/144 278/278/144 227/227/144
+f 227/227/142 273/273/142 280/280/142
+f 279/279/144 227/227/144 280/280/144
+f 275/275/145 281/281/145 282/282/145
+f 275/275/98 203/203/98 281/281/98
+f 276/276/145 275/275/145 282/282/145
+f 215/215/106 216/216/106 275/275/106
+f 216/216/98 203/203/98 275/275/98
+f 283/283/146 278/278/146 284/284/146
+f 285/285/100 278/278/100 283/283/100
+f 286/286/146 284/284/146 278/278/146
+f 278/278/108 219/219/108 218/218/108
+f 286/286/144 278/278/144 279/279/144
+f 285/285/109 219/219/109 278/278/109
+f 287/287/83 288/288/83 281/281/83
+f 289/289/147 281/281/147 288/288/147
+f 282/282/147 281/281/147 289/289/147
+f 203/203/83 287/287/83 281/281/83
+f 283/283/148 290/290/148 291/291/148
+f 292/292/85 283/283/85 291/291/85
+f 290/290/148 283/283/148 284/284/148
+f 208/208/85 283/283/85 292/292/85
+f 208/208/100 285/285/100 283/283/100
+f 293/293/149 294/294/149 288/288/149
+f 293/293/77 288/288/77 295/295/77
+f 288/288/83 287/287/83 160/160/83
+f 295/295/77 288/288/77 160/160/77
+f 294/294/149 296/296/149 288/288/149
+f 296/296/147 289/289/147 288/288/147
+f 297/297/78 291/291/78 293/293/78
+f 298/298/150 293/293/150 291/291/150
+f 291/291/150 299/299/150 300/300/150
+f 300/300/150 298/298/150 291/291/150
+f 290/290/148 299/299/148 291/291/148
+f 291/291/85 185/185/85 292/292/85
+f 185/185/78 291/291/78 297/297/78
+f 293/293/149 301/301/149 294/294/149
+f 293/293/77 295/295/77 297/297/77
+f 298/298/150 301/301/150 293/293/150
+f 269/269/151 270/270/151 302/302/151
+f 270/270/137 267/267/137 268/268/137
+f 270/270/152 268/268/152 303/303/152
+f 304/304/152 270/270/152 303/303/152
+f 302/302/151 270/270/151 304/304/151
+f 266/266/136 265/265/136 269/269/136
+f 266/266/153 269/269/153 305/305/153
+f 305/305/153 269/269/153 306/306/153
+f 306/306/151 269/269/151 302/302/151
+f 307/307/154 274/274/154 271/271/154
+f 280/280/155 274/274/155 308/308/155
+f 273/273/142 274/274/142 280/280/142
+f 308/308/154 274/274/154 307/307/154
+f 271/271/140 272/272/140 277/277/140
+f 271/271/156 277/277/156 309/309/156
+f 271/271/156 309/309/156 307/307/156
+f 268/268/157 310/310/157 311/311/157
+f 310/310/134 268/268/134 264/264/134
+f 303/303/152 268/268/152 312/312/152
+f 312/312/157 268/268/157 311/311/157
+f 264/264/158 268/268/158 263/263/158
+f 313/313/159 314/314/159 280/280/159
+f 314/314/160 279/279/160 280/280/160
+f 280/280/155 308/308/155 315/315/155
+f 280/280/159 315/315/159 313/313/159
+f 316/316/131 261/261/131 266/266/131
+f 316/316/161 266/266/161 317/317/161
+f 318/318/153 266/266/153 305/305/153
+f 317/317/161 266/266/161 318/318/161
+f 277/277/143 276/276/143 319/319/143
+f 319/319/162 320/320/162 277/277/162
+f 309/309/156 277/277/156 321/321/156
+f 321/321/162 277/277/162 320/320/162
+f 322/322/163 323/323/163 310/310/163
+f 322/322/128 310/310/128 324/324/128
+f 323/323/163 325/325/163 310/310/163
+f 310/310/164 264/264/164 324/324/164
+f 311/311/157 310/310/157 325/325/157
+f 314/314/165 326/326/165 327/327/165
+f 314/314/146 327/327/146 284/284/146
+f 314/314/165 328/328/165 326/326/165
+f 314/314/146 284/284/146 286/286/146
+f 328/328/159 314/314/159 313/313/159
+f 314/314/144 286/286/144 279/279/144
+f 316/316/166 329/329/166 330/330/166
+f 330/330/126 255/255/126 316/316/126
+f 316/316/166 317/317/166 329/329/166
+f 261/261/131 316/316/131 262/262/131
+f 255/255/126 262/262/126 316/316/126
+f 319/319/145 282/282/145 331/331/145
+f 331/331/167 332/332/167 319/319/167
+f 319/319/145 276/276/145 282/282/145
+f 319/319/167 332/332/167 320/320/167
+f 306/306/168 304/304/168 333/333/168
+f 306/306/151 302/302/151 304/304/151
+f 334/334/169 304/304/169 312/312/169
+f 304/304/152 303/303/152 312/312/152
+f 334/334/169 333/333/169 304/304/169
+f 305/305/153 306/306/153 318/318/153
+f 318/318/170 306/306/170 335/335/170
+f 336/336/171 306/306/171 333/333/171
+f 335/335/172 306/306/172 336/336/172
+f 308/308/173 307/307/173 337/337/173
+f 337/337/174 315/315/174 308/308/174
+f 321/321/175 338/338/175 307/307/175
+f 307/307/156 309/309/156 321/321/156
+f 337/337/173 307/307/173 339/339/173
+f 307/307/175 338/338/175 339/339/175
+f 340/340/129 322/322/129 341/341/129
+f 342/342/176 322/322/176 340/340/176
+f 341/341/129 322/322/129 260/260/129
+f 323/323/163 322/322/163 343/343/163
+f 343/343/176 322/322/176 342/342/176
+f 322/322/177 324/324/177 260/260/177
+f 344/344/148 290/290/148 327/327/148
+f 327/327/178 345/345/178 344/344/178
+f 290/290/148 284/284/148 327/327/148
+f 345/345/178 327/327/178 346/346/178
+f 346/346/165 327/327/165 326/326/165
+f 347/347/179 348/348/179 330/330/179
+f 330/330/122 348/348/122 349/349/122
+f 330/330/179 350/350/179 347/347/179
+f 350/350/166 330/330/166 329/329/166
+f 330/330/122 349/349/122 255/255/122
+f 351/351/180 331/331/180 352/352/180
+f 352/352/147 331/331/147 353/353/147
+f 331/331/147 282/282/147 353/353/147
+f 332/332/180 331/331/180 351/351/180
+f 354/354/169 334/334/169 312/312/169
+f 355/355/181 354/354/181 312/312/181
+f 312/312/157 311/311/157 356/356/157
+f 356/356/181 355/355/181 312/312/181
+f 315/315/174 337/337/174 357/357/174
+f 315/315/182 357/357/182 358/358/182
+f 358/358/182 359/359/182 315/315/182
+f 313/313/159 315/315/159 359/359/159
+f 360/360/183 318/318/183 361/361/183
+f 335/335/170 361/361/170 318/318/170
+f 362/362/183 318/318/183 360/360/183
+f 362/362/161 317/317/161 318/318/161
+f 321/321/175 363/363/175 338/338/175
+f 363/363/184 321/321/184 364/364/184
+f 364/364/184 321/321/184 365/365/184
+f 365/365/162 321/321/162 320/320/162
+f 336/366/185 333/367/185 366/368/185
+f 367/369/186 333/367/186 368/370/186
+f 366/368/185 333/367/185 367/369/185
+f 354/371/169 333/367/169 334/372/169
+f 368/370/186 333/367/186 354/371/186
+f 367/369/187 337/337/187 339/339/187
+f 369/373/188 337/337/188 367/369/188
+f 357/357/188 337/337/188 369/373/188
+f 370/374/189 336/366/189 371/375/189
+f 371/375/185 336/366/185 366/368/185
+f 335/376/190 336/366/190 361/377/190
+f 361/377/189 336/366/189 370/374/189
+f 367/369/187 339/339/187 371/375/187
+f 371/375/191 339/339/191 372/378/191
+f 338/338/175 363/363/175 339/339/175
+f 372/378/191 339/339/191 363/363/191
+f 371/375/185 366/368/185 367/369/185
+f 369/373/188 367/369/188 373/379/188
+f 373/379/186 367/369/186 368/370/186
+f 371/375/191 372/378/191 374/380/191
+f 374/380/189 370/374/189 371/375/189
+f 375/381/192 340/340/192 376/382/192
+f 377/383/121 376/382/121 340/340/121
+f 340/340/129 341/341/129 378/384/129
+f 340/340/121 378/384/121 377/383/121
+f 342/342/192 340/340/192 375/381/192
+f 379/385/119 348/348/119 376/382/119
+f 376/382/193 348/348/193 380/386/193
+f 381/387/179 348/348/179 347/347/179
+f 248/248/119 348/348/119 379/385/119
+f 349/349/122 348/348/122 248/248/122
+f 380/386/193 348/348/193 381/387/193
+f 382/388/150 383/389/150 344/344/150
+f 382/388/194 344/344/194 384/390/194
+f 299/299/148 290/290/148 344/344/148
+f 344/344/150 383/389/150 299/299/150
+f 345/345/178 385/391/178 344/344/178
+f 384/390/194 344/344/194 385/391/194
+f 352/352/149 386/392/149 382/388/149
+f 387/393/195 352/352/195 382/388/195
+f 351/351/195 352/352/195 387/393/195
+f 352/352/147 353/353/147 388/394/147
+f 352/352/149 388/394/149 386/392/149
+f 389/395/119 379/385/119 376/382/119
+f 389/395/121 376/382/121 377/383/121
+f 375/381/192 376/382/192 390/396/192
+f 390/396/193 376/382/193 380/386/193
+f 382/388/150 391/397/150 383/389/150
+f 386/392/149 391/397/149 382/388/149
+f 382/388/195 384/390/195 387/393/195
+f 354/371/196 392/398/196 373/379/196
+f 373/379/186 368/370/186 354/371/186
+f 354/371/196 393/399/196 394/400/196
+f 355/401/181 393/399/181 354/371/181
+f 354/371/197 394/400/197 392/398/197
+f 373/379/198 395/402/198 357/357/198
+f 373/379/188 357/357/188 369/373/188
+f 358/358/182 357/357/182 396/403/182
+f 395/402/198 396/403/198 357/357/198
+f 374/380/199 397/404/199 361/377/199
+f 361/377/189 370/374/189 374/380/189
+f 360/405/183 361/377/183 398/406/183
+f 397/404/200 398/406/200 361/377/200
+f 374/380/191 372/378/191 363/363/191
+f 363/363/201 399/407/201 374/380/201
+f 363/363/202 400/408/202 399/407/202
+f 400/408/184 363/363/184 364/364/184
+f 356/356/163 323/323/163 401/409/163
+f 402/410/203 356/356/203 401/409/203
+f 323/323/163 356/356/163 325/325/163
+f 311/311/157 325/325/157 356/356/157
+f 355/355/203 356/356/203 402/410/203
+f 403/411/204 359/359/204 404/412/204
+f 403/411/165 326/326/165 359/359/165
+f 404/412/204 359/359/204 405/413/204
+f 328/328/165 359/359/165 326/326/165
+f 405/413/182 359/359/182 358/358/182
+f 359/359/159 328/328/159 313/313/159
+f 406/414/166 329/329/166 362/362/166
+f 362/362/205 407/415/205 406/414/205
+f 317/317/166 362/362/166 329/329/166
+f 362/362/205 408/416/205 407/415/205
+f 408/416/205 362/362/205 409/417/205
+f 409/417/183 362/362/183 360/360/183
+f 410/418/206 411/419/206 365/365/206
+f 365/365/167 332/332/167 410/418/167
+f 364/364/206 365/365/206 411/419/206
+f 320/320/167 332/332/167 365/365/167
+f 412/420/198 395/402/198 373/379/198
+f 373/379/196 392/398/196 412/420/196
+f 413/421/199 397/404/199 374/380/199
+f 374/380/202 399/407/202 413/421/202
+f 414/422/176 401/409/176 415/423/176
+f 416/424/207 401/409/207 414/422/207
+f 323/323/163 343/343/163 401/409/163
+f 401/409/176 343/343/176 415/423/176
+f 416/424/207 402/410/207 401/409/207
+f 417/425/178 418/426/178 403/411/178
+f 417/425/208 403/411/208 419/427/208
+f 418/426/178 346/346/178 403/411/178
+f 403/411/208 420/428/208 419/427/208
+f 404/412/204 420/428/204 403/411/204
+f 403/411/165 346/346/165 326/326/165
+f 381/387/179 406/414/179 421/429/179
+f 421/429/209 406/414/209 422/430/209
+f 381/387/179 350/350/179 406/414/179
+f 406/414/166 350/350/166 329/329/166
+f 406/414/209 407/415/209 422/430/209
+f 423/431/210 410/418/210 424/432/210
+f 424/432/180 410/418/180 332/332/180
+f 423/431/210 425/433/210 410/418/210
+f 410/418/206 425/433/206 411/419/206
+f 393/434/203 402/410/203 426/435/203
+f 427/436/211 393/399/211 426/437/211
+f 393/434/203 355/355/203 402/410/203
+f 428/438/211 393/399/211 427/436/211
+f 394/400/196 393/399/196 428/438/196
+f 396/403/204 429/439/204 404/412/204
+f 429/439/212 396/403/212 430/440/212
+f 404/412/204 405/413/204 396/403/204
+f 430/440/212 396/403/212 431/441/212
+f 396/403/182 405/413/182 358/358/182
+f 431/441/198 396/403/198 395/402/198
+f 432/442/213 408/443/213 398/406/213
+f 398/406/214 433/444/214 432/442/214
+f 408/443/215 409/445/215 398/406/215
+f 409/445/183 360/405/183 398/406/183
+f 434/446/216 398/406/216 397/404/216
+f 398/406/217 434/446/217 433/444/217
+f 400/408/206 411/419/206 435/447/206
+f 435/447/218 436/448/218 400/408/218
+f 400/408/206 364/364/206 411/419/206
+f 399/407/218 400/408/218 436/448/218
+f 437/449/212 430/440/212 412/420/212
+f 437/449/211 412/420/211 427/436/211
+f 430/440/212 431/441/212 412/420/212
+f 412/420/198 431/441/198 395/402/198
+f 412/420/211 428/438/211 427/436/211
+f 392/398/196 428/438/196 412/420/196
+f 413/421/218 436/448/218 438/450/218
+f 438/450/219 433/444/219 413/421/219
+f 413/421/218 399/407/218 436/448/218
+f 413/421/199 434/446/199 397/404/199
+f 434/446/219 413/421/219 433/444/219
+f 439/451/220 440/452/220 414/422/220
+f 414/422/192 441/453/192 439/451/192
+f 416/424/220 414/422/220 440/452/220
+f 414/422/192 442/454/192 441/453/192
+f 415/423/176 442/454/176 414/422/176
+f 443/455/193 421/429/193 439/451/193
+f 439/451/221 421/429/221 444/456/221
+f 381/387/193 421/429/193 443/455/193
+f 445/457/209 421/429/209 422/430/209
+f 444/456/221 421/429/221 445/457/221
+f 446/458/194 447/459/194 417/425/194
+f 417/425/222 448/460/222 446/458/222
+f 447/459/194 385/391/194 417/425/194
+f 417/425/178 385/391/178 418/426/178
+f 419/427/208 449/461/208 417/425/208
+f 417/425/222 449/461/222 448/460/222
+f 450/462/223 424/432/223 446/458/223
+f 424/432/195 451/463/195 446/458/195
+f 452/464/210 423/431/210 424/432/210
+f 452/464/223 424/432/223 450/462/223
+f 332/332/180 351/351/180 424/432/180
+f 424/432/195 351/351/195 451/463/195
+f 439/451/220 453/465/220 440/452/220
+f 454/466/193 443/455/193 439/451/193
+f 441/453/192 454/466/192 439/451/192
+f 444/456/221 453/465/221 439/451/221
+f 455/467/194 447/459/194 446/458/194
+f 450/462/223 446/458/223 456/468/223
+f 446/458/222 448/460/222 456/468/222
+f 455/467/195 446/458/195 451/463/195
+f 457/469/224 426/437/224 458/470/224
+f 426/437/207 416/471/207 458/470/207
+f 427/436/224 426/437/224 457/469/224
+f 402/410/207 416/424/207 426/435/207
+f 429/439/225 459/472/225 460/473/225
+f 460/473/208 449/461/208 429/439/208
+f 429/439/225 430/440/225 459/472/225
+f 404/412/204 429/439/204 420/428/204
+f 429/439/208 449/461/208 420/428/208
+f 461/474/226 462/475/226 432/442/226
+f 462/475/209 463/476/209 432/442/209
+f 408/443/205 432/442/205 407/477/205
+f 407/477/209 432/442/209 463/476/209
+f 432/442/226 433/444/226 461/474/226
+f 464/478/210 435/447/210 452/464/210
+f 464/478/227 465/479/227 435/447/227
+f 425/433/206 435/447/206 411/419/206
+f 435/447/218 466/480/218 436/448/218
+f 452/464/210 435/447/210 425/433/210
+f 435/447/227 465/479/227 466/480/227
+f 467/481/225 468/482/225 437/449/225
+f 467/481/224 437/449/224 469/483/224
+f 437/449/225 468/482/225 430/440/225
+f 469/483/224 437/449/224 427/436/224
+f 470/484/227 438/450/227 471/485/227
+f 472/486/226 438/450/226 470/484/226
+f 466/480/227 471/485/227 438/450/227
+f 466/480/218 438/450/218 436/448/218
+f 438/450/226 472/486/226 433/444/226
+f 473/487/220 458/470/220 474/488/220
+f 458/470/228 473/487/228 475/489/228
+f 474/488/220 458/470/220 416/471/220
+f 458/470/224 476/490/224 457/469/224
+f 458/470/228 475/489/228 476/490/228
+f 477/491/229 460/473/229 478/492/229
+f 477/491/222 479/493/222 460/473/222
+f 459/472/225 480/494/225 460/473/225
+f 478/492/229 460/473/229 480/494/229
+f 479/493/222 449/461/222 460/473/222
+f 473/487/230 462/475/230 481/495/230
+f 482/496/221 462/475/221 473/487/221
+f 462/475/226 461/474/226 483/497/226
+f 462/475/230 483/497/230 481/495/230
+f 482/496/221 445/498/221 462/475/221
+f 462/475/209 445/498/209 463/476/209
+f 464/478/223 484/499/223 477/491/223
+f 485/500/231 464/478/231 477/491/231
+f 464/478/231 485/500/231 471/485/231
+f 464/478/227 471/485/227 465/479/227
+f 464/478/223 452/464/223 484/499/223
+f 486/501/220 473/487/220 474/488/220
+f 482/496/221 473/487/221 486/501/221
+f 481/495/230 487/502/230 473/487/230
+f 475/489/228 473/487/228 487/502/228
+f 484/499/223 488/503/223 477/491/223
+f 478/492/232 489/504/232 477/491/232
+f 477/491/231 489/504/231 485/500/231
+f 488/503/222 479/493/222 477/491/222
+f 490/505/229 491/506/229 467/481/229
+f 467/481/228 492/507/228 490/505/228
+f 491/506/229 468/482/229 467/481/229
+f 493/508/228 492/507/228 467/481/228
+f 469/483/224 493/508/224 467/481/224
+f 470/484/231 494/509/231 490/505/231
+f 495/510/230 470/484/230 490/505/230
+f 471/485/231 494/509/231 470/484/231
+f 483/497/226 472/486/226 470/484/226
+f 483/497/230 470/484/230 495/510/230
+f 494/509/231 496/511/231 490/505/231
+f 490/505/229 496/511/229 491/506/229
+f 497/512/228 490/505/228 492/507/228
+f 497/512/230 495/510/230 490/505/230
+f 378/384/129 341/341/129 252/252/129
+f 252/252/129 341/341/129 260/260/129
+f 486/513/220 416/424/220 440/452/220
+f 453/465/220 486/513/220 440/452/220
+f 381/387/193 443/455/193 454/466/193
+f 350/350/179 381/387/179 347/347/179
+f 430/440/225 468/482/225 459/472/225
+f 459/472/225 468/482/225 480/494/225
+f 383/389/150 300/300/150 299/299/150
+f 300/300/150 391/397/150 301/301/150
+f 383/389/150 391/397/150 300/300/150
+f 300/300/150 301/301/150 298/298/150
+f 384/390/194 447/459/194 455/467/194
+f 384/390/194 385/391/194 447/459/194
+f 26/26/9 85/85/9 86/86/9
+f 86/86/9 40/40/9 21/21/9
+f 86/86/9 81/81/9 40/40/9
+f 86/86/9 21/21/9 26/26/9
+f 484/499/223 450/462/223 456/468/223
+f 448/460/222 449/461/222 456/468/222
+f 456/468/222 449/461/222 479/493/222
+f 484/499/223 456/468/223 488/503/223
+f 456/468/222 479/493/222 488/503/222
+f 455/467/195 451/463/195 387/393/195
+f 384/390/195 455/467/195 387/393/195
+f 49/49/10 84/84/10 85/85/10
+f 49/49/10 85/85/10 24/24/10
+f 27/27/11 85/85/11 26/26/11
+f 24/24/10 85/85/10 27/27/10
+f 380/386/193 381/387/193 454/466/193
+f 342/342/192 454/466/192 441/453/192
+f 342/342/192 375/381/192 454/466/192
+f 375/381/192 390/396/192 454/466/192
+f 390/396/193 380/386/193 454/466/193
+f 486/501/220 474/488/220 416/471/220
+f 445/457/221 486/513/221 444/456/221
+f 445/498/221 482/496/221 486/501/221
+f 444/456/221 486/513/221 453/465/221
+f 96/96/233 173/173/233 106/106/233
+f 106/106/234 173/173/234 172/172/234
+f 95/95/38 173/173/38 96/96/38
+f 466/480/227 465/479/227 471/485/227
+f 471/485/231 489/504/231 494/509/231
+f 485/500/231 489/504/231 471/485/231
+f 287/287/83 176/176/83 160/160/83
+f 165/165/77 295/295/77 160/160/77
+f 248/248/119 379/385/119 389/395/119
+f 389/395/119 237/237/119 248/248/119
+f 349/349/122 248/248/122 255/255/122
+f 248/248/122 247/247/122 255/255/122
+f 181/181/84 178/178/84 246/246/84
+f 181/181/80 239/239/80 167/167/80
+f 385/391/178 345/345/178 346/346/178
+f 385/391/178 346/346/178 418/426/178
+f 12/12/2 30/30/2 11/11/2
+f 43/43/5 12/12/5 59/59/5
+f 12/12/5 8/8/5 59/59/5
+f 483/497/226 461/474/226 472/486/226
+f 483/497/230 495/510/230 481/495/230
+f 44/44/16 41/41/16 70/70/16
+f 44/44/16 70/70/16 94/94/16
+f 452/464/210 425/433/210 423/431/210
+f 49/49/17 71/71/17 99/99/17
+f 419/427/208 420/428/208 449/461/208
+f 217/217/99 205/205/99 204/204/99
+f 115/115/22 63/63/22 61/61/22
+f 59/59/235 58/58/235 67/67/235
+f 498/514/35 87/87/35 82/82/35
+f 82/82/35 101/101/35 498/514/35
+f 87/87/34 499/515/34 79/79/34
+f 499/515/34 91/91/34 79/79/34
+f 162/162/78 185/185/78 165/165/78
+f 106/106/40 188/188/40 105/105/40
+f 172/172/41 188/188/41 106/106/41
+f 30/30/2 35/35/2 11/11/2
+f 7/7/2 11/11/2 35/35/2
+f 4/4/2 11/11/2 7/7/2
+f 353/353/147 282/282/147 296/296/147
+f 282/282/147 289/289/147 296/296/147
+f 407/477/209 463/476/209 422/516/209
+f 183/183/39 100/100/39 132/132/39
+f 13/13/6 39/39/6 57/57/6
+f 13/13/6 57/57/6 10/10/6
+f 57/57/6 39/39/6 38/38/6
+f 415/423/176 343/343/176 342/342/176
+f 77/77/236 52/52/236 103/103/236
+f 71/71/17 49/49/17 47/47/17
+f 14/14/7 63/63/7 17/17/7
+f 17/17/7 63/63/7 46/46/7
+f 73/73/22 63/63/22 115/115/22
+f 53/53/13 32/32/13 500/517/13
+f 53/53/13 500/517/13 107/107/13
+f 107/107/13 54/54/13 53/53/13
+f 116/116/23 62/62/23 76/76/23
+f 324/324/237 264/264/237 260/260/237
+f 52/52/18 54/54/18 103/103/18
+f 87/87/34 166/166/34 499/515/34
+f 161/161/34 91/91/34 499/515/34
+f 499/515/34 166/166/34 161/161/34
+f 96/96/12 500/517/12 97/97/12
+f 107/107/13 500/517/13 96/96/13
+f 177/177/36 89/89/36 91/91/36
+f 177/177/36 127/127/36 89/89/36
+f 164/164/35 87/87/35 498/514/35
+f 183/183/35 164/164/35 498/514/35
+f 498/514/35 101/101/35 183/183/35
+f 166/166/238 87/87/238 164/164/238
+f 176/176/83 287/287/83 203/203/83
+f 442/454/192 342/342/192 441/453/192
+f 205/205/84 246/246/84 178/178/84
+f 167/167/80 239/239/80 174/174/80
+f 170/170/81 253/253/81 187/187/81
+f 210/210/86 187/187/86 253/253/86
+f 16/16/8 55/55/8 51/51/8
+f 55/55/4 16/16/4 34/34/4
+f 378/384/121 252/252/121 377/383/121
+f 46/46/7 48/48/7 17/17/7
+f 22/22/3 17/17/3 48/48/3
+f 478/492/229 480/494/229 468/482/229
+f 242/242/81 170/170/81 174/174/81
+f 170/170/81 242/242/81 253/253/81
+f 13/13/1 25/25/1 18/18/1
+f 39/39/1 13/13/1 18/18/1
+f 494/509/231 489/504/231 496/511/231
+f 241/241/121 243/243/121 389/395/121
+f 389/395/121 377/383/121 241/241/121
+f 377/383/121 252/252/121 241/241/121
+f 391/397/149 296/296/149 294/294/149
+f 301/301/149 391/397/149 294/294/149
+f 463/476/209 445/498/209 422/516/209
+f 433/444/226 472/486/226 461/474/226
+f 491/506/229 496/511/229 468/482/229
+f 497/512/230 487/502/230 481/495/230
+f 481/495/230 495/510/230 497/512/230
+f 475/489/228 492/507/228 493/508/228
+f 487/502/228 497/512/228 492/507/228
+f 475/489/228 487/502/228 492/507/228
+f 25/25/1 13/13/1 3/3/1
+f 25/25/3 6/6/3 17/17/3
+f 25/25/3 17/17/3 22/22/3
+f 25/25/239 3/3/239 6/6/239
+f 501/518/4 7/7/4 35/35/4
+f 34/34/4 501/518/4 35/35/4
+f 16/16/8 51/51/8 62/62/8
+f 16/16/4 7/7/4 501/518/4
+f 34/34/4 16/16/4 501/518/4
+f 40/40/15 90/90/15 36/36/15
+f 36/36/15 90/90/15 66/66/15
+f 40/40/15 81/81/15 90/90/15
+f 100/100/240 183/183/240 101/101/240
+f 129/129/37 180/180/37 95/95/37
+f 415/423/176 342/342/176 442/454/176
+f 31/31/12 97/97/12 500/517/12
+f 31/31/12 500/517/12 32/32/12
+f 44/44/12 97/97/12 31/31/12
+f 484/499/223 452/464/223 450/462/223
+f 165/165/78 185/185/78 297/297/78
+f 297/297/77 295/295/77 165/165/77
+f 239/239/80 244/244/80 174/174/80
+f 244/244/81 242/242/81 174/174/81
+f 208/208/85 185/185/85 184/184/85
+f 292/292/85 185/185/85 208/208/85
+f 210/210/86 253/253/86 251/251/86
+f 256/256/99 205/205/99 217/217/99
+f 208/208/100 207/207/100 219/219/100
+f 208/208/100 219/219/100 285/285/100
+f 259/259/101 221/221/101 210/210/101
+f 389/395/119 243/243/119 237/237/119
+f 250/250/129 252/252/129 260/260/129
+f 264/264/241 258/258/241 260/260/241
+f 386/392/149 296/296/149 391/397/149
+f 388/394/147 353/353/147 296/296/147
+f 388/394/149 296/296/149 386/392/149
+f 451/463/195 351/351/195 387/393/195
+f 457/469/224 493/508/224 427/436/224
+f 476/490/224 493/508/224 457/469/224
+f 427/436/224 493/508/224 469/483/224
+f 476/490/228 475/489/228 493/508/228
+f 496/511/242 489/504/242 468/482/242
+f 489/504/243 478/492/243 468/482/243
+f 394/400/197 428/438/197 392/398/197
diff --git a/examples/pybullet/gym/pybullet_data/torus_deform.urdf b/examples/pybullet/gym/pybullet_data/torus_deform.urdf
new file mode 100644
index 000000000..a3b49dc94
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/torus_deform.urdf
@@ -0,0 +1,14 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/examples/pybullet/gym/pybullet_data/toys/LICENSE.txt b/examples/pybullet/gym/pybullet_data/toys/LICENSE.txt
new file mode 100644
index 000000000..655f41b0f
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/toys/LICENSE.txt
@@ -0,0 +1,14 @@
+URDF created by Erwin Coumans
+
+Bullet Continuous Collision Detection and Physics Library
+http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
diff --git a/examples/pybullet/gym/pybullet_data/toys/concave_box.cdf b/examples/pybullet/gym/pybullet_data/toys/concave_box.cdf
new file mode 100644
index 000000000..1183762d8
Binary files /dev/null and b/examples/pybullet/gym/pybullet_data/toys/concave_box.cdf differ
diff --git a/examples/pybullet/gym/pybullet_data/toys/concave_box.mtl b/examples/pybullet/gym/pybullet_data/toys/concave_box.mtl
new file mode 100644
index 000000000..35c3a209b
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/toys/concave_box.mtl
@@ -0,0 +1,11 @@
+# Blender MTL File: 'None'
+# Material Count: 1
+
+newmtl None
+Ns 0
+Ka 0.000000 0.000000 0.000000
+Kd 0.8 0.8 0.8
+Ks 0.8 0.8 0.8
+d 1
+illum 2
+map_Kd ../checker_grid.jpg
diff --git a/examples/pybullet/gym/pybullet_data/toys/concave_box.obj b/examples/pybullet/gym/pybullet_data/toys/concave_box.obj
new file mode 100644
index 000000000..bc58715ec
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/toys/concave_box.obj
@@ -0,0 +1,949 @@
+# Blender v2.78 (sub 0) OBJ File: ''
+# www.blender.org
+mtllib concave_box.mtl
+o Cube_Cube.003
+v 0.252358 -0.051546 0.073254
+v 0.196823 -0.040500 0.026785
+v 0.133463 -0.027897 -0.007745
+v 0.064714 -0.014222 -0.029009
+v 0.237323 -0.101112 0.073254
+v 0.185009 -0.079443 0.026785
+v 0.125326 -0.054721 -0.007745
+v 0.060565 -0.027897 -0.029009
+v 0.212906 -0.146792 0.073254
+v 0.165826 -0.115334 0.026785
+v 0.112112 -0.079443 -0.007745
+v 0.053829 -0.040500 -0.029009
+v 0.180047 -0.186831 0.073254
+v 0.140008 -0.146792 0.026785
+v 0.094329 -0.101112 -0.007745
+v 0.044763 -0.051546 -0.029009
+v 0.140008 -0.219690 0.073254
+v 0.108550 -0.172609 0.026785
+v 0.072660 -0.118895 -0.007745
+v 0.033716 -0.060612 -0.029009
+v -0.006783 -0.000000 -0.036189
+v 0.094329 -0.244106 0.073254
+v 0.072660 -0.191793 0.026785
+v 0.047938 -0.132109 -0.007745
+v 0.021113 -0.067349 -0.029009
+v 0.044763 -0.259142 0.073254
+v 0.033716 -0.203606 0.026785
+v 0.021113 -0.140246 -0.007745
+v 0.007438 -0.071497 -0.029009
+v -0.006783 -0.264218 0.073254
+v -0.006783 -0.207595 0.026785
+v -0.006783 -0.142994 -0.007745
+v -0.006783 -0.072898 -0.029009
+v -0.058330 -0.259142 0.073254
+v -0.047283 -0.203606 0.026785
+v -0.034680 -0.140246 -0.007745
+v -0.021005 -0.071497 -0.029009
+v -0.107895 -0.244106 0.073254
+v -0.086227 -0.191793 0.026785
+v -0.061505 -0.132109 -0.007745
+v -0.034680 -0.067349 -0.029009
+v -0.153575 -0.219690 0.073254
+v -0.122117 -0.172609 0.026785
+v -0.086227 -0.118895 -0.007745
+v -0.047283 -0.060612 -0.029009
+v -0.193614 -0.186831 0.073254
+v -0.153575 -0.146792 0.026785
+v -0.107895 -0.101112 -0.007745
+v -0.058330 -0.051546 -0.029009
+v -0.226473 -0.146792 0.073254
+v -0.179392 -0.115334 0.026785
+v -0.125679 -0.079443 -0.007745
+v -0.067396 -0.040500 -0.029009
+v -0.250889 -0.101112 0.073254
+v -0.198576 -0.079443 0.026785
+v -0.138893 -0.054721 -0.007745
+v -0.074132 -0.027897 -0.029009
+v -0.265925 -0.051546 0.073254
+v -0.210390 -0.040500 0.026785
+v -0.147030 -0.027897 -0.007745
+v -0.078280 -0.014222 -0.029009
+v -0.271002 0.000000 0.073254
+v -0.214378 0.000000 0.026785
+v -0.149777 -0.000000 -0.007745
+v -0.079681 -0.000000 -0.029009
+v -0.265925 0.051546 0.073254
+v -0.210390 0.040500 0.026785
+v -0.147030 0.027897 -0.007745
+v -0.078280 0.014222 -0.029009
+v -0.250889 0.101112 0.073254
+v -0.198576 0.079443 0.026785
+v -0.138893 0.054721 -0.007745
+v -0.074132 0.027897 -0.029009
+v -0.226473 0.146792 0.073254
+v -0.179392 0.115334 0.026785
+v -0.125679 0.079443 -0.007745
+v -0.067396 0.040500 -0.029009
+v -0.193614 0.186831 0.073254
+v -0.153575 0.146792 0.026785
+v -0.107895 0.101112 -0.007745
+v -0.058330 0.051546 -0.029009
+v -0.153575 0.219690 0.073254
+v -0.122117 0.172609 0.026785
+v -0.086227 0.118895 -0.007745
+v -0.047283 0.060612 -0.029009
+v -0.107895 0.244106 0.073254
+v -0.086227 0.191793 0.026785
+v -0.061505 0.132109 -0.007745
+v -0.034680 0.067349 -0.029009
+v -0.058330 0.259141 0.073254
+v -0.047283 0.203606 0.026785
+v -0.034680 0.140246 -0.007745
+v -0.021005 0.071497 -0.029009
+v -0.006783 0.264218 0.073254
+v -0.006783 0.207595 0.026785
+v -0.006783 0.142994 -0.007745
+v -0.006783 0.072898 -0.029009
+v 0.044763 0.259141 0.073254
+v 0.033716 0.203606 0.026785
+v 0.021113 0.140246 -0.007745
+v 0.007438 0.071497 -0.029009
+v 0.094329 0.244106 0.073254
+v 0.072660 0.191793 0.026785
+v 0.047938 0.132109 -0.007745
+v 0.021113 0.067349 -0.029009
+v 0.140008 0.219689 0.073254
+v 0.108550 0.172609 0.026785
+v 0.072660 0.118895 -0.007745
+v 0.033716 0.060612 -0.029009
+v 0.180047 0.186831 0.073254
+v 0.140008 0.146792 0.026785
+v 0.094328 0.101112 -0.007745
+v 0.044763 0.051546 -0.029009
+v 0.212906 0.146792 0.073254
+v 0.165825 0.115334 0.026785
+v 0.112112 0.079443 -0.007745
+v 0.053829 0.040500 -0.029009
+v 0.237322 0.101112 0.073254
+v 0.185009 0.079443 0.026785
+v 0.125326 0.054721 -0.007745
+v 0.060565 0.027897 -0.029009
+v 0.252358 0.051546 0.073254
+v 0.196823 0.040500 0.026785
+v 0.133463 0.027897 -0.007745
+v 0.064713 0.014222 -0.029009
+v 0.257435 -0.000000 0.073254
+v 0.200811 -0.000000 0.026785
+v 0.136210 -0.000000 -0.007745
+v 0.066114 -0.000000 -0.029009
+v -1.000000 1.000000 -0.100000
+v -1.000000 1.000000 0.100000
+v 1.000000 1.000000 -0.100000
+v 1.000000 1.000000 0.100000
+v -1.000000 -1.000000 -0.100000
+v -1.000000 -1.000000 0.100000
+v 1.000000 -1.000000 -0.100000
+v 1.000000 -1.000000 0.100000
+v -0.006783 -0.286168 0.100000
+v -0.062612 -0.280669 0.100000
+v 0.195568 -0.202351 0.100000
+v 0.231157 -0.158987 0.100000
+v 0.257601 -0.109512 0.100000
+v 0.195568 0.202351 0.100000
+v -0.209135 0.202351 0.100000
+v 0.273886 -0.055829 0.100000
+v 0.273886 0.055829 0.100000
+v 0.257601 0.109512 0.100000
+v 0.152203 0.237940 0.100000
+v -0.209135 -0.202351 0.100000
+v -0.165770 -0.237940 0.100000
+v -0.244724 -0.158986 0.100000
+v -0.116295 -0.264385 0.100000
+v -0.244723 0.158986 0.100000
+v -0.165770 0.237940 0.100000
+v -0.116295 0.264385 0.100000
+v 0.231156 0.158986 0.100000
+v -0.271168 -0.109512 0.100000
+v -0.287453 0.055829 0.100000
+v -0.292951 0.000000 0.100000
+v 0.279384 -0.000000 0.100000
+v 0.102728 -0.264385 0.100000
+v 0.049045 0.280669 0.100000
+v 0.049045 -0.280669 0.100000
+v -0.287453 -0.055829 0.100000
+v -0.062612 0.280669 0.100000
+v -0.006783 0.286168 0.100000
+v 0.152203 -0.237940 0.100000
+v 0.102728 0.264385 0.100000
+v -0.271168 0.109512 0.100000
+vt 0.5258 0.8663
+vt 0.5000 1.0000
+vt 0.5000 0.8663
+vt 0.5202 0.6339
+vt 0.5000 0.6339
+vt 0.5139 0.4613
+vt 0.5000 0.4613
+vt 0.5071 0.3550
+vt 0.5000 0.3550
+vt 0.5000 0.4966
+vt 0.5071 0.5324
+vt 0.5000 0.5331
+vt 0.5139 0.5303
+vt 0.5506 0.8663
+vt 0.5279 1.0000
+vt 0.5274 0.4613
+vt 0.5139 0.3550
+vt 0.5734 0.8663
+vt 0.5548 1.0000
+vt 0.5577 0.6339
+vt 0.5397 0.6339
+vt 0.5397 0.4613
+vt 0.5202 0.3550
+vt 0.5202 0.5269
+vt 0.5258 0.5224
+vt 0.6012 1.0000
+vt 0.5795 1.0000
+vt 0.5734 0.6339
+vt 0.5506 0.4613
+vt 0.5258 0.3550
+vt 0.5700 0.6339
+vt 0.5700 0.8663
+vt 0.5900 0.8663
+vt 0.5363 0.4613
+vt 0.5472 0.4613
+vt 0.5169 0.3550
+vt 0.5224 0.3550
+vt 0.5303 0.5169
+vt 0.5978 1.0000
+vt 0.5514 1.0000
+vt 0.5761 1.0000
+vt 0.5543 0.6339
+vt 0.5472 0.8663
+vt 0.5363 0.6339
+vt 0.5240 0.4613
+vt 0.5337 0.5106
+vt 0.5224 0.8663
+vt 0.5106 0.4613
+vt 0.5106 0.3550
+vt 0.5357 0.5037
+vt 0.4966 1.0000
+vt 0.5245 1.0000
+vt 0.5169 0.6339
+vt 0.4966 0.8663
+vt 0.4966 0.6339
+vt 0.5037 0.3550
+vt 0.4966 0.4613
+vt 0.5364 0.4966
+vt 0.4827 0.4613
+vt 0.4895 0.3550
+vt 0.4966 0.3550
+vt 0.5357 0.4895
+vt 0.4708 0.8663
+vt 0.4461 0.8663
+vt 0.4687 1.0000
+vt 0.4764 0.6339
+vt 0.4569 0.6339
+vt 0.4692 0.4613
+vt 0.5337 0.4827
+vt 0.4827 0.3550
+vt 0.4569 0.4613
+vt 0.5303 0.4764
+vt 0.4232 0.8663
+vt 0.4419 1.0000
+vt 0.4389 0.6339
+vt 0.4032 0.8663
+vt 0.4171 1.0000
+vt 0.4232 0.6339
+vt 0.4708 0.3550
+vt 0.4764 0.3550
+vt 0.5258 0.4708
+vt 0.5202 0.3550
+vt 0.5506 0.4613
+vt 0.5258 0.3550
+vt 0.5202 0.4663
+vt 0.5734 0.8663
+vt 0.6012 1.0000
+vt 0.5934 0.8663
+vt 0.5734 0.6339
+vt 0.5577 0.6339
+vt 0.5506 0.8663
+vt 0.5795 1.0000
+vt 0.5274 0.4613
+vt 0.5397 0.4613
+vt 0.5139 0.4629
+vt 0.5071 0.4609
+vt 0.5279 1.0000
+vt 0.5548 1.0000
+vt 0.5202 0.6339
+vt 0.5397 0.6339
+vt 0.5139 0.4613
+vt 0.5071 0.3550
+vt 0.5139 0.3550
+vt 0.5000 0.8663
+vt 0.5258 0.8663
+vt 0.5000 0.6339
+vt 0.5000 0.4613
+vt 0.5000 0.4602
+vt 0.4929 0.4609
+vt 0.4742 0.8663
+vt 0.5000 1.0000
+vt 0.4798 0.6339
+vt 0.4861 0.4613
+vt 0.4929 0.3550
+vt 0.5000 0.3550
+vt 0.4603 0.6339
+vt 0.4726 0.4613
+vt 0.4861 0.4629
+vt 0.4494 0.8663
+vt 0.4721 1.0000
+vt 0.4266 0.8663
+vt 0.4452 1.0000
+vt 0.4603 0.4613
+vt 0.4861 0.3550
+vt 0.4798 0.4663
+vt 0.4266 0.6339
+vt 0.4423 0.6339
+vt 0.4742 0.3550
+vt 0.4798 0.3550
+vt 0.4742 0.4708
+vt 0.4066 0.8663
+vt 0.4205 1.0000
+vt 0.4232 0.8663
+vt 0.3954 1.0000
+vt 0.4032 0.8663
+vt 0.4389 0.6339
+vt 0.4232 0.6339
+vt 0.4569 0.4613
+vt 0.4461 0.4613
+vt 0.4708 0.3550
+vt 0.4697 0.4764
+vt 0.4569 0.6339
+vt 0.4827 0.3550
+vt 0.4764 0.3550
+vt 0.4663 0.4827
+vt 0.4461 0.8663
+vt 0.4171 1.0000
+vt 0.4708 0.8663
+vt 0.4419 1.0000
+vt 0.4764 0.6339
+vt 0.4827 0.4613
+vt 0.4692 0.4613
+vt 0.4895 0.3550
+vt 0.4643 0.4895
+vt 0.4966 0.3550
+vt 0.4636 0.4966
+vt 0.4966 0.8663
+vt 0.4687 1.0000
+vt 0.4966 0.6339
+vt 0.5224 0.8663
+vt 0.4966 1.0000
+vt 0.5169 0.6339
+vt 0.4966 0.4613
+vt 0.5037 0.3550
+vt 0.4643 0.5037
+vt 0.5106 0.3550
+vt 0.5106 0.4613
+vt 0.4663 0.5106
+vt 0.5472 0.8663
+vt 0.5245 1.0000
+vt 0.5363 0.6339
+vt 0.5240 0.4613
+vt 0.5700 0.8663
+vt 0.5514 1.0000
+vt 0.5363 0.4613
+vt 0.5169 0.3550
+vt 0.4697 0.5169
+vt 0.4742 0.5224
+vt 0.5900 0.8663
+vt 0.5761 1.0000
+vt 0.5700 0.6339
+vt 0.5543 0.6339
+vt 0.5224 0.3550
+vt 0.4423 0.6339
+vt 0.4066 0.8663
+vt 0.4266 0.6339
+vt 0.4603 0.4613
+vt 0.4494 0.4613
+vt 0.4798 0.3550
+vt 0.4742 0.3550
+vt 0.4798 0.5269
+vt 0.4266 0.8663
+vt 0.3988 1.0000
+vt 0.4494 0.8663
+vt 0.4205 1.0000
+vt 0.4603 0.6339
+vt 0.4861 0.3550
+vt 0.4861 0.5303
+vt 0.4798 0.6339
+vt 0.4861 0.4613
+vt 0.4726 0.4613
+vt 0.4929 0.3550
+vt 0.4929 0.5324
+vt 0.4742 0.8663
+vt 0.4452 1.0000
+vt 0.4721 1.0000
+vt 0.0000 1.0000
+vt 1.0000 0.0000
+vt 0.0000 0.0000
+vt 0.0000 1.0000
+vt 1.0000 0.0000
+vt 0.0000 0.0000
+vt 0.0000 1.0000
+vt -1.0000 0.0000
+vt 0.0000 0.0000
+vt 0.0000 1.0000
+vt -1.0000 0.0000
+vt 0.0000 0.0000
+vt 0.0000 -1.0000
+vt -1.0000 0.0000
+vt -0.5279 -0.3631
+vt -0.5000 -0.3603
+vt -0.5000 0.3535
+vt -0.5279 0.3563
+vt -1.0000 0.0000
+vt 0.5934 0.8663
+vt 0.3954 1.0000
+vt 0.4461 0.4613
+vt 0.4494 0.4613
+vt 0.3988 1.0000
+vt 0.5978 1.0000
+vt 0.5472 0.4613
+vt 1.0000 1.0000
+vt 1.0000 1.0000
+vt -1.0000 1.0000
+vt -1.0000 1.0000
+vt 1.0000 -1.0000
+vt -1.0000 -1.0000
+vt -0.6431 -0.5034
+vt -0.6403 -0.4755
+vt -0.3569 -0.5034
+vt 0.0000 -1.0000
+vt 0.0000 0.0000
+vt -0.3597 -0.4755
+vt -0.6322 -0.4486
+vt -0.6190 -0.4239
+vt -0.3678 -0.4486
+vt -0.3810 -0.4239
+vt -0.6012 -0.4022
+vt -0.5795 -0.3844
+vt -0.3988 -0.4022
+vt -0.4205 -0.3844
+vt -0.5548 -0.3712
+vt -0.4452 -0.3712
+vt -0.4721 -0.3631
+vt 0.0000 0.0000
+vt -0.3569 0.4966
+vt -0.3597 0.4687
+vt -0.3678 0.4419
+vt -0.6403 0.4687
+vt -0.6431 0.4966
+vt -0.6322 0.4419
+vt -0.3810 0.4171
+vt -0.3988 0.3954
+vt -0.6190 0.4171
+vt -0.6012 0.3954
+vt -0.4205 0.3776
+vt -0.4452 0.3644
+vt -0.5795 0.3776
+vt -0.5548 0.3644
+vt -0.4721 0.3563
+vn -0.7708 0.0759 0.6326
+vn -0.6332 0.0624 0.7715
+vn -0.4709 0.0464 0.8810
+vn -0.2902 0.0286 0.9565
+vn -0.0980 0.0097 0.9951
+vn -0.0942 0.0286 0.9951
+vn -0.7412 0.2248 0.6326
+vn -0.6088 0.1847 0.7715
+vn -0.4528 0.1374 0.8810
+vn -0.2790 0.0846 0.9565
+vn -0.6831 0.3651 0.6326
+vn -0.5611 0.2999 0.7715
+vn -0.4173 0.2230 0.8810
+vn -0.2571 0.1374 0.9565
+vn -0.0869 0.0464 0.9951
+vn -0.0761 0.0625 0.9951
+vn -0.5987 0.4913 0.6326
+vn -0.4918 0.4036 0.7715
+vn -0.3658 0.3002 0.8810
+vn -0.2254 0.1850 0.9565
+vn -0.4036 0.4918 0.7715
+vn -0.3002 0.3658 0.8810
+vn -0.1850 0.2254 0.9565
+vn -0.0625 0.0761 0.9951
+vn -0.4913 0.5987 0.6326
+vn -0.3651 0.6831 0.6326
+vn -0.2999 0.5611 0.7715
+vn -0.2230 0.4173 0.8810
+vn -0.1374 0.2571 0.9565
+vn -0.0464 0.0869 0.9951
+vn -0.1847 0.6088 0.7715
+vn -0.1374 0.4528 0.8810
+vn -0.0846 0.2790 0.9565
+vn -0.0286 0.0942 0.9951
+vn -0.2248 0.7412 0.6326
+vn -0.0759 0.7708 0.6326
+vn -0.0624 0.6332 0.7715
+vn -0.0464 0.4709 0.8810
+vn -0.0286 0.2902 0.9565
+vn -0.0097 0.0980 0.9951
+vn 0.0464 0.4709 0.8810
+vn 0.0286 0.2902 0.9565
+vn 0.0097 0.0980 0.9951
+vn 0.0759 0.7708 0.6326
+vn 0.0624 0.6332 0.7715
+vn 0.2248 0.7412 0.6326
+vn 0.1847 0.6088 0.7715
+vn 0.1374 0.4528 0.8810
+vn 0.0846 0.2790 0.9565
+vn 0.0286 0.0942 0.9951
+vn 0.1374 0.2571 0.9565
+vn 0.0464 0.0869 0.9951
+vn 0.3651 0.6831 0.6326
+vn 0.2999 0.5611 0.7715
+vn 0.2230 0.4173 0.8810
+vn 0.4913 0.5987 0.6326
+vn 0.4036 0.4918 0.7715
+vn 0.3002 0.3658 0.8810
+vn 0.1850 0.2254 0.9565
+vn 0.0625 0.0761 0.9951
+vn 0.2254 0.1850 0.9565
+vn 0.0761 0.0625 0.9951
+vn 0.5987 0.4913 0.6326
+vn 0.4918 0.4036 0.7715
+vn 0.3658 0.3002 0.8810
+vn 0.6831 0.3651 0.6326
+vn 0.5611 0.2999 0.7715
+vn 0.4173 0.2230 0.8810
+vn 0.2571 0.1374 0.9565
+vn 0.0869 0.0464 0.9951
+vn 0.0942 0.0286 0.9951
+vn 0.7412 0.2248 0.6326
+vn 0.6088 0.1847 0.7715
+vn 0.4528 0.1374 0.8810
+vn 0.2790 0.0846 0.9565
+vn 0.7708 0.0759 0.6326
+vn 0.6332 0.0624 0.7715
+vn 0.4709 0.0464 0.8810
+vn 0.2902 0.0286 0.9565
+vn 0.0980 0.0097 0.9951
+vn 0.0980 -0.0097 0.9951
+vn 0.7708 -0.0759 0.6326
+vn 0.6332 -0.0624 0.7715
+vn 0.4709 -0.0464 0.8810
+vn 0.2902 -0.0286 0.9565
+vn 0.6088 -0.1847 0.7715
+vn 0.4528 -0.1374 0.8810
+vn 0.2790 -0.0846 0.9565
+vn 0.0942 -0.0286 0.9951
+vn 0.7412 -0.2248 0.6326
+vn 0.6831 -0.3651 0.6326
+vn 0.5611 -0.2999 0.7715
+vn 0.4173 -0.2230 0.8810
+vn 0.2571 -0.1374 0.9565
+vn 0.0869 -0.0464 0.9951
+vn 0.3658 -0.3002 0.8810
+vn 0.2254 -0.1850 0.9565
+vn 0.0761 -0.0625 0.9951
+vn 0.5987 -0.4913 0.6326
+vn 0.4918 -0.4036 0.7715
+vn 0.4913 -0.5987 0.6326
+vn 0.4036 -0.4918 0.7715
+vn 0.3002 -0.3658 0.8810
+vn 0.1850 -0.2254 0.9565
+vn 0.0625 -0.0761 0.9951
+vn 0.2230 -0.4173 0.8810
+vn 0.1374 -0.2571 0.9565
+vn 0.0464 -0.0869 0.9951
+vn 0.3651 -0.6831 0.6326
+vn 0.2999 -0.5611 0.7715
+vn 0.2248 -0.7412 0.6326
+vn 0.1847 -0.6088 0.7715
+vn 0.1374 -0.4528 0.8810
+vn 0.0846 -0.2790 0.9565
+vn 0.0286 -0.0942 0.9951
+vn 0.0286 -0.2902 0.9565
+vn 0.0097 -0.0980 0.9951
+vn 0.0759 -0.7708 0.6326
+vn 0.0624 -0.6332 0.7715
+vn 0.0464 -0.4709 0.8810
+vn -0.0759 -0.7708 0.6326
+vn -0.0624 -0.6332 0.7715
+vn -0.0464 -0.4709 0.8810
+vn -0.0286 -0.2902 0.9565
+vn -0.0097 -0.0980 0.9951
+vn -0.0846 -0.2790 0.9565
+vn -0.0286 -0.0942 0.9951
+vn -0.2248 -0.7412 0.6326
+vn -0.1847 -0.6088 0.7715
+vn -0.1374 -0.4528 0.8810
+vn -0.3651 -0.6831 0.6326
+vn -0.2999 -0.5611 0.7715
+vn -0.2231 -0.4173 0.8810
+vn -0.1374 -0.2571 0.9565
+vn -0.0464 -0.0869 0.9951
+vn -0.0625 -0.0761 0.9951
+vn -0.4913 -0.5987 0.6326
+vn -0.4036 -0.4918 0.7715
+vn -0.3002 -0.3658 0.8810
+vn -0.1850 -0.2254 0.9565
+vn -0.4918 -0.4036 0.7715
+vn -0.3658 -0.3002 0.8810
+vn -0.2254 -0.1850 0.9565
+vn -0.0761 -0.0625 0.9951
+vn -0.5987 -0.4913 0.6326
+vn -0.6831 -0.3651 0.6326
+vn -0.5611 -0.2999 0.7715
+vn -0.4173 -0.2231 0.8810
+vn -0.2571 -0.1374 0.9565
+vn -0.0869 -0.0464 0.9951
+vn -0.6088 -0.1847 0.7715
+vn -0.4528 -0.1374 0.8810
+vn -0.2790 -0.0846 0.9565
+vn -0.0942 -0.0286 0.9951
+vn -0.7412 -0.2248 0.6326
+vn -0.7708 -0.0759 0.6326
+vn -0.6332 -0.0624 0.7715
+vn -0.4709 -0.0464 0.8810
+vn -0.2902 -0.0286 0.9565
+vn -0.0980 -0.0097 0.9951
+vn 0.0000 1.0000 0.0000
+vn 1.0000 0.0000 0.0000
+vn 0.0000 -1.0000 0.0000
+vn -1.0000 0.0000 0.0000
+vn 0.0000 0.0000 -1.0000
+vn -0.0000 0.0000 1.0000
+vn 0.2231 0.4173 0.8810
+vn 0.4173 -0.2231 0.8810
+vn -0.4173 -0.2230 0.8810
+usemtl None
+s off
+f 1/1/1 160/2/1 126/3/1
+f 2/4/2 126/3/2 127/5/2
+f 3/6/3 127/5/3 128/7/3
+f 4/8/4 128/7/4 129/9/4
+f 21/10/5 4/11/5 129/12/5
+f 21/10/6 8/13/6 4/11/6
+f 5/14/7 145/15/7 1/1/7
+f 2/4/8 5/14/8 1/1/8
+f 7/16/9 2/4/9 3/6/9
+f 8/17/10 3/6/10 4/8/10
+f 9/18/11 142/19/11 5/14/11
+f 10/20/12 5/14/12 6/21/12
+f 11/22/13 6/21/13 7/16/13
+f 12/23/14 7/16/14 8/17/14
+f 21/10/15 12/24/15 8/13/15
+f 21/10/16 16/25/16 12/24/16
+f 9/18/17 140/26/17 141/27/17
+f 14/28/18 9/18/18 10/20/18
+f 15/29/19 10/20/19 11/22/19
+f 16/30/20 11/22/20 12/23/20
+f 14/31/21 17/32/21 13/33/21
+f 19/34/22 14/31/22 15/35/22
+f 20/36/23 15/35/23 16/37/23
+f 21/10/24 20/38/24 16/25/24
+f 17/32/25 140/39/25 13/33/25
+f 17/32/26 161/40/26 167/41/26
+f 18/42/27 22/43/27 17/32/27
+f 19/34/28 23/44/28 18/42/28
+f 20/36/29 24/45/29 19/34/29
+f 21/10/30 25/46/30 20/38/30
+f 23/44/31 26/47/31 22/43/31
+f 28/48/32 23/44/32 24/45/32
+f 25/49/33 28/48/33 24/45/33
+f 21/10/34 29/50/34 25/46/34
+f 26/47/35 161/40/35 22/43/35
+f 26/47/36 138/51/36 163/52/36
+f 27/53/37 30/54/37 26/47/37
+f 28/48/38 31/55/38 27/53/38
+f 29/56/39 32/57/39 28/48/39
+f 21/10/40 33/58/40 29/50/40
+f 36/59/41 31/55/41 32/57/41
+f 37/60/42 32/57/42 33/61/42
+f 21/10/43 37/62/43 33/58/43
+f 34/63/44 138/51/44 30/54/44
+f 31/55/45 34/63/45 30/54/45
+f 38/64/46 139/65/46 34/63/46
+f 35/66/47 38/64/47 34/63/47
+f 36/59/48 39/67/48 35/66/48
+f 37/60/49 40/68/49 36/59/49
+f 21/10/50 41/69/50 37/62/50
+f 41/70/51 44/71/51 40/68/51
+f 21/10/52 45/72/52 41/69/52
+f 42/73/53 152/74/53 38/64/53
+f 43/75/54 38/64/54 39/67/54
+f 44/71/55 39/67/55 40/68/55
+f 46/76/56 150/77/56 42/73/56
+f 43/75/57 46/76/57 42/73/57
+f 44/71/58 47/78/58 43/75/58
+f 49/79/59 44/71/59 45/80/59
+f 21/10/60 49/81/60 45/72/60
+f 53/82/61 48/83/61 49/84/61
+f 21/10/62 53/85/62 49/81/62
+f 50/86/63 149/87/63 46/88/63
+f 47/89/64 50/86/64 46/88/64
+f 48/83/65 51/90/65 47/89/65
+f 54/91/66 151/92/66 50/86/66
+f 51/90/67 54/91/67 50/86/67
+f 56/93/68 51/90/68 52/94/68
+f 53/82/69 56/93/69 52/94/69
+f 21/10/70 57/95/70 53/85/70
+f 21/10/71 61/96/71 57/95/71
+f 54/91/72 164/97/72 157/98/72
+f 59/99/73 54/91/73 55/100/73
+f 60/101/74 55/100/74 56/93/74
+f 61/102/75 56/93/75 57/103/75
+f 62/104/76 164/97/76 58/105/76
+f 63/106/77 58/105/77 59/99/77
+f 64/107/78 59/99/78 60/101/78
+f 61/102/79 64/107/79 60/101/79
+f 21/10/80 65/108/80 61/96/80
+f 21/10/81 69/109/81 65/108/81
+f 66/110/82 159/111/82 62/104/82
+f 67/112/83 62/104/83 63/106/83
+f 68/113/84 63/106/84 64/107/84
+f 69/114/85 64/107/85 65/115/85
+f 71/116/86 66/110/86 67/112/86
+f 72/117/87 67/112/87 68/113/87
+f 69/114/88 72/117/88 68/113/88
+f 21/10/89 73/118/89 69/109/89
+f 70/119/90 158/120/90 66/110/90
+f 74/121/91 169/122/91 70/119/91
+f 71/116/92 74/121/92 70/119/92
+f 76/123/93 71/116/93 72/117/93
+f 73/124/94 76/123/94 72/117/94
+f 21/10/95 77/125/95 73/118/95
+f 76/123/96 79/126/96 75/127/96
+f 81/128/97 76/123/97 77/129/97
+f 21/10/98 81/130/98 77/125/98
+f 78/131/99 153/132/99 74/121/99
+f 79/126/100 74/121/100 75/127/100
+f 82/133/101 144/134/101 78/135/101
+f 83/136/102 78/135/102 79/137/102
+f 84/138/103 79/137/103 80/139/103
+f 81/140/104 84/138/104 80/139/104
+f 21/10/105 85/141/105 81/130/105
+f 84/138/106 87/142/106 83/136/106
+f 89/143/107 84/138/107 85/144/107
+f 21/10/108 89/145/108 85/141/108
+f 86/146/109 154/147/109 82/133/109
+f 87/142/110 82/133/110 83/136/110
+f 90/148/111 155/149/111 86/146/111
+f 91/150/112 86/146/112 87/142/112
+f 92/151/113 87/142/113 88/152/113
+f 93/153/114 88/152/114 89/143/114
+f 21/10/115 93/154/115 89/145/115
+f 97/155/116 92/151/116 93/153/116
+f 21/10/117 97/156/117 93/154/117
+f 94/157/118 165/158/118 90/148/118
+f 95/159/119 90/148/119 91/150/119
+f 92/151/120 95/159/120 91/150/120
+f 98/160/121 166/161/121 94/157/121
+f 99/162/122 94/157/122 95/159/122
+f 96/163/123 99/162/123 95/159/123
+f 101/164/124 96/163/124 97/155/124
+f 21/10/125 101/165/125 97/156/125
+f 105/166/126 100/167/126 101/164/126
+f 21/10/127 105/168/127 101/165/127
+f 102/169/128 162/170/128 98/160/128
+f 103/171/129 98/160/129 99/162/129
+f 104/172/130 99/162/130 100/167/130
+f 106/173/131 168/174/131 102/169/131
+f 103/171/132 106/173/132 102/169/132
+f 108/175/133 103/171/133 104/172/133
+f 109/176/134 104/172/134 105/166/134
+f 21/10/135 109/177/135 105/168/135
+f 21/10/136 113/178/136 109/177/136
+f 110/179/137 148/180/137 106/173/137
+f 111/181/138 106/173/138 107/182/138
+f 108/175/139 111/181/139 107/182/139
+f 113/183/140 108/175/140 109/176/140
+f 115/184/141 110/185/141 111/186/141
+f 116/187/142 111/186/142 112/188/142
+f 117/189/143 112/188/143 113/190/143
+f 21/10/144 117/191/144 113/178/144
+f 114/192/145 143/193/145 110/185/145
+f 118/194/146 156/195/146 114/192/146
+f 119/196/147 114/192/147 115/184/147
+f 116/187/148 119/196/148 115/184/148
+f 121/197/149 116/187/149 117/189/149
+f 21/10/150 121/198/150 117/191/150
+f 123/199/151 118/194/151 119/196/151
+f 124/200/152 119/196/152 120/201/152
+f 125/202/153 120/201/153 121/197/153
+f 21/10/154 125/203/154 121/198/154
+f 122/204/155 147/205/155 118/194/155
+f 126/3/156 146/206/156 122/204/156
+f 127/5/157 122/204/157 123/199/157
+f 128/7/158 123/199/158 124/200/158
+f 129/9/159 124/200/159 125/202/159
+f 21/10/160 129/12/160 125/203/160
+f 131/207/161 132/208/161 130/209/161
+f 133/210/162 136/211/162 132/212/162
+f 137/213/163 134/214/163 136/215/163
+f 135/216/164 130/217/164 134/218/164
+f 136/211/165 130/219/165 132/212/165
+f 133/220/166 146/221/166 160/222/166
+f 159/223/166 158/224/166 131/225/166
+f 1/1/1 145/15/1 160/2/1
+f 2/4/2 1/1/2 126/3/2
+f 3/6/3 2/4/3 127/5/3
+f 4/8/4 3/6/4 128/7/4
+f 5/14/7 142/19/7 145/15/7
+f 2/4/8 6/21/8 5/14/8
+f 7/16/9 6/21/9 2/4/9
+f 8/17/10 7/16/10 3/6/10
+f 9/18/11 141/27/11 142/19/11
+f 10/20/12 9/18/12 5/14/12
+f 11/22/13 10/20/13 6/21/13
+f 12/23/14 11/22/14 7/16/14
+f 9/18/17 13/226/17 140/26/17
+f 14/28/18 13/226/18 9/18/18
+f 15/29/19 14/28/19 10/20/19
+f 16/30/20 15/29/20 11/22/20
+f 14/31/21 18/42/21 17/32/21
+f 19/34/22 18/42/22 14/31/22
+f 20/36/23 19/34/23 15/35/23
+f 17/32/25 167/41/25 140/39/25
+f 17/32/26 22/43/26 161/40/26
+f 18/42/27 23/44/27 22/43/27
+f 19/34/28 24/45/28 23/44/28
+f 20/36/29 25/49/29 24/45/29
+f 23/44/31 27/53/31 26/47/31
+f 28/48/32 27/53/32 23/44/32
+f 25/49/33 29/56/33 28/48/33
+f 26/47/35 163/52/35 161/40/35
+f 26/47/36 30/54/36 138/51/36
+f 27/53/37 31/55/37 30/54/37
+f 28/48/38 32/57/38 31/55/38
+f 29/56/39 33/61/39 32/57/39
+f 36/59/41 35/66/41 31/55/41
+f 37/60/42 36/59/42 32/57/42
+f 34/63/44 139/65/44 138/51/44
+f 31/55/45 35/66/45 34/63/45
+f 38/64/46 152/74/46 139/65/46
+f 35/66/47 39/67/47 38/64/47
+f 36/59/48 40/68/48 39/67/48
+f 37/60/49 41/70/49 40/68/49
+f 41/70/51 45/80/51 44/71/51
+f 42/73/53 150/77/53 152/74/53
+f 43/75/54 42/73/54 38/64/54
+f 44/71/167 43/75/167 39/67/167
+f 46/76/56 149/227/56 150/77/56
+f 43/75/57 47/78/57 46/76/57
+f 44/71/58 48/228/58 47/78/58
+f 49/79/59 48/228/59 44/71/59
+f 53/82/61 52/94/61 48/83/61
+f 50/86/63 151/92/63 149/87/63
+f 47/89/64 51/90/64 50/86/64
+f 48/83/65 52/94/65 51/90/65
+f 54/91/66 157/98/66 151/92/66
+f 51/90/67 55/100/67 54/91/67
+f 56/93/68 55/100/68 51/90/68
+f 53/82/69 57/103/69 56/93/69
+f 54/91/72 58/105/72 164/97/72
+f 59/99/73 58/105/73 54/91/73
+f 60/101/74 59/99/74 55/100/74
+f 61/102/75 60/101/75 56/93/75
+f 62/104/76 159/111/76 164/97/76
+f 63/106/77 62/104/77 58/105/77
+f 64/107/78 63/106/78 59/99/78
+f 61/102/79 65/115/79 64/107/79
+f 66/110/82 158/120/82 159/111/82
+f 67/112/83 66/110/83 62/104/83
+f 68/113/84 67/112/84 63/106/84
+f 69/114/85 68/113/85 64/107/85
+f 71/116/86 70/119/86 66/110/86
+f 72/117/87 71/116/87 67/112/87
+f 69/114/88 73/124/88 72/117/88
+f 70/119/90 169/122/90 158/120/90
+f 74/121/91 153/132/91 169/122/91
+f 71/116/92 75/127/92 74/121/92
+f 76/123/168 75/127/168 71/116/168
+f 73/124/94 77/129/94 76/123/94
+f 76/123/96 80/229/96 79/126/96
+f 81/128/97 80/229/97 76/123/97
+f 78/131/99 144/230/99 153/132/99
+f 79/126/100 78/131/100 74/121/100
+f 82/133/101 154/147/101 144/134/101
+f 83/136/102 82/133/102 78/135/102
+f 84/138/103 83/136/103 79/137/103
+f 81/140/104 85/144/104 84/138/104
+f 84/138/106 88/152/106 87/142/106
+f 89/143/107 88/152/107 84/138/107
+f 86/146/109 155/149/109 154/147/109
+f 87/142/110 86/146/110 82/133/110
+f 90/148/111 165/158/111 155/149/111
+f 91/150/112 90/148/112 86/146/112
+f 92/151/113 91/150/113 87/142/113
+f 93/153/114 92/151/114 88/152/114
+f 97/155/116 96/163/116 92/151/116
+f 94/157/118 166/161/118 165/158/118
+f 95/159/119 94/157/119 90/148/119
+f 92/151/120 96/163/120 95/159/120
+f 98/160/121 162/170/121 166/161/121
+f 99/162/122 98/160/122 94/157/122
+f 96/163/123 100/167/123 99/162/123
+f 101/164/124 100/167/124 96/163/124
+f 105/166/126 104/172/126 100/167/126
+f 102/169/128 168/174/128 162/170/128
+f 103/171/129 102/169/129 98/160/129
+f 104/172/130 103/171/130 99/162/130
+f 106/173/131 148/180/131 168/174/131
+f 103/171/132 107/182/132 106/173/132
+f 108/175/133 107/182/133 103/171/133
+f 109/176/134 108/175/134 104/172/134
+f 110/179/137 143/231/137 148/180/137
+f 111/181/138 110/179/138 106/173/138
+f 108/175/139 112/232/139 111/181/139
+f 113/183/140 112/232/140 108/175/140
+f 115/184/141 114/192/141 110/185/141
+f 116/187/142 115/184/142 111/186/142
+f 117/189/143 116/187/143 112/188/143
+f 114/192/145 156/195/145 143/193/145
+f 118/194/146 147/205/146 156/195/146
+f 119/196/147 118/194/147 114/192/147
+f 116/187/169 120/201/169 119/196/169
+f 121/197/149 120/201/149 116/187/149
+f 123/199/151 122/204/151 118/194/151
+f 124/200/152 123/199/152 119/196/152
+f 125/202/153 124/200/153 120/201/153
+f 122/204/155 146/206/155 147/205/155
+f 126/3/156 160/2/156 146/206/156
+f 127/5/157 126/3/157 122/204/157
+f 128/7/158 127/5/158 123/199/158
+f 129/9/159 128/7/159 124/200/159
+f 131/207/161 133/233/161 132/208/161
+f 133/210/162 137/234/162 136/211/162
+f 137/213/163 135/235/163 134/214/163
+f 135/216/164 131/236/164 130/217/164
+f 136/211/165 134/237/165 130/219/165
+f 133/220/166 131/238/166 166/239/166
+f 133/220/166 166/239/166 162/240/166
+f 138/241/166 135/242/166 137/243/166
+f 163/244/166 138/241/166 137/243/166
+f 133/220/166 162/240/166 168/245/166
+f 133/220/166 168/245/166 148/246/166
+f 161/247/166 163/244/166 137/243/166
+f 167/248/166 161/247/166 137/243/166
+f 133/220/166 148/246/166 143/249/166
+f 133/220/166 143/249/166 156/250/166
+f 140/251/166 167/248/166 137/243/166
+f 141/252/166 140/251/166 137/243/166
+f 137/243/166 133/220/166 160/222/166
+f 133/220/166 156/250/166 147/253/166
+f 142/254/166 141/252/166 137/243/166
+f 145/255/166 142/254/166 137/243/166
+f 133/220/166 147/253/166 146/221/166
+f 160/222/166 145/255/166 137/243/166
+f 135/256/166 138/257/166 139/258/166
+f 135/256/166 139/258/166 152/259/166
+f 165/260/166 166/261/166 131/225/166
+f 155/262/166 165/260/166 131/225/166
+f 135/256/166 152/259/166 150/263/166
+f 135/256/166 150/263/166 149/264/166
+f 154/265/166 155/262/166 131/225/166
+f 144/266/166 154/265/166 131/225/166
+f 135/256/166 149/264/166 151/267/166
+f 135/256/166 151/267/166 157/268/166
+f 153/269/166 144/266/166 131/225/166
+f 169/270/166 153/269/166 131/225/166
+f 131/225/166 135/256/166 159/223/166
+f 135/256/166 157/268/166 164/271/166
+f 158/224/166 169/270/166 131/225/166
+f 135/256/166 164/271/166 159/223/166
diff --git a/examples/pybullet/gym/pybullet_data/toys/concave_box.urdf b/examples/pybullet/gym/pybullet_data/toys/concave_box.urdf
new file mode 100644
index 000000000..c2180ccdd
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/toys/concave_box.urdf
@@ -0,0 +1,30 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/examples/pybullet/gym/pybullet_data/toys/cube.mtl b/examples/pybullet/gym/pybullet_data/toys/cube.mtl
new file mode 100644
index 000000000..c221299b1
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/toys/cube.mtl
@@ -0,0 +1,11 @@
+# Blender MTL File: 'shape_sort.blend'
+# Material Count: 1
+
+newmtl Material.002
+Ns 96.078431
+Ka 0.000000 0.000000 0.000000
+Kd 0.017444 0.640000 0.032216
+Ks 0.034126 0.500000 0.031333
+Ni 1.000000
+d 1.000000
+illum 2
diff --git a/examples/pybullet/gym/pybullet_data/toys/cube.obj b/examples/pybullet/gym/pybullet_data/toys/cube.obj
new file mode 100644
index 000000000..c2849fe05
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/toys/cube.obj
@@ -0,0 +1,64 @@
+# Blender v2.71 (sub 0) OBJ File: 'shape_sort.blend'
+# www.blender.org
+mtllib cube.mtl
+o Cube.001_Cube.002
+v -0.231854 0.040516 -0.056463
+v -0.231854 0.040516 -0.121937
+v -0.144556 0.040516 -0.121937
+v -0.144556 0.040516 -0.056463
+v -0.231854 0.127815 -0.056463
+v -0.231854 0.127815 -0.121937
+v -0.144556 0.127815 -0.121937
+v -0.144556 0.127815 -0.056463
+v -0.231854 0.040516 -0.056463
+v -0.231854 0.040516 -0.121937
+v -0.144556 0.040516 -0.121937
+v -0.144556 0.040516 -0.056463
+v -0.231854 0.127815 -0.056463
+v -0.231854 0.127815 -0.121937
+v -0.144556 0.127815 -0.121937
+v -0.144556 0.127815 -0.056463
+vt 1.044600 0.042083
+vt 1.044600 -0.957917
+vt 0.044600 -0.957917
+vt 1.905897 0.042083
+vt 1.905897 -0.957917
+vt 0.905898 -0.957917
+vt -0.955400 0.042083
+vt -0.955400 -0.957917
+vt -0.094102 0.042083
+vt -0.094102 -0.957917
+vt 0.905898 1.044600
+vt 1.905897 1.044600
+vt 1.905897 0.044600
+vt -0.094102 1.044600
+vt -0.094102 0.044600
+vt 0.044600 0.042083
+vt 0.905898 0.042083
+vt 0.905898 0.044600
+usemtl Material.002
+s off
+f 6/1 2/2 1/3
+f 7/4 3/5 2/6
+f 8/7 4/8 3/3
+f 5/9 1/10 4/6
+f 2/11 3/12 4/13
+f 7/11 6/14 5/15
+f 14/1 10/2 9/3
+f 15/4 11/5 10/6
+f 16/7 12/8 11/3
+f 13/9 9/10 12/6
+f 10/11 11/12 12/13
+f 15/11 14/14 13/15
+f 5/16 6/1 1/3
+f 6/17 7/4 2/6
+f 7/16 8/7 3/3
+f 8/17 5/9 4/6
+f 1/18 2/11 4/13
+f 8/18 7/11 5/15
+f 13/16 14/1 9/3
+f 14/17 15/4 10/6
+f 15/16 16/7 11/3
+f 16/17 13/9 12/6
+f 9/18 10/11 12/13
+f 16/18 15/11 13/15
diff --git a/examples/pybullet/gym/pybullet_data/toys/cylinder.mtl b/examples/pybullet/gym/pybullet_data/toys/cylinder.mtl
new file mode 100644
index 000000000..3efd48560
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/toys/cylinder.mtl
@@ -0,0 +1,11 @@
+# Blender MTL File: 'shape_sort.blend'
+# Material Count: 1
+
+newmtl Material.001
+Ns 96.078431
+Ka 0.000000 0.000000 0.000000
+Kd 0.013473 0.018536 0.640000
+Ks 0.500000 0.500000 0.500000
+Ni 1.000000
+d 1.000000
+illum 2
diff --git a/examples/pybullet/gym/pybullet_data/toys/cylinder.obj b/examples/pybullet/gym/pybullet_data/toys/cylinder.obj
new file mode 100644
index 000000000..b2abf963d
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/toys/cylinder.obj
@@ -0,0 +1,282 @@
+# Blender v2.71 (sub 0) OBJ File: 'shape_sort.blend'
+# www.blender.org
+mtllib cylinder.mtl
+o Cylinder.001
+v -0.291246 0.045696 0.165546
+v -0.214241 0.045696 0.165546
+v -0.291246 0.034429 0.166100
+v -0.214241 0.034429 0.166100
+v -0.291246 0.023595 0.167744
+v -0.214241 0.023595 0.167744
+v -0.291246 0.013610 0.170412
+v -0.214241 0.013610 0.170412
+v -0.291246 0.004859 0.174003
+v -0.214241 0.004858 0.174003
+v -0.291246 -0.002324 0.178379
+v -0.214241 -0.002324 0.178379
+v -0.291246 -0.007661 0.183372
+v -0.214241 -0.007661 0.183372
+v -0.291246 -0.010947 0.188789
+v -0.214241 -0.010947 0.188789
+v -0.291246 -0.012057 0.194422
+v -0.214241 -0.012057 0.194422
+v -0.291246 -0.010947 0.200056
+v -0.214241 -0.010947 0.200056
+v -0.291246 -0.007661 0.205473
+v -0.214241 -0.007661 0.205473
+v -0.291246 -0.002324 0.210465
+v -0.214241 -0.002324 0.210465
+v -0.291246 0.004859 0.214841
+v -0.214241 0.004858 0.214841
+v -0.291246 0.013610 0.218432
+v -0.214241 0.013610 0.218432
+v -0.291246 0.023595 0.221101
+v -0.214241 0.023595 0.221101
+v -0.291246 0.034429 0.222744
+v -0.214241 0.034429 0.222744
+v -0.291246 0.045696 0.223299
+v -0.214241 0.045696 0.223299
+v -0.291246 0.056963 0.222744
+v -0.214241 0.056963 0.222744
+v -0.291246 0.067797 0.221101
+v -0.214241 0.067797 0.221101
+v -0.291246 0.077782 0.218432
+v -0.214241 0.077782 0.218432
+v -0.291246 0.086534 0.214841
+v -0.214241 0.086534 0.214841
+v -0.291246 0.093716 0.210465
+v -0.214241 0.093716 0.210465
+v -0.291246 0.099053 0.205473
+v -0.214241 0.099053 0.205473
+v -0.291246 0.102340 0.200056
+v -0.214241 0.102340 0.200056
+v -0.291246 0.103449 0.194422
+v -0.214241 0.103449 0.194422
+v -0.291246 0.102340 0.188789
+v -0.214241 0.102340 0.188789
+v -0.291246 0.099053 0.183371
+v -0.214241 0.099053 0.183371
+v -0.291246 0.093716 0.178379
+v -0.214241 0.093716 0.178379
+v -0.291246 0.086534 0.174003
+v -0.214241 0.086534 0.174003
+v -0.291246 0.077782 0.170412
+v -0.214241 0.077782 0.170412
+v -0.291246 0.067797 0.167744
+v -0.214241 0.067797 0.167744
+v -0.291246 0.056963 0.166100
+v -0.214241 0.056963 0.166100
+vt 0.306049 0.488177
+vt 0.092448 0.519423
+vt 0.067655 0.516411
+vt 0.270128 0.485165
+vt 0.049219 0.513369
+vt 0.232794 0.482123
+vt 0.034123 0.510414
+vt 0.020864 0.507658
+vt 0.163092 0.476412
+vt 0.008587 0.505209
+vt 0.133302 0.473963
+vt 1.008587 0.505209
+vt 0.996728 0.503160
+vt 1.106812 0.471914
+vt 0.984865 0.501590
+vt 1.082707 0.470344
+vt 0.972628 0.500559
+vt 0.959652 0.500107
+vt 1.036012 0.468861
+vt 0.945535 0.500252
+vt 1.008811 0.469006
+vt 0.929810 0.500987
+vt 0.972352 0.469741
+vt 0.911918 0.502285
+vt 0.913273 0.471038
+vt 0.891203 0.504095
+vt 0.866959 0.506348
+vt 0.698338 0.475102
+vt 0.838607 0.508958
+vt 0.630535 0.477711
+vt 0.806049 0.511823
+vt 0.592448 0.480577
+vt 0.770128 0.514835
+vt 0.732794 0.517877
+vt 0.549219 0.486631
+vt 0.696491 0.520833
+vt 0.534123 0.489586
+vt 0.663092 0.523588
+vt 0.520864 0.492342
+vt 0.633302 0.526037
+vt 0.508586 0.494791
+vt 0.606812 0.528086
+vt 0.496728 0.496840
+vt 0.582707 0.529656
+vt 0.559695 0.530687
+vt 0.472628 0.499441
+vt 0.536012 0.531139
+vt 0.459652 0.499893
+vt 0.508810 0.530994
+vt 0.445535 0.499748
+vt 0.472352 0.530259
+vt 0.413273 0.528962
+vt 0.411918 0.497715
+vt 0.310810 0.527152
+vt 0.391202 0.495905
+vt 0.198337 0.524898
+vt 1.020864 0.507658
+vt 1.034123 0.510414
+vt 0.130535 0.522289
+vt 0.366959 0.493652
+vt 1.391202 0.495905
+vt 0.196491 0.479167
+vt 1.133302 0.473963
+vt 1.059695 0.469313
+vt 0.810811 0.472848
+vt 0.567655 0.483589
+vt 0.484865 0.498410
+vt 0.429810 0.499013
+vt 1.092448 0.519423
+vt 1.067655 0.516411
+vt 1.198337 0.524898
+vt 1.413273 0.528962
+vt 1.310810 0.527152
+vt 1.472352 0.530259
+vt 1.130535 0.522289
+vt 1.049219 0.513369
+vt 0.338607 0.491042
+vt 1.270128 0.485165
+vt 1.306049 0.488177
+vt 1.196491 0.479167
+vt 1.232794 0.482123
+vt 1.163092 0.476412
+vt 1.366959 0.493652
+vt 1.338607 0.491042
+vt 1.429810 0.499013
+vt 1.411918 0.497715
+vt 1.445535 0.499748
+vt 1.459652 0.499893
+usemtl Material.001
+s off
+f 1/1 2/2 4/3
+f 3/4 4/3 6/5
+f 5/6 6/5 8/7
+f 8/7 10/8 9/9
+f 10/8 12/10 11/11
+f 12/12 14/13 13/14
+f 14/13 16/15 15/16
+f 15/16 16/15 18/17
+f 18/17 20/18 19/19
+f 20/18 22/20 21/21
+f 22/20 24/22 23/23
+f 24/22 26/24 25/25
+f 25/25 26/24 28/26
+f 28/26 30/27 29/28
+f 30/27 32/29 31/30
+f 32/29 34/31 33/32
+f 33/32 34/31 36/33
+f 36/33 38/34 37/35
+f 38/34 40/36 39/37
+f 40/36 42/38 41/39
+f 42/38 44/40 43/41
+f 44/40 46/42 45/43
+f 45/43 46/42 48/44
+f 48/44 50/45 49/46
+f 50/45 52/47 51/48
+f 52/47 54/49 53/50
+f 53/50 54/49 56/51
+f 56/51 58/52 57/53
+f 58/52 60/54 59/55
+f 59/55 60/54 62/56
+f 26/24 10/57 8/58
+f 64/59 2/2 1/1
+f 61/60 62/56 64/59
+f 37/35 39/37 59/61
+f 3/4 1/1 4/3
+f 5/6 3/4 6/5
+f 7/62 5/6 8/7
+f 7/62 8/7 9/9
+f 9/9 10/8 11/11
+f 11/63 12/12 13/14
+f 13/14 14/13 15/16
+f 17/64 15/16 18/17
+f 17/64 18/17 19/19
+f 19/19 20/18 21/21
+f 21/21 22/20 23/23
+f 23/23 24/22 25/25
+f 27/65 25/25 28/26
+f 27/65 28/26 29/28
+f 29/28 30/27 31/30
+f 31/30 32/29 33/32
+f 35/66 33/32 36/33
+f 35/66 36/33 37/35
+f 37/35 38/34 39/37
+f 39/37 40/36 41/39
+f 41/39 42/38 43/41
+f 43/41 44/40 45/43
+f 47/67 45/43 48/44
+f 47/67 48/44 49/46
+f 49/46 50/45 51/48
+f 51/48 52/47 53/50
+f 55/68 53/50 56/51
+f 55/68 56/51 57/53
+f 57/53 58/52 59/55
+f 61/60 59/55 62/56
+f 2/69 34/31 4/70
+f 38/34 36/33 62/71
+f 58/72 40/36 60/73
+f 54/49 44/40 56/74
+f 50/45 48/44 52/47
+f 46/42 54/49 48/44
+f 14/13 22/20 20/18
+f 12/12 10/57 24/22
+f 2/69 64/75 34/31
+f 30/27 6/76 32/29
+f 26/24 8/58 28/26
+f 63/77 61/60 64/59
+f 64/75 36/33 34/31
+f 14/13 20/18 16/15
+f 44/40 42/38 56/74
+f 40/36 38/34 60/73
+f 38/34 62/71 60/73
+f 10/57 26/24 24/22
+f 48/44 54/49 52/47
+f 34/31 32/29 4/70
+f 8/58 6/76 28/26
+f 54/49 46/42 44/40
+f 20/18 18/17 16/15
+f 42/38 58/72 56/74
+f 63/77 64/59 1/1
+f 32/29 6/76 4/70
+f 12/12 24/22 14/13
+f 58/72 42/38 40/36
+f 36/33 64/75 62/71
+f 24/22 22/20 14/13
+f 6/76 30/27 28/26
+f 3/78 31/30 1/79
+f 7/80 29/28 5/81
+f 11/63 23/23 9/82
+f 15/16 21/21 13/14
+f 15/16 17/64 19/19
+f 61/83 63/84 35/66
+f 27/65 7/80 25/25
+f 41/39 55/85 57/86
+f 35/66 63/84 33/32
+f 53/87 55/85 43/41
+f 43/41 55/85 41/39
+f 47/67 53/87 45/43
+f 47/67 49/46 51/88
+f 59/61 39/37 57/86
+f 29/28 31/30 5/81
+f 7/80 27/65 29/28
+f 45/43 53/87 43/41
+f 47/67 51/88 53/87
+f 61/83 37/35 59/61
+f 31/30 3/78 5/81
+f 15/16 19/19 21/21
+f 21/21 23/23 13/14
+f 39/37 41/39 57/86
+f 7/80 9/82 25/25
+f 1/79 31/30 33/32
+f 23/23 11/63 13/14
+f 9/82 23/23 25/25
+f 37/35 61/83 35/66
+f 63/84 1/79 33/32
diff --git a/examples/pybullet/gym/pybullet_data/toys/prism.mtl b/examples/pybullet/gym/pybullet_data/toys/prism.mtl
new file mode 100644
index 000000000..ed3e8a1ae
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/toys/prism.mtl
@@ -0,0 +1,11 @@
+# Blender MTL File: 'shape_sort.blend'
+# Material Count: 1
+
+newmtl Material.003
+Ns 96.078431
+Ka 0.000000 0.000000 0.000000
+Kd 0.640000 0.007339 0.006282
+Ks 0.500000 0.500000 0.500000
+Ni 1.000000
+d 1.000000
+illum 2
diff --git a/examples/pybullet/gym/pybullet_data/toys/prism.obj b/examples/pybullet/gym/pybullet_data/toys/prism.obj
new file mode 100644
index 000000000..b8b0d6f0f
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/toys/prism.obj
@@ -0,0 +1,45 @@
+# Blender v2.71 (sub 0) OBJ File: 'shape_sort.blend'
+# www.blender.org
+mtllib prism.mtl
+o Cube.002_Cube.005
+v -0.233641 -0.103557 0.060897
+v -0.233641 -0.103557 -0.057063
+v -0.149383 -0.103557 -0.057063
+v -0.149383 -0.103557 0.060897
+v -0.233013 -0.039217 0.035115
+v -0.233013 -0.039217 -0.031280
+v -0.150011 -0.039217 -0.031280
+v -0.150011 -0.039217 0.035115
+vt 0.780473 0.523151
+vt 0.999041 -0.022288
+vt -0.000959 -0.022288
+vt 1.896793 0.523151
+vt 1.904244 -0.022288
+vt 0.904244 -0.022288
+vt 0.217610 0.523151
+vt -0.088305 0.523151
+vt -0.095756 -0.022288
+vt 0.904244 1.999041
+vt 1.904244 1.999041
+vt 1.904244 0.999041
+vt 0.896793 0.780473
+vt -0.088305 0.780473
+vt -0.088305 0.217610
+vt 0.911695 0.523151
+vt 0.896793 0.523151
+vt 0.904244 0.999041
+vt 0.896793 0.217610
+usemtl Material.003
+s off
+f 6/1 2/2 1/3
+f 7/4 3/5 2/6
+f 8/7 4/3 3/2
+f 5/8 1/9 4/6
+f 2/10 3/11 4/12
+f 7/13 6/14 5/15
+f 5/7 6/1 1/3
+f 6/16 7/4 2/6
+f 7/1 8/7 3/2
+f 8/17 5/8 4/6
+f 1/18 2/10 4/12
+f 8/19 7/13 5/15
diff --git a/examples/pybullet/gym/pybullet_data/toys/shape_sorter.mtl b/examples/pybullet/gym/pybullet_data/toys/shape_sorter.mtl
new file mode 100644
index 000000000..4e5935766
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/toys/shape_sorter.mtl
@@ -0,0 +1,21 @@
+# Blender MTL File: 'shape_sort.blend'
+# Material Count: 2
+
+newmtl Material.004
+Ns 96.078431
+Ka 0.000000 0.000000 0.000000
+Kd 0.640000 0.640000 0.640000
+Ks 0.500000 0.500000 0.500000
+Ni 1.000000
+d 1.000000
+illum 2
+map_Kd E:\develop\bullet3\data\table\table.png
+
+newmtl Material.004_NONE
+Ns 96.078431
+Ka 0.000000 0.000000 0.000000
+Kd 0.640000 0.640000 0.640000
+Ks 0.500000 0.500000 0.500000
+Ni 1.000000
+d 1.000000
+illum 2
diff --git a/examples/pybullet/gym/pybullet_data/toys/shape_sorter.obj b/examples/pybullet/gym/pybullet_data/toys/shape_sorter.obj
new file mode 100644
index 000000000..a8b6dfd84
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/toys/shape_sorter.obj
@@ -0,0 +1,400 @@
+# Blender v2.71 (sub 0) OBJ File: 'shape_sort.blend'
+# www.blender.org
+mtllib shape_sorter.mtl
+o Cube
+v -0.200000 0.200000 -0.200000
+v -0.200000 -0.200000 -0.200000
+v -0.200000 0.037707 -0.055248
+v -0.200000 0.037707 -0.124929
+v -0.200000 0.130615 -0.124929
+v -0.200000 0.130615 -0.055248
+v -0.200000 -0.200000 0.200000
+v -0.200000 0.014752 0.076444
+v -0.200000 -0.109627 0.071917
+v -0.200000 -0.109627 -0.068083
+v -0.200000 -0.033266 -0.037483
+v -0.200000 -0.033266 0.041318
+v -0.200000 0.015905 0.070592
+v -0.200000 0.019320 0.064964
+v -0.200000 0.024864 0.059777
+v -0.200000 0.032326 0.055231
+v -0.200000 0.041418 0.051500
+v -0.200000 0.051791 0.048728
+v -0.200000 0.063047 0.047021
+v -0.200000 0.074753 0.046444
+v -0.200000 0.086458 0.047021
+v -0.200000 0.097713 0.048728
+v -0.200000 0.108087 0.051500
+v -0.200000 0.117179 0.055231
+v -0.200000 0.124641 0.059777
+v -0.200000 0.130185 0.064964
+v -0.200000 0.133600 0.070592
+v -0.200000 0.200000 0.200000
+v 0.200000 0.200000 -0.200000
+v 0.200000 -0.200000 -0.200000
+v -0.179938 0.037707 -0.055248
+v -0.179938 0.037707 -0.124929
+v -0.179938 0.130615 -0.124929
+v -0.179938 0.130615 -0.055248
+v -0.179938 0.200000 0.179938
+v -0.179938 0.200000 -0.179938
+v 0.179938 0.200000 -0.179938
+v 0.200000 -0.200000 0.200000
+v -0.200000 0.134752 0.076444
+v -0.200000 0.133600 0.082297
+v -0.200000 0.130185 0.087925
+v -0.200000 0.124641 0.093111
+v -0.200000 0.117179 0.097657
+v -0.200000 0.108087 0.101388
+v -0.200000 0.097714 0.104161
+v -0.200000 0.086458 0.105868
+v -0.200000 0.074753 0.106444
+v -0.200000 0.063047 0.105868
+v -0.200000 0.051792 0.104161
+v -0.200000 0.041418 0.101388
+v -0.200000 0.032326 0.097657
+v -0.200000 0.024864 0.093111
+v -0.200000 0.019320 0.087925
+v -0.200000 0.015905 0.082297
+v -0.179938 0.014752 0.076444
+v -0.179938 0.015905 0.070592
+v -0.179938 0.019320 0.064964
+v -0.179938 0.024864 0.059777
+v -0.179938 0.032326 0.055231
+v -0.179938 0.041418 0.051500
+v -0.179938 0.051791 0.048728
+v -0.179938 0.063047 0.047021
+v -0.179938 0.074752 0.046444
+v -0.179938 0.086458 0.047021
+v -0.179938 0.097713 0.048728
+v -0.179938 0.108087 0.051500
+v -0.179938 0.117179 0.055231
+v -0.179938 0.124641 0.059777
+v -0.179938 0.130185 0.064964
+v -0.179938 0.133600 0.070592
+v 0.200000 0.200000 0.200000
+v -0.179938 -0.185168 -0.179938
+v 0.179938 0.200000 0.179938
+v -0.179938 -0.185168 0.179938
+v -0.179938 0.015905 0.082297
+v -0.179938 0.019320 0.087925
+v -0.179938 0.024864 0.093111
+v -0.179938 0.032326 0.097657
+v -0.179938 0.041418 0.101388
+v -0.179938 0.051791 0.104161
+v -0.179938 0.063047 0.105868
+v -0.179938 0.074753 0.106444
+v -0.179938 0.086458 0.105868
+v -0.179938 0.097714 0.104161
+v -0.179938 0.108087 0.101388
+v -0.179938 0.117179 0.097657
+v -0.179938 0.124641 0.093111
+v -0.179938 0.130185 0.087925
+v -0.179938 0.133600 0.082297
+v -0.179938 0.134752 0.076444
+v 0.179938 -0.185168 -0.179938
+v -0.179938 -0.109627 0.071917
+v -0.179938 -0.033266 0.041318
+v -0.179938 -0.033266 -0.037483
+v -0.179938 -0.109627 -0.068083
+v 0.179938 -0.185168 0.179938
+vt 0.337529 0.596545
+vt 0.387821 0.596545
+vt 0.283346 0.663317
+vt 0.379674 0.386434
+vt 0.283346 0.278382
+vt 0.572047 0.278383
+vt 0.387821 0.507137
+vt 0.461218 0.542787
+vt 0.461634 0.554051
+vt 0.372847 0.595850
+vt 0.372847 0.369934
+vt 0.391646 0.506894
+vt 0.343069 0.506894
+vt 0.337529 0.507137
+vt 0.343069 0.593253
+vt 0.391646 0.593253
+vt 0.304720 0.657748
+vt 0.378834 0.597878
+vt 0.542284 0.369934
+vt 0.572047 0.663317
+vt 0.504523 0.542787
+vt 0.504107 0.531523
+vt 0.482871 0.485047
+vt 0.483455 0.485557
+vt 0.479374 0.486628
+vt 0.478646 0.486156
+vt 0.475451 0.489802
+vt 0.474584 0.489442
+vt 0.471835 0.494956
+vt 0.467560 0.501959
+vt 0.470841 0.494778
+vt 0.468666 0.501892
+vt 0.466065 0.510343
+vt 0.462866 0.520691
+vt 0.464867 0.510708
+vt 0.461634 0.531522
+vt 0.464132 0.519985
+vt 0.462942 0.530448
+vt 0.462540 0.541328
+vt 0.462942 0.552208
+vt 0.462866 0.564883
+vt 0.464132 0.562671
+vt 0.466065 0.572313
+vt 0.467560 0.583615
+vt 0.464867 0.574866
+vt 0.468666 0.580764
+vt 0.471835 0.587700
+vt 0.474584 0.596132
+vt 0.470841 0.590796
+vt 0.475451 0.592854
+vt 0.479374 0.596028
+vt 0.542284 0.595850
+vt 0.304720 0.299726
+vt 0.555605 0.657748
+vt 0.504369 0.541328
+vt 0.503967 0.552209
+vt 0.378834 0.375736
+vt 0.383150 0.371876
+vt 0.555605 0.299726
+vt 0.478646 0.599418
+vt 0.483455 0.597099
+vt 0.482870 0.600527
+vt 0.487535 0.596028
+vt 0.487095 0.599418
+vt 0.491458 0.592854
+vt 0.491157 0.596132
+vt 0.495074 0.587700
+vt 0.494900 0.590796
+vt 0.498243 0.580764
+vt 0.498181 0.583615
+vt 0.500844 0.572313
+vt 0.502875 0.564883
+vt 0.500874 0.574866
+vt 0.502777 0.562671
+vt 0.504107 0.554052
+vt 0.503967 0.530448
+vt 0.502777 0.519985
+vt 0.502875 0.520691
+vt 0.500844 0.510343
+vt 0.500874 0.510708
+vt 0.498243 0.501892
+vt 0.498181 0.501959
+vt 0.495074 0.494956
+vt 0.494900 0.494778
+vt 0.491458 0.489802
+vt 0.491157 0.489442
+vt 0.487535 0.486628
+vt 0.487095 0.486156
+vt 0.534501 0.375736
+vt 0.534501 0.597878
+vt 0.478009 0.388189
+vt 0.400119 0.458779
+vt 0.457642 0.460616
+vt 0.480077 0.374691
+vt 0.459415 0.445604
+vt 0.404578 0.442049
+vt 0.000000 0.000000
+usemtl Material.004
+s off
+f 5/1 6/2 1/3
+f 10/4 2/5 7/6
+f 3/7 20/8 21/9
+f 29/10 30/11 2/5
+f 3/7 31/12 32/13
+f 4/14 32/13 33/15
+f 34/16 6/2 5/1
+f 36/17 37/18 29/10
+f 31/12 3/7 6/2
+f 2/5 30/11 38/19
+f 28/20 47/21 48/22
+f 8/23 55/24 56/25
+f 13/26 56/25 57/27
+f 14/28 57/27 58/29
+f 16/30 15/31 58/29
+f 16/30 59/32 60/33
+f 18/34 17/35 60/33
+f 19/36 18/34 61/37
+f 20/8 19/36 62/38
+f 20/8 63/39 64/40
+f 22/41 21/9 64/40
+f 22/41 65/42 66/43
+f 24/44 23/45 66/43
+f 24/44 67/46 68/47
+f 26/48 25/49 68/47
+f 26/48 69/50 70/51
+f 71/52 38/19 30/11
+f 32/13 31/12 72/53
+f 71/52 29/10 37/18
+f 35/54 82/55 83/56
+f 36/17 72/53 91/57
+f 62/38 61/37 31/12
+f 95/58 74/59 72/53
+f 28/20 7/6 38/19
+f 27/60 70/51 90/61
+f 39/62 90/61 89/63
+f 40/64 89/63 88/65
+f 41/66 88/65 87/67
+f 42/68 87/67 86/69
+f 43/70 86/69 85/71
+f 45/72 44/73 85/71
+f 45/72 84/74 83/56
+f 46/75 83/56 82/55
+f 47/21 82/55 81/76
+f 48/22 81/76 80/77
+f 49/78 80/77 79/79
+f 50/80 79/79 78/81
+f 51/82 78/81 77/83
+f 52/84 77/83 76/85
+f 53/86 76/85 75/87
+f 54/88 75/87 55/24
+f 37/18 91/57 96/89
+f 73/90 96/89 74/59
+f 74/59 96/89 91/57
+f 2/5 3/7 4/14
+f 7/6 8/23 9/91
+f 1/3 2/5 4/14
+f 1/3 4/14 5/1
+f 10/4 11/92 3/7
+f 9/91 10/4 7/6
+f 10/4 3/7 2/5
+f 12/93 16/30 11/92
+f 28/20 1/3 6/2
+f 12/93 9/91 8/23
+f 27/60 28/20 6/2
+f 14/28 15/31 12/93
+f 12/93 8/23 13/26
+f 26/48 27/60 6/2
+f 12/93 13/26 14/28
+f 25/49 26/48 6/2
+f 1/3 29/10 2/5
+f 24/44 25/49 6/2
+f 3/7 16/30 17/35
+f 23/45 24/44 6/2
+f 3/7 17/35 18/34
+f 22/41 23/45 6/2
+f 3/7 18/34 19/36
+f 21/9 22/41 6/2
+f 16/30 3/7 11/92
+f 6/2 3/7 21/9
+f 12/93 15/31 16/30
+f 4/14 3/7 32/13
+f 3/7 19/36 20/8
+f 5/1 4/14 33/15
+f 33/15 34/16 5/1
+f 1/3 28/20 35/54
+f 34/16 31/12 6/2
+f 1/3 35/54 36/17
+f 36/17 29/10 1/3
+f 7/6 2/5 38/19
+f 28/20 27/60 39/62
+f 54/88 8/23 7/6
+f 28/20 39/62 40/64
+f 53/86 54/88 7/6
+f 28/20 40/64 41/66
+f 52/84 53/86 7/6
+f 28/20 41/66 42/68
+f 51/82 52/84 7/6
+f 28/20 42/68 43/70
+f 50/80 51/82 7/6
+f 28/20 43/70 44/73
+f 49/78 50/80 7/6
+f 44/73 45/72 28/20
+f 7/6 28/20 49/78
+f 28/20 45/72 46/75
+f 14/28 13/26 57/27
+f 13/26 8/23 56/25
+f 28/20 48/22 49/78
+f 28/20 46/75 47/21
+f 15/31 14/28 58/29
+f 59/32 16/30 58/29
+f 17/35 16/30 60/33
+f 61/37 18/34 60/33
+f 62/38 19/36 61/37
+f 63/39 20/8 62/38
+f 21/9 20/8 64/40
+f 65/42 22/41 64/40
+f 23/45 22/41 66/43
+f 67/46 24/44 66/43
+f 25/49 24/44 68/47
+f 69/50 26/48 68/47
+f 27/60 26/48 70/51
+f 29/10 71/52 30/11
+f 36/17 34/16 33/15
+f 35/54 28/20 71/52
+f 33/15 32/13 36/17
+f 74/59 55/24 75/87
+f 32/13 72/53 36/17
+f 73/90 35/54 71/52
+f 71/52 37/18 73/90
+f 36/17 35/54 34/16
+f 74/59 75/87 76/85
+f 31/12 63/39 62/38
+f 74/59 76/85 77/83
+f 35/54 90/61 34/16
+f 74/59 77/83 78/81
+f 88/65 89/63 35/54
+f 78/81 79/79 74/59
+f 87/67 88/65 35/54
+f 74/59 79/79 80/77
+f 86/69 87/67 35/54
+f 35/54 74/59 80/77
+f 85/71 86/69 35/54
+f 35/54 80/77 81/76
+f 84/74 85/71 35/54
+f 35/54 81/76 82/55
+f 83/56 84/74 35/54
+f 37/18 36/17 91/57
+f 35/54 89/63 90/61
+f 34/16 90/61 70/51
+f 92/94 93/95 55/24
+f 92/94 55/24 74/59
+f 34/16 70/51 69/50
+f 59/32 94/96 31/12
+f 34/16 69/50 68/47
+f 31/12 34/16 64/40
+f 34/16 68/47 67/46
+f 93/95 57/27 56/25
+f 34/16 67/46 66/43
+f 93/95 58/29 57/27
+f 34/16 66/43 65/42
+f 93/95 59/32 58/29
+f 34/16 65/42 64/40
+f 93/95 94/96 59/32
+f 63/39 31/12 64/40
+f 61/37 60/33 31/12
+f 93/95 56/25 55/24
+f 94/96 95/58 31/12
+f 60/33 59/32 31/12
+f 39/62 27/60 90/61
+f 95/58 92/94 74/59
+f 71/52 28/20 38/19
+f 95/58 72/53 31/12
+f 40/64 39/62 89/63
+f 41/66 40/64 88/65
+f 42/68 41/66 87/67
+f 43/70 42/68 86/69
+f 44/73 43/70 85/71
+f 84/74 45/72 85/71
+f 46/75 45/72 83/56
+f 47/21 46/75 82/55
+f 48/22 47/21 81/76
+f 49/78 48/22 80/77
+f 50/80 49/78 79/79
+f 51/82 50/80 78/81
+f 52/84 51/82 77/83
+f 53/86 52/84 76/85
+f 54/88 53/86 75/87
+f 8/23 54/88 55/24
+f 73/90 37/18 96/89
+f 35/54 73/90 74/59
+f 72/53 74/59 91/57
+usemtl Material.004_NONE
+f 10/97 95/97 94/97
+f 93/97 12/97 11/97
+f 10/97 9/97 92/97
+f 92/97 9/97 12/97
+f 11/97 10/97 94/97
+f 94/97 93/97 11/97
+f 95/97 10/97 92/97
+f 93/97 92/97 12/97
diff --git a/examples/pybullet/gym/pybullet_data/urdf/mug.obj b/examples/pybullet/gym/pybullet_data/urdf/mug.obj
new file mode 100644
index 000000000..4fb8d9186
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/urdf/mug.obj
@@ -0,0 +1,1352 @@
+# Blender v2.79 (sub 0) OBJ File: 'mug.blend'
+# www.blender.org
+o mug_Cylinder.002
+v 0.000000 0.000000 0.000000
+v 0.000000 0.032649 0.008628
+v 0.000000 -0.000000 0.008628
+v 0.008450 0.031536 0.008628
+v 0.016324 0.028275 0.008628
+v 0.023086 0.023086 0.008628
+v 0.028275 0.016324 0.008628
+v 0.031536 0.008450 0.008628
+v 0.032649 0.000000 0.008628
+v 0.031536 -0.008450 0.008628
+v 0.028275 -0.016324 0.008628
+v 0.023086 -0.023086 0.008628
+v 0.016324 -0.028275 0.008628
+v 0.008450 -0.031536 0.008628
+v 0.000000 -0.032649 0.008628
+v -0.008450 -0.031536 0.008628
+v -0.016324 -0.028275 0.008628
+v -0.023086 -0.023086 0.008628
+v -0.028275 -0.016324 0.008628
+v -0.031536 -0.008450 0.008628
+v -0.032649 -0.000000 0.008628
+v -0.031536 0.008450 0.008628
+v -0.028275 0.016324 0.008628
+v -0.023086 0.023086 0.008628
+v -0.016324 0.028275 0.008628
+v -0.008450 0.031536 0.008628
+v 0.000000 0.041000 0.097171
+v 0.000000 0.038146 0.100000
+v 0.000000 0.040164 0.099171
+v 0.009873 0.036846 0.100000
+v 0.010612 0.039603 0.097171
+v 0.010395 0.038796 0.099171
+v 0.019073 0.033036 0.100000
+v 0.020500 0.035507 0.097171
+v 0.020082 0.034783 0.099171
+v 0.026973 0.026973 0.100000
+v 0.028991 0.028991 0.097171
+v 0.028400 0.028400 0.099171
+v 0.033036 0.019073 0.100000
+v 0.035507 0.020500 0.097171
+v 0.034783 0.020082 0.099171
+v 0.036846 0.009873 0.100000
+v 0.039603 0.010612 0.097171
+v 0.038796 0.010395 0.099171
+v 0.038146 0.000000 0.100000
+v 0.041000 0.000000 0.097171
+v 0.040164 0.000000 0.099171
+v 0.036846 -0.009873 0.100000
+v 0.039603 -0.010612 0.097171
+v 0.038796 -0.010395 0.099171
+v 0.033036 -0.019073 0.100000
+v 0.035507 -0.020500 0.097171
+v 0.034783 -0.020082 0.099171
+v 0.026974 -0.026973 0.100000
+v 0.028991 -0.028991 0.097171
+v 0.028400 -0.028400 0.099171
+v 0.019073 -0.033036 0.100000
+v 0.020500 -0.035507 0.097171
+v 0.020082 -0.034783 0.099171
+v 0.009873 -0.036846 0.100000
+v 0.010612 -0.039603 0.097171
+v 0.010395 -0.038796 0.099171
+v 0.000000 -0.038146 0.100000
+v 0.000000 -0.041000 0.097171
+v 0.000000 -0.040164 0.099171
+v -0.009873 -0.036846 0.100000
+v -0.010612 -0.039603 0.097171
+v -0.010395 -0.038796 0.099171
+v -0.019073 -0.033036 0.100000
+v -0.020500 -0.035507 0.097171
+v -0.020082 -0.034783 0.099171
+v -0.026973 -0.026974 0.100000
+v -0.028991 -0.028991 0.097171
+v -0.028400 -0.028400 0.099171
+v -0.033036 -0.019073 0.100000
+v -0.035507 -0.020500 0.097171
+v -0.034783 -0.020082 0.099171
+v -0.036846 -0.009873 0.100000
+v -0.039603 -0.010612 0.097171
+v -0.038796 -0.010395 0.099171
+v -0.038146 -0.000000 0.100000
+v -0.041000 -0.000000 0.097171
+v -0.040164 -0.000000 0.099171
+v -0.036846 0.009873 0.100000
+v -0.039603 0.010612 0.097171
+v -0.038796 0.010395 0.099171
+v -0.033036 0.019073 0.100000
+v -0.035507 0.020500 0.097171
+v -0.034783 0.020082 0.099171
+v -0.026974 0.026973 0.100000
+v -0.028991 0.028991 0.097171
+v -0.028400 0.028400 0.099171
+v -0.019073 0.033036 0.100000
+v -0.020500 0.035507 0.097171
+v -0.020082 0.034783 0.099171
+v -0.009873 0.036846 0.100000
+v -0.010612 0.039603 0.097171
+v -0.010395 0.038796 0.099171
+v 0.000000 0.035502 0.100000
+v 0.000000 0.032649 0.097171
+v 0.000000 0.033484 0.099171
+v 0.008450 0.031536 0.097171
+v 0.009189 0.034293 0.100000
+v 0.008666 0.032344 0.099171
+v 0.016324 0.028275 0.097171
+v 0.017751 0.030746 0.100000
+v 0.016742 0.028998 0.099171
+v 0.023086 0.023086 0.097171
+v 0.025104 0.025104 0.100000
+v 0.023677 0.023677 0.099171
+v 0.028275 0.016324 0.097171
+v 0.030746 0.017751 0.100000
+v 0.028998 0.016742 0.099171
+v 0.031536 0.008450 0.097171
+v 0.034293 0.009189 0.100000
+v 0.032344 0.008666 0.099171
+v 0.032649 0.000000 0.097171
+v 0.035502 0.000000 0.100000
+v 0.033484 0.000000 0.099171
+v 0.031536 -0.008450 0.097171
+v 0.034293 -0.009189 0.100000
+v 0.032344 -0.008666 0.099171
+v 0.028275 -0.016324 0.097171
+v 0.030746 -0.017751 0.100000
+v 0.028998 -0.016742 0.099171
+v 0.023086 -0.023086 0.097171
+v 0.025104 -0.025104 0.100000
+v 0.023677 -0.023677 0.099171
+v 0.016324 -0.028275 0.097171
+v 0.017751 -0.030746 0.100000
+v 0.016742 -0.028998 0.099171
+v 0.008450 -0.031536 0.097171
+v 0.009189 -0.034293 0.100000
+v 0.008666 -0.032343 0.099171
+v 0.000000 -0.032649 0.097171
+v 0.000000 -0.035502 0.100000
+v 0.000000 -0.033484 0.099171
+v -0.008450 -0.031536 0.097171
+v -0.009189 -0.034293 0.100000
+v -0.008666 -0.032344 0.099171
+v -0.016324 -0.028275 0.097171
+v -0.017751 -0.030746 0.100000
+v -0.016742 -0.028998 0.099171
+v -0.023086 -0.023086 0.097171
+v -0.025104 -0.025104 0.100000
+v -0.023677 -0.023677 0.099171
+v -0.028275 -0.016324 0.097171
+v -0.030746 -0.017751 0.100000
+v -0.028998 -0.016742 0.099171
+v -0.031536 -0.008450 0.097171
+v -0.034293 -0.009189 0.100000
+v -0.032343 -0.008666 0.099171
+v -0.032649 -0.000000 0.097171
+v -0.035502 -0.000000 0.100000
+v -0.033484 -0.000000 0.099171
+v -0.031536 0.008450 0.097171
+v -0.034293 0.009189 0.100000
+v -0.032344 0.008666 0.099171
+v -0.028275 0.016324 0.097171
+v -0.030746 0.017751 0.100000
+v -0.028998 0.016742 0.099171
+v -0.023086 0.023086 0.097171
+v -0.025104 0.025104 0.100000
+v -0.023677 0.023677 0.099171
+v -0.016324 0.028275 0.097171
+v -0.017751 0.030746 0.100000
+v -0.016742 0.028998 0.099171
+v -0.008450 0.031536 0.097171
+v -0.009189 0.034293 0.100000
+v -0.008666 0.032344 0.099171
+v 0.000000 0.039460 0.000000
+v 0.000000 0.041000 0.001527
+v 0.000000 0.040549 0.000447
+v 0.010612 0.039603 0.001527
+v 0.010213 0.038116 0.000000
+v 0.010495 0.039167 0.000447
+v 0.020500 0.035507 0.001527
+v 0.019730 0.034174 0.000000
+v 0.020275 0.035116 0.000447
+v 0.028991 0.028991 0.001527
+v 0.027903 0.027903 0.000000
+v 0.028672 0.028672 0.000447
+v 0.035507 0.020500 0.001527
+v 0.034174 0.019730 0.000000
+v 0.035116 0.020275 0.000447
+v 0.039603 0.010612 0.001527
+v 0.038116 0.010213 0.000000
+v 0.039167 0.010495 0.000447
+v 0.041000 0.000000 0.001527
+v 0.039460 0.000000 0.000000
+v 0.040549 0.000000 0.000447
+v 0.039603 -0.010612 0.001527
+v 0.038116 -0.010213 0.000000
+v 0.039167 -0.010495 0.000447
+v 0.035507 -0.020500 0.001527
+v 0.034174 -0.019730 0.000000
+v 0.035116 -0.020275 0.000447
+v 0.028991 -0.028991 0.001527
+v 0.027903 -0.027903 0.000000
+v 0.028672 -0.028672 0.000447
+v 0.020500 -0.035507 0.001527
+v 0.019730 -0.034174 0.000000
+v 0.020275 -0.035116 0.000447
+v 0.010612 -0.039603 0.001527
+v 0.010213 -0.038116 0.000000
+v 0.010495 -0.039167 0.000447
+v 0.000000 -0.041000 0.001527
+v 0.000000 -0.039460 0.000000
+v 0.000000 -0.040549 0.000447
+v -0.010612 -0.039603 0.001527
+v -0.010213 -0.038116 0.000000
+v -0.010495 -0.039167 0.000447
+v -0.020500 -0.035507 0.001527
+v -0.019730 -0.034174 0.000000
+v -0.020274 -0.035116 0.000447
+v -0.028991 -0.028991 0.001527
+v -0.027903 -0.027903 0.000000
+v -0.028672 -0.028672 0.000447
+v -0.035507 -0.020500 0.001527
+v -0.034174 -0.019730 0.000000
+v -0.035116 -0.020275 0.000447
+v -0.039603 -0.010612 0.001527
+v -0.038116 -0.010213 0.000000
+v -0.039167 -0.010495 0.000447
+v -0.041000 -0.000000 0.001527
+v -0.039460 -0.000000 0.000000
+v -0.040549 -0.000000 0.000447
+v -0.039603 0.010612 0.001527
+v -0.038116 0.010213 0.000000
+v -0.039167 0.010495 0.000447
+v -0.035507 0.020500 0.001527
+v -0.034174 0.019730 0.000000
+v -0.035116 0.020274 0.000447
+v -0.028991 0.028991 0.001527
+v -0.027903 0.027903 0.000000
+v -0.028673 0.028672 0.000447
+v -0.020500 0.035507 0.001527
+v -0.019730 0.034174 0.000000
+v -0.020275 0.035116 0.000447
+v -0.010612 0.039603 0.001527
+v -0.010213 0.038116 0.000000
+v -0.010495 0.039167 0.000447
+v -0.003627 0.038527 0.081890
+v -0.005507 0.038527 0.080294
+v -0.005059 0.038527 0.081441
+v 0.005507 0.038527 0.080294
+v 0.003627 0.038527 0.081890
+v 0.005059 0.038527 0.081441
+v -0.003600 0.080633 0.059432
+v -0.005474 0.079067 0.059309
+v -0.005036 0.080199 0.059398
+v -0.005474 0.056411 0.081965
+v -0.003600 0.056534 0.083531
+v -0.005036 0.056500 0.083097
+v -0.005477 0.077925 0.066460
+v -0.003600 0.079455 0.066958
+v -0.005046 0.079026 0.066818
+v -0.005477 0.074748 0.072697
+v -0.003600 0.076049 0.073642
+v -0.005046 0.075684 0.073377
+v -0.005477 0.069799 0.077646
+v -0.003600 0.070744 0.078947
+v -0.005046 0.070479 0.078582
+v -0.005477 0.063562 0.080823
+v -0.003600 0.064059 0.082353
+v -0.005046 0.063920 0.081924
+v 0.003600 0.056534 0.083531
+v 0.005474 0.056411 0.081965
+v 0.005036 0.056500 0.083097
+v 0.005474 0.079067 0.059309
+v 0.003600 0.080633 0.059432
+v 0.005036 0.080199 0.059398
+v 0.003600 0.064059 0.082353
+v 0.005477 0.063562 0.080823
+v 0.005046 0.063920 0.081924
+v 0.003600 0.070744 0.078947
+v 0.005477 0.069799 0.077646
+v 0.005046 0.070479 0.078582
+v 0.003600 0.076049 0.073642
+v 0.005477 0.074748 0.072697
+v 0.005046 0.075684 0.073377
+v 0.003600 0.079455 0.066958
+v 0.005477 0.077925 0.066460
+v 0.005046 0.079026 0.066818
+v -0.003631 0.038527 0.018578
+v -0.005510 0.038527 0.020185
+v -0.005068 0.038527 0.019031
+v 0.003631 0.038527 0.018578
+v 0.005510 0.038527 0.020185
+v 0.005068 0.038527 0.019031
+v -0.003599 0.080629 0.050000
+v -0.005471 0.079081 0.050000
+v -0.005023 0.080207 0.050000
+v 0.005471 0.079081 0.050000
+v 0.003599 0.080629 0.050000
+v 0.005023 0.080207 0.050000
+v -0.005474 0.079067 0.040691
+v -0.003600 0.080633 0.040568
+v -0.005036 0.080199 0.040602
+v -0.003600 0.056534 0.016469
+v -0.005474 0.056411 0.018035
+v -0.005036 0.056500 0.016903
+v -0.003600 0.079455 0.033042
+v -0.005477 0.077925 0.033540
+v -0.005046 0.079026 0.033182
+v -0.003600 0.076049 0.026358
+v -0.005477 0.074748 0.027303
+v -0.005046 0.075684 0.026623
+v -0.003600 0.070744 0.021053
+v -0.005477 0.069799 0.022354
+v -0.005046 0.070479 0.021418
+v -0.003600 0.064059 0.017647
+v -0.005477 0.063562 0.019177
+v -0.005046 0.063920 0.018076
+v 0.005474 0.056411 0.018035
+v 0.003600 0.056534 0.016469
+v 0.005036 0.056500 0.016903
+v 0.003600 0.080633 0.040568
+v 0.005474 0.079067 0.040691
+v 0.005036 0.080199 0.040602
+v 0.005477 0.063562 0.019177
+v 0.003600 0.064059 0.017647
+v 0.005046 0.063920 0.018076
+v 0.005477 0.069799 0.022354
+v 0.003600 0.070744 0.021053
+v 0.005046 0.070479 0.021418
+v 0.005477 0.074748 0.027303
+v 0.003600 0.076049 0.026358
+v 0.005046 0.075684 0.026623
+v 0.005477 0.077925 0.033540
+v 0.003600 0.079455 0.033042
+v 0.005046 0.079026 0.033182
+v 0.003628 0.038527 0.070184
+v 0.005471 0.038527 0.072262
+v 0.004983 0.038527 0.070850
+v -0.003627 0.038527 0.070184
+v -0.005471 0.038527 0.072267
+v -0.004983 0.038527 0.070852
+v -0.005472 0.075182 0.050000
+v -0.003602 0.073666 0.050000
+v -0.005029 0.074082 0.050000
+v 0.003602 0.073666 0.050000
+v 0.005472 0.075177 0.050000
+v 0.005030 0.074081 0.050000
+v 0.005464 0.074276 0.065273
+v 0.003594 0.072835 0.064794
+v 0.004992 0.073209 0.064919
+v 0.003600 0.073692 0.058871
+v 0.005468 0.075191 0.059001
+v 0.005015 0.074095 0.058907
+v -0.005468 0.075196 0.059001
+v -0.003599 0.073692 0.058871
+v -0.005014 0.074096 0.058907
+v -0.003593 0.072835 0.064794
+v -0.005464 0.074281 0.065274
+v -0.004991 0.073210 0.064919
+v 0.005464 0.071645 0.070442
+v 0.003594 0.070415 0.069549
+v 0.004994 0.070735 0.069781
+v -0.003593 0.070415 0.069549
+v -0.005464 0.071649 0.070445
+v -0.004993 0.070735 0.069782
+v 0.005464 0.067544 0.074543
+v 0.003594 0.066651 0.073313
+v 0.004994 0.066883 0.073633
+v -0.003593 0.066650 0.073313
+v -0.005464 0.067547 0.074547
+v -0.004993 0.066884 0.073634
+v 0.005464 0.062374 0.077175
+v 0.003594 0.061896 0.075733
+v 0.004992 0.062021 0.076107
+v -0.003593 0.061896 0.075733
+v -0.005464 0.062376 0.077179
+v -0.004991 0.062021 0.076108
+v 0.005468 0.056103 0.078090
+v 0.003600 0.055974 0.076590
+v 0.005015 0.056009 0.076993
+v -0.003599 0.055974 0.076590
+v -0.005468 0.056103 0.078095
+v -0.005014 0.056010 0.076994
+v 0.005472 0.038527 0.027531
+v 0.003627 0.038527 0.029560
+v 0.004985 0.038527 0.028916
+v -0.003626 0.038527 0.029560
+v -0.005472 0.038527 0.027526
+v -0.004984 0.038527 0.028915
+v 0.003594 0.072835 0.035206
+v 0.005464 0.074276 0.034727
+v 0.004992 0.073209 0.035081
+v 0.005468 0.075191 0.040999
+v 0.003600 0.073692 0.041129
+v 0.005015 0.074095 0.041093
+v -0.003599 0.073692 0.041129
+v -0.005468 0.075196 0.040999
+v -0.005014 0.074096 0.041093
+v -0.005464 0.074281 0.034726
+v -0.003593 0.072835 0.035206
+v -0.004991 0.073210 0.035081
+v 0.003594 0.070415 0.030451
+v 0.005464 0.071645 0.029558
+v 0.004994 0.070735 0.030219
+v -0.005464 0.071649 0.029555
+v -0.003593 0.070415 0.030451
+v -0.004993 0.070735 0.030218
+v 0.003594 0.066651 0.026687
+v 0.005464 0.067544 0.025457
+v 0.004994 0.066883 0.026367
+v -0.005464 0.067547 0.025453
+v -0.003593 0.066650 0.026687
+v -0.004993 0.066884 0.026366
+v 0.003594 0.061896 0.024267
+v 0.005464 0.062374 0.022825
+v 0.004992 0.062021 0.023893
+v -0.005464 0.062376 0.022821
+v -0.003593 0.061896 0.024267
+v -0.004991 0.062021 0.023892
+v 0.003600 0.055974 0.023410
+v 0.005468 0.056103 0.021910
+v 0.005015 0.056009 0.023007
+v -0.005468 0.056103 0.021905
+v -0.003599 0.055974 0.023410
+v -0.005014 0.056010 0.023006
+v -0.005477 0.045795 0.081957
+v -0.003603 0.045916 0.083535
+v -0.005045 0.045879 0.083093
+v 0.005477 0.045795 0.081957
+v 0.003603 0.045916 0.083535
+v 0.005045 0.045879 0.083093
+v -0.003608 0.045257 0.076495
+v -0.005460 0.045676 0.078027
+v -0.004995 0.045382 0.076898
+v 0.003609 0.045257 0.076495
+v 0.005460 0.045676 0.078022
+v 0.004996 0.045381 0.076897
+v -0.003604 0.045959 0.016460
+v -0.005478 0.045808 0.018049
+v -0.005052 0.045914 0.016909
+v 0.003604 0.045959 0.016460
+v 0.005478 0.045808 0.018049
+v 0.005052 0.045914 0.016909
+v -0.005460 0.045677 0.021971
+v -0.003608 0.045270 0.023501
+v -0.004995 0.045392 0.023098
+v 0.005460 0.045677 0.021977
+v 0.003608 0.045270 0.023501
+v 0.004996 0.045392 0.023099
+vn -0.2539 0.9477 -0.1935
+vn -0.2539 0.9477 0.1935
+vn 0.0000 0.9811 0.1935
+vn 0.0000 0.9811 -0.1935
+vn 0.0000 0.0000 -1.0000
+vn -0.1478 0.1478 -0.9779
+vn -0.1045 0.1810 -0.9779
+vn 0.2539 0.9477 0.1935
+vn 0.2539 0.9477 -0.1935
+vn 0.4905 0.8496 0.1935
+vn 0.4905 0.8496 -0.1935
+vn -0.2090 0.0000 -0.9779
+vn -0.2019 0.0541 -0.9779
+vn 0.1045 0.1810 0.9779
+vn 0.0541 0.2019 0.9779
+vn -0.0461 -0.1719 0.9840
+vn -0.0890 -0.1541 0.9840
+vn 0.6937 0.6937 0.1935
+vn 0.6937 0.6937 -0.1935
+vn -0.1478 -0.1478 -0.9779
+vn -0.1810 -0.1045 -0.9779
+vn 0.1478 0.1478 0.9779
+vn -0.1258 -0.1258 0.9840
+vn 0.8496 0.4905 0.1935
+vn 0.8496 0.4905 -0.1935
+vn -0.0541 -0.2019 0.9779
+vn 0.0000 -0.2090 0.9779
+vn 0.0000 0.1780 0.9840
+vn 0.0461 0.1719 0.9840
+vn 0.1810 0.1045 0.9779
+vn -0.1541 -0.0890 0.9840
+vn 0.9477 0.2539 0.1935
+vn 0.9477 0.2539 -0.1935
+vn 0.1045 -0.1810 0.9779
+vn 0.1478 -0.1478 0.9779
+vn -0.1258 0.1258 0.9840
+vn -0.0890 0.1541 0.9840
+vn 0.2019 0.0541 0.9779
+vn -0.1719 -0.0461 0.9840
+vn 0.9811 0.0000 0.1935
+vn 0.9811 0.0000 -0.1935
+vn 0.2019 -0.0541 0.9779
+vn 0.2090 0.0000 0.9779
+vn -0.1780 0.0000 0.9840
+vn -0.1719 0.0461 0.9840
+vn 0.9477 -0.2539 0.1935
+vn 0.9477 -0.2539 -0.1935
+vn 0.1810 0.1045 -0.9779
+vn 0.2019 0.0541 -0.9779
+vn 0.1810 -0.1045 -0.9779
+vn 0.1478 -0.1478 -0.9779
+vn 0.8496 -0.4905 0.1935
+vn 0.8496 -0.4905 -0.1935
+vn 0.0541 0.2019 -0.9779
+vn 0.1045 0.1810 -0.9779
+vn 0.1810 -0.1045 0.9779
+vn -0.1541 0.0890 0.9840
+vn 0.6937 -0.6937 0.1935
+vn 0.6937 -0.6937 -0.1935
+vn -0.0541 0.2019 -0.9779
+vn 0.4905 -0.8496 0.1935
+vn 0.4905 -0.8496 -0.1935
+vn -0.1810 0.1045 -0.9779
+vn 0.0541 -0.2019 -0.9779
+vn 0.0000 -0.2090 -0.9779
+vn 0.2539 -0.9477 0.1935
+vn 0.2539 -0.9477 -0.1935
+vn -0.2019 -0.0541 -0.9779
+vn 0.0541 -0.2019 0.9779
+vn -0.0461 0.1719 0.9840
+vn 0.0000 -0.9811 0.1935
+vn 0.0000 -0.9811 -0.1935
+vn -0.0541 -0.2019 -0.9779
+vn -0.1045 -0.1810 -0.9779
+vn -0.2539 -0.9477 0.1935
+vn -0.2539 -0.9477 -0.1935
+vn 0.1045 -0.1810 -0.9779
+vn -0.4905 -0.8496 0.1935
+vn -0.4905 -0.8496 -0.1935
+vn 0.2019 -0.0541 -0.9779
+vn -0.1045 -0.1810 0.9779
+vn 0.0890 0.1541 0.9840
+vn -0.6937 -0.6937 0.1935
+vn -0.6937 -0.6937 -0.1935
+vn 0.2090 0.0000 -0.9779
+vn -0.1810 -0.1045 0.9779
+vn -0.1478 -0.1478 0.9779
+vn 0.1258 0.1258 0.9840
+vn 0.1541 0.0890 0.9840
+vn -0.8496 -0.4905 0.1935
+vn -0.8496 -0.4905 -0.1935
+vn 0.0000 0.2090 0.9779
+vn -0.0541 0.2019 0.9779
+vn 0.0461 -0.1719 0.9840
+vn 0.0000 -0.1780 0.9840
+vn -0.2019 -0.0541 0.9779
+vn 0.1719 0.0461 0.9840
+vn -0.9477 -0.2539 0.1935
+vn -0.9477 -0.2539 -0.1935
+vn -0.2090 0.0000 0.9779
+vn 0.1780 0.0000 0.9840
+vn -0.9811 0.0000 0.1935
+vn -0.9811 0.0000 -0.1935
+vn -0.2019 0.0541 0.9779
+vn 0.1719 -0.0461 0.9840
+vn -0.9477 0.2539 0.1935
+vn -0.9477 0.2539 -0.1935
+vn -0.1810 0.1045 0.9779
+vn 0.1541 -0.0890 0.9840
+vn -0.8496 0.4905 0.1935
+vn -0.8496 0.4905 -0.1935
+vn -0.1478 0.1478 0.9779
+vn 0.1258 -0.1258 0.9840
+vn -0.6937 0.6937 0.1935
+vn -0.6937 0.6937 -0.1935
+vn -0.1045 0.1810 0.9779
+vn 0.0890 -0.1541 0.9840
+vn -0.4905 0.8496 0.1935
+vn -0.4905 0.8496 -0.1935
+vn -0.2536 -0.9464 0.1998
+vn 0.0000 -0.9798 0.1998
+vn 0.0000 -0.7342 0.6789
+vn -0.1900 -0.7092 0.6789
+vn 0.0000 0.2090 -0.9779
+vn 0.1478 0.1478 -0.9779
+vn 0.2536 0.9464 0.1998
+vn 0.0000 0.9798 0.1998
+vn 0.0000 0.7342 0.6789
+vn 0.1900 0.7092 0.6789
+vn -0.6928 0.6928 0.1998
+vn -0.8485 0.4899 0.1998
+vn -0.6359 0.3671 0.6789
+vn -0.5192 0.5192 0.6789
+vn 0.8485 -0.4899 0.1998
+vn 0.9464 -0.2536 0.1998
+vn 0.7092 -0.1900 0.6789
+vn 0.6359 -0.3671 0.6789
+vn -0.9464 -0.2536 0.1998
+vn -0.8485 -0.4899 0.1998
+vn -0.6359 -0.3671 0.6789
+vn -0.7092 -0.1900 0.6789
+vn 0.8485 0.4899 0.1998
+vn 0.6928 0.6928 0.1998
+vn 0.5192 0.5192 0.6789
+vn 0.6359 0.3671 0.6789
+vn -0.2536 0.9464 0.1998
+vn -0.1900 0.7092 0.6789
+vn 0.2536 -0.9464 0.1998
+vn 0.4899 -0.8485 0.1998
+vn 0.3671 -0.6359 0.6789
+vn 0.1900 -0.7092 0.6789
+vn -0.9464 0.2536 0.1998
+vn -0.7092 0.1900 0.6789
+vn 0.9798 0.0000 0.1998
+vn 0.7342 0.0000 0.6789
+vn -0.6928 -0.6928 0.1998
+vn -0.5192 -0.5192 0.6789
+vn 0.4899 0.8485 0.1998
+vn 0.3671 0.6359 0.6789
+vn -0.4899 0.8485 0.1998
+vn -0.3671 0.6359 0.6789
+vn 0.6928 -0.6928 0.1998
+vn 0.5192 -0.5192 0.6789
+vn -0.9798 0.0000 0.1998
+vn -0.7342 0.0000 0.6789
+vn 0.9464 0.2536 0.1998
+vn 0.7092 0.1900 0.6789
+vn -0.4899 -0.8485 0.1998
+vn -0.3671 -0.6359 0.6789
+vn 0.0000 0.0000 1.0000
+vn 0.0000 0.7203 0.6937
+vn 0.1864 0.6957 0.6937
+vn 0.3601 0.6238 0.6937
+vn 0.5093 0.5093 0.6937
+vn 0.6238 0.3601 0.6937
+vn 0.6957 0.1864 0.6937
+vn 0.7203 0.0000 0.6937
+vn 0.6957 -0.1864 0.6937
+vn 0.6238 -0.3601 0.6937
+vn 0.5093 -0.5093 0.6937
+vn 0.3601 -0.6238 0.6937
+vn 0.1864 -0.6957 0.6937
+vn 0.0000 -0.7203 0.6937
+vn -0.1864 -0.6957 0.6937
+vn -0.3601 -0.6238 0.6937
+vn -0.5093 -0.5093 0.6937
+vn -0.6238 -0.3601 0.6937
+vn -0.6957 -0.1864 0.6937
+vn -0.7203 0.0000 0.6937
+vn -0.6957 0.1864 0.6937
+vn -0.6238 0.3601 0.6937
+vn -0.5093 0.5093 0.6937
+vn -0.3601 0.6238 0.6937
+vn -0.1864 0.6957 0.6937
+vn 0.0000 -0.6882 0.7255
+vn -0.1781 -0.6648 0.7255
+vn -0.3441 -0.5960 0.7255
+vn -0.4866 -0.4866 0.7255
+vn -0.5960 -0.3441 0.7255
+vn -0.6648 -0.1781 0.7255
+vn -0.6882 0.0000 0.7255
+vn -0.6648 0.1781 0.7255
+vn -0.5960 0.3441 0.7255
+vn -0.4866 0.4866 0.7255
+vn -0.3441 0.5960 0.7255
+vn -0.1781 0.6648 0.7255
+vn 0.0000 0.6882 0.7255
+vn 0.1781 0.6648 0.7255
+vn 0.3441 0.5960 0.7255
+vn 0.4866 0.4866 0.7255
+vn 0.5960 0.3441 0.7255
+vn 0.6648 0.1781 0.7255
+vn 0.6882 0.0000 0.7255
+vn 0.6648 -0.1781 0.7255
+vn 0.5960 -0.3441 0.7255
+vn 0.4866 -0.4866 0.7255
+vn 0.3441 -0.5960 0.7255
+vn 0.1781 -0.6648 0.7255
+vn 0.1864 0.6957 -0.6937
+vn 0.0000 0.7203 -0.6937
+vn 0.3601 0.6238 -0.6937
+vn 0.5093 0.5093 -0.6937
+vn 0.6238 0.3601 -0.6937
+vn 0.6957 0.1864 -0.6937
+vn 0.7203 0.0000 -0.6937
+vn 0.6957 -0.1864 -0.6937
+vn 0.6238 -0.3601 -0.6937
+vn 0.5093 -0.5093 -0.6937
+vn 0.3601 -0.6238 -0.6937
+vn 0.1864 -0.6957 -0.6937
+vn 0.0000 -0.7203 -0.6937
+vn -0.1864 -0.6957 -0.6937
+vn -0.3601 -0.6238 -0.6937
+vn -0.5093 -0.5093 -0.6937
+vn -0.6238 -0.3601 -0.6937
+vn -0.6957 -0.1864 -0.6937
+vn -0.7203 0.0000 -0.6937
+vn -0.6957 0.1864 -0.6937
+vn -0.6238 0.3601 -0.6937
+vn -0.5093 0.5093 -0.6937
+vn -0.3601 0.6238 -0.6937
+vn -0.1864 0.6957 -0.6937
+vn 0.9873 -0.0316 0.1558
+vn 0.9365 0.2263 -0.2680
+vn 0.9869 0.0385 -0.1568
+vn 0.9807 -0.0226 0.1941
+vn -0.9809 0.0597 0.1849
+vn -0.9841 -0.0538 -0.1691
+vn -0.9822 -0.0130 -0.1871
+vn -0.9820 0.0147 0.1883
+vn -0.9826 0.1857 0.0000
+vn -0.9812 -0.1929 0.0000
+vn -0.9825 -0.1859 -0.0129
+vn -0.9815 0.1907 0.0146
+vn -0.9809 0.1849 0.0597
+vn -0.9841 -0.1691 -0.0538
+vn -0.9838 -0.1451 -0.1054
+vn -0.9812 0.1562 0.1134
+vn -0.9838 -0.1054 -0.1451
+vn -0.9812 0.1134 0.1562
+vn 0.1468 0.0770 0.9861
+vn -0.1468 0.0770 0.9861
+vn -0.1455 -0.1062 0.9836
+vn 0.1455 -0.1062 0.9836
+vn 0.9802 -0.0302 -0.1954
+vn 0.9866 0.0386 0.1584
+vn 0.9385 0.2178 0.2678
+vn 0.9884 -0.0388 -0.1467
+vn -0.9815 0.1907 -0.0146
+vn -0.9825 -0.1859 0.0129
+vn -0.9809 0.1849 -0.0597
+vn -0.9841 -0.1691 0.0538
+vn -0.9812 0.1562 -0.1134
+vn -0.9838 -0.1451 0.1054
+vn -0.9812 0.1134 -0.1562
+vn -0.9838 -0.1054 0.1451
+vn -0.9809 0.0597 -0.1849
+vn -0.9841 -0.0538 0.1691
+vn 0.1472 0.0772 -0.9861
+vn 0.1452 -0.1337 -0.9803
+vn -0.1452 -0.1337 -0.9803
+vn -0.1472 0.0772 -0.9861
+vn -0.1399 -0.9901 0.0000
+vn -0.1397 -0.9877 0.0698
+vn 0.1394 -0.9878 0.0698
+vn 0.1395 -0.9902 0.0000
+vn 0.1394 -0.9878 -0.0698
+vn 0.1398 -0.9438 -0.2995
+vn -0.1401 -0.9438 -0.2994
+vn -0.1397 -0.9877 -0.0698
+vn 0.1391 -0.8012 -0.5819
+vn -0.1395 -0.8012 -0.5819
+vn 0.1391 -0.5819 -0.8012
+vn -0.1395 -0.5819 -0.8012
+vn 0.1398 -0.2995 -0.9438
+vn -0.1402 -0.2994 -0.9437
+vn 0.1393 -0.0667 -0.9880
+vn -0.1396 -0.0667 -0.9879
+vn 0.1446 0.3836 -0.9121
+vn -0.1450 0.3836 -0.9120
+vn -0.1401 -0.9438 0.2994
+vn 0.1398 -0.9438 0.2995
+vn -0.1395 -0.8012 0.5819
+vn 0.1391 -0.8012 0.5819
+vn -0.1395 -0.5819 0.8012
+vn 0.1391 -0.5819 0.8012
+vn -0.1401 -0.2994 0.9437
+vn 0.1398 -0.2995 0.9438
+vn -0.1396 -0.0670 0.9879
+vn 0.1392 -0.0670 0.9880
+vn -0.1444 0.3729 0.9165
+vn 0.1440 0.3729 0.9166
+vn -0.9884 -0.0388 -0.1467
+vn -0.9387 0.2175 0.2674
+vn -0.9867 0.0385 0.1581
+vn -0.9802 -0.0302 -0.1954
+vn 0.9812 0.1562 0.1135
+vn 0.9837 -0.1454 -0.1056
+vn 0.9840 -0.1694 -0.0539
+vn 0.9809 0.1849 0.0597
+vn 0.9821 0.0148 -0.1877
+vn 0.9821 -0.0130 0.1876
+vn 0.9812 0.1135 -0.1562
+vn 0.9837 -0.1056 0.1454
+vn 0.9840 -0.0539 0.1694
+vn 0.9809 0.0597 -0.1849
+vn 0.1485 0.5813 0.8000
+vn -0.1485 0.5813 0.8000
+vn -0.1478 0.3049 0.9408
+vn 0.1478 0.3049 0.9408
+vn 0.1478 0.3049 -0.9408
+vn -0.1478 0.3049 -0.9408
+vn -0.9821 0.0148 -0.1877
+vn -0.9822 -0.0130 0.1873
+vn 0.9812 0.1135 0.1562
+vn 0.9837 -0.1056 -0.1454
+vn 0.9826 0.1857 0.0000
+vn 0.9811 -0.1932 0.0000
+vn 0.9824 -0.1862 0.0130
+vn 0.9815 0.1907 -0.0146
+vn 0.9820 0.0147 0.1884
+vn 0.9822 -0.0130 -0.1874
+vn 0.9840 -0.0539 -0.1694
+vn 0.9809 0.0597 0.1849
+vn 0.1478 0.9408 -0.3049
+vn 0.1485 0.8000 -0.5813
+vn -0.1485 0.8000 -0.5813
+vn -0.1478 0.9408 -0.3049
+vn -0.9807 -0.0226 0.1941
+vn -0.9869 0.0384 -0.1565
+vn -0.9366 0.2260 -0.2676
+vn -0.9873 -0.0316 0.1558
+vn 0.1123 0.6749 -0.7293
+vn -0.1126 0.6749 -0.7292
+vn -0.1448 0.9894 0.0000
+vn 0.1448 0.9894 0.0000
+vn 0.1456 0.9864 -0.0767
+vn -0.1456 0.9864 -0.0767
+vn 0.1485 0.5813 -0.8000
+vn -0.1485 0.5813 -0.8000
+vn 0.9815 0.1907 0.0146
+vn 0.9824 -0.1862 -0.0130
+vn 0.9809 0.1849 -0.0597
+vn 0.9840 -0.1694 0.0539
+vn 0.9837 -0.1454 0.1056
+vn 0.9812 0.1562 -0.1135
+vn -0.1456 0.9864 0.0767
+vn 0.1456 0.9864 0.0767
+vn -0.1484 -0.2151 0.9652
+vn 0.1484 -0.2151 0.9652
+vn 0.1485 0.8000 0.5813
+vn -0.1485 0.8000 0.5813
+vn -0.1478 0.9408 0.3049
+vn 0.1478 0.9408 0.3049
+vn -0.1134 0.6594 0.7432
+vn 0.1131 0.6594 0.7432
+vn 0.1475 -0.2713 -0.9511
+vn -0.1475 -0.2713 -0.9511
+vn 0.6697 0.7404 0.0575
+vn 0.6653 0.7103 0.2300
+vn -0.6653 0.7103 0.2300
+vn -0.6697 0.7404 0.0575
+vn 0.6664 0.6031 0.4382
+vn -0.6664 0.6031 0.4382
+vn 0.6664 0.4382 0.6031
+vn -0.6664 0.4382 0.6031
+vn 0.6653 0.2300 0.7103
+vn -0.6653 0.2300 0.7103
+vn 0.6718 0.0577 0.7385
+vn -0.6718 0.0577 0.7385
+vn -0.6767 0.7362 0.0000
+vn -0.6652 -0.0813 0.7422
+vn 0.7048 -0.1535 0.6926
+vn 0.6652 -0.0813 0.7422
+vn 0.6767 0.7362 0.0000
+vn 0.6697 0.7404 -0.0575
+vn 0.6653 0.7103 -0.2300
+vn -0.6653 0.7103 -0.2300
+vn -0.6697 0.7404 -0.0575
+vn 0.6664 0.6031 -0.4382
+vn -0.6664 0.6031 -0.4382
+vn 0.6664 0.4382 -0.6031
+vn -0.6664 0.4382 -0.6031
+vn 0.6653 0.2300 -0.7103
+vn -0.6653 0.2300 -0.7103
+vn 0.6724 0.0579 -0.7378
+vn -0.6724 0.0579 -0.7378
+vn -0.6623 -0.1034 -0.7421
+vn 0.7106 -0.1924 -0.6768
+vn 0.6623 -0.1034 -0.7421
+vn 0.6762 -0.7349 -0.0517
+vn 0.6870 -0.6926 -0.2197
+vn -0.6876 -0.6921 -0.2195
+vn -0.6767 -0.7344 -0.0517
+vn 0.6854 -0.5892 -0.4279
+vn -0.6859 -0.5888 -0.4276
+vn 0.6854 -0.4279 -0.5892
+vn -0.6859 -0.4276 -0.5888
+vn 0.6870 -0.2197 -0.6926
+vn -0.6876 -0.2195 -0.6921
+vn 0.6761 -0.0500 -0.7350
+vn -0.6767 -0.0500 -0.7345
+vn -0.6685 -0.7437 0.0000
+vn -0.6990 0.2732 -0.6608
+vn 0.4884 0.5826 -0.6496
+vn 0.6985 0.2735 -0.6613
+vn 0.6680 -0.7442 0.0000
+vn 0.6762 -0.7349 0.0517
+vn 0.6870 -0.6926 0.2197
+vn -0.6876 -0.6921 0.2195
+vn -0.6767 -0.7344 0.0517
+vn 0.6854 -0.5892 0.4279
+vn -0.6859 -0.5888 0.4276
+vn 0.6854 -0.4279 0.5892
+vn -0.6859 -0.4276 0.5888
+vn 0.6870 -0.2197 0.6926
+vn -0.6876 -0.2195 0.6921
+vn 0.6760 -0.0501 0.7352
+vn -0.6765 -0.0501 0.7347
+vn -0.6982 0.2655 0.6648
+vn 0.4946 0.5670 0.6587
+vn 0.6977 0.2658 0.6653
+vn -0.7048 -0.1535 0.6926
+vn -0.4891 0.5823 -0.6494
+vn -0.7106 -0.1924 -0.6768
+vn -0.4952 0.5667 0.6584
+s 1
+f 240//1 97//2 27//3 172//4
+f 1//5 235//6 238//7
+f 172//4 27//3 31//8 174//9
+f 174//9 31//8 34//10 177//11
+f 1//5 226//12 229//13
+f 33//14 30//15 103//16 106//17
+f 177//11 34//10 37//18 180//19
+f 1//5 217//20 220//21
+f 36//22 33//14 106//17 109//23
+f 180//19 37//18 40//24 183//25
+f 66//26 63//27 136//28 139//29
+f 39//30 36//22 109//23 112//31
+f 183//25 40//24 43//32 186//33
+f 57//34 54//35 127//36 130//37
+f 42//38 39//30 112//31 115//39
+f 186//33 43//32 46//40 189//41
+f 48//42 45//43 118//44 121//45
+f 45//43 42//38 115//39 118//44
+f 189//41 46//40 49//46 192//47
+f 1//5 184//48 187//49
+f 1//5 196//50 199//51
+f 192//47 49//46 52//52 195//53
+f 1//5 175//54 178//55
+f 51//56 48//42 121//45 124//57
+f 195//53 52//52 55//58 198//59
+f 1//5 238//7 241//60
+f 54//35 51//56 124//57 127//36
+f 198//59 55//58 58//61 201//62
+f 1//5 229//13 232//63
+f 1//5 205//64 208//65
+f 201//62 58//61 61//66 204//67
+f 1//5 220//21 223//68
+f 60//69 57//34 130//37 133//70
+f 204//67 61//66 64//71 207//72
+f 1//5 211//73 214//74
+f 63//27 60//69 133//70 136//28
+f 207//72 64//71 67//75 210//76
+f 1//5 202//77 205//64
+f 1//5 214//74 217//20
+f 210//76 67//75 70//78 213//79
+f 1//5 193//80 196//50
+f 69//81 66//26 139//29 142//82
+f 213//79 70//78 73//83 216//84
+f 1//5 187//49 190//85
+f 75//86 72//87 145//88 148//89
+f 216//84 73//83 76//90 219//91
+f 28//92 96//93 169//94 99//95
+f 78//96 75//86 148//89 151//97
+f 219//91 76//90 79//98 222//99
+f 81//100 78//96 151//97 154//101
+f 222//99 79//98 82//102 225//103
+f 1//5 232//63 235//6
+f 84//104 81//100 154//101 157//105
+f 225//103 82//102 85//106 228//107
+f 1//5 223//68 226//12
+f 87//108 84//104 157//105 160//109
+f 228//107 85//106 88//110 231//111
+f 72//87 69//81 142//82 145//88
+f 90//112 87//108 160//109 163//113
+f 231//111 88//110 91//114 234//115
+f 1//5 208//65 211//73
+f 93//116 90//112 163//113 166//117
+f 234//115 91//114 94//118 237//119
+f 1//5 199//51 202//77
+f 96//93 93//116 166//117 169//94
+f 237//119 94//118 97//2 240//1
+f 1//5 190//85 193//80
+f 102//120 100//121 2//122 4//123
+f 1//5 171//124 175//54
+f 1//5 181//125 184//48
+f 1//5 178//55 181//125
+f 138//126 135//127 15//128 16//129
+f 126//130 123//131 11//132 12//133
+f 159//134 156//135 22//136 23//137
+f 114//138 111//139 7//140 8//141
+f 147//142 144//143 18//144 19//145
+f 135//127 132//146 14//147 15//128
+f 168//148 165//149 25//150 26//151
+f 123//131 120//152 10//153 11//132
+f 156//135 153//154 21//155 22//136
+f 111//139 108//156 6//157 7//140
+f 144//143 141//158 17//159 18//144
+f 132//146 129//160 13//161 14//147
+f 165//149 162//162 24//163 25//150
+f 120//152 117//164 9//165 10//153
+f 153//154 150//166 20//167 21//155
+f 108//156 105//168 5//169 6//157
+f 141//158 138//126 16//129 17//159
+f 129//160 126//130 12//133 13//161
+f 162//162 159//134 23//137 24//163
+f 117//164 114//138 8//141 9//165
+f 105//168 102//120 4//123 5//169
+f 150//166 147//142 19//145 20//167
+f 100//121 168//148 26//151 2//122
+f 3//170 4//123 2//122
+f 3//170 5//169 4//123
+f 3//170 6//157 5//169
+f 3//170 7//140 6//157
+f 3//170 8//141 7//140
+f 3//170 9//165 8//141
+f 3//170 10//153 9//165
+f 3//170 11//132 10//153
+f 3//170 12//133 11//132
+f 3//170 13//161 12//133
+f 3//170 14//147 13//161
+f 3//170 15//128 14//147
+f 3//170 16//129 15//128
+f 3//170 17//159 16//129
+f 3//170 18//144 17//159
+f 3//170 19//145 18//144
+f 3//170 20//167 19//145
+f 3//170 21//155 20//167
+f 3//170 22//136 21//155
+f 3//170 23//137 22//136
+f 3//170 24//163 23//137
+f 3//170 25//150 24//163
+f 3//170 26//151 25//150
+f 3//170 2//122 26//151
+f 31//8 27//3 29//171 32//172
+f 32//172 29//171 28//92 30//15
+f 34//10 31//8 32//172 35//173
+f 35//173 32//172 30//15 33//14
+f 37//18 34//10 35//173 38//174
+f 38//174 35//173 33//14 36//22
+f 40//24 37//18 38//174 41//175
+f 41//175 38//174 36//22 39//30
+f 43//32 40//24 41//175 44//176
+f 44//176 41//175 39//30 42//38
+f 46//40 43//32 44//176 47//177
+f 47//177 44//176 42//38 45//43
+f 49//46 46//40 47//177 50//178
+f 50//178 47//177 45//43 48//42
+f 52//52 49//46 50//178 53//179
+f 53//179 50//178 48//42 51//56
+f 55//58 52//52 53//179 56//180
+f 56//180 53//179 51//56 54//35
+f 58//61 55//58 56//180 59//181
+f 59//181 56//180 54//35 57//34
+f 61//66 58//61 59//181 62//182
+f 62//182 59//181 57//34 60//69
+f 64//71 61//66 62//182 65//183
+f 65//183 62//182 60//69 63//27
+f 67//75 64//71 65//183 68//184
+f 68//184 65//183 63//27 66//26
+f 70//78 67//75 68//184 71//185
+f 71//185 68//184 66//26 69//81
+f 73//83 70//78 71//185 74//186
+f 74//186 71//185 69//81 72//87
+f 76//90 73//83 74//186 77//187
+f 77//187 74//186 72//87 75//86
+f 79//98 76//90 77//187 80//188
+f 80//188 77//187 75//86 78//96
+f 82//102 79//98 80//188 83//189
+f 83//189 80//188 78//96 81//100
+f 85//106 82//102 83//189 86//190
+f 86//190 83//189 81//100 84//104
+f 88//110 85//106 86//190 89//191
+f 89//191 86//190 84//104 87//108
+f 91//114 88//110 89//191 92//192
+f 92//192 89//191 87//108 90//112
+f 94//118 91//114 92//192 95//193
+f 95//193 92//192 90//112 93//116
+f 97//2 94//118 95//193 98//194
+f 98//194 95//193 93//116 96//93
+f 27//3 97//2 98//194 29//171
+f 29//171 98//194 96//93 28//92
+f 103//16 99//95 101//195 104//196
+f 104//196 101//195 100//121 102//120
+f 106//17 103//16 104//196 107//197
+f 107//197 104//196 102//120 105//168
+f 109//23 106//17 107//197 110//198
+f 110//198 107//197 105//168 108//156
+f 112//31 109//23 110//198 113//199
+f 113//199 110//198 108//156 111//139
+f 115//39 112//31 113//199 116//200
+f 116//200 113//199 111//139 114//138
+f 118//44 115//39 116//200 119//201
+f 119//201 116//200 114//138 117//164
+f 121//45 118//44 119//201 122//202
+f 122//202 119//201 117//164 120//152
+f 124//57 121//45 122//202 125//203
+f 125//203 122//202 120//152 123//131
+f 127//36 124//57 125//203 128//204
+f 128//204 125//203 123//131 126//130
+f 130//37 127//36 128//204 131//205
+f 131//205 128//204 126//130 129//160
+f 133//70 130//37 131//205 134//206
+f 134//206 131//205 129//160 132//146
+f 136//28 133//70 134//206 137//207
+f 137//207 134//206 132//146 135//127
+f 139//29 136//28 137//207 140//208
+f 140//208 137//207 135//127 138//126
+f 142//82 139//29 140//208 143//209
+f 143//209 140//208 138//126 141//158
+f 145//88 142//82 143//209 146//210
+f 146//210 143//209 141//158 144//143
+f 148//89 145//88 146//210 149//211
+f 149//211 146//210 144//143 147//142
+f 151//97 148//89 149//211 152//212
+f 152//212 149//211 147//142 150//166
+f 154//101 151//97 152//212 155//213
+f 155//213 152//212 150//166 153//154
+f 157//105 154//101 155//213 158//214
+f 158//214 155//213 153//154 156//135
+f 160//109 157//105 158//214 161//215
+f 161//215 158//214 156//135 159//134
+f 163//113 160//109 161//215 164//216
+f 164//216 161//215 159//134 162//162
+f 166//117 163//113 164//216 167//217
+f 167//217 164//216 162//162 165//149
+f 169//94 166//117 167//217 170//218
+f 170//218 167//217 165//149 168//148
+f 99//95 169//94 170//218 101//195
+f 101//195 170//218 168//148 100//121
+f 30//15 28//92 99//95 103//16
+f 172//4 174//9 176//219 173//220
+f 173//220 176//219 175//54 171//124
+f 174//9 177//11 179//221 176//219
+f 176//219 179//221 178//55 175//54
+f 177//11 180//19 182//222 179//221
+f 179//221 182//222 181//125 178//55
+f 180//19 183//25 185//223 182//222
+f 182//222 185//223 184//48 181//125
+f 183//25 186//33 188//224 185//223
+f 185//223 188//224 187//49 184//48
+f 186//33 189//41 191//225 188//224
+f 188//224 191//225 190//85 187//49
+f 189//41 192//47 194//226 191//225
+f 191//225 194//226 193//80 190//85
+f 192//47 195//53 197//227 194//226
+f 194//226 197//227 196//50 193//80
+f 195//53 198//59 200//228 197//227
+f 197//227 200//228 199//51 196//50
+f 198//59 201//62 203//229 200//228
+f 200//228 203//229 202//77 199//51
+f 201//62 204//67 206//230 203//229
+f 203//229 206//230 205//64 202//77
+f 204//67 207//72 209//231 206//230
+f 206//230 209//231 208//65 205//64
+f 207//72 210//76 212//232 209//231
+f 209//231 212//232 211//73 208//65
+f 210//76 213//79 215//233 212//232
+f 212//232 215//233 214//74 211//73
+f 213//79 216//84 218//234 215//233
+f 215//233 218//234 217//20 214//74
+f 216//84 219//91 221//235 218//234
+f 218//234 221//235 220//21 217//20
+f 219//91 222//99 224//236 221//235
+f 221//235 224//236 223//68 220//21
+f 222//99 225//103 227//237 224//236
+f 224//236 227//237 226//12 223//68
+f 225//103 228//107 230//238 227//237
+f 227//237 230//238 229//13 226//12
+f 228//107 231//111 233//239 230//238
+f 230//238 233//239 232//63 229//13
+f 231//111 234//115 236//240 233//239
+f 233//239 236//240 235//6 232//63
+f 234//115 237//119 239//241 236//240
+f 236//240 239//241 238//7 235//6
+f 237//119 240//1 242//242 239//241
+f 239//241 242//242 241//60 238//7
+f 240//1 172//4 173//220 242//242
+f 242//242 173//220 171//124 241//60
+f 1//5 241//60 171//124
+f 246//243 334//244 433//245 426//246
+f 264//247 373//248 379//249 252//250
+f 292//251 339//252 351//253 250//254
+f 255//255 355//256 361//257 258//258
+f 258//258 361//257 367//259 261//260
+f 261//260 367//259 373//248 264//247
+f 267//261 253//262 424//263 427//264
+f 439//265 444//266 381//267 289//268
+f 297//269 394//270 339//252 292//251
+f 304//271 396//272 394//270 297//269
+f 307//273 402//274 396//272 304//271
+f 310//275 408//276 402//274 307//273
+f 313//277 414//278 408//276 310//275
+f 316//279 438//280 435//281 300//282
+f 340//283 393//284 391//285 342//286
+f 348//287 346//288 354//289 352//290
+f 346//288 358//291 360//292 354//289
+f 358//291 364//293 366//294 360//292
+f 364//293 370//295 372//296 366//294
+f 370//295 376//297 378//298 372//296
+f 376//297 432//299 429//300 378//298
+f 391//285 393//284 397//301 387//302
+f 340//283 342//286 348//287 352//290
+f 387//302 397//301 403//303 399//304
+f 399//304 403//303 409//305 405//306
+f 405//306 409//305 415//307 411//308
+f 411//308 415//307 421//309 417//310
+f 417//310 421//309 442//311 445//312
+f 286//313 385//314 441//315 436//316
+f 280//317 357//318 345//319 283//320
+f 315//321 418//322 444//266 439//265
+f 324//323 406//324 412//325 321//326
+f 276//327 262//328 265//329 273//330
+f 322//331 316//279 300//282 312//332
+f 301//333 420//334 414//278 313//277
+f 277//335 363//336 357//318 280//317
+f 294//337 343//338 390//339 319//340
+f 268//341 375//342 369//343 274//344
+f 331//345 328//346 306//347 303//348
+f 423//349 430//350 337//351 244//352
+f 432//299 333//353 336//354 429//300
+f 291//355 295//356 318//357 298//358
+f 328//346 325//359 309//360 306//347
+f 426//246 433//245 375//342 268//341
+f 318//357 331//345 303//348 298//358
+f 270//361 349//362 343//338 294//337
+f 273//330 265//329 253//262 267//261
+f 330//363 388//364 400//365 327//366
+f 291//355 249//367 271//368 295//356
+f 325//359 322//331 312//332 309//360
+f 250//254 351//253 355//256 255//255
+f 427//264 424//263 243//369 247//370
+f 327//366 400//365 406//324 324//323
+f 283//320 345//319 349//362 270//361
+f 279//371 259//372 262//328 276//327
+f 321//326 412//325 418//322 315//321
+f 274//344 369//343 363//336 277//335
+f 271//368 249//367 256//373 282//374
+f 282//374 256//373 259//372 279//371
+f 445//312 442//311 384//375 382//376
+f 252//250 379//249 430//350 423//349
+f 319//340 390//339 388//364 330//363
+f 438//280 288//377 285//378 435//281
+f 436//316 441//315 420//334 301//333
+f 283//320 270//361 272//379 284//380
+f 284//380 272//379 271//368 282//374
+f 250//254 255//255 257//381 251//382
+f 251//382 257//381 256//373 249//367
+f 280//317 283//320 284//380 281//383
+f 281//383 284//380 282//374 279//371
+f 255//255 258//258 260//384 257//381
+f 257//381 260//384 259//372 256//373
+f 277//335 280//317 281//383 278//385
+f 278//385 281//383 279//371 276//327
+f 258//258 261//260 263//386 260//384
+f 260//384 263//386 262//328 259//372
+f 274//344 277//335 278//385 275//387
+f 275//387 278//385 276//327 273//330
+f 261//260 264//247 266//388 263//386
+f 263//386 266//388 265//329 262//328
+f 268//341 274//344 275//387 269//389
+f 269//389 275//387 273//330 267//261
+f 264//247 252//250 254//390 266//388
+f 266//388 254//390 253//262 265//329
+f 249//367 291//355 293//391 251//382
+f 251//382 293//391 292//251 250//254
+f 424//263 253//262 254//390 425//392
+f 425//392 254//390 252//250 423//349
+f 427//264 247//370 248//393 428//394
+f 428//394 248//393 246//243 426//246
+f 295//356 271//368 272//379 296//395
+f 296//395 272//379 270//361 294//337
+f 331//345 318//357 320//396 332//397
+f 332//397 320//396 319//340 330//363
+f 298//358 303//348 305//398 299//399
+f 299//399 305//398 304//271 297//269
+f 328//346 331//345 332//397 329//400
+f 329//400 332//397 330//363 327//366
+f 303//348 306//347 308//401 305//398
+f 305//398 308//401 307//273 304//271
+f 325//359 328//346 329//400 326//402
+f 326//402 329//400 327//366 324//323
+f 306//347 309//360 311//403 308//401
+f 308//401 311//403 310//275 307//273
+f 322//331 325//359 326//402 323//404
+f 323//404 326//402 324//323 321//326
+f 309//360 312//332 314//405 311//403
+f 311//403 314//405 313//277 310//275
+f 316//279 322//331 323//404 317//406
+f 317//406 323//404 321//326 315//321
+f 312//332 300//282 302//407 314//405
+f 314//405 302//407 301//333 313//277
+f 297//269 292//251 293//391 299//399
+f 299//399 293//391 291//355 298//358
+f 436//316 301//333 302//407 437//408
+f 437//408 302//407 300//282 435//281
+f 439//265 289//268 290//409 440//410
+f 440//410 290//409 288//377 438//280
+f 294//337 319//340 320//396 296//395
+f 296//395 320//396 318//357 295//356
+f 346//288 348//287 350//411 347//412
+f 347//412 350//411 349//362 345//319
+f 352//290 354//289 356//413 353//414
+f 353//414 356//413 355//256 351//253
+f 358//291 346//288 347//412 359//415
+f 359//415 347//412 345//319 357//318
+f 354//289 360//292 362//416 356//413
+f 356//413 362//416 361//257 355//256
+f 364//293 358//291 359//415 365//417
+f 365//417 359//415 357//318 363//336
+f 360//292 366//294 368//418 362//416
+f 362//416 368//418 367//259 361//257
+f 370//295 364//293 365//417 371//419
+f 371//419 365//417 363//336 369//343
+f 366//294 372//296 374//420 368//418
+f 368//418 374//420 373//248 367//259
+f 376//297 370//295 371//419 377//421
+f 377//421 371//419 369//343 375//342
+f 372//296 378//298 380//422 374//420
+f 374//420 380//422 379//249 373//248
+f 351//253 339//252 341//423 353//414
+f 353//414 341//423 340//283 352//290
+f 430//350 379//249 380//422 431//424
+f 431//424 380//422 378//298 429//300
+f 433//245 334//244 335//425 434//426
+f 434//426 335//425 333//353 432//299
+f 343//338 349//362 350//411 344//427
+f 344//427 350//411 348//287 342//286
+f 388//364 390//339 392//428 389//429
+f 389//429 392//428 391//285 387//302
+f 394//270 396//272 398//430 395//431
+f 395//431 398//430 397//301 393//284
+f 400//365 388//364 389//429 401//432
+f 401//432 389//429 387//302 399//304
+f 396//272 402//274 404//433 398//430
+f 398//430 404//433 403//303 397//301
+f 406//324 400//365 401//432 407//434
+f 407//434 401//432 399//304 405//306
+f 402//274 408//276 410//435 404//433
+f 404//433 410//435 409//305 403//303
+f 412//325 406//324 407//434 413//436
+f 413//436 407//434 405//306 411//308
+f 408//276 414//278 416//437 410//435
+f 410//435 416//437 415//307 409//305
+f 418//322 412//325 413//436 419//438
+f 419//438 413//436 411//308 417//310
+f 414//278 420//334 422//439 416//437
+f 416//437 422//439 421//309 415//307
+f 393//284 340//283 341//423 395//431
+f 395//431 341//423 339//252 394//270
+f 442//311 421//309 422//439 443//440
+f 443//440 422//439 420//334 441//315
+f 445//312 382//376 383//441 446//442
+f 446//442 383//441 381//267 444//266
+f 342//286 391//285 392//428 344//427
+f 344//427 392//428 390//339 343//338
+f 243//369 424//263 425//392 245//443
+f 245//443 425//392 423//349 244//352
+f 267//261 427//264 428//394 269//389
+f 269//389 428//394 426//246 268//341
+f 337//351 430//350 431//424 338//444
+f 338//444 431//424 429//300 336//354
+f 375//342 433//245 434//426 377//421
+f 377//421 434//426 432//299 376//297
+f 286//313 436//316 437//408 287//445
+f 287//445 437//408 435//281 285//378
+f 315//321 439//265 440//410 317//406
+f 317//406 440//410 438//280 316//279
+f 384//375 442//311 443//440 386//446
+f 386//446 443//440 441//315 385//314
+f 417//310 445//312 446//442 419//438
+f 419//438 446//442 444//266 418//322
diff --git a/examples/pybullet/gym/pybullet_data/urdf/mug.urdf b/examples/pybullet/gym/pybullet_data/urdf/mug.urdf
new file mode 100644
index 000000000..ab105ea54
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/urdf/mug.urdf
@@ -0,0 +1,28 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/examples/pybullet/gym/pybullet_data/urdf/mug_col.obj b/examples/pybullet/gym/pybullet_data/urdf/mug_col.obj
new file mode 100644
index 000000000..4d56449df
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/urdf/mug_col.obj
@@ -0,0 +1,1310 @@
+# Blender v2.79 (sub 0) OBJ File: 'mug.blend'
+# www.blender.org
+o mug_col_Cylinder.003
+v 0.000000 0.000000 0.000000
+v 0.000000 0.041000 0.097171
+v 0.000000 0.038146 0.100000
+v 0.000000 0.040164 0.099171
+v 0.009873 0.036846 0.100000
+v 0.010612 0.039603 0.097171
+v 0.010395 0.038796 0.099171
+v 0.019073 0.033036 0.100000
+v 0.020500 0.035507 0.097171
+v 0.020082 0.034783 0.099171
+v 0.026973 0.026973 0.100000
+v 0.028991 0.028991 0.097171
+v 0.028400 0.028400 0.099171
+v 0.033036 0.019073 0.100000
+v 0.035507 0.020500 0.097171
+v 0.034783 0.020082 0.099171
+v 0.036846 0.009873 0.100000
+v 0.039603 0.010612 0.097171
+v 0.038796 0.010395 0.099171
+v 0.038146 0.000000 0.100000
+v 0.041000 0.000000 0.097171
+v 0.040164 0.000000 0.099171
+v 0.036846 -0.009873 0.100000
+v 0.039603 -0.010612 0.097171
+v 0.038796 -0.010395 0.099171
+v 0.033036 -0.019073 0.100000
+v 0.035507 -0.020500 0.097171
+v 0.034783 -0.020082 0.099171
+v 0.026974 -0.026973 0.100000
+v 0.028991 -0.028991 0.097171
+v 0.028400 -0.028400 0.099171
+v 0.019073 -0.033036 0.100000
+v 0.020500 -0.035507 0.097171
+v 0.020082 -0.034783 0.099171
+v 0.009873 -0.036846 0.100000
+v 0.010612 -0.039603 0.097171
+v 0.010395 -0.038796 0.099171
+v 0.000000 -0.038146 0.100000
+v 0.000000 -0.041000 0.097171
+v 0.000000 -0.040164 0.099171
+v -0.009873 -0.036846 0.100000
+v -0.010612 -0.039603 0.097171
+v -0.010395 -0.038796 0.099171
+v -0.019073 -0.033036 0.100000
+v -0.020500 -0.035507 0.097171
+v -0.020082 -0.034783 0.099171
+v -0.026973 -0.026974 0.100000
+v -0.028991 -0.028991 0.097171
+v -0.028400 -0.028400 0.099171
+v -0.033036 -0.019073 0.100000
+v -0.035507 -0.020500 0.097171
+v -0.034783 -0.020082 0.099171
+v -0.036846 -0.009873 0.100000
+v -0.039603 -0.010612 0.097171
+v -0.038796 -0.010395 0.099171
+v -0.038146 -0.000000 0.100000
+v -0.041000 -0.000000 0.097171
+v -0.040164 -0.000000 0.099171
+v -0.036846 0.009873 0.100000
+v -0.039603 0.010612 0.097171
+v -0.038796 0.010395 0.099171
+v -0.033036 0.019073 0.100000
+v -0.035507 0.020500 0.097171
+v -0.034783 0.020082 0.099171
+v -0.026974 0.026973 0.100000
+v -0.028991 0.028991 0.097171
+v -0.028400 0.028400 0.099171
+v -0.019073 0.033036 0.100000
+v -0.020500 0.035507 0.097171
+v -0.020082 0.034783 0.099171
+v -0.009873 0.036846 0.100000
+v -0.010612 0.039603 0.097171
+v -0.010395 0.038796 0.099171
+v 0.000000 -0.000000 0.100000
+v 0.000000 0.039460 0.000000
+v 0.000000 0.041000 0.001527
+v 0.000000 0.040549 0.000447
+v 0.010612 0.039603 0.001527
+v 0.010213 0.038116 0.000000
+v 0.010495 0.039167 0.000447
+v 0.020500 0.035507 0.001527
+v 0.019730 0.034174 0.000000
+v 0.020275 0.035116 0.000447
+v 0.028991 0.028991 0.001527
+v 0.027903 0.027903 0.000000
+v 0.028672 0.028672 0.000447
+v 0.035507 0.020500 0.001527
+v 0.034174 0.019730 0.000000
+v 0.035116 0.020275 0.000447
+v 0.039603 0.010612 0.001527
+v 0.038116 0.010213 0.000000
+v 0.039167 0.010495 0.000447
+v 0.041000 0.000000 0.001527
+v 0.039460 0.000000 0.000000
+v 0.040549 0.000000 0.000447
+v 0.039603 -0.010612 0.001527
+v 0.038116 -0.010213 0.000000
+v 0.039167 -0.010495 0.000447
+v 0.035507 -0.020500 0.001527
+v 0.034174 -0.019730 0.000000
+v 0.035116 -0.020275 0.000447
+v 0.028991 -0.028991 0.001527
+v 0.027903 -0.027903 0.000000
+v 0.028672 -0.028672 0.000447
+v 0.020500 -0.035507 0.001527
+v 0.019730 -0.034174 0.000000
+v 0.020275 -0.035116 0.000447
+v 0.010612 -0.039603 0.001527
+v 0.010213 -0.038116 0.000000
+v 0.010495 -0.039167 0.000447
+v 0.000000 -0.041000 0.001527
+v 0.000000 -0.039460 0.000000
+v 0.000000 -0.040549 0.000447
+v -0.010612 -0.039603 0.001527
+v -0.010213 -0.038116 0.000000
+v -0.010495 -0.039167 0.000447
+v -0.020500 -0.035507 0.001527
+v -0.019730 -0.034174 0.000000
+v -0.020274 -0.035116 0.000447
+v -0.028991 -0.028991 0.001527
+v -0.027903 -0.027903 0.000000
+v -0.028672 -0.028672 0.000447
+v -0.035507 -0.020500 0.001527
+v -0.034174 -0.019730 0.000000
+v -0.035116 -0.020275 0.000447
+v -0.039603 -0.010612 0.001527
+v -0.038116 -0.010213 0.000000
+v -0.039167 -0.010495 0.000447
+v -0.041000 -0.000000 0.001527
+v -0.039460 -0.000000 0.000000
+v -0.040549 -0.000000 0.000447
+v -0.039603 0.010612 0.001527
+v -0.038116 0.010213 0.000000
+v -0.039167 0.010495 0.000447
+v -0.035507 0.020500 0.001527
+v -0.034174 0.019730 0.000000
+v -0.035116 0.020274 0.000447
+v -0.028991 0.028991 0.001527
+v -0.027903 0.027903 0.000000
+v -0.028673 0.028672 0.000447
+v -0.020500 0.035507 0.001527
+v -0.019730 0.034174 0.000000
+v -0.020275 0.035116 0.000447
+v -0.010612 0.039603 0.001527
+v -0.010213 0.038116 0.000000
+v -0.010495 0.039167 0.000447
+vn -0.2539 0.9477 -0.1935
+vn -0.2539 0.9477 0.1935
+vn 0.0000 0.9811 0.1935
+vn 0.0000 0.9811 -0.1935
+vn 0.0000 0.0000 -1.0000
+vn -0.1478 0.1478 -0.9779
+vn -0.1045 0.1810 -0.9779
+vn 0.2539 0.9477 0.1935
+vn 0.2539 0.9477 -0.1935
+vn 0.4905 0.8496 0.1935
+vn 0.4905 0.8496 -0.1935
+vn -0.2090 0.0000 -0.9779
+vn -0.2019 0.0541 -0.9779
+vn 0.6937 0.6937 0.1935
+vn 0.6937 0.6937 -0.1935
+vn -0.1478 -0.1478 -0.9779
+vn -0.1810 -0.1045 -0.9779
+vn 0.8496 0.4905 0.1935
+vn 0.8496 0.4905 -0.1935
+vn 0.9477 0.2539 0.1935
+vn 0.9477 0.2539 -0.1935
+vn 0.9811 0.0000 0.1935
+vn 0.9811 0.0000 -0.1935
+vn 0.9477 -0.2539 0.1935
+vn 0.9477 -0.2539 -0.1935
+vn 0.1810 0.1045 -0.9779
+vn 0.2019 0.0541 -0.9779
+vn 0.1810 -0.1045 -0.9779
+vn 0.1478 -0.1478 -0.9779
+vn 0.8496 -0.4905 0.1935
+vn 0.8496 -0.4905 -0.1935
+vn 0.0541 0.2019 -0.9779
+vn 0.1045 0.1810 -0.9779
+vn 0.6937 -0.6937 0.1935
+vn 0.6937 -0.6937 -0.1935
+vn -0.0541 0.2019 -0.9779
+vn 0.4905 -0.8496 0.1935
+vn 0.4905 -0.8496 -0.1935
+vn -0.1810 0.1045 -0.9779
+vn 0.0541 -0.2019 -0.9779
+vn 0.0000 -0.2090 -0.9779
+vn 0.2539 -0.9477 0.1935
+vn 0.2539 -0.9477 -0.1935
+vn -0.2019 -0.0541 -0.9779
+vn 0.0000 -0.9811 0.1935
+vn 0.0000 -0.9811 -0.1935
+vn -0.0541 -0.2019 -0.9779
+vn -0.1045 -0.1810 -0.9779
+vn -0.2539 -0.9477 0.1935
+vn -0.2539 -0.9477 -0.1935
+vn 0.1045 -0.1810 -0.9779
+vn -0.4905 -0.8496 0.1935
+vn -0.4905 -0.8496 -0.1935
+vn 0.2019 -0.0541 -0.9779
+vn -0.6937 -0.6937 0.1935
+vn -0.6937 -0.6937 -0.1935
+vn 0.2090 0.0000 -0.9779
+vn -0.8496 -0.4905 0.1935
+vn -0.8496 -0.4905 -0.1935
+vn -0.9477 -0.2539 0.1935
+vn -0.9477 -0.2539 -0.1935
+vn -0.9811 0.0000 0.1935
+vn -0.9811 0.0000 -0.1935
+vn -0.9477 0.2539 0.1935
+vn -0.9477 0.2539 -0.1935
+vn -0.8496 0.4905 0.1935
+vn -0.8496 0.4905 -0.1935
+vn -0.6937 0.6937 0.1935
+vn -0.6937 0.6937 -0.1935
+vn -0.4905 0.8496 0.1935
+vn -0.4905 0.8496 -0.1935
+vn 0.0000 0.2090 -0.9779
+vn 0.1478 0.1478 -0.9779
+vn 0.0000 0.7203 0.6937
+vn 0.1864 0.6957 0.6937
+vn 0.0000 0.2090 0.9779
+vn 0.0541 0.2019 0.9779
+vn 0.3601 0.6238 0.6937
+vn 0.1045 0.1810 0.9779
+vn 0.5093 0.5093 0.6937
+vn 0.1478 0.1478 0.9779
+vn 0.6238 0.3601 0.6937
+vn 0.1810 0.1045 0.9779
+vn 0.6957 0.1864 0.6937
+vn 0.2019 0.0541 0.9779
+vn 0.7203 0.0000 0.6937
+vn 0.2090 0.0000 0.9779
+vn 0.6957 -0.1864 0.6937
+vn 0.2019 -0.0541 0.9779
+vn 0.6238 -0.3601 0.6937
+vn 0.1810 -0.1045 0.9779
+vn 0.5093 -0.5093 0.6937
+vn 0.1478 -0.1478 0.9779
+vn 0.3601 -0.6238 0.6937
+vn 0.1045 -0.1810 0.9779
+vn 0.1864 -0.6957 0.6937
+vn 0.0541 -0.2019 0.9779
+vn 0.0000 -0.7203 0.6937
+vn 0.0000 -0.2090 0.9779
+vn -0.1864 -0.6957 0.6937
+vn -0.0541 -0.2019 0.9779
+vn -0.3601 -0.6238 0.6937
+vn -0.1045 -0.1810 0.9779
+vn -0.5093 -0.5093 0.6937
+vn -0.1478 -0.1478 0.9779
+vn -0.6238 -0.3601 0.6937
+vn -0.1810 -0.1045 0.9779
+vn -0.6957 -0.1864 0.6937
+vn -0.2019 -0.0541 0.9779
+vn -0.7203 0.0000 0.6937
+vn -0.2090 0.0000 0.9779
+vn -0.6957 0.1864 0.6937
+vn -0.2019 0.0541 0.9779
+vn -0.6238 0.3601 0.6937
+vn -0.1810 0.1045 0.9779
+vn -0.5093 0.5093 0.6937
+vn -0.1478 0.1478 0.9779
+vn -0.3601 0.6238 0.6937
+vn -0.1045 0.1810 0.9779
+vn -0.1864 0.6957 0.6937
+vn -0.0541 0.2019 0.9779
+vn 0.0000 0.0000 1.0000
+vn 0.1864 0.6957 -0.6937
+vn 0.0000 0.7203 -0.6937
+vn 0.3601 0.6238 -0.6937
+vn 0.5093 0.5093 -0.6937
+vn 0.6238 0.3601 -0.6937
+vn 0.6957 0.1864 -0.6937
+vn 0.7203 0.0000 -0.6937
+vn 0.6957 -0.1864 -0.6937
+vn 0.6238 -0.3601 -0.6937
+vn 0.5093 -0.5093 -0.6937
+vn 0.3601 -0.6238 -0.6937
+vn 0.1864 -0.6957 -0.6937
+vn 0.0000 -0.7203 -0.6937
+vn -0.1864 -0.6957 -0.6937
+vn -0.3601 -0.6238 -0.6937
+vn -0.5093 -0.5093 -0.6937
+vn -0.6238 -0.3601 -0.6937
+vn -0.6957 -0.1864 -0.6937
+vn -0.7203 0.0000 -0.6937
+vn -0.6957 0.1864 -0.6937
+vn -0.6238 0.3601 -0.6937
+vn -0.5093 0.5093 -0.6937
+vn -0.3601 0.6238 -0.6937
+vn -0.1864 0.6957 -0.6937
+s 1
+f 144//1 72//2 2//3 76//4
+f 1//5 139//6 142//7
+f 76//4 2//3 6//8 78//9
+f 78//9 6//8 9//10 81//11
+f 1//5 130//12 133//13
+f 81//11 9//10 12//14 84//15
+f 1//5 121//16 124//17
+f 84//15 12//14 15//18 87//19
+f 87//19 15//18 18//20 90//21
+f 90//21 18//20 21//22 93//23
+f 93//23 21//22 24//24 96//25
+f 1//5 88//26 91//27
+f 1//5 100//28 103//29
+f 96//25 24//24 27//30 99//31
+f 1//5 79//32 82//33
+f 99//31 27//30 30//34 102//35
+f 1//5 142//7 145//36
+f 102//35 30//34 33//37 105//38
+f 1//5 133//13 136//39
+f 1//5 109//40 112//41
+f 105//38 33//37 36//42 108//43
+f 1//5 124//17 127//44
+f 108//43 36//42 39//45 111//46
+f 1//5 115//47 118//48
+f 111//46 39//45 42//49 114//50
+f 1//5 106//51 109//40
+f 1//5 118//48 121//16
+f 114//50 42//49 45//52 117//53
+f 1//5 97//54 100//28
+f 117//53 45//52 48//55 120//56
+f 1//5 91//27 94//57
+f 120//56 48//55 51//58 123//59
+f 123//59 51//58 54//60 126//61
+f 126//61 54//60 57//62 129//63
+f 1//5 136//39 139//6
+f 129//63 57//62 60//64 132//65
+f 1//5 127//44 130//12
+f 132//65 60//64 63//66 135//67
+f 135//67 63//66 66//68 138//69
+f 1//5 112//41 115//47
+f 138//69 66//68 69//70 141//71
+f 1//5 103//29 106//51
+f 141//71 69//70 72//2 144//1
+f 1//5 94//57 97//54
+f 1//5 75//72 79//32
+f 1//5 85//73 88//26
+f 1//5 82//33 85//73
+f 6//8 2//3 4//74 7//75
+f 7//75 4//74 3//76 5//77
+f 9//10 6//8 7//75 10//78
+f 10//78 7//75 5//77 8//79
+f 12//14 9//10 10//78 13//80
+f 13//80 10//78 8//79 11//81
+f 15//18 12//14 13//80 16//82
+f 16//82 13//80 11//81 14//83
+f 18//20 15//18 16//82 19//84
+f 19//84 16//82 14//83 17//85
+f 21//22 18//20 19//84 22//86
+f 22//86 19//84 17//85 20//87
+f 24//24 21//22 22//86 25//88
+f 25//88 22//86 20//87 23//89
+f 27//30 24//24 25//88 28//90
+f 28//90 25//88 23//89 26//91
+f 30//34 27//30 28//90 31//92
+f 31//92 28//90 26//91 29//93
+f 33//37 30//34 31//92 34//94
+f 34//94 31//92 29//93 32//95
+f 36//42 33//37 34//94 37//96
+f 37//96 34//94 32//95 35//97
+f 39//45 36//42 37//96 40//98
+f 40//98 37//96 35//97 38//99
+f 42//49 39//45 40//98 43//100
+f 43//100 40//98 38//99 41//101
+f 45//52 42//49 43//100 46//102
+f 46//102 43//100 41//101 44//103
+f 48//55 45//52 46//102 49//104
+f 49//104 46//102 44//103 47//105
+f 51//58 48//55 49//104 52//106
+f 52//106 49//104 47//105 50//107
+f 54//60 51//58 52//106 55//108
+f 55//108 52//106 50//107 53//109
+f 57//62 54//60 55//108 58//110
+f 58//110 55//108 53//109 56//111
+f 60//64 57//62 58//110 61//112
+f 61//112 58//110 56//111 59//113
+f 63//66 60//64 61//112 64//114
+f 64//114 61//112 59//113 62//115
+f 66//68 63//66 64//114 67//116
+f 67//116 64//114 62//115 65//117
+f 69//70 66//68 67//116 70//118
+f 70//118 67//116 65//117 68//119
+f 72//2 69//70 70//118 73//120
+f 73//120 70//118 68//119 71//121
+f 2//3 72//2 73//120 4//74
+f 4//74 73//120 71//121 3//76
+f 20//87 17//85 74//122
+f 11//81 8//79 74//122
+f 3//76 71//121 74//122
+f 68//119 65//117 74//122
+f 59//113 56//111 74//122
+f 50//107 47//105 74//122
+f 41//101 38//99 74//122
+f 32//95 29//93 74//122
+f 23//89 20//87 74//122
+f 14//83 11//81 74//122
+f 5//77 3//76 74//122
+f 71//121 68//119 74//122
+f 62//115 59//113 74//122
+f 53//109 50//107 74//122
+f 44//103 41//101 74//122
+f 35//97 32//95 74//122
+f 26//91 23//89 74//122
+f 17//85 14//83 74//122
+f 8//79 5//77 74//122
+f 65//117 62//115 74//122
+f 56//111 53//109 74//122
+f 47//105 44//103 74//122
+f 38//99 35//97 74//122
+f 29//93 26//91 74//122
+f 76//4 78//9 80//123 77//124
+f 77//124 80//123 79//32 75//72
+f 78//9 81//11 83//125 80//123
+f 80//123 83//125 82//33 79//32
+f 81//11 84//15 86//126 83//125
+f 83//125 86//126 85//73 82//33
+f 84//15 87//19 89//127 86//126
+f 86//126 89//127 88//26 85//73
+f 87//19 90//21 92//128 89//127
+f 89//127 92//128 91//27 88//26
+f 90//21 93//23 95//129 92//128
+f 92//128 95//129 94//57 91//27
+f 93//23 96//25 98//130 95//129
+f 95//129 98//130 97//54 94//57
+f 96//25 99//31 101//131 98//130
+f 98//130 101//131 100//28 97//54
+f 99//31 102//35 104//132 101//131
+f 101//131 104//132 103//29 100//28
+f 102//35 105//38 107//133 104//132
+f 104//132 107//133 106//51 103//29
+f 105//38 108//43 110//134 107//133
+f 107//133 110//134 109//40 106//51
+f 108//43 111//46 113//135 110//134
+f 110//134 113//135 112//41 109//40
+f 111//46 114//50 116//136 113//135
+f 113//135 116//136 115//47 112//41
+f 114//50 117//53 119//137 116//136
+f 116//136 119//137 118//48 115//47
+f 117//53 120//56 122//138 119//137
+f 119//137 122//138 121//16 118//48
+f 120//56 123//59 125//139 122//138
+f 122//138 125//139 124//17 121//16
+f 123//59 126//61 128//140 125//139
+f 125//139 128//140 127//44 124//17
+f 126//61 129//63 131//141 128//140
+f 128//140 131//141 130//12 127//44
+f 129//63 132//65 134//142 131//141
+f 131//141 134//142 133//13 130//12
+f 132//65 135//67 137//143 134//142
+f 134//142 137//143 136//39 133//13
+f 135//67 138//69 140//144 137//143
+f 137//143 140//144 139//6 136//39
+f 138//69 141//71 143//145 140//144
+f 140//144 143//145 142//7 139//6
+f 141//71 144//1 146//146 143//145
+f 143//145 146//146 145//36 142//7
+f 144//1 76//4 77//124 146//146
+f 146//146 77//124 75//72 145//36
+f 1//5 145//36 75//72
+o handle_col.001_Cube.009
+v -0.003600 0.080633 0.059432
+v -0.005474 0.079067 0.059309
+v -0.005036 0.080199 0.059398
+v -0.005477 0.077925 0.066460
+v -0.003600 0.079455 0.066958
+v -0.005046 0.079026 0.066818
+v 0.005474 0.079067 0.059309
+v 0.003600 0.080633 0.059432
+v 0.005036 0.080199 0.059398
+v 0.003600 0.079455 0.066958
+v 0.005477 0.077925 0.066460
+v 0.005046 0.079026 0.066818
+v -0.003599 0.080629 0.050000
+v -0.005471 0.079081 0.050000
+v -0.005023 0.080207 0.050000
+v 0.005471 0.079081 0.050000
+v 0.003599 0.080629 0.050000
+v 0.005023 0.080207 0.050000
+v -0.005474 0.079067 0.040691
+v -0.003600 0.080633 0.040568
+v -0.005036 0.080199 0.040602
+v -0.003600 0.079455 0.033042
+v -0.005477 0.077925 0.033540
+v -0.005046 0.079026 0.033182
+v 0.003600 0.080633 0.040568
+v 0.005474 0.079067 0.040691
+v 0.005036 0.080199 0.040602
+v 0.005477 0.077925 0.033540
+v 0.003600 0.079455 0.033042
+v 0.005046 0.079026 0.033182
+v -0.005472 0.075182 0.050000
+v -0.003602 0.073666 0.050000
+v -0.005029 0.074082 0.050000
+v 0.003602 0.073666 0.050000
+v 0.005472 0.075177 0.050000
+v 0.005030 0.074081 0.050000
+v 0.005464 0.074276 0.065273
+v 0.003594 0.072835 0.064794
+v 0.004992 0.073209 0.064919
+v 0.003600 0.073692 0.058871
+v 0.005468 0.075191 0.059001
+v 0.005015 0.074095 0.058907
+v -0.005468 0.075196 0.059001
+v -0.003599 0.073692 0.058871
+v -0.005014 0.074096 0.058907
+v -0.003593 0.072835 0.064794
+v -0.005464 0.074281 0.065274
+v -0.004991 0.073210 0.064919
+v 0.003594 0.072835 0.035206
+v 0.005464 0.074276 0.034727
+v 0.004992 0.073209 0.035081
+v 0.005468 0.075191 0.040999
+v 0.003600 0.073692 0.041129
+v 0.005015 0.074095 0.041093
+v -0.003599 0.073692 0.041129
+v -0.005468 0.075196 0.040999
+v -0.005014 0.074096 0.041093
+v -0.005464 0.074281 0.034726
+v -0.003593 0.072835 0.035206
+v -0.004991 0.073210 0.035081
+v 0.000000 0.076121 0.065870
+v 0.000000 0.076121 0.034130
+vn -0.9826 0.1857 0.0000
+vn -0.9812 -0.1929 0.0000
+vn -0.9825 -0.1859 -0.0129
+vn -0.9815 0.1907 0.0146
+vn -0.9815 0.1907 -0.0146
+vn -0.9825 -0.1859 0.0129
+vn -0.7388 -0.0633 -0.6709
+vn -0.7237 -0.3338 -0.6040
+vn -0.1399 -0.9901 0.0000
+vn -0.1397 -0.9877 0.0698
+vn 0.1394 -0.9878 0.0698
+vn 0.1395 -0.9902 0.0000
+vn 0.1394 -0.9878 -0.0698
+vn 0.0944 -0.8650 0.4928
+vn -0.0946 -0.8650 0.4927
+vn -0.1397 -0.9877 -0.0698
+vn -0.0946 -0.8650 -0.4927
+vn 0.0944 -0.8650 -0.4928
+vn 0.9826 0.1857 0.0000
+vn 0.9811 -0.1932 0.0000
+vn 0.9824 -0.1862 0.0130
+vn 0.9815 0.1907 -0.0146
+vn -0.1448 0.9894 0.0000
+vn 0.1448 0.9894 0.0000
+vn 0.1456 0.9864 -0.0767
+vn -0.1456 0.9864 -0.0767
+vn 0.1159 0.5509 -0.8265
+vn -0.1159 0.5509 -0.8265
+vn 0.9815 0.1907 0.0146
+vn 0.9824 -0.1862 -0.0130
+vn -0.1456 0.9864 0.0767
+vn 0.1456 0.9864 0.0767
+vn -0.7237 -0.3338 0.6040
+vn -0.7388 -0.0633 0.6709
+vn 0.7388 -0.0632 0.6709
+vn 0.7237 -0.3340 0.6039
+vn -0.1159 0.5509 0.8265
+vn 0.1159 0.5509 0.8265
+vn 0.7237 -0.3340 -0.6039
+vn 0.7388 -0.0633 -0.6709
+vn 0.6697 0.7404 0.0575
+vn 0.5433 0.4040 0.7359
+vn -0.5433 0.4040 0.7359
+vn -0.6697 0.7404 0.0575
+vn -0.6767 0.7362 0.0000
+vn 0.6767 0.7362 0.0000
+vn 0.6697 0.7404 -0.0575
+vn 0.5433 0.4040 -0.7359
+vn -0.5433 0.4040 -0.7359
+vn -0.6697 0.7404 -0.0575
+vn 0.6762 -0.7349 -0.0517
+vn 0.5124 -0.7210 0.4665
+vn -0.5128 -0.7206 0.4666
+vn -0.6767 -0.7344 -0.0517
+vn -0.6685 -0.7437 0.0000
+vn 0.6680 -0.7442 0.0000
+vn 0.6762 -0.7349 0.0517
+vn 0.5124 -0.7210 -0.4665
+vn -0.5128 -0.7206 -0.4666
+vn -0.6767 -0.7344 0.0517
+vn 0.0000 -0.3106 0.9505
+vn 0.0000 -0.3106 -0.9505
+s 1
+f 160//147 177//148 189//149 148//150
+f 165//151 202//152 177//148 160//147
+f 169//153 204//154 202//152 165//151
+f 178//155 201//156 199//157 180//158
+f 186//159 184//160 192//161 190//162
+f 199//157 201//156 205//163 195//164
+f 178//155 180//158 186//159 190//162
+f 162//165 181//166 198//167 172//168
+f 159//169 163//170 171//171 166//172
+f 171//171 175//173 168//174 166//172
+f 153//175 187//176 181//166 162//165
+f 159//169 147//177 154//178 163//170
+f 148//150 189//149 193//179 150//180
+f 157//181 183//182 187//176 153//175
+f 154//178 147//177 151//183 156//184
+f 172//168 198//167 196//185 174//186
+f 157//181 153//175 155//187 158//188
+f 158//188 155//187 154//178 156//184
+f 148//150 150//180 152//189 149//190
+f 149//190 152//189 151//183 147//177
+f 147//177 159//169 161//191 149//190
+f 149//190 161//191 160//147 148//150
+f 163//170 154//178 155//187 164//192
+f 164//192 155//187 153//175 162//165
+f 175//173 171//171 173//193 176//194
+f 176//194 173//193 172//168 174//186
+f 166//172 168//174 170//195 167//196
+f 167//196 170//195 169//153 165//151
+f 165//151 160//147 161//191 167//196
+f 167//196 161//191 159//169 166//172
+f 162//165 172//168 173//193 164//192
+f 164//192 173//193 171//171 163//170
+f 184//160 186//159 188//197 185//198
+f 185//198 188//197 187//176 183//182
+f 190//162 192//161 194//199 191//200
+f 191//200 194//199 193//179 189//149
+f 189//149 177//148 179//201 191//200
+f 191//200 179//201 178//155 190//162
+f 181//166 187//176 188//197 182//202
+f 182//202 188//197 186//159 180//158
+f 196//185 198//167 200//203 197//204
+f 197//204 200//203 199//157 195//164
+f 202//152 204//154 206//205 203//206
+f 203//206 206//205 205//163 201//156
+f 201//156 178//155 179//201 203//206
+f 203//206 179//201 177//148 202//152
+f 180//158 199//157 200//203 182//202
+f 182//202 200//203 198//167 181//166
+f 184//160 185//198 207//207
+f 157//181 158//188 207//207
+f 150//180 193//179 207//207
+f 185//198 183//182 207//207
+f 192//161 184//160 207//207
+f 156//184 151//183 207//207
+f 158//188 156//184 207//207
+f 193//179 194//199 207//207
+f 151//183 152//189 207//207
+f 194//199 192//161 207//207
+f 183//182 157//181 207//207
+f 152//189 150//180 207//207
+f 174//186 196//185 208//208
+f 196//185 197//204 208//208
+f 175//173 176//194 208//208
+f 197//204 195//164 208//208
+f 176//194 174//186 208//208
+f 168//174 175//173 208//208
+f 195//164 205//163 208//208
+f 205//163 206//205 208//208
+f 169//153 170//195 208//208
+f 206//205 204//154 208//208
+f 204//154 169//153 208//208
+f 170//195 168//174 208//208
+o handle_col.002_Cube.008
+v -0.003600 0.079455 0.033042
+v -0.005477 0.077925 0.033540
+v -0.005046 0.079026 0.033182
+v -0.003600 0.076049 0.026358
+v -0.005477 0.074748 0.027303
+v -0.005046 0.075684 0.026623
+v -0.003600 0.070744 0.021053
+v -0.005477 0.069799 0.022354
+v -0.005046 0.070479 0.021418
+v -0.003600 0.064059 0.017647
+v -0.005477 0.063562 0.019177
+v -0.005046 0.063920 0.018076
+v 0.005477 0.063562 0.019177
+v 0.003600 0.064059 0.017647
+v 0.005046 0.063920 0.018076
+v 0.005477 0.069799 0.022354
+v 0.003600 0.070744 0.021053
+v 0.005046 0.070479 0.021418
+v 0.005477 0.074748 0.027303
+v 0.003600 0.076049 0.026358
+v 0.005046 0.075684 0.026623
+v 0.005477 0.077925 0.033540
+v 0.003600 0.079455 0.033042
+v 0.005046 0.079026 0.033182
+v 0.003594 0.072835 0.035206
+v 0.005464 0.074276 0.034727
+v 0.004992 0.073209 0.035081
+v -0.005464 0.074281 0.034726
+v -0.003593 0.072835 0.035206
+v -0.004991 0.073210 0.035081
+v 0.003594 0.070415 0.030451
+v 0.005464 0.071645 0.029558
+v 0.004994 0.070735 0.030219
+v -0.005464 0.071649 0.029555
+v -0.003593 0.070415 0.030451
+v -0.004993 0.070735 0.030218
+v 0.003594 0.066651 0.026687
+v 0.005464 0.067544 0.025457
+v 0.004994 0.066883 0.026367
+v -0.005464 0.067547 0.025453
+v -0.003593 0.066650 0.026687
+v -0.004993 0.066884 0.026366
+v 0.003594 0.061896 0.024267
+v 0.005464 0.062374 0.022825
+v 0.004992 0.062021 0.023893
+v -0.005464 0.062376 0.022821
+v -0.003593 0.061896 0.024267
+v -0.004991 0.062021 0.023892
+v 0.000000 0.076121 0.034130
+v 0.000000 0.062972 0.020980
+vn -0.9812 0.1562 -0.1134
+vn -0.9838 -0.1451 0.1054
+vn -0.7259 0.0844 0.6826
+vn -0.7394 0.3416 0.5801
+vn -0.9812 0.1134 -0.1562
+vn -0.9838 -0.1054 0.1451
+vn -0.7394 -0.5801 -0.3416
+vn -0.7259 -0.6826 -0.0844
+vn 0.0953 -0.4129 0.9057
+vn -0.0955 -0.4130 0.9057
+vn -0.1395 -0.8012 0.5819
+vn 0.1391 -0.8012 0.5819
+vn -0.1395 -0.5819 0.8012
+vn 0.1391 -0.5819 0.8012
+vn -0.0955 -0.9057 0.4130
+vn 0.0953 -0.9057 0.4129
+vn 0.9812 0.1135 -0.1562
+vn 0.9837 -0.1056 0.1454
+vn 0.7259 -0.6826 -0.0842
+vn 0.7394 -0.5801 -0.3416
+vn 0.1171 0.9313 0.3449
+vn 0.1485 0.8000 -0.5813
+vn -0.1485 0.8000 -0.5813
+vn -0.1171 0.9313 0.3449
+vn 0.1485 0.5813 -0.8000
+vn -0.1485 0.5813 -0.8000
+vn 0.7394 0.3416 0.5801
+vn 0.7259 0.0842 0.6826
+vn 0.9837 -0.1454 0.1056
+vn 0.9812 0.1562 -0.1135
+vn 0.1171 -0.3449 -0.9313
+vn -0.1171 -0.3449 -0.9313
+vn 0.5451 0.7577 0.3587
+vn 0.6664 0.6031 -0.4382
+vn -0.6664 0.6031 -0.4382
+vn -0.5451 0.7577 0.3587
+vn 0.6664 0.4382 -0.6031
+vn -0.6664 0.4382 -0.6031
+vn 0.5451 -0.3587 -0.7577
+vn -0.5451 -0.3587 -0.7577
+vn 0.5130 -0.3110 0.8000
+vn 0.6854 -0.5892 0.4279
+vn -0.6859 -0.5888 0.4276
+vn -0.5135 -0.3107 0.7998
+vn 0.6854 -0.4279 0.5892
+vn -0.6859 -0.4276 0.5888
+vn 0.5130 -0.8000 0.3110
+vn -0.5135 -0.7998 0.3107
+vn 0.0000 0.3106 0.9505
+vn 0.0000 -0.9505 -0.3106
+s 1
+f 213//209 242//210 236//211 210//212
+f 216//213 248//214 242//210 213//209
+f 219//215 254//216 248//214 216//213
+f 233//217 237//218 243//219 239//220
+f 239//220 243//219 249//221 245//222
+f 245//222 249//221 255//223 251//224
+f 224//225 246//226 252//227 221//228
+f 231//229 228//230 212//231 209//232
+f 228//230 225//233 215//234 212//231
+f 230//235 234//236 240//237 227//238
+f 225//233 222//239 218//240 215//234
+f 227//238 240//237 246//226 224//225
+f 228//230 231//229 232//241 229//242
+f 229//242 232//241 230//235 227//238
+f 209//232 212//231 214//243 211//244
+f 211//244 214//243 213//209 210//212
+f 225//233 228//230 229//242 226//245
+f 226//245 229//242 227//238 224//225
+f 212//231 215//234 217//246 214//243
+f 214//243 217//246 216//213 213//209
+f 222//239 225//233 226//245 223//247
+f 223//247 226//245 224//225 221//228
+f 215//234 218//240 220//248 217//246
+f 217//246 220//248 219//215 216//213
+f 240//237 234//236 235//249 241//250
+f 241//250 235//249 233//217 239//220
+f 236//211 242//210 244//251 238//252
+f 238//252 244//251 243//219 237//218
+f 246//226 240//237 241//250 247//253
+f 247//253 241//250 239//220 245//222
+f 242//210 248//214 250//254 244//251
+f 244//251 250//254 249//221 243//219
+f 252//227 246//226 247//253 253//255
+f 253//255 247//253 245//222 251//224
+f 248//214 254//216 256//256 250//254
+f 250//254 256//256 255//223 249//221
+f 211//244 210//212 257//257
+f 238//252 237//218 257//257
+f 235//249 234//236 257//257
+f 232//241 231//229 257//257
+f 231//229 209//232 257//257
+f 210//212 236//211 257//257
+f 230//235 232//241 257//257
+f 233//217 235//249 257//257
+f 237//218 233//217 257//257
+f 236//211 238//252 257//257
+f 209//232 211//244 257//257
+f 234//236 230//235 257//257
+f 218//240 222//239 258//258
+f 223//247 221//228 258//258
+f 253//255 251//224 258//258
+f 252//227 253//255 258//258
+f 222//239 223//247 258//258
+f 221//228 252//227 258//258
+f 251//224 255//223 258//258
+f 255//223 256//256 258//258
+f 219//215 220//248 258//258
+f 254//216 219//215 258//258
+f 256//256 254//216 258//258
+f 220//248 218//240 258//258
+o handle_col_Cube.007
+v -0.005477 0.077925 0.066460
+v -0.003600 0.079455 0.066958
+v -0.005046 0.079026 0.066818
+v -0.005477 0.074748 0.072697
+v -0.003600 0.076049 0.073642
+v -0.005046 0.075684 0.073377
+v -0.005477 0.069799 0.077646
+v -0.003600 0.070744 0.078947
+v -0.005046 0.070479 0.078582
+v -0.005477 0.063562 0.080823
+v -0.003600 0.064059 0.082353
+v -0.005046 0.063920 0.081924
+v 0.003600 0.064059 0.082353
+v 0.005477 0.063562 0.080823
+v 0.005046 0.063920 0.081924
+v 0.003600 0.070744 0.078947
+v 0.005477 0.069799 0.077646
+v 0.005046 0.070479 0.078582
+v 0.003600 0.076049 0.073642
+v 0.005477 0.074748 0.072697
+v 0.005046 0.075684 0.073377
+v 0.003600 0.079455 0.066958
+v 0.005477 0.077925 0.066460
+v 0.005046 0.079026 0.066818
+v 0.005464 0.074276 0.065273
+v 0.003594 0.072835 0.064794
+v 0.004992 0.073209 0.064919
+v -0.003593 0.072835 0.064794
+v -0.005464 0.074281 0.065274
+v -0.004991 0.073210 0.064919
+v 0.005464 0.071645 0.070442
+v 0.003594 0.070415 0.069549
+v 0.004994 0.070735 0.069781
+v -0.003593 0.070415 0.069549
+v -0.005464 0.071649 0.070445
+v -0.004993 0.070735 0.069782
+v 0.005464 0.067544 0.074543
+v 0.003594 0.066651 0.073313
+v 0.004994 0.066883 0.073633
+v -0.003593 0.066650 0.073313
+v -0.005464 0.067547 0.074547
+v -0.004993 0.066884 0.073634
+v 0.005464 0.062374 0.077175
+v 0.003594 0.061896 0.075733
+v 0.004992 0.062021 0.076107
+v -0.003593 0.061896 0.075733
+v -0.005464 0.062376 0.077179
+v -0.004991 0.062021 0.076108
+v 0.000000 0.062972 0.079020
+v 0.000000 0.076121 0.065870
+vn -0.7394 0.3416 -0.5801
+vn -0.7259 0.0844 -0.6826
+vn -0.9838 -0.1451 -0.1054
+vn -0.9812 0.1562 0.1134
+vn -0.9838 -0.1054 -0.1451
+vn -0.9812 0.1134 0.1562
+vn -0.7259 -0.6826 0.0844
+vn -0.7394 -0.5801 0.3416
+vn 0.0953 -0.4129 -0.9057
+vn 0.1391 -0.8012 -0.5819
+vn -0.1395 -0.8012 -0.5819
+vn -0.0955 -0.4130 -0.9057
+vn 0.1391 -0.5819 -0.8012
+vn -0.1395 -0.5819 -0.8012
+vn 0.0953 -0.9057 -0.4129
+vn -0.0955 -0.9057 -0.4130
+vn 0.9812 0.1562 0.1135
+vn 0.9837 -0.1454 -0.1056
+vn 0.7259 0.0842 -0.6826
+vn 0.7394 0.3416 -0.5801
+vn 0.1485 0.5813 0.8000
+vn -0.1485 0.5813 0.8000
+vn -0.1171 -0.3449 0.9313
+vn 0.1171 -0.3449 0.9313
+vn 0.9812 0.1135 0.1562
+vn 0.9837 -0.1056 -0.1454
+vn 0.1485 0.8000 0.5813
+vn -0.1485 0.8000 0.5813
+vn 0.7394 -0.5801 0.3416
+vn 0.7259 -0.6826 0.0842
+vn 0.1171 0.9313 -0.3449
+vn -0.1171 0.9313 -0.3449
+vn 0.5451 0.7577 -0.3587
+vn 0.6664 0.6031 0.4382
+vn -0.6664 0.6031 0.4382
+vn -0.5451 0.7577 -0.3587
+vn 0.6664 0.4382 0.6031
+vn -0.6664 0.4382 0.6031
+vn 0.5451 -0.3587 0.7577
+vn -0.5451 -0.3587 0.7577
+vn 0.5130 -0.3110 -0.8000
+vn 0.6854 -0.5892 -0.4279
+vn -0.6859 -0.5888 -0.4276
+vn -0.5135 -0.3107 -0.7998
+vn 0.6854 -0.4279 -0.5892
+vn -0.6859 -0.4276 -0.5888
+vn 0.5130 -0.8000 -0.3110
+vn -0.5135 -0.7998 -0.3107
+vn 0.0000 -0.9505 0.3106
+vn 0.0000 0.3106 -0.9505
+s 1
+f 259//259 287//260 293//261 262//262
+f 262//262 293//261 299//263 265//264
+f 265//264 299//263 305//265 268//266
+f 284//267 290//268 292//269 286//270
+f 290//268 296//271 298//272 292//269
+f 296//271 302//273 304//274 298//272
+f 278//275 289//276 283//277 281//278
+f 274//279 266//280 269//281 271//282
+f 275//283 295//284 289//276 278//275
+f 277//285 263//286 266//280 274//279
+f 272//287 301//288 295//284 275//283
+f 280//289 260//290 263//286 277//285
+f 278//275 281//278 282//291 279//292
+f 279//292 282//291 280//289 277//285
+f 259//259 262//262 264//293 261//294
+f 261//294 264//293 263//286 260//290
+f 275//283 278//275 279//292 276//295
+f 276//295 279//292 277//285 274//279
+f 262//262 265//264 267//296 264//293
+f 264//293 267//296 266//280 263//286
+f 272//287 275//283 276//295 273//297
+f 273//297 276//295 274//279 271//282
+f 265//264 268//266 270//298 267//296
+f 267//296 270//298 269//281 266//280
+f 290//268 284//267 285//299 291//300
+f 291//300 285//299 283//277 289//276
+f 286//270 292//269 294//301 288//302
+f 288//302 294//301 293//261 287//260
+f 296//271 290//268 291//300 297//303
+f 297//303 291//300 289//276 295//284
+f 292//269 298//272 300//304 294//301
+f 294//301 300//304 299//263 293//261
+f 302//273 296//271 297//303 303//305
+f 303//305 297//303 295//284 301//288
+f 298//272 304//274 306//306 300//304
+f 300//304 306//306 305//265 299//263
+f 301//288 272//287 307//307
+f 304//274 302//273 307//307
+f 305//265 306//306 307//307
+f 269//281 270//298 307//307
+f 268//266 305//265 307//307
+f 270//298 268//266 307//307
+f 306//306 304//274 307//307
+f 271//282 269//281 307//307
+f 303//305 301//288 307//307
+f 273//297 271//282 307//307
+f 272//287 273//297 307//307
+f 302//273 303//305 307//307
+f 284//267 286//270 308//308
+f 259//259 261//294 308//308
+f 286//270 288//302 308//308
+f 281//278 283//277 308//308
+f 288//302 287//260 308//308
+f 261//294 260//290 308//308
+f 282//291 281//278 308//308
+f 260//290 280//289 308//308
+f 285//299 284//267 308//308
+f 287//260 259//259 308//308
+f 283//277 285//299 308//308
+f 280//289 282//291 308//308
+o handle_col.004_Cube.006
+v -0.003627 0.038527 0.081890
+v -0.005507 0.038527 0.080294
+v -0.005059 0.038527 0.081441
+v 0.005507 0.038527 0.080294
+v 0.003627 0.038527 0.081890
+v 0.005059 0.038527 0.081441
+v -0.005474 0.056411 0.081965
+v -0.003600 0.056534 0.083531
+v -0.005036 0.056500 0.083097
+v -0.005477 0.063562 0.080823
+v -0.003600 0.064059 0.082353
+v -0.005046 0.063920 0.081924
+v 0.003600 0.056534 0.083531
+v 0.005474 0.056411 0.081965
+v 0.005036 0.056500 0.083097
+v 0.003600 0.064059 0.082353
+v 0.005477 0.063562 0.080823
+v 0.005046 0.063920 0.081924
+v 0.003628 0.038527 0.070184
+v 0.005471 0.038527 0.072262
+v 0.004983 0.038527 0.070850
+v -0.003627 0.038527 0.070184
+v -0.005471 0.038527 0.072267
+v -0.004983 0.038527 0.070852
+v 0.005464 0.062374 0.077175
+v 0.003594 0.061896 0.075733
+v 0.004992 0.062021 0.076107
+v -0.003593 0.061896 0.075733
+v -0.005464 0.062376 0.077179
+v -0.004991 0.062021 0.076108
+v 0.005468 0.056103 0.078090
+v 0.003600 0.055974 0.076590
+v 0.005015 0.056009 0.076993
+v -0.003599 0.055974 0.076590
+v -0.005468 0.056103 0.078095
+v -0.005014 0.056010 0.076994
+v -0.005477 0.045795 0.081957
+v -0.003603 0.045916 0.083535
+v -0.005045 0.045879 0.083093
+v 0.005477 0.045795 0.081957
+v 0.003603 0.045916 0.083535
+v 0.005045 0.045879 0.083093
+v -0.003608 0.045257 0.076495
+v -0.005460 0.045676 0.078027
+v -0.004995 0.045382 0.076898
+v 0.003609 0.045257 0.076495
+v 0.005460 0.045676 0.078022
+v 0.004996 0.045381 0.076897
+v 0.000000 0.062972 0.079020
+v 0.000000 0.038527 0.076154
+vn 0.7254 -0.6787 0.1145
+vn 0.7789 -0.5862 -0.2228
+vn 0.9869 0.0385 -0.1568
+vn 0.9807 -0.0226 0.1941
+vn -0.7388 0.6709 -0.0633
+vn -0.7237 0.6040 -0.3338
+vn -0.9822 -0.0130 -0.1871
+vn -0.9820 0.0147 0.1883
+vn 0.1468 0.0770 0.9861
+vn -0.1468 0.0770 0.9861
+vn -0.1455 -0.1062 0.9836
+vn 0.1455 -0.1062 0.9836
+vn 0.0944 0.4928 -0.8650
+vn 0.1393 -0.0667 -0.9880
+vn -0.1396 -0.0667 -0.9879
+vn -0.0946 0.4927 -0.8650
+vn 0.1446 0.3836 -0.9121
+vn -0.1450 0.3836 -0.9120
+vn 0.9820 0.0147 0.1884
+vn 0.9822 -0.0130 -0.1874
+vn 0.7237 0.6039 -0.3340
+vn 0.7388 0.6709 -0.0633
+vn -0.9807 -0.0226 0.1941
+vn -0.9869 0.0384 -0.1565
+vn -0.7789 -0.5864 -0.2225
+vn -0.7254 -0.6787 0.1145
+vn 0.1423 -0.3544 -0.9242
+vn -0.1427 -0.3542 -0.9242
+vn 0.1159 0.8265 0.5509
+vn -0.1159 0.8265 0.5509
+vn -0.1004 -0.7506 0.6530
+vn 0.1004 -0.7506 0.6530
+vn 0.5433 0.7359 0.4040
+vn 0.6718 0.0577 0.7385
+vn -0.6718 0.0577 0.7385
+vn -0.5433 0.7359 0.4040
+vn -0.6652 -0.0813 0.7422
+vn 0.5236 -0.6789 0.5146
+vn 0.6652 -0.0813 0.7422
+vn 0.5124 0.4665 -0.7209
+vn 0.6761 -0.0500 -0.7350
+vn -0.6767 -0.0500 -0.7345
+vn -0.5128 0.4666 -0.7206
+vn -0.6990 0.2732 -0.6608
+vn 0.5548 -0.3842 -0.7379
+vn 0.6985 0.2735 -0.6613
+vn -0.5236 -0.6789 0.5146
+vn -0.5553 -0.3845 -0.7374
+vn 0.0000 0.9505 -0.3106
+vn 0.0000 -1.0000 0.0000
+s 1
+f 312//309 328//310 355//311 348//312
+f 318//313 337//314 343//315 315//316
+f 321//317 316//318 346//319 349//320
+f 334//321 340//322 342//323 336//324
+f 340//322 354//325 351//326 342//323
+f 322//327 339//328 333//329 325//330
+f 345//331 352//332 331//333 310//334
+f 354//325 327//335 330//336 351//326
+f 348//312 355//311 339//328 322//327
+f 324//337 319//338 316//318 321//317
+f 349//320 346//319 309//339 313//340
+f 315//316 343//315 352//332 345//331
+f 322//327 325//330 326//341 323//342
+f 323//342 326//341 324//337 321//317
+f 318//313 315//316 317//343 320//344
+f 320//344 317//343 316//318 319//338
+f 346//319 316//318 317//343 347//345
+f 347//345 317//343 315//316 345//331
+f 349//320 313//340 314//346 350//347
+f 350//347 314//346 312//309 348//312
+f 340//322 334//321 335//348 341//349
+f 341//349 335//348 333//329 339//328
+f 336//324 342//323 344//350 338//351
+f 338//351 344//350 343//315 337//314
+f 352//332 343//315 344//350 353//352
+f 353//352 344//350 342//323 351//326
+f 355//311 328//310 329//353 356//354
+f 356//354 329//353 327//335 354//325
+f 309//339 346//319 347//345 311//355
+f 311//355 347//345 345//331 310//334
+f 321//317 349//320 350//347 323//342
+f 323//342 350//347 348//312 322//327
+f 331//333 352//332 353//352 332//356
+f 332//356 353//352 351//326 330//336
+f 339//328 355//311 356//354 341//349
+f 341//349 356//354 354//325 340//322
+f 325//330 333//329 357//357
+f 320//344 319//338 357//357
+f 336//324 338//351 357//357
+f 319//338 324//337 357//357
+f 326//341 325//330 357//357
+f 338//351 337//314 357//357
+f 324//337 326//341 357//357
+f 335//348 334//321 357//357
+f 337//314 318//313 357//357
+f 334//321 336//324 357//357
+f 318//313 320//344 357//357
+f 333//329 335//348 357//357
+f 314//346 313//340 358//358
+f 309//339 311//355 358//358
+f 328//310 312//309 358//358
+f 331//333 332//356 358//358
+f 327//335 329//353 358//358
+f 311//355 310//334 358//358
+f 310//334 331//333 358//358
+f 313//340 309//339 358//358
+f 329//353 328//310 358//358
+f 312//309 314//346 358//358
+f 330//336 327//335 358//358
+f 332//356 330//336 358//358
+o handle_col.003_Cube.005
+v -0.003631 0.038527 0.018578
+v -0.005510 0.038527 0.020185
+v -0.005068 0.038527 0.019031
+v 0.003631 0.038527 0.018578
+v 0.005510 0.038527 0.020185
+v 0.005068 0.038527 0.019031
+v -0.003600 0.056534 0.016469
+v -0.005474 0.056411 0.018035
+v -0.005036 0.056500 0.016903
+v -0.003600 0.064059 0.017647
+v -0.005477 0.063562 0.019177
+v -0.005046 0.063920 0.018076
+v 0.005474 0.056411 0.018035
+v 0.003600 0.056534 0.016469
+v 0.005036 0.056500 0.016903
+v 0.005477 0.063562 0.019177
+v 0.003600 0.064059 0.017647
+v 0.005046 0.063920 0.018076
+v 0.005472 0.038527 0.027531
+v 0.003627 0.038527 0.029560
+v 0.004985 0.038527 0.028916
+v -0.003626 0.038527 0.029560
+v -0.005472 0.038527 0.027526
+v -0.004984 0.038527 0.028915
+v 0.003594 0.061896 0.024267
+v 0.005464 0.062374 0.022825
+v 0.004992 0.062021 0.023893
+v -0.005464 0.062376 0.022821
+v -0.003593 0.061896 0.024267
+v -0.004991 0.062021 0.023892
+v 0.003600 0.055974 0.023410
+v 0.005468 0.056103 0.021910
+v 0.005015 0.056009 0.023007
+v -0.005468 0.056103 0.021905
+v -0.003599 0.055974 0.023410
+v -0.005014 0.056010 0.023006
+v -0.003604 0.045959 0.016460
+v -0.005478 0.045808 0.018049
+v -0.005052 0.045914 0.016909
+v 0.003604 0.045959 0.016460
+v 0.005478 0.045808 0.018049
+v 0.005052 0.045914 0.016909
+v -0.005460 0.045677 0.021971
+v -0.003608 0.045270 0.023501
+v -0.004995 0.045392 0.023098
+v 0.005460 0.045677 0.021977
+v 0.003608 0.045270 0.023501
+v 0.004996 0.045392 0.023099
+v 0.000000 0.062972 0.020980
+v 0.000000 0.038527 0.023966
+vn 0.9802 -0.0302 -0.1954
+vn 0.9866 0.0386 0.1584
+vn 0.7770 -0.5891 0.2217
+vn 0.7235 -0.6819 -0.1074
+vn 0.1472 0.0772 -0.9861
+vn 0.1452 -0.1337 -0.9803
+vn -0.1452 -0.1337 -0.9803
+vn -0.1472 0.0772 -0.9861
+vn 0.0944 0.4928 0.8650
+vn -0.0946 0.4927 0.8650
+vn -0.1396 -0.0670 0.9879
+vn 0.1392 -0.0670 0.9880
+vn -0.1444 0.3729 0.9165
+vn 0.1440 0.3729 0.9166
+vn -0.7235 -0.6819 -0.1074
+vn -0.7770 -0.5892 0.2214
+vn -0.9867 0.0385 0.1581
+vn -0.9802 -0.0302 -0.1954
+vn 0.9821 0.0148 -0.1877
+vn 0.9821 -0.0130 0.1876
+vn 0.1159 0.8265 -0.5509
+vn -0.1159 0.8265 -0.5509
+vn -0.9821 0.0148 -0.1877
+vn -0.9822 -0.0130 0.1873
+vn -0.7237 0.6040 0.3338
+vn -0.7388 0.6709 0.0633
+vn 0.7388 0.6709 0.0632
+vn 0.7237 0.6039 0.3340
+vn -0.1404 -0.3648 0.9204
+vn 0.1400 -0.3650 0.9204
+vn 0.0978 -0.7696 -0.6309
+vn -0.0978 -0.7696 -0.6309
+vn 0.5433 0.7359 -0.4040
+vn 0.6724 0.0579 -0.7378
+vn -0.6724 0.0579 -0.7378
+vn -0.5433 0.7359 -0.4040
+vn -0.6623 -0.1034 -0.7421
+vn 0.5222 -0.6927 -0.4974
+vn 0.6623 -0.1034 -0.7421
+vn 0.5124 0.4665 0.7209
+vn 0.6760 -0.0501 0.7352
+vn -0.6765 -0.0501 0.7347
+vn -0.5128 0.4666 0.7206
+vn -0.6982 0.2655 0.6648
+vn 0.5524 -0.3917 0.7357
+vn 0.6977 0.2658 0.6653
+vn -0.5222 -0.6927 -0.4974
+vn -0.5529 -0.3920 0.7352
+vn 0.0000 0.9505 0.3106
+vn 0.0000 -1.0000 0.0000
+s 1
+f 399//359 404//360 377//361 363//362
+f 372//363 398//364 395//365 365//366
+f 383//367 387//368 393//369 389//370
+f 389//370 393//369 402//371 405//372
+f 360//373 381//374 401//375 396//376
+f 371//377 390//378 404//360 399//359
+f 375//379 372//363 365//366 368//380
+f 366//381 392//382 386//383 369//384
+f 374//385 384//386 390//378 371//377
+f 405//372 402//371 380//387 378//388
+f 398//364 362//389 359//390 395//365
+f 396//376 401//375 392//382 366//381
+f 372//363 375//379 376//391 373//392
+f 373//392 376//391 374//385 371//377
+f 368//380 365//366 367//393 370//394
+f 370//394 367//393 366//381 369//384
+f 396//376 366//381 367//393 397//395
+f 397//395 367//393 365//366 395//365
+f 399//359 363//362 364//396 400//397
+f 400//397 364//396 362//389 398//364
+f 390//378 384//386 385//398 391//399
+f 391//399 385//398 383//367 389//370
+f 386//383 392//382 394//400 388//401
+f 388//401 394//400 393//369 387//368
+f 402//371 393//369 394//400 403//402
+f 403//402 394//400 392//382 401//375
+f 405//372 378//388 379//403 406//404
+f 406//404 379//403 377//361 404//360
+f 360//373 396//376 397//395 361//405
+f 361//405 397//395 395//365 359//390
+f 371//377 399//359 400//397 373//392
+f 373//392 400//397 398//364 372//363
+f 380//387 402//371 403//402 382//406
+f 382//406 403//402 401//375 381//374
+f 389//370 405//372 406//404 391//399
+f 391//399 406//404 404//360 390//378
+f 388//401 387//368 407//407
+f 374//385 376//391 407//407
+f 385//398 384//386 407//407
+f 387//368 383//367 407//407
+f 384//386 374//385 407//407
+f 368//380 370//394 407//407
+f 383//367 385//398 407//407
+f 369//384 386//383 407//407
+f 370//394 369//384 407//407
+f 386//383 388//401 407//407
+f 375//379 368//380 407//407
+f 376//391 375//379 407//407
+f 364//396 363//362 408//408
+f 361//405 359//390 408//408
+f 381//374 360//373 408//408
+f 382//406 381//374 408//408
+f 377//361 379//403 408//408
+f 360//373 361//405 408//408
+f 363//362 377//361 408//408
+f 359//390 362//389 408//408
+f 379//403 378//388 408//408
+f 362//389 364//396 408//408
+f 378//388 380//387 408//408
+f 380//387 382//406 408//408
diff --git a/examples/pybullet/gym/pybullet_data/uvmap.png b/examples/pybullet/gym/pybullet_data/uvmap.png
new file mode 100644
index 000000000..81b00f401
Binary files /dev/null and b/examples/pybullet/gym/pybullet_data/uvmap.png differ
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/__init__.py b/examples/pybullet/gym/pybullet_envs/minitaur/__init__.py
index 8b1378917..e69de29bb 100644
--- a/examples/pybullet/gym/pybullet_envs/minitaur/__init__.py
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/__init__.py
@@ -1 +0,0 @@
-
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/__init__.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/__init__.py
index 8b1378917..e69de29bb 100644
--- a/examples/pybullet/gym/pybullet_envs/minitaur/agents/__init__.py
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/__init__.py
@@ -1 +0,0 @@
-
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/__init__.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/com_height_estimator.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/com_height_estimator.py
new file mode 100644
index 000000000..3838c74aa
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/com_height_estimator.py
@@ -0,0 +1,76 @@
+"""State estimator for robot height."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import copy
+from typing import Any, Sequence
+
+import gin
+
+from pybullet_envs.minitaur.agents.baseline_controller import state_estimator
+
+
+@gin.configurable
+class COMHeightEstimator(state_estimator.StateEstimatorBase):
+ """Estimate the CoM height using base orientation and local toe positions."""
+
+ def __init__(
+ self,
+ robot: Any,
+ com_estimate_leg_indices: Sequence[int] = (0, 1, 2, 3),
+ initial_com_height: float = 0.45,
+ ):
+ """Initializes the class.
+
+ Args:
+ robot: A quadruped robot.
+ com_estimate_leg_indices: Leg indices used in estimating the CoM height.
+ initial_com_height: CoM height used during reset.
+ """
+ self._robot = robot
+ self._com_estimate_leg_indices = com_estimate_leg_indices
+ self._initial_com_estimate_leg_indices = copy.copy(com_estimate_leg_indices)
+ self._initial_com_height = initial_com_height
+ self.reset(0)
+
+ @property
+ def estimated_com_height(self):
+ return self._com_height
+
+ def reset(self, current_time):
+ del current_time
+ self._com_height = self._initial_com_height
+ self._com_estimate_leg_indices = copy.copy(
+ self._initial_com_estimate_leg_indices)
+
+ def update(self, current_time):
+ del current_time
+ local_toe_poses = self._robot.foot_positions(
+ position_in_world_frame=False)
+ # We rotate the local toe positions into the world orientation centered
+ # at the robot base to estimate the height of the robot.
+ world_toe_poses = []
+ for toe_p in local_toe_poses:
+ world_toe_poses.append(
+ self._robot.pybullet_client.multiplyTransforms(
+ positionA=(0, 0, 0),
+ orientationA=self._robot.base_orientation_quaternion,
+ positionB=toe_p,
+ orientationB=(0, 0, 0, 1))[0])
+ mean_height = 0.0
+ num_toe_in_contact = 0
+ for leg_id in self._com_estimate_leg_indices:
+ mean_height += world_toe_poses[leg_id][2]
+ num_toe_in_contact += 1
+ mean_height /= num_toe_in_contact
+ self._com_height = abs(mean_height)
+
+ @property
+ def com_estimate_leg_indices(self):
+ return self._com_estimate_leg_indices
+
+ @com_estimate_leg_indices.setter
+ def com_estimate_leg_indices(self, leg_indices):
+ self._com_estimate_leg_indices = leg_indices
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/com_velocity_estimator.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/com_velocity_estimator.py
new file mode 100644
index 000000000..609f83271
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/com_velocity_estimator.py
@@ -0,0 +1,83 @@
+"""State estimator."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+from typing import Any, Sequence
+
+import gin
+import numpy as np
+
+from pybullet_envs.minitaur.agents.baseline_controller import state_estimator
+from pybullet_envs.minitaur.robots.safety.python import moving_window_filter
+
+_DEFAULT_WINDOW_SIZE = 20
+
+
+@gin.configurable
+class COMVelocityEstimator(state_estimator.StateEstimatorBase):
+ """Estimate the CoM velocity using on board sensors.
+
+
+ Requires knowledge about the base velocity in world frame, which for example
+ can be obtained from a MoCap system. This estimator will filter out the high
+ frequency noises in the velocity so the results can be used with controllers
+ reliably.
+
+ """
+
+ def __init__(
+ self,
+ robot: Any,
+ window_size: int = _DEFAULT_WINDOW_SIZE,
+ ):
+ self._robot = robot
+ self._window_size = window_size
+ self.reset(0)
+
+ @property
+ def com_velocity_body_yaw_aligned_frame(self) -> Sequence[float]:
+ """The base velocity projected in the body aligned inertial frame.
+
+ The body aligned frame is a intertia frame that coincides with the body
+ frame, but has a zero relative velocity/angular velocity to the world frame.
+
+ Returns:
+ The com velocity in body aligned frame.
+ """
+ return self._com_velocity_body_yaw_aligned_frame
+
+ @property
+ def com_velocity_world_frame(self) -> Sequence[float]:
+ return self._com_velocity_world_frame
+
+ def reset(self, current_time):
+ del current_time
+ # We use a moving window filter to reduce the noise in velocity estimation.
+ self._velocity_filter_x = moving_window_filter.MovingWindowFilter(
+ window_size=self._window_size)
+ self._velocity_filter_y = moving_window_filter.MovingWindowFilter(
+ window_size=self._window_size)
+ self._velocity_filter_z = moving_window_filter.MovingWindowFilter(
+ window_size=self._window_size)
+ self._com_velocity_world_frame = np.array((0, 0, 0))
+ self._com_velocity_body_yaw_aligned_frame = np.array((0, 0, 0))
+
+ def update(self, current_time):
+ del current_time
+ velocity = self._robot.base_velocity
+
+ vx = self._velocity_filter_x.CalculateAverage(velocity[0])
+ vy = self._velocity_filter_y.CalculateAverage(velocity[1])
+ vz = self._velocity_filter_z.CalculateAverage(velocity[2])
+ self._com_velocity_world_frame = np.array((vx, vy, vz))
+
+ base_orientation = self._robot.base_orientation_quaternion
+ _, inverse_rotation = self._robot.pybullet_client.invertTransform(
+ (0, 0, 0), base_orientation)
+
+ self._com_velocity_body_yaw_aligned_frame, _ = (
+ self._robot.pybullet_client.multiplyTransforms(
+ (0, 0, 0), inverse_rotation, self._com_velocity_world_frame,
+ (0, 0, 0, 1)))
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/dummy_gait_generator.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/dummy_gait_generator.py
new file mode 100644
index 000000000..51747904f
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/dummy_gait_generator.py
@@ -0,0 +1,87 @@
+"""A dummy gait generator module for storing gait patterns from higher-level controller."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import google_type_annotations
+from __future__ import print_function
+
+import copy
+from typing import Any, Sequence
+
+import gin
+
+from pybullet_envs.minitaur.agents.baseline_controller import gait_generator
+
+LAIKAGO_STANDING = (
+ gait_generator.LegState.STANCE,
+ gait_generator.LegState.STANCE,
+ gait_generator.LegState.STANCE,
+ gait_generator.LegState.STANCE,
+)
+
+
+@gin.configurable
+class DummyGaitGenerator(gait_generator.GaitGenerator):
+ """A module for storing quadruped gait patterns from high-level controller.
+
+ This module stores the state for each leg of a quadruped robot. The data is
+ used by the stance leg controller to determine the appropriate contact forces.
+ A high-level controller, such as a neural network policy, can be used to
+ control the gait pattern and set corresponding leg states.
+ """
+
+ def __init__(
+ self,
+ robot: Any,
+ initial_leg_state: Sequence[gait_generator.LegState] = LAIKAGO_STANDING,
+ ):
+ """Initializes the class.
+
+ Args:
+ robot: A quadruped robot.
+ initial_leg_state: The desired initial swing/stance state of legs indexed
+ by their id.
+ """
+ self._robot = robot
+ if len(initial_leg_state) != len(
+ list(self._robot.urdf_loader.get_end_effector_id_dict().values())):
+ raise ValueError(
+ "The number of leg states should be the same of number of legs.")
+ self._initial_leg_state = initial_leg_state
+ self._leg_state = list(initial_leg_state)
+ self._desired_leg_state = list(initial_leg_state)
+
+ self.reset(0)
+
+ def reset(self, current_time):
+ del current_time
+ self._leg_state = list(self._initial_leg_state)
+ self._desired_leg_state = list(self._initial_leg_state)
+
+ @property
+ def desired_leg_state(self) -> Sequence[gait_generator.LegState]:
+ """The desired leg SWING/STANCE states.
+
+ Returns:
+ The SWING/STANCE states for all legs.
+
+ """
+ return self._desired_leg_state
+
+ @desired_leg_state.setter
+ def desired_leg_state(self, state):
+ self._desired_leg_state = copy.deepcopy(state)
+
+ @property
+ def leg_state(self) -> Sequence[gait_generator.LegState]:
+ """The leg state after considering contact with ground.
+
+ Returns:
+ The actual state of each leg after accounting for contacts.
+ """
+ return self._leg_state
+
+ @leg_state.setter
+ def leg_state(self, state):
+ self._leg_state = copy.deepcopy(state)
+
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/foot_stepper.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/foot_stepper.py
new file mode 100644
index 000000000..8a79a236f
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/foot_stepper.py
@@ -0,0 +1,199 @@
+# Lint as: python3
+"""A state machine that steps each foot for a static gait. Experimental code."""
+
+import copy
+import math
+
+import numpy as np
+
+
+class StepInput(object):
+
+ def __init__(self):
+ self.base_com_pos = np.array([0, 0, 0])
+ self.base_com_orn = np.array([0, 0, 0, 1])
+ self.toe_pos_world = np.array([0, 0, 0] * 4)
+ self.new_pos_world = np.array([0, 0, 0])
+
+
+class StepOutput(object):
+
+ def __init__(self, new_toe_pos_world):
+ self.new_toe_pos_world = new_toe_pos_world
+
+
+class FootStepper(object):
+ """This class computes desired foot placement for a quadruped robot."""
+
+ def __init__(self, bullet_client, toe_ids, toe_pos_local_ref):
+ self.bullet_client = bullet_client
+ self.state_time = 0.
+ self.toe_ids = toe_ids
+ self.toe_pos_local_ref = toe_pos_local_ref
+ self.sphere_uid = self.bullet_client.loadURDF(
+ "sphere_small.urdf", [0, 0, 0], useFixedBase=True)
+ self.is_far = True
+ self.max_shift = 0.0008
+ self.far_bound = 0.005
+ self.close_bound = 0.03
+
+ self.move_swing_foot = False
+ self.amp = 0.2
+ alpha = 1
+
+ # Loads/draws spheres for debugging purpose. The spheres visualize the
+ # target COM, the current COM and the target foothold location.
+ self.sphere_uid_centroid = self.bullet_client.loadURDF(
+ "sphere_small.urdf", [0, 0, 0], useFixedBase=True)
+ self.bullet_client.changeVisualShape(
+ self.sphere_uid_centroid, -1, rgbaColor=[1, 1, 0, alpha])
+
+ # Disable collision since visualization spheres should not collide with the
+ # robot.
+ self.bullet_client.setCollisionFilterGroupMask(self.sphere_uid_centroid, -1,
+ 0, 0)
+
+ self.sphere_uid_com = self.bullet_client.loadURDF(
+ "sphere_small.urdf", [0, 0, 0], useFixedBase=True)
+ self.bullet_client.changeVisualShape(
+ self.sphere_uid_com, -1, rgbaColor=[1, 0, 1, alpha])
+ self.bullet_client.setCollisionFilterGroupMask(self.sphere_uid_com, -1, 0,
+ 0)
+
+ self.bullet_client.setCollisionFilterGroupMask(self.sphere_uid, -1, 0, 0)
+ self.feetindices = [1, 3, 0, 2]
+ self.swing_foot_index1 = 0
+ self.swing_foot_index = self.feetindices[self.swing_foot_index1]
+ self.colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1]]
+ self.support_vertices = [[1, 2, 3], [0, 2, 3], [0, 1, 3], [0, 1, 2]]
+ self.local_diff_y_threshold = 0.05
+ self.local_diff_y = 100
+ self.is_far = True
+ self.get_reference_pos_swing_foot()
+
+ def next_foot(self):
+ self.swing_foot_index1 = (self.swing_foot_index1 + 1) % 4
+ self.swing_foot_index = self.feetindices[self.swing_foot_index1]
+
+ def swing_foot(self):
+ self.move_swing_foot = True
+
+ def get_reference_pos_swing_foot(self):
+ self.new_pos_local = np.array(
+ self.toe_pos_local_ref[self.swing_foot_index])
+ return self.new_pos_local
+
+ def set_reference_pos_swing_foot(self, new_pos_local):
+ self.new_pos_local = new_pos_local
+
+ def is_com_stable(self):
+ ld2 = self.local_diff_y * self.local_diff_y
+ yaw_ok = ld2 < (self.local_diff_y_threshold * self.local_diff_y_threshold)
+ com_ok = not self.is_far
+ return com_ok and yaw_ok
+
+ def update(self, step_input):
+ """Updates the state machine and toe movements per state."""
+ base_com_pos = step_input.base_com_pos
+ base_com_orn = step_input.base_com_orn
+ base_com_pos_inv, base_com_orn_inv = self.bullet_client.invertTransform(
+ base_com_pos, base_com_orn)
+
+ dt = step_input.dt
+ self.bullet_client.resetBasePositionAndOrientation(self.sphere_uid,
+ step_input.new_pos_world,
+ [0, 0, 0, 1])
+ self.bullet_client.changeVisualShape(
+ self.sphere_uid, -1, rgbaColor=self.colors[self.swing_foot_index])
+
+ all_toes_pos_locals = []
+ for toe_pos_world in step_input.toe_pos_world:
+ toe_pos_local, _ = self.bullet_client.multiplyTransforms(
+ base_com_pos_inv, base_com_orn_inv, toe_pos_world, [0, 0, 0, 1])
+ all_toes_pos_locals.append(toe_pos_local)
+ all_toes_pos_locals = np.array(all_toes_pos_locals)
+ centroid_world = np.zeros(3)
+ for v in self.support_vertices[self.swing_foot_index]:
+ vtx_pos_world = step_input.toe_pos_world[v]
+ centroid_world += vtx_pos_world
+ centroid_world /= 3.
+
+ sphere_z_offset = 0.05
+ self.diff_world = base_com_pos - centroid_world
+ self.diff_world[2] = 0.
+ self.bullet_client.resetBasePositionAndOrientation(self.sphere_uid_centroid,
+ centroid_world,
+ [0, 0, 0, 1])
+ self.bullet_client.resetBasePositionAndOrientation(
+ self.sphere_uid_com,
+ [base_com_pos[0], base_com_pos[1], sphere_z_offset], [0, 0, 0, 1])
+
+ l = np.linalg.norm(self.diff_world)
+ if self.is_far:
+ bound = self.far_bound
+ else:
+ bound = self.close_bound
+
+ if l > bound:
+ self.diff_world *= self.max_shift * 0.5 / l
+ if not self.is_far:
+ self.is_far = True
+ else:
+ if self.is_far:
+ self.is_far = False
+
+ if not self.is_far:
+ self.diff_world = np.zeros(3)
+ for i in range(len(self.toe_pos_local_ref)):
+ toe = self.toe_pos_local_ref[i]
+ toe = [
+ toe[0] + self.diff_world[0], toe[1] + self.diff_world[1],
+ toe[2] + self.diff_world[2]
+ ]
+ self.toe_pos_local_ref[i] = toe
+
+ self.local_diff_y = self.toe_pos_local_ref[0][
+ 1] + self.toe_pos_local_ref[1][1] - self.toe_pos_local_ref[
+ 2][1] - self.toe_pos_local_ref[3][1]
+
+ self.yaw = 0
+ if self.local_diff_y < -self.local_diff_y_threshold:
+ self.yaw = 0.001
+ if self.local_diff_y > self.local_diff_y_threshold:
+ self.yaw = -0.001
+
+ yaw_trans = self.bullet_client.getQuaternionFromEuler([0, 0, self.yaw])
+
+ if not self.is_far:
+ for i in range(len(self.toe_pos_local_ref)):
+ toe = self.toe_pos_local_ref[i]
+ toe, _ = self.bullet_client.multiplyTransforms([0, 0, 0], yaw_trans,
+ toe, [0, 0, 0, 1])
+ self.toe_pos_local_ref[i] = toe
+
+ new_toe_pos_world = []
+
+ # Moves the swing foot to the target location.
+ if self.move_swing_foot:
+ if self.state_time <= 1:
+ self.state_time += 4 * dt
+ if self.state_time >= 1:
+ self.move_swing_foot = False
+ self.state_time = 0
+ self.toe_pos_local_ref[self.swing_foot_index] = self.new_pos_local
+ toe_pos_local_ref_copy = copy.deepcopy(self.toe_pos_local_ref)
+ old_pos = self.toe_pos_local_ref[self.swing_foot_index]
+ new_pos = [
+ old_pos[0] * (1 - self.state_time) + self.new_pos_local[0] *
+ (self.state_time), old_pos[1] * (1 - self.state_time) +
+ self.new_pos_local[1] * (self.state_time),
+ old_pos[2] * (1 - self.state_time) + self.new_pos_local[2] *
+ (self.state_time) + self.amp * math.sin(self.state_time * math.pi)
+ ]
+ toe_pos_local_ref_copy[self.swing_foot_index] = new_pos
+ for toe_pos_local in toe_pos_local_ref_copy:
+ new_toe_pos_world.append(self.bullet_client.multiplyTransforms(
+ base_com_pos, base_com_orn, toe_pos_local, [0, 0, 0, 1])[0])
+
+ step_output = StepOutput(new_toe_pos_world)
+ return step_output
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/gait_generator.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/gait_generator.py
new file mode 100644
index 000000000..61bd849ac
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/gait_generator.py
@@ -0,0 +1,32 @@
+"""Gait pattern planning module."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import abc
+import enum
+
+
+class LegState(enum.Enum):
+ """The state of a leg during locomotion."""
+ SWING = 0
+ STANCE = 1
+ # A swing leg that collides with the ground.
+ EARLY_CONTACT = 2
+ # A stance leg that loses contact.
+ LOSE_CONTACT = 3
+
+
+class GaitGenerator(object): # pytype: disable=ignored-metaclass
+ """Generates the leg swing/stance pattern for the robot."""
+
+ __metaclass__ = abc.ABCMeta
+
+ @abc.abstractmethod
+ def reset(self, current_time):
+ pass
+
+ @abc.abstractmethod
+ def update(self, current_time):
+ pass
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/imu_based_com_velocity_estimator.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/imu_based_com_velocity_estimator.py
new file mode 100644
index 000000000..4caea0906
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/imu_based_com_velocity_estimator.py
@@ -0,0 +1,216 @@
+"""State estimator."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+from typing import Any, Sequence
+
+from filterpy import kalman
+import gin
+import numpy as np
+
+from pybullet_envs.minitaur.agents.baseline_controller import state_estimator
+from pybullet_envs.minitaur.agents.baseline_controller import time_based_moving_window_filter
+from pybullet_envs.minitaur.envs_v2.sensors import accelerometer_sensor
+from pybullet_envs.minitaur.envs_v2.sensors import imu_sensor
+
+_DEFAULT_VELOCITY_FILTER_WINDOW = 0.2
+_DEFAULT_GYRO_FILTER_WINDOW = 0.1
+_DEFAULT_VELOCITY_CLIPPING = 0.8
+_STATE_DIMENSION = 3
+_GRAVITY = (0.0, 0.0, -9.8)
+
+
+@gin.configurable
+class IMUBasedCOMVelocityEstimator(state_estimator.StateEstimatorBase):
+ """Estimate the CoM velocity using IMU sensors and velocities of stance feet.
+
+
+ Estimates the com velocity of the robot using IMU data and stance feet
+ velocities fused by a Kalman Filter. Kalman Filter assumes the true state x
+ follows a linear dynamics: x'=Fx+Bu+w, where x' and x denots the
+ current and previous state of the robot, F is the state transition matrix,
+ B is the control-input matrix, and w is the noise in the dynamics, assuming to
+ follow a zero-mean multivariate normal distribution with covariance Q. It also
+ assumes that we can obtain a noisy observation of the state with the model
+ z=Hx+v, where z is the observed state, H is the observation matrix, and v is
+ a noise model, assuming to follow a zero-mean multivariate normal distribution
+ with covariance R. In our case, x is the CoM velocity of the robot,
+ F=B=H=eye(3), u=dt * CoM acceleration, which is obtained from accelerometer,
+ and the noisy observation z is obtained from the negated average velocities at
+ the end-effectors in contact with the ground.
+
+ """
+
+ def __init__(
+ self,
+ robot: Any,
+ use_sensor_interface: bool = True,
+ accelerometer_variance=0.1,
+ observation_variance=0.1,
+ initial_variance=0.1,
+ velocity_filter_window: float = _DEFAULT_VELOCITY_FILTER_WINDOW,
+ gyroscope_filter_window: float = _DEFAULT_GYRO_FILTER_WINDOW,
+ contact_detection_threshold: float = 0.0,
+ velocity_clipping: float = _DEFAULT_VELOCITY_CLIPPING,
+ ):
+ """Initializes the class.
+
+ Args:
+ robot: A quadruped robot.
+ use_sensor_interface: Whether to use the sensor interface to obtain the
+ IMU readings or directly get them from the robot class. Former should
+ be used in simulation to enable added latency and noise while latter
+ should be used on real robot for better performance.
+ accelerometer_variance: The estimated variance in the accelerometer
+ readings, used in the Kalman Filter.
+ observation_variance: The estimated variance in the observed CoM velocity
+ from the stance feet velocities, used in the Kalman Filter.
+ initial_variance: The variance of the initial distribution for the
+ estimated CoM variance.
+ velocity_filter_window: The filtering window (in time) used to smooth the
+ estimated CoM velocity.
+ gyroscope_filter_window: The filtering window (in time) used to smooth the
+ input gyroscope readings.
+ contact_detection_threshold: Threshold on the contact sensor readings to
+ determine whether the foot is in contact with the ground.
+ velocity_clipping: Clipping value for the estimated velocity to prevent
+ unrealistically large velocity estimations.
+ """
+ self._robot = robot
+ self._contact_detection_threshold = contact_detection_threshold
+ self._velocity_clipping = velocity_clipping
+ self._use_sensor_interface = use_sensor_interface
+
+ # Use the accelerometer and gyroscope sensor from the robot
+ if self._use_sensor_interface:
+ for sensor in self._robot.sensors:
+ if isinstance(sensor, accelerometer_sensor.AccelerometerSensor):
+ self._accelerometer = sensor
+ if isinstance(sensor, imu_sensor.IMUSensor):
+ self._gyroscope = sensor
+ assert hasattr(self, "_accelerometer") and self._accelerometer is not None
+ assert hasattr(self, "_gyroscope") and self._gyroscope is not None
+
+ # x is the underlying CoM velocity we want to estimate, z is the observed
+ # CoM velocity from the stance feet velocities, and u is the accelerometer
+ # readings.
+ self._filter = kalman.KalmanFilter(
+ dim_x=_STATE_DIMENSION,
+ dim_z=_STATE_DIMENSION,
+ dim_u=_STATE_DIMENSION)
+ # Initialize the state distribution to be a zero-mean multi-variate normal
+ # distribution with initial variance.
+ self._filter.x = np.zeros(_STATE_DIMENSION)
+ self._initial_variance = initial_variance
+ self._filter.P = np.eye(_STATE_DIMENSION) * self._initial_variance
+ # Covariance matrix for the control variable.
+ self._filter.Q = np.eye(_STATE_DIMENSION) * accelerometer_variance
+ # Covariance matrix for the observed states.
+ self._filter.R = np.eye(_STATE_DIMENSION) * observation_variance
+
+ # observation function (z=H*x+N(0,R))
+ self._filter.H = np.eye(_STATE_DIMENSION)
+ # state transition matrix (x'=F*x+B*u+N(0,Q))
+ self._filter.F = np.eye(_STATE_DIMENSION)
+ # Control transition matrix
+ self._filter.B = np.eye(_STATE_DIMENSION)
+
+ self._velocity_filter = time_based_moving_window_filter.TimeBasedMovingWindowFilter(
+ velocity_filter_window)
+ self._gyroscope_filter = time_based_moving_window_filter.TimeBasedMovingWindowFilter(
+ gyroscope_filter_window)
+
+ self.reset(0)
+
+ @property
+ def com_velocity_body_yaw_aligned_frame(self) -> Sequence[float]:
+ """The base velocity projected in the body yaw aligned inertial frame.
+
+ The body yaw aligned frame is a intertia frame where the z axis coincides
+ with the yaw of the robot base and the x and y axis coincides with the world
+ frame. It has a zero relative velocity/angular velocity
+ to the world frame.
+
+ Returns:
+ The com velocity in body yaw aligned frame.
+ """
+ clipped_velocity = np.clip(self._com_velocity_body_yaw_aligned_frame,
+ -self._velocity_clipping,
+ self._velocity_clipping)
+
+ return clipped_velocity
+
+ def reset(self, current_time):
+ del current_time
+ self._filter.x = np.zeros(_STATE_DIMENSION)
+ self._filter.P = np.eye(_STATE_DIMENSION) * self._initial_variance
+
+ self._com_velocity_body_yaw_aligned_frame = np.zeros(_STATE_DIMENSION)
+
+ self._velocity_filter.reset()
+ self._gyroscope_filter.reset()
+
+ # Use None instead of 0 in case of a big gap between reset and first step.
+ self._last_timestamp = None
+
+ def update(self, current_time):
+ del current_time
+ current_timestamp = self._robot.timestamp
+
+ # First time step
+ if self._last_timestamp is None:
+ delta_time_s = 0.0
+ else:
+ delta_time_s = current_timestamp - self._last_timestamp
+ self._last_timestamp = current_timestamp
+
+ if self._use_sensor_interface:
+ sensor_acc = np.array(self._accelerometer.get_observation())
+ gyroscope_reading = self._gyroscope.get_observation()
+ else:
+ sensor_acc = np.array(self._robot.base_acceleration_accelerometer)
+ gyroscope_reading = self._robot.base_roll_pitch_yaw
+
+ filtered_gyroscope_reading = self._gyroscope_filter.calculate_average(
+ gyroscope_reading, current_timestamp)
+ # The yaw angle is not used here because reliably estimating the yaw angle
+ # of the robot is in general difficult. This leads to a body yaw aligned
+ # inertia frame for the estimated velocity.
+ yaw_aligned_base_orientation = self._robot.pybullet_client.getQuaternionFromEuler(
+ (filtered_gyroscope_reading[0], filtered_gyroscope_reading[1], 0.0))
+
+ rot_mat = self._robot.pybullet_client.getMatrixFromQuaternion(
+ yaw_aligned_base_orientation)
+ rot_mat = np.array(rot_mat).reshape((_STATE_DIMENSION, _STATE_DIMENSION))
+ calibrated_acc = rot_mat.dot(sensor_acc) + np.array(_GRAVITY)
+ self._filter.predict(u=calibrated_acc * delta_time_s)
+ observed_velocities = []
+
+ foot_contact = [
+ np.linalg.norm(contact_force) > self._contact_detection_threshold
+ for contact_force in self._robot.feet_contact_forces()
+ ]
+ for leg_id in range(4):
+ if foot_contact[leg_id]:
+ jacobian = self._robot.compute_jacobian_for_one_leg(leg_id)
+ # Only pick the jacobian related to joint motors
+ # TODO(magicmelon): standardize the process of picking out relevant dofs
+ com_dof = self._robot.urdf_loader.com_dof
+ jacobian = jacobian[:,
+ com_dof + leg_id * 3:com_dof + (leg_id + 1) * 3]
+ joint_velocities = self._robot.motor_velocities[leg_id *
+ 3:(leg_id + 1) * 3]
+ leg_velocity_in_base_frame = jacobian.dot(joint_velocities)
+ base_velocity_in_base_frame = -leg_velocity_in_base_frame
+ observed_velocities.append(rot_mat.dot(base_velocity_in_base_frame))
+
+ if observed_velocities:
+ observed_velocities = np.mean(observed_velocities, axis=0)
+ self._filter.update(observed_velocities)
+
+ velocity = self._filter.x.copy()
+
+ self._com_velocity_body_yaw_aligned_frame = self._velocity_filter.calculate_average(
+ velocity, current_timestamp)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/leg_controller.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/leg_controller.py
new file mode 100644
index 000000000..731f7ae9f
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/leg_controller.py
@@ -0,0 +1,29 @@
+"""The leg controller class interface."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import abc
+from typing import Any
+
+
+class LegController(object): # pytype: disable=ignored-metaclass
+ """Generates the leg control signal."""
+
+ __metaclass__ = abc.ABCMeta
+
+ @abc.abstractmethod
+ def reset(self, current_time: float):
+ """Resets the controller's internal state."""
+ pass
+
+ @abc.abstractmethod
+ def update(self, current_time: float):
+ """Updates the controller's internal state."""
+ pass
+
+ @abc.abstractmethod
+ def get_action(self) -> Any:
+ """Gets the control signal e.g. torques/positions for the leg."""
+ pass
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/locomotion_controller.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/locomotion_controller.py
new file mode 100644
index 000000000..d9bca69f5
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/locomotion_controller.py
@@ -0,0 +1,96 @@
+"""A model based controller framework."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import time
+from typing import Any, Callable
+
+from pybullet_envs.minitaur.agents.baseline_controller import gait_generator as gait_generator_lib
+from pybullet_envs.minitaur.agents.baseline_controller import leg_controller as leg_controller_lib
+from pybullet_envs.minitaur.agents.baseline_controller import state_estimator as state_estimator_lib
+
+
+class LocomotionController(object):
+ """Generates the quadruped locomotion.
+
+ The actual effect of this controller depends on the composition of each
+ individual subcomponent.
+
+ """
+
+ def __init__(
+ self,
+ robot: Any,
+ gait_generator: gait_generator_lib.GaitGenerator,
+ state_estimator: state_estimator_lib.StateEstimatorBase,
+ swing_leg_controller: leg_controller_lib.LegController,
+ stance_leg_controller: leg_controller_lib.LegController,
+ clock: Callable[[], float] = None,
+ ):
+ """Initializes the class.
+
+ Args:
+ robot: A robot instance.
+ gait_generator: Generates the leg swing/stance pattern.
+ state_estimator: Estimates the state of the robot (e.g. center of mass
+ position or velocity that may not be observable from sensors).
+ swing_leg_controller: Generates motor actions for swing legs.
+ stance_leg_controller: Generates motor actions for stance legs.
+ clock: A real or fake clock source.
+ """
+ self._robot = robot
+ self._clock = clock
+ if clock is None:
+ self._clock = time.time
+ self._reset_time = self._clock()
+ self._time_since_reset = 0
+ self._gait_generator = gait_generator
+ self._state_estimator = state_estimator
+ self._swing_leg_controller = swing_leg_controller
+ self._stance_leg_controller = stance_leg_controller
+
+ @property
+ def swing_leg_controller(self):
+ return self._swing_leg_controller
+
+ @property
+ def stance_leg_controller(self):
+ return self._stance_leg_controller
+
+ @property
+ def gait_generator(self):
+ return self._gait_generator
+
+ @property
+ def state_estimator(self):
+ return self._state_estimator
+
+ def reset(self):
+ self._reset_time = self._clock()
+ self._time_since_reset = 0
+ self._gait_generator.reset(self._time_since_reset)
+ self._state_estimator.reset(self._time_since_reset)
+ self._swing_leg_controller.reset(self._time_since_reset)
+ self._stance_leg_controller.reset(self._time_since_reset)
+
+ def update(self):
+ self._time_since_reset = self._clock() - self._reset_time
+ self._gait_generator.update(self._time_since_reset)
+ self._state_estimator.update(self._time_since_reset)
+ self._swing_leg_controller.update(self._time_since_reset)
+ self._stance_leg_controller.update(self._time_since_reset)
+
+ def get_action(self):
+ """Returns the control ouputs (e.g. positions/torques) for all motors."""
+ swing_action = self._swing_leg_controller.get_action()
+ stance_action = self._stance_leg_controller.get_action()
+ action = []
+ for joint_id in range(self._robot.num_motors):
+ if joint_id in swing_action:
+ action.extend(swing_action[joint_id])
+ else:
+ assert joint_id in stance_action
+ action.extend(stance_action[joint_id])
+ return action
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/locomotion_controller_example.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/locomotion_controller_example.py
new file mode 100644
index 000000000..fd867950d
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/locomotion_controller_example.py
@@ -0,0 +1,190 @@
+r"""Laikago walking example using the locomotion controller framework.
+
+"""
+
+import gc
+import pickle
+
+from absl import app
+from absl import flags
+import numpy as np
+import scipy.interpolate
+
+
+from pybullet_envs.minitaur.agents.baseline_controller import locomotion_controller_setup
+from pybullet_envs.minitaur.envs_v2 import env_loader
+from pybullet_envs.minitaur.robots import robot_config
+
+FLAGS = flags.FLAGS
+flags.DEFINE_boolean("run_on_robot", False,
+ "whether to run in sim or on real hardware")
+flags.DEFINE_boolean(
+ "use_ground_truth_velocity", False,
+ "whether to use a ground truth velocity estimator (available in sim)")
+flags.DEFINE_enum("gait", "fast_trot",
+ ["fast_trot", "slow_trot", "walk", "stand"],
+ "The gait pattern to use")
+flags.DEFINE_boolean(
+ "use_keyboard_control", False,
+ "whether to use a keyboard to control or demo speed profile.")
+flags.DEFINE_string("log_path", None, "Path to save robot logs")
+flags.DEFINE_boolean("add_random_push", False,
+ "whether to add random push to the robot in simulation")
+
+_MAX_TIME_SECONDS = 100
+
+
+def _load_config(render=True, run_on_robot=False):
+ """Builds the environment for the quadruped robot.
+
+ Args:
+ render: Enable/disable rendering.
+ run_on_robot: Whether deploy to robot or run in sim.
+ """
+ if run_on_robot:
+ locomotion_controller_setup.load_real_config()
+ else:
+ locomotion_controller_setup.load_sim_config(render)
+ if FLAGS.add_random_push:
+ locomotion_controller_setup.add_random_push_config()
+
+
+def _generate_example_linear_angular_speed(t):
+ """Creates an example speed profile based on time for demo purpose."""
+ vx = 0.1
+ vy = 0.1
+ wz = 0.3
+ time_points = (0, 4, 7, 11, 13, 15, 17, 19, 100)
+ speed_points = ((0, 0, 0, 0), (vx, 0, 0, 0), (-vx, 0, 0, 0), (0, -vy, 0, 0),
+ (0, vy, 0, 0), (0, 0, 0, wz), (0, 0, 0, -wz), (0, 0, 0, 0),
+ (0, 0, 0, 0))
+
+ speed = scipy.interpolate.interp1d(
+ time_points,
+ speed_points,
+ kind="previous",
+ fill_value="extrapolate",
+ axis=0)(
+ t)
+
+ return speed[0:3], speed[3]
+
+
+def _update_speed_from_kb(kb, lin_speed, ang_speed):
+ """Updates the controller behavior parameters."""
+ if kb.is_keyboard_hit():
+ c = kb.get_input_character()
+ if c == "w":
+ lin_speed += np.array((0.05, 0, 0))
+ if c == "s":
+ lin_speed += np.array((-0.05, 0, 0))
+ if c == "q":
+ ang_speed += 0.1
+ if c == "e":
+ ang_speed += -0.1
+ if c == "a":
+ lin_speed += np.array((0, 0.05, 0))
+ if c == "d":
+ lin_speed += np.array((0, -0.05, 0))
+ if c == "r":
+ lin_speed = np.array([0.0, 0.0, 0.0])
+ ang_speed = 0.0
+
+ lin_speed[0] = np.clip(lin_speed[0], -0.2, 0.4)
+ lin_speed[1] = np.clip(lin_speed[1], -0.2, 0.2)
+ ang_speed = np.clip(ang_speed, -0.3, 0.3)
+ print("desired speed: ", lin_speed, ang_speed)
+
+ return lin_speed, ang_speed
+
+
+def _update_controller_params(controller, lin_speed, ang_speed):
+ controller.swing_leg_controller.desired_speed = lin_speed
+ controller.swing_leg_controller.desired_twisting_speed = ang_speed
+ controller.stance_leg_controller.desired_speed = lin_speed
+ controller.stance_leg_controller.desired_twisting_speed = ang_speed
+
+
+def _run_example(max_time=_MAX_TIME_SECONDS,
+ run_on_robot=False,
+ use_keyboard=False):
+ """Runs the locomotion controller example."""
+ if use_keyboard:
+ kb = keyboard_utils.KeyboardInput()
+
+ env = env_loader.load()
+ env.reset()
+
+ # To mitigate jittering from the python
+ gc.collect()
+
+ # Wait for the robot to be placed properly.
+ if run_on_robot:
+ input("Press Enter to continue when robot is ready.")
+
+ lin_speed = np.array([0.0, 0.0, 0.0])
+ ang_speed = 0.0
+
+ controller = locomotion_controller_setup.setup_controller(
+ env.robot, FLAGS.gait, run_on_robot, FLAGS.use_ground_truth_velocity)
+ controller.reset()
+
+ loop_start_time = env.get_time_since_reset()
+ loop_elapsed_time = 0
+ robot_log = {
+ "timestamps": [],
+ "motor_angles": [],
+ "motor_velocities": [],
+ "base_velocities": [],
+ "foot_positions": [],
+ "base_rollpitchyaw": [],
+ "base_angular_velocities": [],
+ "actions": []
+ }
+ try:
+ while loop_elapsed_time < max_time:
+ #if use_keyboard:
+ # lin_speed, ang_speed = _update_speed_from_kb(kb, lin_speed, ang_speed)
+ #else:
+ lin_speed, ang_speed = _generate_example_linear_angular_speed(
+ loop_elapsed_time)
+
+ # Needed before every call to get_action().
+ _update_controller_params(controller, lin_speed, ang_speed)
+ controller.update()
+ hybrid_action = controller.get_action()
+
+ # Log the robot data.
+ robot_log["timestamps"].append(env.robot.GetTimeSinceReset())
+ robot_log["motor_angles"].append(env.robot.motor_angles)
+ robot_log["motor_velocities"].append(env.robot.motor_velocities)
+ robot_log["base_velocities"].append(
+ controller.state_estimator.com_velocity_body_yaw_aligned_frame)
+ robot_log["foot_positions"].append(env.robot.foot_positions())
+ robot_log["base_rollpitchyaw"].append(env.robot.base_roll_pitch_yaw)
+ robot_log["base_angular_velocities"].append(
+ env.robot.base_roll_pitch_yaw_rate)
+ robot_log["actions"].append(hybrid_action)
+
+ env.step(hybrid_action)
+ loop_elapsed_time = env.get_time_since_reset() - loop_start_time
+
+ finally:
+ if FLAGS.run_on_robot:
+ # Apply zero torques to the robot.
+ env.robot.apply_action(
+ [0] * env.robot.num_motors,
+ motor_control_mode=robot_config.MotorControlMode.TORQUE)
+ if FLAGS.log_path:
+ pickle.dump(robot_log, gfile.Open(FLAGS.log_path + "/robot.log", "wb"))
+
+
+def main(argv):
+ del argv
+ _load_config(render=True, run_on_robot=FLAGS.run_on_robot)
+ _run_example(
+ run_on_robot=FLAGS.run_on_robot, use_keyboard=FLAGS.use_keyboard_control)
+
+
+if __name__ == "__main__":
+ app.run(main)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/locomotion_controller_in_scenario_set_example.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/locomotion_controller_in_scenario_set_example.py
new file mode 100644
index 000000000..71fefa7b5
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/locomotion_controller_in_scenario_set_example.py
@@ -0,0 +1,180 @@
+r"""ScenarioSet example for Laikago MPC controller.
+
+blaze run -c opt \
+//robotics/reinforcement_learning/minitaur/agents/baseline_controller\
+:locomotion_controller_in_scenario_set_example -- --gait=slow_trot \
+--add_random_push=True
+"""
+
+from absl import app
+from absl import flags
+import gin
+import numpy as np
+import scipy.interpolate
+
+from pybullet_envs.minitaur.agents.baseline_controller import locomotion_controller_setup
+from pybullet_envs.minitaur.envs_v2 import env_loader
+
+FLAGS = flags.FLAGS
+
+SCENARIO_SET_CONFIG = """
+import pybullet_envs.minitaur.envs_v2.scenarios.locomotion_simple_scenario_set
+
+include "google3/robotics/reinforcement_learning/minitaur/envs_v2/scenarios/default_scenario_set.gin"
+
+default_scenario_set/singleton.constructor = @locomotion_simple_scenario_set.LocomotionSimpleScenarioSet
+
+
+locomotion_simple_scenario_set.LocomotionSimpleScenarioSet.selector = "flat_ground"
+locomotion_gym_env.LocomotionGymEnv.task = @scenario_set.task()
+locomotion_gym_env.LocomotionGymEnv.scene = @scenario_set.scene()
+locomotion_gym_env.LocomotionGymEnv.env_randomizers = [
+ @scenario_set.env_randomizer()
+]
+"""
+
+_MAX_TIME_SECONDS = 30
+
+flags.DEFINE_enum("gait", "fast_trot",
+ ["fast_trot", "slow_trot", "walk", "stand"],
+ "The gait pattern to use")
+
+flags.DEFINE_boolean("add_random_push", False,
+ "whether to add random push to the robot in simulation")
+
+
+def _start_stop_profile(max_speed=0.5, axis=0, duration=3):
+ speed_profile = np.zeros((3, 4))
+
+ speed_profile[1, axis] = max_speed
+
+ return (0, 0.5, duration + 0.5), speed_profile.tolist()
+
+
+def _random_speed_profile(max_speed=1, axis=0, time_interval=1.0):
+ num_pts = 11
+ time_points = np.arange(num_pts) * time_interval
+
+ speed_profile = np.zeros((num_pts, 4))
+ speed_profile[:, axis] = np.random.uniform(0, max_speed, num_pts)
+ speed_profile[-1, :] = 0
+ return time_points.tolist(), speed_profile.tolist()
+
+
+def _body_height_profile(z_range=(0.3, 0.55)):
+ del z_range
+ # TODO(tingnan): Implement this.
+
+
+def _generate_linear_angular_speed(t, time_points, speed_points):
+ """Creates an example speed profile based on time for demo purpose."""
+
+ speed = scipy.interpolate.interp1d(
+ time_points,
+ speed_points,
+ kind="previous",
+ fill_value="extrapolate",
+ axis=0)(
+ t)
+
+ return speed[0:3], speed[3]
+
+
+def _update_controller_params(controller, lin_speed, ang_speed):
+ controller.swing_leg_controller.desired_speed = lin_speed
+ controller.swing_leg_controller.desired_twisting_speed = ang_speed
+ controller.stance_leg_controller.desired_speed = lin_speed
+ controller.stance_leg_controller.desired_twisting_speed = ang_speed
+
+
+def _gen_stability_test_start_stop():
+ """Generates the speed profile for start/stop tests."""
+ axis_to_name = {
+ 0: "velocity x",
+ 1: "velocity y",
+ 3: "angular velocity z",
+ }
+
+ axis_to_max_speed = {
+ 0: 1.0,
+ 1: 0.5,
+ 3: 2.5,
+ }
+
+ gait_multiplier = {
+ "slow_trot": 0.7,
+ "walk": 0.3,
+ "fast_trot": 1.0,
+ }
+
+ for axis in (0, 1, 3):
+ yield axis_to_name[axis], _start_stop_profile(
+ axis_to_max_speed[axis] * gait_multiplier[FLAGS.gait], axis)
+
+
+def _gen_stability_test_random():
+ """Generates the speed profile for random walking tests."""
+ axis_to_name = {
+ 0: "velocity x",
+ 1: "velocity y",
+ 3: "angular velocity z",
+ }
+
+ axis_to_max_speed = {
+ 0: 1.0,
+ 1: 0.5,
+ 3: 2.5,
+ }
+
+ gait_multiplier = {
+ "slow_trot": 0.7,
+ "walk": 0.3,
+ "fast_trot": 1.0,
+ }
+
+ for axis in (0, 1, 3):
+ yield axis_to_name[axis], _random_speed_profile(
+ axis_to_max_speed[axis] * gait_multiplier[FLAGS.gait], axis)
+
+
+def _test_stability(max_time=5, render=False, test_generator=None):
+ """Tests the stability of the controller using speed profiles."""
+ locomotion_controller_setup.load_sim_config(render=render)
+ gin.parse_config(SCENARIO_SET_CONFIG)
+ if FLAGS.add_random_push:
+ locomotion_controller_setup.add_random_push_config()
+
+ env = env_loader.load()
+ controller = locomotion_controller_setup.setup_controller(
+ env.robot, gait=FLAGS.gait)
+
+ for name, speed_profile in test_generator():
+ env.reset()
+ controller.reset()
+ current_time = 0
+ while current_time < max_time:
+ current_time = env.get_time_since_reset()
+ lin_speed, ang_speed = _generate_linear_angular_speed(
+ current_time, speed_profile[0], speed_profile[1])
+ _update_controller_params(controller, lin_speed, ang_speed)
+
+ # Needed before every call to get_action().
+ controller.update()
+ hybrid_action = controller.get_action()
+
+ _, _, done, _ = env.step(hybrid_action)
+ if done:
+ break
+ print(f"Scene name: flat ground. Random push: {FLAGS.add_random_push}. "
+ f"Survival time for {name} = {speed_profile[1]} is {current_time}")
+
+
+def main(argv):
+ del argv
+ _test_stability(render=True, test_generator=_gen_stability_test_start_stop)
+ _test_stability(
+ max_time=15, render=True, test_generator=_gen_stability_test_random)
+
+
+if __name__ == "__main__":
+ app.run(main)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/locomotion_controller_setup.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/locomotion_controller_setup.py
new file mode 100644
index 000000000..afaed6cb7
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/locomotion_controller_setup.py
@@ -0,0 +1,175 @@
+"""The common setups for MPC based locoomtion controller environments."""
+import time
+import gin
+import numpy as np
+
+from pybullet_envs.minitaur.agents.baseline_controller import com_velocity_estimator
+from pybullet_envs.minitaur.agents.baseline_controller import imu_based_com_velocity_estimator
+from pybullet_envs.minitaur.agents.baseline_controller import locomotion_controller
+from pybullet_envs.minitaur.agents.baseline_controller import openloop_gait_generator
+from pybullet_envs.minitaur.agents.baseline_controller import raibert_swing_leg_controller
+from pybullet_envs.minitaur.agents.baseline_controller import torque_stance_leg_controller
+#from pybullet_envs.minitaur.envs.env_randomizers import minitaur_push_randomizer
+from pybullet_envs.minitaur.envs.env_randomizers import minitaur_push_randomizer
+from pybullet_envs.minitaur.robots import robot_config
+import pybullet_data as pd
+
+CONFIG_FILE = (pd.getDataPath()+"/configs_v2/base/laikago_reactive.gin")
+
+_MOTOR_KD = [1.0, 2.0, 2.0] * 4
+_BODY_HEIGHT = 0.45
+_MAX_TIME_SECONDS = 1000000000
+_MOTOR_KD = [1.0, 2.0, 2.0] * 4
+# TODO(tingnan): This is for tunining the moments of inertia of the model.
+# Once we identified the correct value we can remove this.
+_SCALE = 4
+_INERTIA = (0.07335 * _SCALE, 0, 0, 0, 0.25068 * _SCALE, 0, 0, 0,
+ 0.25447 * _SCALE)
+
+
+def load_sim_config(render=True):
+ """Builds the environment for the quadruped robot.
+
+ Args:
+ render: Enable/disable rendering.
+ """
+ gin.clear_config(clear_constants=False)
+ config_file = CONFIG_FILE
+ gin.parse_config_file(config_file)
+
+ # Sim bindings
+ # Overwrite a few parameters.
+
+ action_repeat = 4
+ gin.bind_parameter("SimulationParameters.num_action_repeat", action_repeat)
+ gin.bind_parameter("laikago_v2.Laikago.action_repeat", action_repeat)
+
+ # Control latency is NOT modeled properly for inverse kinematics and
+ # jacobians, as we are directly calling the pybullet API. We will try to fix
+ # this by loading a separate pybullet instance, set the pose and joint
+ # angles which has latency in them, and then run the jacobian/IK.
+ gin.bind_parameter("laikago_v2.Laikago.motor_control_mode",
+ robot_config.MotorControlMode.HYBRID)
+ # Bump up a bit the adduction/abduction motor d gain for a better tracking.
+ gin.bind_parameter("hybrid_motor_model.HybridMotorModel.kd", _MOTOR_KD)
+ gin.bind_parameter("SimulationParameters.enable_rendering", render)
+ gin.bind_parameter("env_loader.load.wrapper_classes", [])
+
+
+
+def add_random_push_config():
+ """Adds a random push randomizers to the config."""
+ try:
+ current_env_randomizers = gin.query_parameter(
+ "locomotion_gym_env.LocomotionGymEnv.env_randomizers")
+
+ current_env_randomizers.append(
+ minitaur_push_randomizer.MinitaurPushRandomizer(
+ horizontal_force_bound=(500, 900),
+ vertical_force_bound=(50, 100),
+ visualize_perturbation_force=True))
+ gin.bind_parameter("locomotion_gym_env.LocomotionGymEnv.env_randomizers",
+ current_env_randomizers)
+ except ValueError:
+ # No randoimzers bind so far
+ gin.bind_parameter("locomotion_gym_env.LocomotionGymEnv.env_randomizers", [
+ minitaur_push_randomizer.MinitaurPushRandomizer(
+ horizontal_force_bound=(500, 900),
+ vertical_force_bound=(50, 100),
+ visualize_perturbation_force=True)
+ ])
+
+
+def select_gait(gait_type="fast_trot"):
+ """Selects a gait pattern.
+
+ Args:
+ gait_type: which gait to use.
+
+ Returns:
+ A tuple of (stance_duration, duty_factor, initial_phase)
+ """
+ # Each gait is composed of stance_duration, duty_factor, and
+ # init_phase_full_cycle.
+ if gait_type == "fast_trot":
+ return [0.25] * 4, [0.6] * 4, [0, 0.5, 0.5, 0]
+ elif gait_type == "slow_trot":
+ return [0.4] * 4, [0.65] * 4, [0, 0.5, 0.5, 0]
+ elif gait_type == "walk":
+ return [0.75] * 4, [0.8] * 4, [0.25, 0.75, 0.5, 0]
+ else:
+ # Means four leg stand for as long as possible.
+ return [_MAX_TIME_SECONDS] * 4, [0.99] * 4, [0, 0, 0, 0]
+
+
+def setup_controller(robot,
+ gait="fast_trot",
+ run_on_robot=False,
+ use_ground_truth_velocity=False):
+ """Demonstrates how to create a locomotion controller.
+
+ Args:
+ robot: A robot instance.
+ gait: The type of gait to use.
+ run_on_robot: Whether this controller is running on the real robot or not.
+ use_ground_truth_velocity: Whether to use ground truth velocity or velocity
+ estimator.
+
+ Returns:
+ A locomotion controller.
+ """
+ desired_speed = (0, 0)
+ desired_twisting_speed = 0
+
+ feet_positions = np.array(robot.foot_positions())
+ feet_positions[:, 2] = 0
+
+ # Sim and real robots have different mass and contact detection parameters.
+ body_weight, contact_force_threshold = (200, 20) if run_on_robot else (215, 0)
+
+ stance_duration, duty_factor, init_phase = select_gait(gait)
+ gait_generator = openloop_gait_generator.OpenloopGaitGenerator(
+ robot,
+ stance_duration=stance_duration,
+ duty_factor=duty_factor,
+ initial_leg_phase=init_phase,
+ contact_detection_force_threshold=contact_force_threshold,
+ )
+ state_estimator = (
+ imu_based_com_velocity_estimator.IMUBasedCOMVelocityEstimator(
+ robot,
+ contact_detection_threshold=contact_force_threshold,
+ ))
+
+ # Use this in sim to test ground truth velocity estimation.
+ if use_ground_truth_velocity:
+ state_estimator = com_velocity_estimator.COMVelocityEstimator(robot)
+
+ sw_controller = raibert_swing_leg_controller.RaibertSwingLegController(
+ robot,
+ gait_generator,
+ state_estimator,
+ desired_speed=desired_speed,
+ desired_twisting_speed=desired_twisting_speed,
+ desired_height=_BODY_HEIGHT,
+ local_hip_positions=feet_positions,
+ )
+ st_controller = torque_stance_leg_controller.TorqueStanceLegController(
+ robot,
+ gait_generator,
+ state_estimator,
+ desired_speed=desired_speed,
+ desired_twisting_speed=desired_twisting_speed,
+ desired_body_height=_BODY_HEIGHT,
+ body_mass=body_weight / 9.8,
+ body_inertia=_INERTIA,
+ )
+
+ controller = locomotion_controller.LocomotionController(
+ robot=robot,
+ gait_generator=gait_generator,
+ state_estimator=state_estimator,
+ swing_leg_controller=sw_controller,
+ stance_leg_controller=st_controller,
+ clock=robot.GetTimeSinceReset)
+ return controller
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/minitaur_raibert_controller.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/minitaur_raibert_controller.py
new file mode 100644
index 000000000..82ddebcab
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/minitaur_raibert_controller.py
@@ -0,0 +1,448 @@
+"""A Raibert style controller for Minitaur."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import math
+import attr
+import gin
+import numpy as np
+
+from pybullet_envs.minitaur.agents.baseline_controller import minitaur_raibert_controller_utils
+from pybullet_envs.minitaur.envs.utilities import minitaur_pose_utils
+
+LEFT_FRONT_LEG_ID = 0
+LEFT_HIND_LEG_ID = 1
+RIGHT_FRONT_LEG_ID = 2
+RIGHT_HIND_LEG_ID = 3
+
+DIAGONAL_LEG_PAIR_1 = (LEFT_FRONT_LEG_ID, RIGHT_HIND_LEG_ID)
+DIAGONAL_LEG_PAIR_2 = (LEFT_HIND_LEG_ID, RIGHT_FRONT_LEG_ID)
+
+LEFT_LEG_IDS = (LEFT_FRONT_LEG_ID, LEFT_HIND_LEG_ID)
+RIGHT_LEG_IDS = (RIGHT_FRONT_LEG_ID, RIGHT_HIND_LEG_ID)
+
+# The max horizontal foot offset (in meters) for turning.
+_FOOT_HORIZONTAL_OFFSET_FOR_TURNING = 0.1
+
+_STANCE_TG_PHASE_COMPRESSION = 1.5
+_STANCE_TG_DELTA_EXTENSION = 0.2
+_STANCE_HORIZONTAL_SCALING_FACTOR = 1.2
+
+_DEFAULT_SWING_SPEED_GAIN = 0.015
+
+_DEFAULT_SWING_FOOT_CLEARANCE = 0.005
+
+_PITCH_SWING_FEEDBACK_SCALING_FACTOR = 1.2
+
+
+# A POD container to describe the controller's high level behavior.
+@attr.s
+class BehaviorParameters(object):
+ """Highlevel parameters for Raibert style controller."""
+ stance_duration = attr.ib(type=float, default=0.25)
+ desired_forward_speed = attr.ib(type=float, default=0.)
+ desired_turning_speed = attr.ib(type=float, default=0.)
+ standing_height = attr.ib(type=float, default=0.2)
+ desired_incline_angle = attr.ib(type=float, default=0.)
+
+
+def generate_default_swing_trajectory(phase, init_pose, end_pose):
+ """A swing trajectory generator.
+
+ Args:
+ phase: Float. Between [0, 1].
+ init_pose: A tuple. The leg pose (swing, extension) at phase == 0 the
+ beginning of swing.
+ end_pose: A tuple. The leg pose at phase == 1 the end of swing.
+
+ Returns:
+ The desired leg pose for the current phase.
+ """
+ # Try phase compression
+ normalized_phase = math.sqrt(min(phase * 1.5, 1))
+
+ # For swing, we use a linear interpolation:
+ swing = (end_pose[0] - init_pose[0]) * normalized_phase + init_pose[0]
+
+ # For extension, we can fit a second order polynomial:
+ min_ext = (init_pose[1] + end_pose[1]) / 2 - 0.8
+ min_ext = max(min_ext, 0.5)
+
+ # The phase value at which the extension reaches the minimal value min_ext.
+ # phi is small, the swing leg will try to lift higher in the first half of
+ # swing.
+ phi = 0.1
+
+ # We convert the extension back into the cartesion space. In this way we can
+ # guarantee a lift-up trajectory. The ankle to hip distance is easier to
+ # compute than a full forward-kinematics.
+ min_ankle_dist = minitaur_raibert_controller_utils.extension_to_ankle_dist(
+ min_ext)
+ init_ankle_dist = minitaur_raibert_controller_utils.extension_to_ankle_dist(
+ init_pose[1])
+ end_ankle_dist = minitaur_raibert_controller_utils.extension_to_ankle_dist(
+ end_pose[1])
+
+ # The polynomial is: a * phi^2 + b * phi + c
+ delta_1 = min_ankle_dist - init_ankle_dist
+ delta_2 = end_ankle_dist - init_ankle_dist
+ delta_p = phi * phi - phi
+
+ a = (delta_1 - phi * delta_2) / delta_p
+
+ b = (phi * phi * delta_2 - delta_1) / delta_p
+
+ ankle_dist = (
+ a * normalized_phase * normalized_phase + b * normalized_phase +
+ init_ankle_dist)
+
+ l1 = minitaur_raibert_controller_utils.UPPER_LEG_LEN
+ l2 = minitaur_raibert_controller_utils.LOWER_SHORT_LEG_LEN
+
+ ankle_dist = min(max(ankle_dist, l2 - l1 + 0.01), l2 + l1 - 0.01)
+
+ extension = minitaur_raibert_controller_utils.ankle_dist_to_extension(
+ ankle_dist)
+
+ return (swing, extension)
+
+
+@gin.configurable
+def generate_default_stance_trajectory(phase,
+ init_pose,
+ end_pose,
+ use_constant_extension=False):
+ """A stance strajectory generator.
+
+ Args:
+ phase: Float. Between [0, 1].
+ init_pose: A tuple. The leg pose (swing, extension) at phase == 0, i.e. the
+ beginning of stance.
+ end_pose: A tuple. The leg pose at phase == 1, i.e. the end of stance.
+ use_constant_extension: Boolean. Whether or not to fix the extension during
+ stance.
+
+ Returns:
+ The desired leg pose for the current phase.
+ """
+ normalized_phase = min(_STANCE_TG_PHASE_COMPRESSION * math.sqrt(phase), 1)
+ swing = (end_pose[0] - init_pose[0]) * normalized_phase + init_pose[0]
+
+ # The extension evolves nonlinearly according to the parabola equation.
+ if use_constant_extension:
+ extension = end_pose[1]
+ else:
+ extension = end_pose[1] - 4 * _STANCE_TG_DELTA_EXTENSION * (
+ normalized_phase**2 - normalized_phase)
+ return (swing, extension)
+
+
+def get_stance_foot_offset_for_turning(leg_id, steering_signal):
+ """Modify the stance foot position to achieve turning.
+
+ The strategy works for trotting gaits.
+
+ Args:
+ leg_id: Integer. The leg index.
+ steering_signal: Float. The desired turning signal in [-1, 1]. Because we
+ don't have an accurate mapping from angular velocity to the foot offset,
+ the steering signal should be treated as a reference and only its relative
+ magnitude matters.
+
+ Returns:
+ The stance foot's horizontal offset.
+
+ """
+ clipped_steering_signal = np.clip(steering_signal, -1, 1)
+
+ if leg_id in LEFT_LEG_IDS:
+ return _FOOT_HORIZONTAL_OFFSET_FOR_TURNING * clipped_steering_signal
+ else:
+ return -(_FOOT_HORIZONTAL_OFFSET_FOR_TURNING * clipped_steering_signal)
+
+
+def get_leg_swing_offset_for_pitching(body_pitch, desired_incline_angle):
+ """Get the leg swing zero point when the body is tilted.
+
+ For example, when climbing up or down stairs/slopes, the robot body will tilt
+ up or down. By compensating the body pitch, the leg's trajectory will be
+ centered around the vertical direction (not perpendicular to the surface).
+ This helps the robot to generate thrust when going upwards, and braking when
+ going downwards.
+
+ Args:
+ body_pitch: Float. Current body pitch angle.
+ desired_incline_angle: Float. The desired body pitch angle.
+
+ Returns:
+ The stance and swing leg swing offset.
+
+ """
+ kp = 0.2
+ return -((1 - kp) * body_pitch + kp * desired_incline_angle)
+
+
+@gin.configurable
+class RaibertSwingLegController(object):
+ """The swing leg controller."""
+
+ def __init__(self,
+ speed_gain=_DEFAULT_SWING_SPEED_GAIN,
+ foot_clearance=_DEFAULT_SWING_FOOT_CLEARANCE,
+ leg_trajectory_generator=generate_default_swing_trajectory):
+ """Initializes the controller.
+
+ Args:
+ speed_gain: Float. The speed feedback gain to modulate the target foot
+ position.
+ foot_clearance: Float. The foot clearance (at the end of the swing phase)
+ in meters.
+ leg_trajectory_generator: A trajectory generator function.
+ """
+ self._speed_gain = speed_gain
+ self._foot_clearance = foot_clearance
+ self._leg_trajectory_generator = leg_trajectory_generator
+
+ def get_action(self, raibert_controller):
+ """Get the swing legs' desired pose."""
+ current_speed = raibert_controller.estimate_base_velocity()
+ phase = raibert_controller.get_phase()
+ rpy = raibert_controller.robot.base_roll_pitch_yaw
+
+ leg_pose_set = {}
+ for i in raibert_controller.swing_set:
+ # Target foot horizontal position is calculated according to Raibert's
+ # original formula in "Legged robots that balance".
+ target_foot_horizontal_position = (
+ raibert_controller.behavior_parameters.stance_duration / 2 *
+ current_speed + self._speed_gain *
+ (current_speed -
+ raibert_controller.behavior_parameters.desired_forward_speed))
+
+ # 1) Convert the target foot position to leg pose space.
+ # Lift the foot a little bit.
+ target_foot_vertical_position = -(
+ raibert_controller.behavior_parameters.standing_height -
+ self._foot_clearance)
+ target_foot_position = (target_foot_horizontal_position,
+ target_foot_vertical_position)
+ target_leg_pose = minitaur_raibert_controller_utils.foot_position_to_leg_pose(
+ target_foot_position)
+
+ # 2) Generates the curve from the swing start leg pose to the target leg
+ # pose and find the next leg pose on the curve based on current swing
+ # phase.
+
+ desired_leg_pose = self._leg_trajectory_generator(
+ phase, raibert_controller.swing_start_leg_pose, target_leg_pose)
+
+ swing_offset = get_leg_swing_offset_for_pitching(
+ rpy[1], raibert_controller.behavior_parameters.desired_incline_angle)
+
+ leg_pose_set[i] = (desired_leg_pose[0] + swing_offset,
+ desired_leg_pose[1])
+
+ return leg_pose_set
+
+
+@gin.configurable
+class RaibertStanceLegController(object):
+ """The controller that modulates the behavior of the stance legs."""
+
+ def __init__(self,
+ speed_gain=0.1,
+ leg_trajectory_generator=generate_default_stance_trajectory):
+ """Initializes the controller.
+
+ Args:
+ speed_gain: Float. The speed feedback gain to modulate the target stance
+ foot position.
+ leg_trajectory_generator: A trajectory generator function. Generates the
+ desired leg pose during the stance phase.
+ """
+ self._speed_gain = speed_gain
+ self._leg_trajectory_generator = leg_trajectory_generator
+
+ def get_action(self, raibert_controller):
+ """Get the desired leg pose for the stance legs."""
+
+ phase = raibert_controller.get_phase()
+ current_speed = raibert_controller.estimate_base_velocity()
+ rpy = raibert_controller.robot.base_roll_pitch_yaw
+
+ leg_pose_set = {}
+ for i in raibert_controller.stance_set:
+ desired_forward_speed = (
+ raibert_controller.behavior_parameters.desired_forward_speed)
+
+ target_foot_horizontal_position = -_STANCE_HORIZONTAL_SCALING_FACTOR * (
+ raibert_controller.behavior_parameters.stance_duration / 2 *
+ current_speed - self._speed_gain *
+ (current_speed - desired_forward_speed))
+
+ target_foot_horizontal_position += get_stance_foot_offset_for_turning(
+ i, raibert_controller.behavior_parameters.desired_turning_speed)
+
+ target_foot_position = (
+ target_foot_horizontal_position,
+ -raibert_controller.behavior_parameters.standing_height)
+ target_leg_pose = minitaur_raibert_controller_utils.foot_position_to_leg_pose(
+ target_foot_position)
+
+ desired_leg_pose = (
+ self._leg_trajectory_generator(
+ phase, raibert_controller.stance_start_leg_pose, target_leg_pose))
+
+ swing_offset = _PITCH_SWING_FEEDBACK_SCALING_FACTOR * get_leg_swing_offset_for_pitching(
+ rpy[1], raibert_controller.behavior_parameters.desired_incline_angle)
+
+ leg_pose_set[i] = (desired_leg_pose[0] + swing_offset,
+ desired_leg_pose[1])
+
+ return leg_pose_set
+
+
+@gin.configurable
+class MinitaurRaibertController(object):
+ """A Raibert style controller for trotting gait."""
+
+ def __init__(self,
+ robot,
+ behavior_parameters=BehaviorParameters(),
+ swing_leg_controller=RaibertSwingLegController(),
+ stance_leg_controller=RaibertStanceLegController(),
+ pose_feedback_controller=None):
+ self._time = 0
+ self._robot = robot
+ self.behavior_parameters = behavior_parameters
+
+ self._last_recorded_speed = 0
+
+ self._swing_leg_controller = swing_leg_controller
+ self._stance_leg_controller = stance_leg_controller
+ self._pose_feeback_controller = pose_feedback_controller
+
+ # The leg order is FL, RL, FR, RR -> [0, 1, 2, 3]
+ self._swing_set = DIAGONAL_LEG_PAIR_1
+ self._stance_set = DIAGONAL_LEG_PAIR_2
+
+ # Compute the initial leg pose.
+ self._swing_start_leg_pose = self.get_swing_leg_pose()
+ self._stance_start_leg_pose = self.get_stance_leg_pose()
+
+ @property
+ def robot(self):
+ return self._robot
+
+ @property
+ def swing_set(self):
+ return self._swing_set
+
+ @property
+ def stance_set(self):
+ return self._stance_set
+
+ @property
+ def swing_start_leg_pose(self):
+ return self._swing_start_leg_pose
+
+ @property
+ def stance_start_leg_pose(self):
+ return self._stance_start_leg_pose
+
+ def _get_average_leg_pose(self, leg_indices):
+ """Get the average leg pose."""
+ current_leg_pose = minitaur_pose_utils.motor_angles_to_leg_pose(
+ self._robot.motor_angles)
+
+ # extract the swing leg pose from the current_leg_pose
+ leg_pose = []
+ for index in leg_indices:
+ leg_pose.append([
+ current_leg_pose[index],
+ current_leg_pose[index + minitaur_pose_utils.NUM_LEGS]
+ ])
+
+ leg_pose = np.array(leg_pose)
+ return np.mean(leg_pose, axis=0)
+
+ def get_swing_leg_pose(self):
+ """Get the current swing legs' average pose."""
+ return self._get_average_leg_pose(self._swing_set)
+
+ def get_stance_leg_pose(self):
+ """Get the current stance legs' average pose."""
+ return self._get_average_leg_pose(self._stance_set)
+
+ def get_phase(self):
+ """Compute the current stance/swing phase."""
+ return math.fmod(self._time, self.behavior_parameters.stance_duration
+ ) / self.behavior_parameters.stance_duration
+
+ def _get_new_swing_stance_set(self):
+ """Switch the set of swing/stance legs based on timing."""
+ swing_stance_phase = math.fmod(self._time,
+ 2 * self.behavior_parameters.stance_duration)
+ if swing_stance_phase < self.behavior_parameters.stance_duration:
+ return (DIAGONAL_LEG_PAIR_1, DIAGONAL_LEG_PAIR_2)
+ return (DIAGONAL_LEG_PAIR_2, DIAGONAL_LEG_PAIR_1)
+
+ def update(self, t):
+ """Update the internal status of the controller.
+
+ Args:
+ t: Float. The current time after reset in seconds.
+ """
+ self._time = t
+ new_swing_set, new_stance_set = self._get_new_swing_stance_set()
+
+ # If there is a stance/swing switch.
+ if new_swing_set[0] is not self._swing_set[0]:
+ self._swing_set = new_swing_set
+ self._stance_set = new_stance_set
+
+ # Also records the starting pose.
+ self._swing_start_leg_pose = self.get_swing_leg_pose()
+ self._stance_start_leg_pose = self.get_stance_leg_pose()
+
+ def estimate_base_velocity(self):
+ """Estimate the current CoM speed."""
+ # It is best to use a sensor fusion approach.
+ stance_leg_pose = self.get_stance_leg_pose()
+
+ delta_sw = stance_leg_pose[0] - self._stance_start_leg_pose[0]
+
+ x, y = minitaur_raibert_controller_utils.leg_pose_to_foot_position(
+ stance_leg_pose)
+ toe_hip_len = math.sqrt(x**2 + y**2)
+ horizontal_dist = toe_hip_len * delta_sw
+ phase = self.get_phase()
+ speed = self._last_recorded_speed
+ if phase > 0.1:
+ speed = horizontal_dist / (
+ self.behavior_parameters.stance_duration * phase)
+ self._last_recorded_speed = speed
+ return speed
+
+ def get_swing_leg_action(self):
+ return self._swing_leg_controller.get_action(self)
+
+ def get_stance_leg_action(self):
+ return self._stance_leg_controller.get_action(self)
+
+ def get_action(self):
+ """Gets the desired motor angles."""
+ leg_pose = [0] * minitaur_pose_utils.NUM_MOTORS
+ swing_leg_pose = self.get_swing_leg_action()
+ for i in self._swing_set:
+ leg_pose[i] = swing_leg_pose[i][0]
+ leg_pose[i + minitaur_pose_utils.NUM_LEGS] = swing_leg_pose[i][1]
+
+ stance_leg_pose = self.get_stance_leg_action()
+ for i in self._stance_set:
+ leg_pose[i] = stance_leg_pose[i][0]
+ leg_pose[i + minitaur_pose_utils.NUM_LEGS] = stance_leg_pose[i][1]
+
+ return minitaur_pose_utils.leg_pose_to_motor_angles(leg_pose)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/minitaur_raibert_controller_utils.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/minitaur_raibert_controller_utils.py
new file mode 100644
index 000000000..cc992a197
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/minitaur_raibert_controller_utils.py
@@ -0,0 +1,82 @@
+"""Utility functions for the Minitaur Raibert controller."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import math
+
+UPPER_LEG_LEN = 0.112
+LOWER_SHORT_LEG_LEN = 0.199
+LOWER_LONG_LEG_LEN = 0.2315
+
+
+def leg_pose_to_foot_position(leg_pose):
+ """The forward kinematics."""
+ l1 = UPPER_LEG_LEN
+ l2 = LOWER_SHORT_LEG_LEN
+ l3 = LOWER_LONG_LEG_LEN
+
+ ext = leg_pose[1]
+ alpha = math.asin(l1 * math.sin(ext) / l2)
+
+ sw = -leg_pose[0]
+ x = l3 * math.sin(alpha + sw) - l1 * math.sin(ext + sw)
+ y = l3 * math.cos(alpha + sw) - l1 * math.cos(ext + sw)
+
+ return (x, -y)
+
+
+def foot_position_to_leg_pose(foot_position):
+ """The inverse kinematics."""
+ l1 = UPPER_LEG_LEN
+ l2 = LOWER_SHORT_LEG_LEN
+ l3 = LOWER_LONG_LEG_LEN
+
+ x = foot_position[0]
+ y = foot_position[1]
+
+ assert y < 0
+ hip_toe_sqr = x**2 + y**2
+ cos_beta = (l1 * l1 + l3 * l3 - hip_toe_sqr) / (2 * l1 * l3)
+ assert -1 <= cos_beta <= 1
+ hip_ankle_sqr = l1 * l1 + l2 * l2 - 2 * l1 * l2 * cos_beta
+ hip_ankle = math.sqrt(hip_ankle_sqr)
+ cos_ext = -(l1 * l1 + hip_ankle_sqr - l2 * l2) / (2 * l1 * hip_ankle)
+ ext = math.acos(cos_ext)
+
+ hip_toe = math.sqrt(hip_toe_sqr)
+ cos_theta = (hip_toe_sqr + hip_ankle_sqr -
+ (l3 - l2)**2) / (2 * hip_ankle * hip_toe)
+
+ assert cos_theta > 0
+ theta = math.acos(cos_theta)
+ sw = math.asin(x / hip_toe) - theta
+ return (-sw, ext)
+
+
+def extension_to_ankle_dist(extension):
+ """Converts leg extension to ankle-hip distance in meters.
+
+ The ankle is defined as the joint of the two lower links, which is different
+ from the toe which is the tip of the longer lower limb.
+
+ Args:
+ extension: Float. the leg extension.
+
+ Returns:
+ Float. The hip to ankle distance in meters.
+
+ """
+ l1 = UPPER_LEG_LEN
+ l2 = LOWER_SHORT_LEG_LEN
+ alpha = math.asin(l1 / l2 * math.sin(extension))
+ return l2 * math.cos(alpha) - l1 * math.cos(extension)
+
+
+def ankle_dist_to_extension(dist):
+ """Converts ankle-hip distance (meters) to extension."""
+ l1 = UPPER_LEG_LEN
+ l2 = LOWER_SHORT_LEG_LEN
+ cos_extension = -(l1**2 + dist**2 - l2**2) / (2 * l1 * dist)
+ return math.acos(cos_extension)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/model_predictive_control.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/model_predictive_control.py
new file mode 100644
index 000000000..739424360
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/model_predictive_control.py
@@ -0,0 +1,149 @@
+# Lint as: python3
+"""Classic model predictive control methods."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import math
+from typing import Sequence
+import numpy as np
+
+_MAX_ABS_RPY = 0.3
+_MAX_ABS_ANGULAR_VELOCITY = math.pi
+
+# The center of mass torque is computed using a simple PD control: tau = -KP *
+# delta_rotation - KD * delta_angular_velocity
+_TORQUE_KP = 2000
+_TORQUE_KD = 150
+
+# For center of mass force, we only need to track position in the z direction
+# (i.e. maintain the body height), and speed in x-y plane.
+_FORCE_KP = 500
+_FORCE_KD = 200
+
+
+def compute_contact_force_projection_matrix(
+ foot_positions_in_com_frame: Sequence[Sequence[float]],
+ stance_foot_ids: Sequence[int],
+) -> np.ndarray:
+ r"""Computes the 6 x 3n matrix to map contact force to com dynamics.
+
+ This is essentially the vectorized rhs of com dynamics equation:
+ ma = \sum f
+ I\omega_dot = \sum r \cross f
+
+ where the summation if over all feet in contact with ground.
+
+ Caveats: Current we have not taken the com rotation into account as we assume
+ the com rotation would be small. Ideally we should rotate the foot_positions
+ to a world frame centered at com. Also, since absolute yaw angles are not
+ accurate dute to drifting, we should use (roll, pitch, 0) to do the foot
+ position projection. This feature will be quite useful for MPC.
+ TODO(b/143378213): Fix this.
+
+ Args:
+ foot_positions_in_com_frame: the local position of each foot.
+ stance_foot_ids: The stance foot to be used to assemble the matrix.
+
+ Returns:
+ The contact force projection matrix.
+
+ """
+ jacobians = []
+ for foot_id in stance_foot_ids:
+ jv = np.identity(3)
+ foot_position = foot_positions_in_com_frame[foot_id]
+ x, y, z = foot_position[:3]
+ jw = np.array(((0, -z, y), (z, 0, -x), (-y, x, 0)))
+ jacobians.append(np.vstack((jv, jw)))
+
+ return np.hstack(jacobians)
+
+
+def plan_foot_contact_force(
+ mass: float,
+ inertia: np.ndarray,
+ com_position: np.ndarray,
+ com_velocity: np.ndarray,
+ com_roll_pitch_yaw: np.ndarray,
+ com_angular_velocity: np.ndarray,
+ foot_positions_in_com_frame: Sequence[Sequence[float]],
+ foot_contact_state: Sequence[bool],
+ desired_com_position: np.ndarray,
+ desired_com_velocity: np.ndarray,
+ desired_com_roll_pitch_yaw: np.ndarray,
+ desired_com_angular_velocity: np.ndarray,
+):
+ """Plan the foot contact forces using robot states.
+
+ TODO(b/143382305): Wrap this interface in a MPC class so we can use other
+ planning algorithms.
+
+ Args:
+ mass: The total mass of the robot.
+ inertia: The diagnal elements [Ixx, Iyy, Izz] of the robot.
+ com_position: Center of mass position in world frame. Usually we cannot
+ accurrately obtain this without motion capture.
+ com_velocity: Center of mass velocity in world frame.
+ com_roll_pitch_yaw: Center of mass rotation wrt world frame in euler angles.
+ com_angular_velocity: The angular velocity (roll_dot, pitch_dot, yaw_dot).
+ foot_positions_in_com_frame: The position of all feet/toe joints in the body
+ frame.
+ foot_contact_state: Indicates if a foot is in contact with the ground.
+ desired_com_position: We usually just care about the body height.
+ desired_com_velocity: In world frame.
+ desired_com_roll_pitch_yaw: We usually care about roll and pitch, since yaw
+ measurement can be unreliable.
+ desired_com_angular_velocity: Roll and pitch change rate are usually zero.
+ Yaw rate is the turning speed of the robot.
+
+ Returns:
+ The desired stance foot contact forces.
+ """
+ del inertia
+ del com_position
+ body_height = []
+ stance_foot_ids = []
+ for foot_id, foot_position in enumerate(foot_positions_in_com_frame):
+ if not foot_contact_state[foot_id]:
+ continue
+ stance_foot_ids.append(foot_id)
+ body_height.append(foot_position[2])
+
+ avg_bogy_height = abs(sum(body_height) / len(body_height))
+
+ rpy = com_roll_pitch_yaw
+ rpy[:2] = np.clip(rpy[:2], -_MAX_ABS_RPY, _MAX_ABS_RPY)
+ rpy_dot = com_angular_velocity
+ rpy_dot = np.clip(rpy_dot, -_MAX_ABS_ANGULAR_VELOCITY,
+ _MAX_ABS_ANGULAR_VELOCITY)
+
+ com_torque = -avg_bogy_height * (
+ _TORQUE_KP * (rpy - desired_com_roll_pitch_yaw) + _TORQUE_KD * rpy_dot)
+
+ # We don't care about the absolute yaw angle in the low level controller.
+ # Instead, we stabialize the angular velocity in the z direction.
+ com_torque[2] = -avg_bogy_height * _TORQUE_KD * (
+ rpy_dot[2] - desired_com_angular_velocity[2])
+
+ # Track a desired com velocity.
+ com_force = -_FORCE_KD * (com_velocity - desired_com_velocity)
+
+ # In the z-direction we also want to track the body height.
+ com_force[2] += mass * 9.8 - _FORCE_KP * (
+ avg_bogy_height - desired_com_position[2])
+
+ com_force_torque = np.concatenate((com_force, com_torque)).transpose()
+
+ # Map the com force torque to foot contact forces.
+ foot_force_to_com = compute_contact_force_projection_matrix(
+ foot_positions_in_com_frame, stance_foot_ids)
+ all_contact_force = -np.matmul(
+ np.linalg.pinv(foot_force_to_com), com_force_torque).transpose()
+ contact_force = {}
+
+ for i, foot_id in enumerate(stance_foot_ids):
+ contact_force[foot_id] = all_contact_force[3 * i:3 * i + 3]
+
+ return contact_force
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/multi_state_estimator.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/multi_state_estimator.py
new file mode 100644
index 000000000..60f8cd86b
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/multi_state_estimator.py
@@ -0,0 +1,48 @@
+"""A class for combining multiple state estimators."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+from typing import Any, Sequence
+
+import gin
+
+from pybullet_envs.minitaur.agents.baseline_controller import state_estimator
+
+
+@gin.configurable
+class MultiStateEstimator(state_estimator.StateEstimatorBase):
+ """Combine multiple state estimators.
+
+
+ This class can be used to combine multiple state estimators into one. For
+ example, one can use the COMVelocityEstimator to estimate the com velocity
+ and COMHeightEstimator to estimate the com height.
+
+ """
+
+ def __init__(
+ self,
+ robot: Any,
+ state_estimators: Sequence[state_estimator.StateEstimatorBase],
+ ):
+ self._robot = robot
+ self._state_estimators = state_estimators
+ self.reset(0)
+
+ def reset(self, current_time):
+ for single_state_estimator in self._state_estimators:
+ single_state_estimator.reset(current_time)
+
+ def update(self, current_time):
+ for single_state_estimator in self._state_estimators:
+ single_state_estimator.update(current_time)
+
+ def __getattr__(self, attr):
+ for single_state_estimator in self._state_estimators:
+ if hasattr(single_state_estimator, attr):
+ return getattr(single_state_estimator, attr)
+ raise ValueError(
+ "{} is not found in any of the state estimators".format(attr))
+
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/openloop_gait_generator.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/openloop_gait_generator.py
new file mode 100644
index 000000000..c8696dc27
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/openloop_gait_generator.py
@@ -0,0 +1,194 @@
+"""Gait pattern planning module."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import logging
+import math
+from typing import Any, Sequence
+
+import gin
+import numpy as np
+
+from pybullet_envs.minitaur.agents.baseline_controller import gait_generator
+
+_DEFAULT_INITIAL_LEG_STATE = (
+ gait_generator.LegState.STANCE,
+ gait_generator.LegState.STANCE,
+ gait_generator.LegState.STANCE,
+ gait_generator.LegState.STANCE,
+)
+
+_NOMINAL_STANCE_DURATION = (0.25, 0.25, 0.25, 0.25)
+_NOMINAL_DUTY_FACTOR = (0.6, 0.6, 0.6, 0.6)
+_TROTTING_LEG_PHASE = (0, 0.5, 0.5, 0)
+_NOMINAL_CONTACT_DETECTION_PHASE = 0.4
+
+
+@gin.configurable
+class OpenloopGaitGenerator(gait_generator.GaitGenerator):
+ """Generates openloop gaits for quadruped robots.
+
+ A flexible open-loop gait generator. Each leg has its own cycle and duty
+ factor. And the state of each leg alternates between stance and swing. One can
+ easily formuate a set of common quadruped gaits like trotting, pacing,
+ pronking, bounding, etc by tweaking the input parameters.
+ """
+
+ def __init__(
+ self,
+ robot: Any,
+ stance_duration: Sequence[float] = _NOMINAL_STANCE_DURATION,
+ duty_factor: Sequence[float] = _NOMINAL_DUTY_FACTOR,
+ initial_leg_phase: Sequence[float] = _TROTTING_LEG_PHASE,
+ contact_detection_force_threshold: float = 0,
+ contact_detection_phase_threshold:
+ float = _NOMINAL_CONTACT_DETECTION_PHASE,
+ ):
+ """Initializes the class.
+
+ Args:
+ robot: A quadruped robot that at least implements the GetFootContacts API
+ and num_legs property.
+ stance_duration: The desired stance duration.
+ duty_factor: The ratio stance_duration / total_gait_cycle.
+ initial_leg_phase: The desired initial phase [0, 1] of the legs within the
+ full swing + stance cycle.
+ contact_detection_force_threshold: The minimal contact force required to
+ detect if a foot is in contact with the ground. For real robots this
+ needs to be larger (i.e. 25 for Laikago).
+ contact_detection_phase_threshold: Updates the state of each leg based on
+ contact info, when the current normalized phase is greater than this
+ threshold. This is essential to remove false positives in contact
+ detection when phase switches. For example, a swing foot at at the
+ beginning of the gait cycle might be still on the ground.
+ """
+ self._robot = robot
+ self._stance_duration = stance_duration
+ self._duty_factor = duty_factor
+ self._swing_duration = np.array(stance_duration) / np.array(
+ duty_factor) - np.array(stance_duration)
+ if len(initial_leg_phase) != len(
+ list(self._robot.urdf_loader.get_end_effector_id_dict().values())):
+ raise ValueError(
+ "The number of leg phases should be the same as number of legs.")
+ self._initial_leg_phase = initial_leg_phase
+
+ self._initial_leg_state = _DEFAULT_INITIAL_LEG_STATE
+ self._next_leg_state = []
+ # The ratio in cycle is duty factor if initial state of the leg is STANCE,
+ # and 1 - duty_factory if the initial state of the leg is SWING.
+ self._initial_state_ratio_in_cycle = []
+ for state, duty in zip(self._initial_leg_state, duty_factor):
+ assert state == gait_generator.LegState.STANCE
+ self._initial_state_ratio_in_cycle.append(duty)
+ self._next_leg_state.append(gait_generator.LegState.SWING)
+
+ self._contact_detection_force_threshold = contact_detection_force_threshold
+ self._contact_detection_phase_threshold = contact_detection_phase_threshold
+
+ # The normalized phase within swing or stance duration.
+ self._normalized_phase = None
+
+ # The current leg state, when contact is considered.
+ self._leg_state = None
+
+ # The desired leg state (i.e. SWING or STANCE).
+ self._desired_leg_state = None
+
+ self.reset(0)
+
+ def reset(self, current_time):
+ # The normalized phase within swing or stance duration.
+ self._normalized_phase = np.zeros(
+ len(list(self._robot.urdf_loader.get_end_effector_id_dict().values())))
+ self._leg_state = list(self._initial_leg_state)
+ self._desired_leg_state = list(self._initial_leg_state)
+
+ @property
+ def desired_leg_state(self) -> Sequence[gait_generator.LegState]:
+ """The desired leg SWING/STANCE states.
+
+ Returns:
+ The SWING/STANCE states for all legs.
+
+ """
+ return self._desired_leg_state
+
+ @property
+ def leg_state(self) -> Sequence[gait_generator.LegState]:
+ """The leg state after considering contact with ground.
+
+ Returns:
+ The actual state of each leg after accounting for contacts.
+ """
+ return self._leg_state
+
+ @property
+ def swing_duration(self) -> Sequence[float]:
+ return self._swing_duration
+
+ @property
+ def stance_duration(self) -> Sequence[float]:
+ return self._stance_duration
+
+ @property
+ def normalized_phase(self) -> Sequence[float]:
+ """The phase within the current swing or stance cycle.
+
+ Reflects the leg's phase within the curren swing or stance stage. For
+ example, at the end of the current swing duration, the phase will
+ be set to 1 for all swing legs. Same for stance legs.
+
+ Returns:
+ Normalized leg phase for all legs.
+
+ """
+ return self._normalized_phase
+
+ def update(self, current_time):
+ contact_state = [
+ np.linalg.norm(contact_force) > self._contact_detection_force_threshold
+ for contact_force in self._robot.feet_contact_forces()
+ ]
+
+ for leg_id in range(
+ len(list(self._robot.urdf_loader.get_end_effector_id_dict().values()))):
+ # Here is the explanation behind this logic: We use the phase within the
+ # full swing/stance cycle to determine if a swing/stance switch occurs
+ # for a leg. The threshold value is the "initial_state_ratio_in_cycle" as
+ # explained before. If the current phase is less than the initial state
+ # ratio, the leg is either in the initial state or has switched back after
+ # one or more full cycles.
+ full_cycle_period = (
+ self._stance_duration[leg_id] / self._duty_factor[leg_id])
+ # To account for the non-zero initial phase, we offset the time duration
+ # with the effect time contribution from the initial leg phase.
+ augmented_time = current_time + self._initial_leg_phase[
+ leg_id] * full_cycle_period
+ phase_in_full_cycle = math.fmod(augmented_time,
+ full_cycle_period) / full_cycle_period
+ ratio = self._initial_state_ratio_in_cycle[leg_id]
+ if phase_in_full_cycle < ratio:
+ self._desired_leg_state[leg_id] = self._initial_leg_state[leg_id]
+ self._normalized_phase[leg_id] = phase_in_full_cycle / ratio
+ else:
+ # A phase switch happens for this leg.
+ self._desired_leg_state[leg_id] = self._next_leg_state[leg_id]
+ self._normalized_phase[leg_id] = (phase_in_full_cycle - ratio) / (1 -
+ ratio)
+
+ self._leg_state[leg_id] = self._desired_leg_state[leg_id]
+
+ # No contact detection at the beginning of each SWING/STANCE phase.
+ if (self._normalized_phase[leg_id] <
+ self._contact_detection_phase_threshold):
+ continue
+ if (self._leg_state[leg_id] == gait_generator.LegState.SWING and
+ contact_state[leg_id]):
+ logging.info("early touch down detected")
+ self._leg_state[leg_id] = gait_generator.LegState.EARLY_CONTACT
+ if (self._leg_state[leg_id] == gait_generator.LegState.STANCE and
+ not contact_state[leg_id]):
+ self._leg_state[leg_id] = gait_generator.LegState.LOSE_CONTACT
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/raibert_swing_leg_controller.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/raibert_swing_leg_controller.py
new file mode 100644
index 000000000..aad4b9b67
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/raibert_swing_leg_controller.py
@@ -0,0 +1,242 @@
+"""The swing leg controller class."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import copy
+import math
+from typing import Any, Mapping, Sequence, Tuple
+
+import gin
+import numpy as np
+
+from pybullet_envs.minitaur.agents.baseline_controller import gait_generator as gait_generator_lib
+from pybullet_envs.minitaur.agents.baseline_controller import leg_controller
+
+# The position correction coefficients in Raibert's formula.
+_KP = 0.025
+
+# At the end of swing, we leave a small clearance to prevent unexpected foot
+# collision.
+_FOOT_CLEARANCE_M = 0.0
+_DEFAULT_LOCAL_HIP_POSITIONS = ((0.21, -0.1157, 0), (0.21, 0.1157, 0),
+ (-0.21, -0.1157, 0), (-0.21, 0.1157, 0))
+_DEFAULT_SWING_EASE_UP_PHASE = 1.0
+_DEFAULT_SWING_EASE_UP_PERCENT = 1.0
+_FEED_FORWARD_TORQUES = (-0.7, 0, 0, 0.7, 0, 0, -0.7, 0, 0, 0.7, 0, 0)
+
+
+def _gen_parabola(phase: float, start: float, mid: float, end: float) -> float:
+ """Gets a point on a parabola y = a x^2 + b x + c.
+
+ The Parabola is determined by three points (0, start), (0.5, mid), (1, end) in
+ the plane.
+
+ Args:
+ phase: Normalized to [0, 1]. A point on the x-axis of the parabola.
+ start: The y value at x == 0.
+ mid: The y value at x == 0.5.
+ end: The y value at x == 1.
+
+ Returns:
+ The y value at x == phase.
+ """
+ mid_phase = 0.5
+ delta_1 = mid - start
+ delta_2 = end - start
+ delta_3 = mid_phase**2 - mid_phase
+ coef_a = (delta_1 - delta_2 * mid_phase) / delta_3
+ coef_b = (delta_2 * mid_phase**2 - delta_1) / delta_3
+ coef_c = start
+
+ return coef_a * phase**2 + coef_b * phase + coef_c
+
+
+def _gen_swing_foot_trajectory(input_phase: float, start_pos: Sequence[float],
+ end_pos: Sequence[float], ease_up_phase: float,
+ ease_up_percent: float) -> Tuple[float]:
+ """Generates the swing trajectory using a parabola.
+
+ Args:
+ input_phase: the swing/stance phase value between [0, 1].
+ start_pos: The foot's position at the beginning of swing cycle.
+ end_pos: The foot's desired position at the end of swing cycle.
+ ease_up_phase: Time length for the initial ease up phase dueing swing cycle.
+ ease_up_percent: Percentage of the swing cycle completed after
+ ease_up_phase.
+
+ Returns:
+ The desired foot position at the current phase.
+ """
+ # We augment the swing speed using the below formula. For the first portion of
+ # the swing cycle (ease_up_phase), the swing leg moves faster and finishes
+ # ease_up_percent% of the full swing trajectory. The rest of trajectory takes
+ # the rest of the swing cycle. Intuitely, we want to move the swing foot
+ # quickly to the target landing location and stay above the ground, in this
+ # way the control is more robust to perturbations to the body that may cause
+ # the swing foot to drop onto the ground earlier than expected. This is a
+ # common practice similar to the MIT cheetah and Marc Raibert's original
+ # controllers.
+ assert 0 <= ease_up_percent <= 1
+ assert 0 <= ease_up_phase <= 1
+ phase = input_phase
+ if input_phase <= ease_up_phase:
+ phase = ease_up_percent * math.sin(input_phase /
+ (2 * ease_up_phase) * math.pi)
+ else:
+ phase = ease_up_percent + (input_phase - ease_up_phase) * (
+ 1 - ease_up_percent) / (1 - ease_up_phase)
+
+ x = (1 - phase) * start_pos[0] + phase * end_pos[0]
+ y = (1 - phase) * start_pos[1] + phase * end_pos[1]
+ max_clearance = 0.1
+ mid = max(end_pos[2], start_pos[2]) + max_clearance
+ z = _gen_parabola(phase, start_pos[2], mid, end_pos[2])
+
+ # PyType detects the wrong return type here.
+ return (x, y, z) # pytype: disable=bad-return-type
+
+
+@gin.configurable
+class RaibertSwingLegController(leg_controller.LegController):
+ """Controls the swing leg position using Raibert's formula.
+
+ For details, please refer to chapter 2 in "Legged robbots that balance" by
+ Marc Raibert. The key idea is to stablize the swing foot's location based on
+ the CoM moving speed.
+
+ """
+
+ def __init__(
+ self,
+ robot: Any,
+ gait_generator: Any,
+ state_estimator: Any,
+ desired_speed: Tuple[float] = (0, 0),
+ desired_twisting_speed: float = 0,
+ desired_height: float = 0.45,
+ foot_clearance: float = _FOOT_CLEARANCE_M,
+ local_hip_positions: Tuple[Tuple[float]] = _DEFAULT_LOCAL_HIP_POSITIONS,
+ ease_up_phase: float = _DEFAULT_SWING_EASE_UP_PHASE,
+ ease_up_percent: float = _DEFAULT_SWING_EASE_UP_PERCENT,
+ feed_forward_torques: Sequence[float] = _FEED_FORWARD_TORQUES
+ ):
+ """Initializes the class.
+
+ Args:
+ robot: A robot instance.
+ gait_generator: Generates the stance/swing pattern.
+ state_estimator: Estiamtes the CoM speeds.
+ desired_speed: Behavior parameters. X-Y speed.
+ desired_twisting_speed: Behavior control parameters.
+ desired_height: Desired standing height.
+ foot_clearance: The foot clearance on the ground at the end of the swing
+ cycle.
+ local_hip_positions: Positions of the robot's hips in local frames.
+ ease_up_phase: Time length for the initial ease up phase dueing swing
+ cycle.
+ ease_up_percent: Percentage of the swing cycle completed after
+ ease_up_phase.
+ feed_forward_torques: A feed-forward torque applied to the actuators on
+ the swing legs (e.g. for gravity compensation).
+ """
+ self._robot = robot
+ self._state_estimator = state_estimator
+ self._gait_generator = gait_generator
+ self._last_leg_state = gait_generator.desired_leg_state
+ self.desired_speed = np.array((desired_speed[0], desired_speed[1], 0))
+ self.desired_twisting_speed = desired_twisting_speed
+ self._desired_height = np.array((0, 0, desired_height - foot_clearance))
+ self._local_hip_positions = local_hip_positions
+ self._ease_up_phase = ease_up_phase
+ self._ease_up_percent = ease_up_percent
+ self._feed_forward_torques = feed_forward_torques
+
+ self._joint_angles = None
+ self._phase_switch_foot_local_position = None
+ self.reset(0)
+
+ def reset(self, current_time: float) -> None:
+ """Called during the start of a swing cycle.
+
+ Args:
+ current_time: The wall time in seconds.
+ """
+ del current_time
+ self._last_leg_state = self._gait_generator.desired_leg_state
+ self._phase_switch_foot_local_position = (self._robot.foot_positions())
+ self._joint_angles = {}
+
+ def update(self, current_time: float) -> None:
+ """Called at each control step.
+
+ Args:
+ current_time: The wall time in seconds.
+ """
+ del current_time
+ new_leg_state = self._gait_generator.desired_leg_state
+
+ # Detects phase switch for each leg so we can remember the feet position at
+ # the beginning of the swing phase.
+ for leg_id, state in enumerate(new_leg_state):
+ if (state == gait_generator_lib.LegState.SWING and
+ state != self._last_leg_state[leg_id]):
+ self._phase_switch_foot_local_position[leg_id] = (
+ self._robot.foot_positions()[leg_id])
+
+ self._last_leg_state = copy.deepcopy(new_leg_state)
+
+ def get_action(self) -> Mapping[Any, Any]:
+ com_velocity = self._state_estimator.com_velocity_body_yaw_aligned_frame
+ com_velocity = np.array((com_velocity[0], com_velocity[1], 0))
+
+ _, _, yaw_dot = self._robot.base_roll_pitch_yaw_rate
+
+ local_toe_positions = np.array(self._robot.foot_positions())
+
+ for leg_id, leg_state in enumerate(self._gait_generator.leg_state):
+ if (leg_state == gait_generator_lib.LegState.STANCE or
+ leg_state == gait_generator_lib.LegState.EARLY_CONTACT):
+ continue
+
+ # For now we did not consider the body pitch/roll and all calculation is
+ # in the body frame. TODO(b/143378213): Calculate the foot_target_position
+ # in world farme and then project back to calculate the joint angles.
+ hip_offset = self._local_hip_positions[leg_id]
+ twisting_vector = np.array((-hip_offset[1], hip_offset[0], 0))
+ hip_horizontal_velocity = com_velocity + yaw_dot * twisting_vector
+ target_hip_horizontal_velocity = (
+ self.desired_speed + self.desired_twisting_speed * twisting_vector)
+
+ foot_target_position = (
+ hip_horizontal_velocity *
+ self._gait_generator.swing_duration[leg_id] / 2 - _KP *
+ (target_hip_horizontal_velocity - hip_horizontal_velocity)
+ ) - self._desired_height + np.array((hip_offset[0], hip_offset[1], 0))
+
+ foot_position = _gen_swing_foot_trajectory(
+ self._gait_generator.normalized_phase[leg_id],
+ self._phase_switch_foot_local_position[leg_id], foot_target_position,
+ self._ease_up_phase, self._ease_up_percent)
+
+ local_toe_positions[leg_id] = foot_position
+ joint_ids, joint_angles = self._robot.motor_angles_from_foot_positions(
+ local_toe_positions, position_in_world_frame=False)
+ # Update the stored joint angles as needed.
+ motors_per_leg = len(joint_ids) // len(local_toe_positions)
+ for joint_id, joint_angle in zip(joint_ids, joint_angles):
+ self._joint_angles[joint_id] = (joint_angle, joint_id // motors_per_leg)
+
+ action = {}
+ kps, kds = self._robot.motor_model.get_motor_gains()
+
+ for joint_id, joint_angle_leg_id in self._joint_angles.items():
+ leg_id = joint_angle_leg_id[1]
+ if self._gait_generator.leg_state[
+ leg_id] == gait_generator_lib.LegState.SWING:
+ # This is a hybrid action for PD control.
+ action[joint_id] = (joint_angle_leg_id[0], kps[joint_id], 0,
+ kds[joint_id], self._feed_forward_torques[joint_id])
+
+ return action
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/state_estimator.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/state_estimator.py
new file mode 100644
index 000000000..6b12a05ad
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/state_estimator.py
@@ -0,0 +1,21 @@
+"""State estimator."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import abc
+
+
+class StateEstimatorBase(object): # pytype: disable=ignored-metaclass
+ """Estimates the unmeasurable state of the robot."""
+
+ __metaclass__ = abc.ABCMeta
+
+ @abc.abstractmethod
+ def reset(self, current_time):
+ pass
+
+ @abc.abstractmethod
+ def update(self, current_time):
+ pass
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/static_gait_controller.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/static_gait_controller.py
new file mode 100644
index 000000000..eb4ccdd1c
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/static_gait_controller.py
@@ -0,0 +1,63 @@
+# Lint as: python3
+"""A static gait controller for a quadruped robot. Experimental code."""
+
+import gin
+import numpy as np
+from pybullet_envs.minitaur.agents.baseline_controller import foot_stepper
+
+toe_pos_local_ref = np.array([[0.1478, -0.11459, -0.45576],
+ [0.1478, 0.11688, -0.45576],
+ [-0.2895, -0.11459, -0.45576],
+ [-0.2895, 0.11688, -0.45576]])
+
+
+@gin.configurable
+class StaticGaitController(object):
+ """A static gait controller for a quadruped robot."""
+
+ def __init__(self, robot):
+ self._robot = robot
+ self._toe_ids = tuple(robot.urdf_loader.get_end_effector_id_dict().values())
+ self._wait_count = 0
+ self._stepper = foot_stepper.FootStepper(self._robot.pybullet_client,
+ self._toe_ids, toe_pos_local_ref)
+
+ def act(self, observation):
+ """Computes actions based on observations."""
+ del observation
+ p = self._robot.pybullet_client
+ quadruped = self._robot.robot_id
+ step_input = foot_stepper.StepInput()
+ ls = p.getLinkStates(
+ quadruped, self._toe_ids, computeForwardKinematics=True)
+ toe_pos_world = np.array([ls[0][0], ls[1][0], ls[2][0], ls[3][0]])
+ base_com_pos, base_com_orn = p.getBasePositionAndOrientation(quadruped)
+ new_pos_world = np.array([0, 0, 0])
+
+ if self._stepper.is_com_stable() and not self._stepper.move_swing_foot:
+ self._wait_count += 1
+ if self._wait_count == 20:
+ self._stepper.next_foot()
+ if self._wait_count > 50:
+ self._wait_count = 0
+ step_dist = 0.15
+ print("time {}, make a step of {}".format(
+ self._robot.GetTimeSinceReset(), step_dist))
+ new_pos_local = self._stepper.get_reference_pos_swing_foot()
+ new_pos_local[0] += step_dist
+ new_pos_world, _ = p.multiplyTransforms(base_com_pos, base_com_orn,
+ new_pos_local, [0, 0, 0, 1])
+ self._stepper.swing_foot()
+
+ step_input.new_pos_world = new_pos_world
+ step_input.base_com_pos = base_com_pos
+ step_input.base_com_orn = base_com_orn
+ step_input.toe_pos_world = toe_pos_world
+ step_input.dt = 1.0 / 250
+ step_output = self._stepper.update(step_input)
+
+ # Finds joint poses to achieve toePosWorld
+ desired_joint_angles = self._robot.motor_angles_from_foot_positions(
+ foot_positions=step_output.new_toe_pos_world,
+ position_in_world_frame=True)[1]
+ return desired_joint_angles
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/time_based_moving_window_filter.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/time_based_moving_window_filter.py
new file mode 100644
index 000000000..42868795e
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/time_based_moving_window_filter.py
@@ -0,0 +1,45 @@
+"""A moving-window filter for smoothing the signals within certain time interval."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+
+import gin
+import numpy as np
+
+
+@gin.configurable
+class TimeBasedMovingWindowFilter:
+ """A moving-window filter for smoothing the signals within certain time interval."""
+
+ def __init__(
+ self,
+ filter_window: float = 0.1,
+ ):
+ """Initializes the class.
+
+ Args:
+ filter_window: The filtering window (in time) used to smooth the input
+ signal.
+ """
+ self._filter_window = filter_window
+ self.reset()
+
+ def reset(self):
+ self._timestamp_buffer = []
+ self._value_buffer = []
+
+ def calculate_average(self, new_value, timestamp):
+ """Compute the filtered signals based on the time-based moving window."""
+ self._timestamp_buffer.append(timestamp)
+ self._value_buffer.append(new_value)
+
+ while len(self._value_buffer) > 1:
+ if self._timestamp_buffer[
+ 0] < timestamp - self._filter_window:
+ self._timestamp_buffer.pop(0)
+ self._value_buffer.pop(0)
+ else:
+ break
+ return np.mean(self._value_buffer, axis=0)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/torque_stance_leg_controller.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/torque_stance_leg_controller.py
new file mode 100644
index 000000000..87ebed458
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/torque_stance_leg_controller.py
@@ -0,0 +1,229 @@
+# Lint as: python3
+"""A torque based stance controller framework."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import functools
+import logging
+from typing import Any, Sequence, Tuple
+
+import gin
+import numpy as np
+
+from pybullet_envs.minitaur.agents.baseline_controller import gait_generator as gait_generator_lib
+from pybullet_envs.minitaur.agents.baseline_controller import leg_controller
+#from pybullet_envs.minitaur.agents.baseline_controller.convex_mpc.python import convex_mpc
+#from google3.util.task.python import error
+
+try:
+ import mpc_osqp as convex_mpc # pytype: disable=import-error
+except: #pylint: disable=W0702
+ print("You need to install motion_imitation")
+ print("or use pip3 install motion_imitation --user")
+ print("see also https://github.com/google-research/motion_imitation")
+ import sys
+ sys.exit()
+
+
+_FORCE_DIMENSION = 3
+# The QP weights in the convex MPC formulation. See the MIT paper for details:
+# https://ieeexplore.ieee.org/document/8594448/
+# Intuitively, this is the weights of each state dimension when tracking a
+# desired CoM trajectory. The full CoM state is represented by
+# (roll_pitch_yaw, position, angular_velocity, velocity, gravity_place_holder).
+_MPC_WEIGHTS = (5, 5, 0.2, 0, 0, 10, 0.5, 0.5, 0.2, 0.2, 0.2, 0.1, 0)
+_PLANNING_HORIZON_STEPS = 10
+_PLANNING_TIMESTEP = 0.025
+#_MPC_CONSTRUCTOR = functools.partial(
+# convex_mpc.ConvexMpc, qp_solver_name=convex_mpc.QPSolverName.QPOASES)
+
+
+@gin.configurable
+class TorqueStanceLegController(leg_controller.LegController):
+ """A torque based stance leg controller framework.
+
+ Takes in high level parameters like walking speed and turning speed, and
+ generates necessary the torques for stance legs.
+ """
+
+ def __init__(
+ self,
+ robot: Any,
+ gait_generator: Any,
+ state_estimator: Any,
+ desired_speed: Tuple[float] = (0, 0),
+ desired_twisting_speed: float = 0,
+ desired_roll_pitch: Tuple[float] = (0, 0),
+ desired_body_height: float = 0.45,
+ body_mass: float = 220 / 9.8,
+ body_inertia: Tuple[float] = (0.183375, 0, 0, 0, 0.6267, 0, 0, 0,
+ 0.636175),
+ num_legs: int = 4,
+ friction_coeffs: Sequence[float] = (0.5, 0.5, 0.5, 0.5),
+ qp_weights: Sequence[float] = _MPC_WEIGHTS,
+ planning_horizon: int = _PLANNING_HORIZON_STEPS,
+ planning_timestep: int = _PLANNING_TIMESTEP,
+ ):
+ """Initializes the class.
+
+ Tracks the desired position/velocity of the robot by computing proper joint
+ torques using MPC module.
+
+ Args:
+ robot: A robot instance.
+ gait_generator: Used to query the locomotion phase and leg states.
+ state_estimator: Estimate the robot states (e.g. CoM velocity).
+ desired_speed: desired CoM speed in x-y plane.
+ desired_twisting_speed: desired CoM rotating speed in z direction.
+ desired_roll_pitch: desired CoM roll and pitch.
+ desired_body_height: The standing height of the robot.
+ body_mass: The total mass of the robot.
+ body_inertia: The inertia matrix in the body principle frame. We assume
+ the body principle coordinate frame has x-forward and z-up.
+ num_legs: The number of legs used for force planning.
+ friction_coeffs: The friction coeffs on the contact surfaces.
+ qp_weights: The weights used in solving the QP problem.
+ planning_horizon: Number of steps to roll-out in the QP formulation.
+ planning_timestep: Timestep between each step in the QP formulation.
+ """
+
+ self._robot = robot
+ self._gait_generator = gait_generator
+ self._state_estimator = state_estimator
+ self._desired_speed = desired_speed
+ self._desired_twisting_speed = desired_twisting_speed
+ self._desired_roll_pitch = desired_roll_pitch
+ self._desired_body_height = desired_body_height
+ self._body_mass = body_mass
+ self._num_legs = num_legs
+ self._friction_coeffs = np.array(friction_coeffs)
+ self._qp_solver_fail = False
+ self._com_estimate_leg_indices = None
+ qp_solver = convex_mpc.QPOASES #convex_mpc.OSQP #
+
+ body_inertia_list = list(body_inertia)
+ weights_list = list(qp_weights)
+
+ self._mpc = convex_mpc.ConvexMpc(
+ body_mass,
+ body_inertia_list,
+ self._num_legs,
+ planning_horizon,
+ planning_timestep,
+ weights_list,
+ 1e-6,
+ qp_solver
+ )
+
+
+ def reset(self, current_time):
+ del current_time
+ self._qp_solver_fail = False
+ self._com_estimate_leg_indices = None
+
+ def update(self, current_time):
+ del current_time
+
+ def get_action(self):
+ """Computes the torque for stance legs."""
+ desired_com_position = np.array((0., 0., self._desired_body_height),
+ dtype=np.float64)
+ desired_com_velocity = np.array(
+ (self.desired_speed[0], self.desired_speed[1], 0.), dtype=np.float64)
+ desired_com_roll_pitch_yaw = np.array(
+ (self.desired_roll_pitch[0], self.desired_roll_pitch[1], 0.),
+ dtype=np.float64)
+ desired_com_angular_velocity = np.array(
+ (0., 0., self.desired_twisting_speed), dtype=np.float64)
+ foot_contact_state = np.array(
+ [(leg_state == gait_generator_lib.LegState.STANCE or
+ leg_state == gait_generator_lib.LegState.EARLY_CONTACT)
+ for leg_state in self._gait_generator.desired_leg_state],
+ dtype=np.int32)
+
+ # We use the body yaw aligned world frame for MPC computation.
+ com_roll_pitch_yaw = np.array(
+ self._robot.base_roll_pitch_yaw, dtype=np.float64)
+ com_roll_pitch_yaw[2] = 0
+ #try:
+ estimated_com_position = np.array(())
+ if hasattr(self._state_estimator, "estimated_com_height"):
+ estimated_com_position = np.array(
+ (0, 0, self._state_estimator.estimated_com_height))
+ try:
+ predicted_contact_forces = self._mpc.compute_contact_forces(
+ estimated_com_position, #com_position
+ np.asarray(self._state_estimator.com_velocity_body_yaw_aligned_frame,
+ dtype=np.float64), #com_velocity
+ np.array(com_roll_pitch_yaw, dtype=np.float64), #com_roll_pitch_yaw
+ # Angular velocity in the yaw aligned world frame is actually different
+ # from rpy rate. We use it here as a simple approximation.
+ np.asarray(self._robot.base_roll_pitch_yaw_rate,
+ dtype=np.float64), #com_angular_velocity
+ foot_contact_state, #foot_contact_states
+ np.array(self._robot.foot_positions(
+ position_in_world_frame=False).flatten(),
+ dtype=np.float64), #foot_positions_base_frame
+ self._friction_coeffs, #foot_friction_coeffs
+ desired_com_position, #desired_com_position
+ desired_com_velocity, #desired_com_velocity
+ desired_com_roll_pitch_yaw, #desired_com_roll_pitch_yaw
+ desired_com_angular_velocity #desired_com_angular_velocity
+ )
+ except:# error.StatusNotOk as e:
+ logging.error("Error in Torque Stance Leg")#e.message)
+ self._qp_solver_fail = True
+ predicted_contact_forces = np.zeros(self._num_legs * _FORCE_DIMENSION)
+
+ contact_forces = {}
+ for i in range(self._num_legs):
+ contact_forces[i] = np.array(
+ predicted_contact_forces[i * _FORCE_DIMENSION:(i + 1) *
+ _FORCE_DIMENSION])
+
+ _, kds = self._robot.motor_model.get_motor_gains()
+ action = {}
+ for leg_id, force in contact_forces.items():
+ motor_torques = self._robot.map_contact_force_to_joint_torques(
+ leg_id, force)
+ for joint_id, torque in motor_torques.items():
+ action[joint_id] = (0, 0, 0, kds[joint_id], torque)
+ return action
+
+ @property
+ def qp_solver_fail(self):
+ return self._qp_solver_fail
+
+ @property
+ def desired_speed(self):
+ return self._desired_speed
+
+ @desired_speed.setter
+ def desired_speed(self, speed):
+ self._desired_speed = speed
+
+ @property
+ def desired_twisting_speed(self):
+ return self._desired_twisting_speed
+
+ @desired_twisting_speed.setter
+ def desired_twisting_speed(self, twisting_speed):
+ self._desired_twisting_speed = twisting_speed
+
+ @property
+ def desired_roll_pitch(self):
+ return self._desired_roll_pitch
+
+ @desired_roll_pitch.setter
+ def desired_roll_pitch(self, roll_pitch):
+ self._desired_roll_pitch = roll_pitch
+
+ @property
+ def desired_body_height(self):
+ return self._desired_body_height
+
+ @desired_body_height.setter
+ def desired_body_height(self, body_height):
+ self._desired_body_height = body_height
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/trajectory_generator/__init__.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/trajectory_generator/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/trajectory_generator/controller_simple.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/trajectory_generator/controller_simple.py
new file mode 100644
index 000000000..746e4cfd5
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/trajectory_generator/controller_simple.py
@@ -0,0 +1,87 @@
+"""Asymmetric sine controller for quadruped locomotion.
+
+Asymmetric sine uses cosine and sine waves to generate swinging and extension
+for leg motion. It's asymmetric because sine waves are split into two phases
+(swing forward and stance) and these phases have different frequencies according
+to what proportion of a period will be spend on swinging forward forward
+swinging backwards. In addition, the sine wave for extension has different
+amplitudes during these two phases.
+"""
+import math
+
+TWO_PI = 2 * math.pi
+_DEFAULT_LEG_AMPLITUDE_EXTENSION = -0.02
+_DEFAULT_LEG_AMPLITUDE_SWING = 0.5
+_DEFAULT_LEG_AMPLITUDE_LIFT = 0.9
+_DEFAULT_WALKING_HEIGHT = 0.0
+_DEFAULT_LEG_CENTER_SWING = 0.0
+_DELTA_CENTER_EXTENSION_CAP = 1
+_DELTA_INTENSITY_CAP = 0.1
+
+
+class SimpleLegController(object):
+ """Controller that gives swing and extension based on phase and parameters.
+
+
+ The controller returns the swing-extend pair based on a parameterized
+ ellipsoid trajectory that depends on center of motion, amplitude and phase.
+ The parameters are
+ amplitude_extension: Amplitude for extension during stance (phase < pi).
+ amplitude_lift: Amplitude for extension during swing (phase > pi).
+ amplitude_swing: Amplitude for swing.
+ center_extension: The value extension signal oscillates around.
+ center_swing: The value swing signal oscillates around.
+ intensity: A coefficient that scales the motion of the legs.
+ The formula to calculate motion and more detailed information about these
+ parameters can be found at go/pmtg-refactored.
+ """
+
+ def __init__(self, init_phase=0):
+ self.amplitude_extension = _DEFAULT_LEG_AMPLITUDE_EXTENSION
+ self.amplitude_swing = _DEFAULT_LEG_AMPLITUDE_SWING
+ self.amplitude_lift = _DEFAULT_LEG_AMPLITUDE_LIFT
+ self.center_extension = _DEFAULT_WALKING_HEIGHT
+ self.center_swing = _DEFAULT_LEG_CENTER_SWING
+ self.intensity = 1.0
+ self._init_phase = init_phase
+ self.phase = init_phase
+ self.phase_offset = 0
+
+ def reset(self):
+ self.phase = self._init_phase
+
+ def get_swing_extend(self):
+ """Returns the swing and extend parameters for the leg.
+
+ Returns:
+ swing: Desired swing of the leg.
+ extend: Desired extension amount of the leg.
+ """
+
+ # Increase default extension by the extra extension scaled by intensity.
+ # Extend reduces to default center extension when intensity goes to 0,
+ # because we prefer the legs to stay at walking height when intensity is
+ # 0.
+ amplitude_extension = self.amplitude_extension
+ # The leg is in swing phase when phase > pi.
+ if self.phase > math.pi:
+ amplitude_extension = self.amplitude_lift
+ extend = self.center_extension + (
+ amplitude_extension * math.sin(self.phase)) * self.intensity
+ # Calculate the swing based on the signal and scale it with intensity.
+ # Swing reduces to 0 when intensity goes to 0, because we would prefer the
+ # legs to stay neutral (standing position instead of center swing) when
+ # intensity is 0.
+ swing = self.center_swing + self.amplitude_swing * math.cos(self.phase)
+ swing *= self.intensity
+
+ return swing, extend
+
+ def adjust_center_extension(self, target_center_extension):
+ delta = min(_DELTA_CENTER_EXTENSION_CAP,
+ target_center_extension - self.center_extension)
+ self.center_extension += delta
+
+ def adjust_intensity(self, target_intensity):
+ delta = min(_DELTA_INTENSITY_CAP, target_intensity - self.intensity)
+ self.intensity += delta
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/trajectory_generator/tg_inplace.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/trajectory_generator/tg_inplace.py
new file mode 100644
index 000000000..b07048e75
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/trajectory_generator/tg_inplace.py
@@ -0,0 +1,57 @@
+"""Trajectory Generator for in-place stepping motion for quadruped robot."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import math
+import numpy as np
+
+TWO_PI = 2 * math.pi
+
+
+def _get_actions_asymmetric_sine(phase, tg_params):
+ """Returns the leg extension given current phase of TG and parameters.
+
+ Args:
+ phase: a number in [0, 2pi) representing current leg phase
+ tg_params: a dictionary of tg parameters:
+ stance_lift_cutoff -- switches the TG between stance (phase < cutoff) and
+ lift (phase > cutoff) phase
+ amplitude_swing -- amplitude in swing phase
+ amplitude_lift -- amplitude in lift phase
+ center_extension -- center of leg extension
+ """
+ stance_lift_cutoff = tg_params['stance_lift_cutoff']
+ a_prime = np.where(phase < stance_lift_cutoff, tg_params['amplitude_stance'],
+ tg_params['amplitude_lift'])
+ scaled_phase = np.where(
+ phase > stance_lift_cutoff, np.pi + (phase - stance_lift_cutoff) /
+ (TWO_PI - stance_lift_cutoff) * np.pi, phase / stance_lift_cutoff * np.pi)
+ return tg_params['center_extension'] + a_prime * np.sin(scaled_phase)
+
+
+def step(current_phases, leg_frequencies, dt, tg_params):
+ """Steps forward the in-place trajectory generator.
+
+ Args:
+ current_phases: phases of each leg.
+ leg_frequencies: the frequency to proceed the phase of each leg.
+ dt: amount of time (sec) between consecutive time steps.
+ tg_params: a set of parameters for trajectory generator, see the docstring
+ of "_get_actions_asymmetric_sine" for details.
+
+ Returns:
+ actions: leg swing/extensions as output by the trajectory generator.
+ new_state: new swing/extension.
+ """
+ new_phases = np.fmod(current_phases + TWO_PI * leg_frequencies * dt, TWO_PI)
+ extensions = []
+ for leg_id in range(4):
+ extensions.append(
+ _get_actions_asymmetric_sine(new_phases[..., leg_id], tg_params))
+ return new_phases, extensions
+
+
+def reset():
+ return np.array([0, np.pi * 0.5, np.pi, np.pi * 1.5])
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/trajectory_generator/tg_simple.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/trajectory_generator/tg_simple.py
new file mode 100644
index 000000000..c63884d81
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/trajectory_generator/tg_simple.py
@@ -0,0 +1,400 @@
+"""Trajectory Generator generates walking leg motions for a quadruped robot.
+
+Trajectory Generator (TG) has an internal state (phase) and generates
+walking-like motion for 8 motors of minitaur quadruped robot based on
+parameters
+such as:
+ - delta time to progress the TG's internal state.
+ - intensity to control amount of movement (stride length and lift of the legs).
+ - waking height to control the average extension of the legs.
+
+Each time step() is called, the internal state is progressed and 8 motor
+positions are generated. This TG uses the open-loop SineController class to
+provide leg positions. It is mainly a wrapper for ability to modulating the
+time
+and other parameters of the SineController.
+"""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import math
+import numpy as np
+import gin
+from pybullet_envs.minitaur.agents.trajectory_generator import controller_simple
+
+PHASE_LOWER_BOUND = 0.0
+PHASE_UPPER_BOUND = 1.0
+WALK_HEIGHT_LOWER_BOUND = -0.5
+WALK_HEIGHT_UPPER_BOUND = 1.0
+INTENSITY_LOWER_BOUND = 0.0
+INTENSITY_UPPER_BOUND = 1.5
+_SWING_STANCE_LOWER_BOUND = 0.2
+_SWING_STANCE_UPPER_BOUND = 5.0
+_DELTA_SWING_STANCE_CAP = 0.4
+_TWO_PI = math.pi * 2.0
+_LEG_COUPLING_DICT = {
+ "null": [],
+ # All the legs are coupled.
+ "all coupled": [0, 0, 0, 0],
+ # Front legs and back legs are coupled separately.
+ "front back": [0, 1, 0, 1],
+ # Left legs and right legs are coupled separately.
+ "left right": [0, 0, 1, 1],
+ # Diagonal legs are coupled (i.e. trottting).
+ "diagonal": [0, 1, 1, 0],
+ # Each leg is indepenent.
+ "decoupled": [0, 1, 2, 3]
+}
+
+
+@gin.configurable
+class TgSimple(object):
+ """TgSimple class is a simplified trajectory generator for quadruped walking.
+
+ It returns 8 actions for quadruped slow walking behavior
+ based on the parameters provided such as intensity, walking height and delta
+ time. It returns its internal phase as information.
+ """
+
+ def __init__(self,
+ walk_height_lower_bound=WALK_HEIGHT_LOWER_BOUND,
+ walk_height_upper_bound=WALK_HEIGHT_UPPER_BOUND,
+ intensity_lower_bound=INTENSITY_LOWER_BOUND,
+ intensity_upper_bound=INTENSITY_UPPER_BOUND,
+ swing_stance_lower_bound=_SWING_STANCE_LOWER_BOUND,
+ swing_stance_upper_bound=_SWING_STANCE_UPPER_BOUND,
+ integrator_coupling_mode="all coupled",
+ walk_height_coupling_mode="all coupled",
+ variable_swing_stance_ratio=False,
+ swing_stance_ratio=1.0,
+ init_leg_phase_offsets=None):
+ """Initialize the trajectory generator with a controller.
+
+ For trajectory generator, we create an asymmetric sine controller with
+ parameters that was previously optimized as an open-loop controller.
+
+ Args:
+ walk_height_lower_bound: Lower bound for walking height which sets the
+ default leg extension of the gait. Unit is rad, -0.5 by default.
+ walk_height_upper_bound: Lower bound for walking height which sets the
+ default leg extension of the gait. Unit is rad, 1.0 by default.
+ intensity_lower_bound: The upper bound for intensity of the trajectory
+ generator. It can be used to limit the leg movement.
+ intensity_upper_bound: The upper bound for intensity of the trajectory
+ generator. It can be used to limit the leg movement.
+ swing_stance_lower_bound: Lower bound for the swing vs stance ratio
+ parameter. Default value is 0.2.
+ swing_stance_upper_bound: Upper bound for the swing vs stance ratio
+ parameter. Default value is 0.2.
+ integrator_coupling_mode: How the legs should be coupled for integrators.
+ walk_height_coupling_mode: The same coupling mode used for walking
+ heights for the legs.
+ variable_swing_stance_ratio: A boolean to indicate if the swing stance
+ ratio can change per time step or not.
+ swing_stance_ratio: Time taken by swing phase vs stance phase. This is
+ only relevant if variable_swing_stance_ratio is False.
+ init_leg_phase_offsets: The initial phases of the legs. A list of 4
+ variables within [0,1). The order is front-left, rear-left, front-right
+ and rear-right.
+
+ Raises:
+ ValueError: If parameters are not valid values.
+ """
+ self._walk_height_lower_bound = walk_height_lower_bound
+ self._walk_height_upper_bound = walk_height_upper_bound
+ self._intensity_lower_bound = intensity_lower_bound
+ self._intensity_upper_bound = intensity_upper_bound
+ self._swing_stance_lower_bound = swing_stance_lower_bound
+ self._swing_stance_upper_bound = swing_stance_upper_bound
+ if not init_leg_phase_offsets:
+ init_leg_phase_offsets = [0, 0.25, 0.5, 0.75]
+ if len(init_leg_phase_offsets) != 4:
+ raise ValueError("The number of leg phase offsets is not equal to 4.")
+ if min(init_leg_phase_offsets) < 0 or max(init_leg_phase_offsets) >= 1:
+ raise ValueError("Leg phase offsets are not within [0,1)")
+ self._legs = []
+ for period in init_leg_phase_offsets:
+ init_phase = period * 2 * math.pi
+ self._legs.append(controller_simple.SimpleLegController(init_phase))
+
+ if integrator_coupling_mode not in _LEG_COUPLING_DICT:
+ raise ValueError("Invalid integrator_coupling_mode.")
+ if walk_height_coupling_mode not in _LEG_COUPLING_DICT:
+ raise ValueError("Invalid walk_height_coupling_mode.")
+
+ # Set the phase couplings and build a list of legs per phase coupling.
+ self._integrator_id_per_leg = _LEG_COUPLING_DICT[integrator_coupling_mode]
+ self._num_integrators = max(
+ self._integrator_id_per_leg) + 1 if self._integrator_id_per_leg else 0
+ self._legs_per_integrator_id = [[], [], [], []]
+ for idx, phase_id in enumerate(self._integrator_id_per_leg):
+ self._legs_per_integrator_id[phase_id].append(self._legs[idx])
+
+ # For each integrator coupling, create a integrator unit.
+ # For each leg controlled by that phase generator, mark the phase offset.
+ self._integrator_units = []
+ for legs_per_integrator in self._legs_per_integrator_id:
+ if legs_per_integrator:
+ circular_integrator = CircularAsymmetricalIntegratorUnit(
+ legs_per_integrator[0].phase)
+ self._integrator_units.append(circular_integrator)
+ for leg in legs_per_integrator:
+ leg.phase_offset = leg.phase - circular_integrator.phase
+
+ # Set the walking heights couplings.
+ self._walk_height_id_per_leg = _LEG_COUPLING_DICT[walk_height_coupling_mode]
+ self._num_walk_heights = max(
+ self._walk_height_id_per_leg) + 1 if self._walk_height_id_per_leg else 0
+ self._variable_swing_stance_ratio = variable_swing_stance_ratio
+ self._swing_stance_ratio = swing_stance_ratio
+
+ def reset(self):
+ """Resets leg phase offsets to their initial values."""
+ for leg in self._legs:
+ leg.reset()
+ for circular_integrator in self._integrator_units:
+ circular_integrator.reset()
+
+ def get_parameter_bounds(self):
+ """Lower and upper bounds for the parameters generator's parameters.
+
+ Returns:
+ 2-tuple of:
+ - Lower bounds for the parameters such as intensity, walking height and
+ lift fraction.
+ - Upper bounds for the same parameters.
+ """
+ lower_bounds = [self._intensity_lower_bound]
+ upper_bounds = [self._intensity_upper_bound]
+ lower_bounds += [self._walk_height_lower_bound] * self._num_walk_heights
+ upper_bounds += [self._walk_height_upper_bound] * self._num_walk_heights
+ lower_bounds += [self._swing_stance_lower_bound
+ ] * self._variable_swing_stance_ratio
+ upper_bounds += [self._swing_stance_upper_bound
+ ] * self._variable_swing_stance_ratio
+
+ return lower_bounds, upper_bounds
+
+ def get_actions(self, delta_real_time, tg_params):
+ """Get actions for 8 motors after increasing the phase delta_time.
+
+ Args:
+ delta_real_time: Time in seconds that have actually passed since the last
+ step of the trajectory generator.
+ tg_params: An ndarray of the parameters for generating the trajectory. The
+ parameters must be in the correct order (time_scale, intensity,
+ walking_height, and swing vs stance)
+
+ Raises:
+ ValueError: In case the input dimension does not match expected.
+ Returns:
+ The rotations for all the 8 motors for this time step
+ returned in an array [front_left_motor_1, front_left_motor_2, etc].
+ """
+ speeds, intensity, heights, swing_stance_ratio = self._process_tg_params(
+ tg_params)
+ # Adjust the swing stance ratio of the controller (used for all four legs).
+ if swing_stance_ratio:
+ self.adjust_swing_stance_ratio(swing_stance_ratio)
+ # Adjust the walking height, intensity and swing vs stance of the legs.
+ for idx, leg in enumerate(self._legs):
+ leg.adjust_intensity(intensity)
+ if heights:
+ leg.adjust_center_extension(heights[self._walk_height_id_per_leg[idx]])
+
+ # Progress all the phase generators based on delta time.
+ for idx, integrator_unit in enumerate(self._integrator_units):
+ integrator_unit.progress_phase(speeds[idx] * delta_real_time,
+ self._swing_stance_ratio)
+
+ # Set the phases for the legs based on their offsets with phase generators.
+ for phase_id, leg_list in enumerate(self._legs_per_integrator_id):
+ for leg in leg_list:
+ delta_period = leg.phase_offset / (2.0 * math.pi)
+ leg.phase = self._integrator_units[phase_id].calculate_progressed_phase(
+ delta_period, self._swing_stance_ratio)
+
+ # Calculate swingextend and convert it to the motor rotations.
+ actions = []
+ for idx, leg in enumerate(self._legs):
+ swing, extend = leg.get_swing_extend()
+ actions.extend([swing, extend])
+ return actions
+
+ def _process_tg_params(self, tg_params):
+ """Process the trajectory generator parameters and split them.
+
+ Args:
+ tg_params: A list consisting of time_scales, intensity, walking_heights,
+ swing_stance_ratio. The size depends on the configuration and inital
+ flags.
+
+ Returns:
+ time_scales: A list of multipliers of delta time (one per integrator).
+ intensity: Intensity of the trajectory generator (one variable).
+ walking_heights: Walking heights used for the legs. The length depends on
+ the coupling between the legs selected at the initialization.
+ swing_stance_ratio: The ratio of the speed of the leg during swing stance
+ vs stance phase.
+ """
+
+ # Check if the given input's dimension matches the expectation considering
+ # the number of parameters the trajectory generator uses.
+ if isinstance(tg_params, np.ndarray):
+ tg_params = tg_params.tolist()
+ expected_action_dim = 1 + self._num_integrators + self._num_walk_heights
+ if self._variable_swing_stance_ratio:
+ expected_action_dim += 1
+ if len(tg_params) != expected_action_dim:
+ raise ValueError(
+ "Action dimension does not match the expectation {} vs {}".format(
+ len(tg_params), expected_action_dim))
+ # Split input into different parts based on type. The order must match the
+ # order given by the order in get_parameter_bounds
+ time_scales = tg_params[0:self._num_integrators]
+ intensity = tg_params[self._num_integrators]
+ walking_heights = tg_params[(self._num_integrators + 1):(
+ 1 + self._num_integrators + self._num_walk_heights)]
+ swing_stance_ratio = None
+ if self._variable_swing_stance_ratio:
+ swing_stance_ratio = tg_params[1 + self._num_integrators +
+ self._num_walk_heights]
+
+ return time_scales, intensity, walking_heights, swing_stance_ratio
+
+ def get_state(self):
+ """Returns a list of floats representing the phase of the controller.
+
+ The phase of the controller is composed of the phases of the integrators.
+ For each integrator, the phase is composed of 2 floats that represents the
+ sine and cosine of the phase of that integrator.
+
+ Returns:
+ List containing sine and cosine of the phases of all the integrators.
+ """
+ return [x for y in self._integrator_units for x in y.get_state()]
+
+ def get_state_lower_bounds(self):
+ """Lower bounds for the internal state.
+
+ Returns:
+ The list containing the lower bounds.
+ """
+ return [PHASE_LOWER_BOUND] * 2 * self._num_integrators
+
+ def get_state_upper_bounds(self):
+ """Upper bounds for the internal state.
+
+ Returns:
+ The list containing the upper bounds.
+ """
+ return [PHASE_UPPER_BOUND] * 2 * self._num_integrators
+
+ def adjust_swing_stance_ratio(self, target_swing_stance_ratio):
+ """Adjust the parameter swing_stance_ratio towards a given target value.
+
+ Args:
+ target_swing_stance_ratio: The target value for the ratio between swing
+ and stance phases.
+ """
+ delta = min(_DELTA_SWING_STANCE_CAP,
+ target_swing_stance_ratio - self._swing_stance_ratio)
+ self._swing_stance_ratio += delta
+
+ @property
+ def num_integrators(self):
+ """Gets the number of integrators used based on coupling mode."""
+ return self._num_integrators
+
+
+class CircularAsymmetricalIntegratorUnit(object):
+ """A circular integrator with asymmetry between first and second half.
+
+ An integrator is a memory unit that accumulates the given parameter at every
+ time step.
+ A circular integrator is when the integrator cycles within [0,2pi].
+ The phase of a circular integrator indicates the accumulated number and it is
+ stored as fmod of 2Pi.
+ Asymmetrical circular integrator has a further characteristic where it
+ distinguishes between the first half of the period vs the second half. It
+ allows the integrator to move at different speeds during these two periods.
+ From a locomotion perspective these two halves of the period can be considered
+ as swing and stance phases. The speed difference is calculated using the
+ variable swing_stance_ratio provided at every time step.
+ CircularAsymmetricalIntegratorUnit can be used to control one or multiple legs
+ depending on the preference. If more than one leg is assigned to a single unit
+ the other legs are calculated based on their initial phase difference.
+ """
+
+ def __init__(self, init_phase=0):
+ self._init_phase = init_phase
+ self.reset()
+
+ def reset(self):
+ self.phase = self._init_phase
+
+ def calculate_progressed_phase(self, delta_period, swing_stance_speed_ratio):
+ """Calculate a hypotethical phase based on the current phase and args.
+
+ This is used to both calculate the new phase, as well as the current phase
+ of the other legs with a given offset of delta_period.
+
+ Args:
+ delta_period: The fraction of the period to add to the current phase of
+ the integrator. If set to 1, the integrator will accomplish one full
+ period and return the same phase. The calculated phase will depend on
+ the current phase (if it is in first half vs second half) and swing vs
+ stance speed ratio.
+ swing_stance_speed_ratio: The ratio of the speed of the phase when it is
+ in swing (second half) vs stance (first half). Set to 1.0 by default,
+ making it symettric, same as a classical integrator.
+
+ Returns:
+ The new phase between 0 and 2 * pi.
+ """
+ stance_speed_coef = (
+ swing_stance_speed_ratio + 1) * 0.5 / swing_stance_speed_ratio
+ swing_speed_coef = (swing_stance_speed_ratio + 1) * 0.5
+ delta_left = delta_period
+ new_phase = self.phase
+ while delta_left > 0:
+ if 0 <= new_phase < math.pi:
+ delta_phase_multiplier = stance_speed_coef * _TWO_PI
+ new_phase += delta_left * delta_phase_multiplier
+ delta_left = 0
+ if new_phase < math.pi:
+ delta_left = 0
+ else:
+ delta_left = (new_phase - math.pi) / delta_phase_multiplier
+ new_phase = math.pi
+ else:
+ delta_phase_multiplier = swing_speed_coef * _TWO_PI
+ new_phase += delta_left * delta_phase_multiplier
+ if math.pi <= new_phase < _TWO_PI:
+ delta_left = 0
+ else:
+ delta_left = (new_phase - _TWO_PI) / delta_phase_multiplier
+ new_phase = 0
+ return math.fmod(new_phase, _TWO_PI)
+
+ def progress_phase(self, delta_period, swing_stance_ratio):
+ """Updates the phase based on the current phase, delta period and ratio.
+
+ Args:
+ delta_period: The fraction of the period to add to the current phase of
+ the integrator. If set to 1, the integrator will accomplish one full
+ period and return the same phase. The calculated phase will depend on
+ the current phase (if it is in first half vs second half) and swing vs
+ stance speed ratio.
+ swing_stance_ratio: The ratio of the speed of the phase when it is in
+ swing (second half) vs stance (first half). Set to 1.0 by default,
+ making it symettric, same as a classical integrator.
+ """
+ self.phase = self.calculate_progressed_phase(delta_period,
+ swing_stance_ratio)
+
+ def get_state(self):
+ """Returns the sin and cos of the phase as state."""
+ return [(math.cos(self.phase) + 1) / 2.0, (math.sin(self.phase) + 1) / 2.0]
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/__init__.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/base_client.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/base_client.py
new file mode 100644
index 000000000..523780124
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/base_client.py
@@ -0,0 +1,63 @@
+"""Base class for simulation client."""
+
+import enum
+from typing import Text
+
+GRAY = (0.3, 0.3, 0.3, 1)
+
+
+class ClientType(enum.Enum):
+ """Type of client."""
+ BULLET = "pybullet"
+
+
+
+class ClientMode(enum.Enum):
+ """Client running mode."""
+
+ # Client is being initialized, object is being loaded, teleported, etc.
+ CONFIGURATION = 1
+
+ # Client is in a mode that simulate motion according to physics and control.
+ SIMULATION = 2
+
+
+class WrongClientModeError(Exception):
+ """Client mode does not meet expectation (e.g. load object in sim mode)."""
+
+
+class BaseClient(object):
+ """Base class for simulation client."""
+
+ def __init__(self, client_type: Text = ""):
+ self._client_type = client_type
+
+ # Default to configuration mode.
+ self._client_mode = ClientMode.CONFIGURATION
+
+ @property
+ def client_type(self) -> ClientType:
+ return self._client_type
+
+ def switch_mode(self, mode: ClientMode) -> bool:
+ """Switches running mode of simulation client and return if mode changed."""
+ if mode not in (ClientMode.CONFIGURATION, ClientMode.SIMULATION):
+ raise ValueError(f"Invalid client mode {mode}.")
+ if mode == self._client_mode:
+ return False
+ self._client_mode = mode
+ return True
+
+ @property
+ def client_mode(self) -> ClientMode:
+ """Returns current client mode."""
+ return self._client_mode
+
+ def _assert_in_configuration_mode(self, operation: Text = "this operation"):
+ """Raises exception if client is not in configuration mode."""
+ if self._client_mode != ClientMode.CONFIGURATION:
+ raise WrongClientModeError(
+ f"Sim client is expected to be in configuration mode for "
+ f"{operation}.")
+
+
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/curriculum_reset_helpers.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/curriculum_reset_helpers.py
new file mode 100644
index 000000000..86fa8cfc3
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/curriculum_reset_helpers.py
@@ -0,0 +1,62 @@
+"""Implements dynamic locomotion gym env that changes based on iteration."""
+import gin
+
+import pybullet
+
+
+# TODO(b/142642890): Make this reset to the initial world state first.
+@gin.configurable
+def gap_task_curriculum_update(env,
+ num_iter,
+ distance_to_gap_or_hurdle=1.5,
+ initial_gap_length=0.1,
+ max_iterations=500,
+ gap_delta=0.0008):
+ """Linearly increase the gap width wrt the iteration number.
+
+ This is specific to BuildSingleGapWorld.
+
+ Args:
+ env: An instance of a LocomotionGymEnv.
+ num_iter: The training iteration we are on.
+ distance_to_gap_or_hurdle: The distance to the gap.
+ initial_gap_length: The starting gap length.
+ max_iterations: The number of iterations up to which we will modify the
+ environment.
+ gap_delta: The amount to increase the gap width by for each increase of 1 in
+ the iteration.
+ """
+
+ gap_length = initial_gap_length + gap_delta * min(max_iterations, num_iter)
+ env.task.reset(
+ env,
+ distance_to_gap_or_hurdle=distance_to_gap_or_hurdle,
+ gap_or_hurdle_width=gap_length)
+
+
+@gin.configurable
+def gap_world_curriculum_update(env,
+ num_iter,
+ initial_second_block_x=8.15,
+ max_iterations=500,
+ gap_delta=0.0008):
+ """Update the world, linearly increasing gap width wrt iteration number.
+
+ This is specific to SingleGapScene.
+
+ Args:
+ env: An instance of a LocomotionGymEnv.
+ num_iter: The training iteration we are on.
+ initial_second_block_x: The initial x position of the second block.
+ max_iterations: The number of iterations up to which we will modify the
+ environment.
+ gap_delta: The amount to increase the gap width by for each increase of 1 in
+ the iteration.
+ """
+
+ ground = env.scene.ground_ids
+ pos = pybullet.getBasePositionAndOrientation(ground[-1])[0]
+ # Linearly increase the gap width to 0.5m by the last iteration.
+ next_x = initial_second_block_x + gap_delta * min(max_iterations, num_iter)
+ pybullet.resetBasePositionAndOrientation(ground[-1], (next_x, pos[1], pos[2]),
+ [0, 0, 0, 1])
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_loader.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_loader.py
new file mode 100644
index 000000000..aa4afe495
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_loader.py
@@ -0,0 +1,75 @@
+"""Load the locomotion gym env using the gin config files."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import gin
+from pybullet_envs.minitaur.envs_v2 import locomotion_gym_env
+from pybullet_envs.minitaur.envs_v2 import multiagent_mobility_gym_env
+
+ROBOT_FIELD_IN_CONFIG = 'robot_params'
+TASK_FIELD_IN_CONFIG = 'task_params'
+PMTG_FIELD_IN_CONFIG = 'pmtg_params'
+_PMTG_GIN_QUERY = 'pmtg_wrapper_env.PmtgWrapperEnv.'
+
+
+@gin.configurable
+def load(wrapper_classes=None, multiagent=False, **kwargs):
+ """load a pre-defined locomotion gym environment.
+
+ The env specific settings should have been set in the gin files.
+
+ Args:
+ wrapper_classes: A list of wrapper_classes.
+ multiagent: Whether to use multiagent environment.
+ **kwargs: Keyword arguments to be passed to the environment constructor.
+
+ Returns:
+ env: The instance of the minitaur gym environment.
+ """
+ # Gin config are not always specified this way (e.g. namescoped config).
+ # Only guery parameters when it is necessary.
+ if any(
+ k in (PMTG_FIELD_IN_CONFIG, TASK_FIELD_IN_CONFIG, ROBOT_FIELD_IN_CONFIG)
+ for k in kwargs):
+ with gin.unlock_config():
+ if multiagent:
+ # Currently assume robots and tasks are identical
+ robot_class = gin.query_parameter(
+ 'multiagent_mobility_gym_env.MultiagentMobilityGymEnv.robot_classes'
+ )[0].selector
+ task = gin.query_parameter(
+ 'multiagent_mobility_gym_env.MultiagentMobilityGymEnv.tasks'
+ )[0].selector
+ else:
+ robot_class = gin.query_parameter(
+ 'locomotion_gym_env.LocomotionGymEnv.robot_class').selector
+ task = gin.query_parameter(
+ 'locomotion_gym_env.LocomotionGymEnv.task').selector
+ gin_prefix_dict = {
+ PMTG_FIELD_IN_CONFIG: _PMTG_GIN_QUERY,
+ TASK_FIELD_IN_CONFIG: task + '.',
+ ROBOT_FIELD_IN_CONFIG: robot_class + '.',
+ }
+ for field_name, field_values in kwargs.items():
+ if field_name in gin_prefix_dict:
+ for var_name, value in field_values.items():
+ gin.bind_parameter(gin_prefix_dict[field_name] + var_name, value)
+ else:
+ raise ValueError(
+ 'Environment argument type is not found in gin_prefix_dict.')
+ if multiagent:
+ env = multiagent_mobility_gym_env.MultiagentMobilityGymEnv()
+ else:
+ env = locomotion_gym_env.LocomotionGymEnv()
+ if wrapper_classes is not None:
+ # A little macro for the automatic list expansion
+ if not isinstance(wrapper_classes, list):
+ wrapper_classes = [wrapper_classes]
+
+ # Wrap environments with user-provided wrappers
+ # (e.g. TrajectoryGeneratorWrapperEnv)
+ for wrapper_cls in wrapper_classes:
+ env = wrapper_cls(env)
+ return env
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/__init__.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/action_denormalize_wrapper.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/action_denormalize_wrapper.py
new file mode 100644
index 000000000..edfead1ab
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/action_denormalize_wrapper.py
@@ -0,0 +1,41 @@
+"""Denormalize the action from [-1, 1] to the env.action_space."""
+
+import gin
+import gym
+import numpy as np
+
+
+def _denomralize(env, action):
+ action = np.array(action)
+ low = np.array(env.action_space.low)
+ high = np.array(env.action_space.high)
+ return (high - low) / 2.0 * action + (high + low) / 2.0
+
+
+@gin.configurable
+class ActionDenormalizeWrapper(object):
+ """An env wrapper that denormalize the action from [-1, 1] to the bounds."""
+
+ def __init__(self, gym_env):
+ """Initializes the wrapper."""
+ self._gym_env = gym_env
+ self.action_space = gym.spaces.Box(
+ low=-1.0,
+ high=1.0,
+ shape=self._gym_env.action_space.low.shape,
+ dtype=np.float32)
+
+ def __getattr__(self, attr):
+ return getattr(self._gym_env, attr)
+
+ def step(self, action):
+ """Steps the wrapped environment.
+
+ Args:
+ action: Numpy array between [-1.0, 1.0].
+
+ Returns:
+ The tuple containing the observation, the reward, and the epsiode
+ end indicator.
+ """
+ return self._gym_env.step(_denomralize(self._gym_env, action))
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/alternating_legs_openloop.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/alternating_legs_openloop.py
new file mode 100644
index 000000000..85a8d99c2
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/alternating_legs_openloop.py
@@ -0,0 +1,125 @@
+"""A trajectory generator that return signals for alternating legs."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import google_type_annotations
+from __future__ import print_function
+
+import math
+import attr
+import gin
+from gym import spaces
+import numpy as np
+from pybullet_envs.minitaur.envs.utilities import laikago_pose_utils
+
+TROT_GAIT = "trot"
+PACE_GAIT = "pace"
+NUM_MOTORS_LAIKAGO = 12
+STD_FOR_GAUSSIAN_TRAJECTORY = 0.15
+MOTION_FREQUENCY = 1.0
+MOTION_AMPLITUDE = 0.25
+ACTION_BOUND = 0.25
+
+
+# TODO(b/131193449): Add a test to this class.
+@gin.configurable
+class LaikagoAlternatingLegsTrajectoryGenerator(object):
+ """A trajectory generator that return signals for alternating legs."""
+
+ def __init__(
+ self,
+ init_abduction=laikago_pose_utils.LAIKAGO_DEFAULT_ABDUCTION_ANGLE,
+ init_hip=laikago_pose_utils.LAIKAGO_DEFAULT_HIP_ANGLE,
+ init_knee=laikago_pose_utils.LAIKAGO_DEFAULT_KNEE_ANGLE,
+ amplitude=MOTION_AMPLITUDE,
+ frequency=MOTION_FREQUENCY,
+ gait=PACE_GAIT, # can be TROT_GAIT or PACE_GAIT
+ ):
+ """Initializes the controller."""
+ self._pose = np.array(
+ attr.astuple(
+ laikago_pose_utils.LaikagoPose(
+ abduction_angle_0=init_abduction,
+ hip_angle_0=init_hip,
+ knee_angle_0=init_knee,
+ abduction_angle_1=init_abduction,
+ hip_angle_1=init_hip,
+ knee_angle_1=init_knee,
+ abduction_angle_2=init_abduction,
+ hip_angle_2=init_hip,
+ knee_angle_2=init_knee,
+ abduction_angle_3=init_abduction,
+ hip_angle_3=init_hip,
+ knee_angle_3=init_knee)))
+ action_high = np.array([ACTION_BOUND] * NUM_MOTORS_LAIKAGO)
+ self.action_space = spaces.Box(-action_high, action_high, dtype=np.float32)
+ self.amplitude = amplitude
+ self.period = 1.0 / frequency
+ self.gait = gait
+
+ def _alternating_legs_trajectory(self, t):
+ """The reference trajectory of each joint when alternating legs.
+
+ Args:
+ t: The time since the latest robot reset.
+
+ Returns:
+ An array of 12 desired motor angles.
+ """
+ phase_in_period = (t % self.period) / self.period
+ is_first_half_gait = phase_in_period < 0.5
+ if self.gait == TROT_GAIT and is_first_half_gait:
+ phases = [0, 1, 1, 0] # 0 means stance and 1 means retraction.
+ elif self.gait == TROT_GAIT and not is_first_half_gait:
+ phases = [1, 0, 0, 1]
+ elif self.gait == PACE_GAIT and is_first_half_gait:
+ phases = [0, 1, 0, 1]
+ elif self.gait == PACE_GAIT and not is_first_half_gait:
+ phases = [1, 0, 1, 0]
+ else:
+ raise ValueError("{} gait is not supported in alternating legs.".format(
+ self.gait))
+
+ phase_step_center = 0.25 if is_first_half_gait else 0.75
+ std = STD_FOR_GAUSSIAN_TRAJECTORY
+ # Uses Gaussian instead of sine for gentle foot touch down.
+ # The following joint angles are added to self._pose.
+ retract_hip_angle = self.amplitude * math.exp(
+ -(phase_in_period - phase_step_center) *
+ (phase_in_period - phase_step_center) / (std * std))
+ retract_knee_angle = -2.0 * retract_hip_angle
+ retract_abduction_angle = 0.0
+ stance_hip_angle = 0.0
+ stance_knee_angle = 0.0
+ stance_abduction_angle = 0.0
+ angles = []
+ for is_retract in phases:
+ if is_retract:
+ angles.extend([retract_abduction_angle, retract_hip_angle,
+ retract_knee_angle])
+ else:
+ angles.extend([stance_abduction_angle, stance_hip_angle,
+ stance_knee_angle])
+ return np.array(angles)
+
+ def reset(self):
+ pass
+
+ def get_action(self, current_time, input_action):
+ """Computes the trajectory according to input time and action.
+
+ Args:
+ current_time: The time in gym env since reset.
+ input_action: A numpy array. The input leg pose from a NN controller.
+
+ Returns:
+ A numpy array. The desired motor angles.
+ """
+
+ return self._pose + self._alternating_legs_trajectory(
+ current_time) + input_action
+
+ def get_observation(self, input_observation):
+ """Get the trajectory generator's observation."""
+
+ return input_observation
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/curriculum_wrapper_env.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/curriculum_wrapper_env.py
new file mode 100644
index 000000000..1e2f33586
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/curriculum_wrapper_env.py
@@ -0,0 +1,50 @@
+"""A wrapped LocomotionGymEnv with functions that change the world and task."""
+
+import gin
+
+
+@gin.configurable
+class CurriculumWrapperEnv(object):
+ """A wrapped LocomotionGymEnv with an evolving environment."""
+
+ def __init__(self,
+ gym_env,
+ num_iter=0,
+ curriculum_world_update=None,
+ curriculum_task_update=None):
+ """Initializes the wrapped env.
+
+ Args:
+ gym_env: An instance of a (potentially previously wrapped)
+ LocomotionGymEnv.
+ num_iter: The training iteration we are on.
+ curriculum_world_update: A function that updates the environment based on
+ the iteration. Takes in the environment as an argument.
+ curriculum_task_update: A function that updates the task (eg.
+ observations) based on the iteration. Takes in the environment as an
+ argument.
+ """
+ self._gym_env = gym_env
+ self._num_iter = num_iter
+ self._curriculum_world_update = curriculum_world_update
+ self._curriculum_task_update = curriculum_task_update
+
+ def modify(self, step=0):
+ self._num_iter = step
+
+ def __getattr__(self, attr):
+ return getattr(self._gym_env, attr)
+
+ def reset(self, *args, **kwargs):
+ """Reset and adjust the environment."""
+ self._gym_env.reset(*args, **kwargs)
+ if self._curriculum_world_update is not None:
+ self._curriculum_world_update(self._gym_env, self._num_iter)
+ if self._curriculum_task_update is not None:
+ self._curriculum_task_update(self._gym_env, self._num_iter)
+ return self._get_observation()
+
+ # Used for testing.
+ @property
+ def num_iter(self):
+ return self._num_iter
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/depth_uv_to_footplacement_wrapper.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/depth_uv_to_footplacement_wrapper.py
new file mode 100644
index 000000000..4b86b6df6
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/depth_uv_to_footplacement_wrapper.py
@@ -0,0 +1,357 @@
+"""Change the action from uv of the depth map to xyz in world frame."""
+
+import copy
+import math
+from typing import Sequence
+import gin
+import gym
+import numpy as np
+
+from pybullet_envs.minitaur.robots import laikago_kinematic_constants
+from pybullet_envs.minitaur.robots import quadruped_base
+from pybullet_envs.minitaur.robots import wheeled_robot_base_sim
+
+UNIT_QUATERNION = (0, 0, 0, 1)
+# A small gap between the target foot position and the floor.
+FLOOR_HEIGHT_EPSILON = 0.02
+FOOT_HEIGHT_CLEARNCE = 0.075
+BASE_MOVEMENT_SPEED = 0.3
+INIT_ABDUCTION_ANGLE = laikago_kinematic_constants.INIT_ABDUCTION_ANGLE
+INIT_HIP_ANGLE = laikago_kinematic_constants.INIT_HIP_ANGLE
+INIT_KNEE_ANGLE = laikago_kinematic_constants.INIT_KNEE_ANGLE
+NUM_LEGS = laikago_kinematic_constants.NUM_LEGS
+COM_VIZ_BOX_SIZE = [0.025, 0.025, 0.005]
+_DEFAULT_JOINT_POSE = (INIT_ABDUCTION_ANGLE, INIT_HIP_ANGLE,
+ INIT_KNEE_ANGLE) * NUM_LEGS
+# Weights for computing the target COM from the supporting feet locations.
+# The target COM for the front feet are biased forward for better robot
+# stability and seeing farther in distance, which enables larger steps.
+_SUPPORT_WEIGHT_MAP = [[0.0, 0.4, 0.3, 0.3], [0.4, 0.0, 0.3, 0.3],
+ [1.0 / 3, 1.0 / 3, 0.0, 1.0 / 3],
+ [1.0 / 3, 1.0 / 3, 1.0 / 3, 0.0]]
+_TASK_SENSOR_NAME = "sensors"
+
+
+@gin.configurable
+class DepthUVToFootPlacementWrapper(object):
+ """Changes the action from a point in depth map to a point in the world frame.
+
+ Attributes:
+ observation: The current observation of the environment.
+ last_action: The last that was used to step the environment.
+ env_step_counter: The number of control steps that have been elapesed since
+ the environment is reset.
+ action_space: The action space of the environment.
+ """
+
+ def __init__(self,
+ gym_env,
+ visualization=True,
+ foot_movement=False,
+ foot_clearance_height=FOOT_HEIGHT_CLEARNCE,
+ base_movement_speed=BASE_MOVEMENT_SPEED,
+ foothold_update_frequency=4):
+ """Initializes the wrapper.
+
+ Args:
+ gym_env: the wrapped gym environment. The robot is controlled
+ kinematically when gym_env.robot inherits from WheeledRobotBase and is
+ controlled dynamically using a static gait controller when gym_env.robot
+ inherits from QudrupedBase.
+ visualization: whether to draw a sphere that represents the foothold
+ position.
+ foot_movement: whether move the toe to the desired foothold position using
+ IK for visualization/debugging purpose.
+ foot_clearance_height: the maximum height of the swing foot.
+ base_movement_speed: the speed of the robot base.
+ foothold_update_frequency: the frequency of updating the foothold, which
+ is the same as the frequency of the steps. The default value 4 means
+ four steps (1 complete cycle of static gait) per second.
+ """
+ self._gym_env = gym_env
+ self._num_control_steps_per_foothold_update = max(
+ 1, int(1.0 / foothold_update_frequency / self._gym_env.env_time_step))
+ self.last_action = None
+ self._visualization = visualization
+ self._foot_movement = foot_movement
+ self._foot_clearance_height = foot_clearance_height
+ self._base_movement_speed = base_movement_speed
+ self._step_counter = 0
+ self.observation_space.spaces["toe_position"] = gym.spaces.Box(
+ np.array([-1.0] * 3), np.array([1.0] * 3))
+ self.action_space = gym.spaces.Box(
+ np.array([-1.0] * 2), np.array([1.0] * 2))
+ self.task.reset(self)
+ if hasattr(self.task, _TASK_SENSOR_NAME):
+ self.observation_space.spaces[
+ self.task.get_name()] = self.task.observation_space
+
+ def __getattr__(self, attr):
+ return getattr(self._gym_env, attr)
+
+ def reset(self, **kwargs):
+ """Reset the environment."""
+ obs = self._gym_env.reset(**kwargs)
+ self.task.reset(self)
+ self._step_counter = 0
+ current_end_effector_pos = np.array(
+ self._gym_env.robot.foot_positions()[self.task.swing_foot_id])
+ self.last_action = [0, 0]
+ if self._visualization:
+ self._create_foot_placement_visualization()
+ self._create_com_visualization()
+ self._initial_local_toe_positions = np.array(
+ self._gym_env.robot.foot_positions(position_in_world_frame=False))
+
+ # Move COM to prepare for the first swing step.
+ if isinstance(self._gym_env.robot, quadruped_base.QuadrupedBase):
+ obs, _ = self._move_com_dynamic(
+ self.task.swing_foot_id,
+ self._num_control_steps_per_foothold_update // 3 * 2)
+
+ # TODO(b/157614175): Adds a toe_position_sensor.
+ obs["toe_position"] = current_end_effector_pos
+ obs["vision"] = self.task.get_depth_image_for_foot()
+ obs["LastAction"] = self.last_action
+
+ self._observation = obs
+ return self._observation
+
+ def _move_kinematic(self):
+ """Move robot kinematically.
+
+ Returns:
+ The tuple containing the observation and env info.
+ """
+ if self._foot_movement:
+ toe_positions = np.array(
+ self._gym_env.robot.foot_positions(position_in_world_frame=True))
+ toe_positions[:, 2] = FLOOR_HEIGHT_EPSILON
+ destination_foothold_xyz_global = self.task.get_foothold_location(
+ self.last_action, use_world_frame=True)
+ destination_foothold_xyz_global[2] = FLOOR_HEIGHT_EPSILON
+ joint_pose = _DEFAULT_JOINT_POSE
+ for i in range(self._num_control_steps_per_foothold_update):
+ if self._foot_movement:
+ alpha = i / (self._num_control_steps_per_foothold_update - 1)
+ toe_positions_over_time = copy.deepcopy(toe_positions)
+ toe_positions_over_time[self.task.swing_foot_id] = (
+ self._construct_foot_trajectories(
+ alpha, toe_positions[self.task.swing_foot_id],
+ destination_foothold_xyz_global, self._foot_clearance_height))
+ joint_pose = np.array(
+ self.robot.motor_angles_from_foot_positions(
+ toe_positions_over_time, position_in_world_frame=True)[1])
+ action = {
+ wheeled_robot_base_sim.BASE_ACTION_NAME:
+ (self._base_movement_speed, 0),
+ wheeled_robot_base_sim.BODY_ACTION_NAME:
+ joint_pose,
+ }
+ obs, _, _, info = self._gym_env.step(action)
+ return obs, info
+
+ def _move_com_dynamic(self, swing_foot_id, num_control_steps):
+ """Move robot COM to the weightd center of the support polygon through dynamics.
+
+ Args:
+ swing_foot_id: Index of the swing foot.
+ num_control_steps: Total number of control steps for moving the COM.
+
+ Returns:
+ The tuple containing the observation and env info.
+ """
+
+ toe_positions = np.array(
+ self._gym_env.robot.foot_positions(position_in_world_frame=False))
+ support_polygon_center = np.array([0.0, 0.0, 0.0])
+ for i in range(NUM_LEGS):
+ if i != swing_foot_id:
+ support_polygon_center += toe_positions[i] * _SUPPORT_WEIGHT_MAP[
+ swing_foot_id][i]
+ support_polygon_center[2] = 0.0
+
+ for i in range(num_control_steps):
+ alpha = i / (num_control_steps - 1)
+ # Create a flat phase towards the end to stabilize the com movement.
+ alpha = np.clip(alpha * 1.1, 0, 1)
+ toe_positions_over_time = copy.deepcopy(
+ toe_positions) - alpha * support_polygon_center
+ # Use initial toe height to maintain the overal base height.
+ for j in range(len(toe_positions_over_time)):
+ toe_positions_over_time[j][2] = self._initial_local_toe_positions[j][2]
+ joint_pose = np.array(
+ self.robot.motor_angles_from_foot_positions(
+ toe_positions_over_time, position_in_world_frame=False)[1])
+ obs, _, _, info = self._gym_env.step(joint_pose)
+ self._update_com_visualization()
+ return obs, info
+
+ def _swing_leg_dynamic(self, swing_foot_id, destination_foothold_xyz_local,
+ num_control_steps):
+ """Move swing leg to the target foothold through IK and dynamics.
+
+ Args:
+ swing_foot_id: Index of the swing foot.
+ destination_foothold_xyz_local: Target foothold position.
+ num_control_steps: Total number of control steps for swinging the leg.
+
+ Returns:
+ The tuple containing the observation and env info.
+ """
+
+ local_toe_positions = np.array(
+ self._gym_env.robot.foot_positions(position_in_world_frame=False))
+
+ # Move swing leg
+ for i in range(num_control_steps):
+ alpha = i / (num_control_steps - 1)
+ toe_positions_over_time = copy.deepcopy(local_toe_positions)
+ toe_positions_over_time[swing_foot_id] = (
+ self._construct_foot_trajectories(alpha,
+ local_toe_positions[swing_foot_id],
+ destination_foothold_xyz_local,
+ self._foot_clearance_height))
+ joint_pose = np.array(
+ self.robot.motor_angles_from_foot_positions(
+ toe_positions_over_time, position_in_world_frame=False)[1])
+
+ obs, _, _, info = self._gym_env.step(joint_pose)
+ return obs, info
+
+ def _move_dynamic(self):
+ """Move robot dynamically.
+
+ Returns:
+ The tuple containing the observation and env info.
+ """
+ destination_foothold_xyz_local = self.task.get_foothold_location(
+ self.last_action, use_world_frame=False)
+ # Lift the target foothold slightly to account for thickness of the feet.
+ destination_foothold_xyz_local[2] += FLOOR_HEIGHT_EPSILON
+
+ # Swing leg in the first 1/3 of the duration.
+ self._swing_leg_dynamic(self.task.swing_foot_id,
+ destination_foothold_xyz_local,
+ self._num_control_steps_per_foothold_update // 3)
+
+ # Move COM in the rest 2/3 of the duration.
+ obs, info = self._move_com_dynamic(
+ self.task.next_swing_foot_id,
+ self._num_control_steps_per_foothold_update // 3 * 2)
+
+ return obs, info
+
+ def step(self, action: Sequence[float]):
+ """Steps the wrapped environment.
+
+ Args:
+ action: 2 dimensional numpy array between [-1.0, 1.0]. They represents the
+ depth image pixel index. We assume that only one foot is swinging in
+ this wrapper, and this is the foothold location for that swinging leg.
+ The order of the swinging leg and the index of the current swinging leg
+ is defined in stepstone_visiontask.py.
+
+ Returns:
+ The tuple containing the observation, the reward, and the episode
+ end indicator.
+ """
+ self.last_action = action
+ reward = self.task(self)
+ done = self.task.done(self)
+ if self._visualization:
+ self._update_foothold_visualization(action)
+
+ if isinstance(self._gym_env.robot, quadruped_base.QuadrupedBase):
+ obs, info = self._move_dynamic()
+ else:
+ obs, info = self._move_kinematic()
+
+ self._step_counter += 1
+ current_end_effector_pos = np.array(
+ self._gym_env.robot.foot_positions()[self.task.swing_foot_id])
+ obs["toe_position"] = current_end_effector_pos
+ obs["vision"] = self.task.get_depth_image_for_foot()
+ self._observation = obs
+ return obs, reward, done, info
+
+ def _construct_foot_trajectories(self, alpha, swing_foot_start_position,
+ swing_foot_destination,
+ foot_clearance_height):
+ """Construct the target foot position for the swing foot.
+
+ Args:
+ alpha: a float in [0.0, 1.0] indicating the phase of the swing foot.
+ swing_foot_start_position: foot position at the beginning of the swing
+ motion.
+ swing_foot_destination: target foot position at the end of the swing
+ motion.
+ foot_clearance_height: the maximum height of the swing foot.
+
+ Returns:
+ The interpolated swing foot position.
+ """
+ new_swing_foot_position = swing_foot_start_position + alpha * (
+ swing_foot_destination - swing_foot_start_position)
+ new_swing_foot_position[2] += (
+ foot_clearance_height * math.sin((alpha) * math.pi))
+ return new_swing_foot_position
+
+ def _create_foot_placement_visualization(self):
+ """Creates a visualization sphere that represents the foothold position."""
+ visual_id = self._gym_env.pybullet_client.createVisualShape(
+ self._gym_env.pybullet_client.GEOM_SPHERE,
+ radius=0.02,
+ rgbaColor=[0.7, 0.7, 0.7, 1])
+ self._foothold_visual_body = self._gym_env.pybullet_client.createMultiBody(
+ baseMass=0, baseVisualShapeIndex=visual_id, basePosition=[0, 0, 0])
+
+ def _update_foothold_visualization(self, action):
+ """Moves the visualization sphere that represents the next foothold."""
+ foothold_xyz = self.task.get_foothold_location(
+ action, use_world_frame=True)
+ self._gym_env.pybullet_client.resetBasePositionAndOrientation(
+ self._foothold_visual_body, foothold_xyz, UNIT_QUATERNION)
+
+ def _create_com_visualization(self):
+ """Creates visualization boxes for COM and support polygon center."""
+ visual_id = self._gym_env.pybullet_client.createVisualShape(
+ self._gym_env.pybullet_client.GEOM_BOX,
+ halfExtents=COM_VIZ_BOX_SIZE,
+ rgbaColor=[0.7, 0.5, 0.5, 1])
+ self._support_polygon_center_visual_body = self._gym_env.pybullet_client.createMultiBody(
+ baseMass=0, baseVisualShapeIndex=visual_id, basePosition=[0, 0, 0])
+
+ visual_id = self._gym_env.pybullet_client.createVisualShape(
+ self._gym_env.pybullet_client.GEOM_BOX,
+ halfExtents=COM_VIZ_BOX_SIZE,
+ rgbaColor=[0.5, 0.75, 0.5, 1])
+ self._projected_com_visual_body = self._gym_env.pybullet_client.createMultiBody(
+ baseMass=0, baseVisualShapeIndex=visual_id, basePosition=[0, 0, 0])
+
+ def _update_com_visualization(self):
+ """Moves the visualization for the COM and support polygon center."""
+ toe_positions = np.array(
+ self._gym_env.robot.foot_positions(position_in_world_frame=True))
+ support_polygon_center_global = np.array([0.0, 0.0, 0.0])
+ for i in range(NUM_LEGS):
+ if i != self.task.next_swing_foot_id:
+ support_polygon_center_global += toe_positions[i] * _SUPPORT_WEIGHT_MAP[
+ self.task.next_swing_foot_id][i]
+ support_polygon_center_global[2] = 0.0
+ self._gym_env.pybullet_client.resetBasePositionAndOrientation(
+ self._support_polygon_center_visual_body, support_polygon_center_global,
+ UNIT_QUATERNION)
+
+ com = np.copy(self._gym_env.robot.base_position)
+ com[2] = 0.0
+ self._gym_env.pybullet_client.resetBasePositionAndOrientation(
+ self._projected_com_visual_body, com, UNIT_QUATERNION)
+
+ @property
+ def observation(self):
+ return self._observation
+
+ @property
+ def env_step_counter(self):
+ return self._step_counter
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/fixed_steptime_wrapper_env.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/fixed_steptime_wrapper_env.py
new file mode 100644
index 000000000..c06ffacab
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/fixed_steptime_wrapper_env.py
@@ -0,0 +1,92 @@
+"""A wrapper that controls the timing between steps.
+"""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import google_type_annotations
+from __future__ import print_function
+
+import time
+import gin
+
+
+@gin.configurable
+class FixedSteptimeWrapperEnv(object):
+ """A wrapped LocomotionGymEnv with timing control between steps."""
+
+ def __init__(self,
+ gym_env,
+ desired_time_between_steps=None):
+ """Initializes the wrapper env.
+
+ Args:
+ gym_env: An instance of LocomotionGymEnv.
+ desired_time_between_steps: The desired time between steps in seconds.
+ If this is None, it is set to the env_time_step of the gym_env.
+ """
+ self._gym_env = gym_env
+ if desired_time_between_steps is None:
+ self._desired_time_between_steps = gym_env.env_time_step
+ else:
+ self._desired_time_between_steps = desired_time_between_steps
+
+ self._last_reset_time = time.time()
+ self._last_step_time = time.time()
+ self._step_counter = 0
+
+ def __getattr__(self, attr):
+ return getattr(self._gym_env, attr)
+
+ def reset(self, initial_motor_angles=None, reset_duration=1.0):
+ """Reset the environment.
+
+ This function records the timing of the reset.
+
+ Args:
+ initial_motor_angles: Not used.
+ reset_duration: Not used.
+
+ Returns:
+ The observation of the environment after reset.
+ """
+ obs = self._gym_env.reset(initial_motor_angles=initial_motor_angles,
+ reset_duration=reset_duration)
+ self._last_reset_time = time.time()
+ self._last_step_time = time.time()
+ self._step_counter = 0
+ return obs
+
+ def step(self, action):
+ """Steps the wrapped environment.
+
+ Args:
+ action: Numpy array. The input action from an NN agent.
+
+ Returns:
+ The tuple containing the observation, the reward, the epsiode end
+ indicator.
+
+ Raises:
+ ValueError if input action is None.
+ """
+ time_between_steps = time.time() - self._last_step_time
+ if time_between_steps < self._desired_time_between_steps:
+ time.sleep(self._desired_time_between_steps - time_between_steps)
+ self._last_step_time = time.time()
+ self._step_counter += 1
+ return self._gym_env.step(action)
+
+ @property
+ def elapsed_time(self):
+ """Returns the elapsed time in seconds."""
+ return time.time() - self._last_reset_time
+
+ @property
+ def steps_per_second(self):
+ """Returns the average number of time steps for 1 second."""
+ return self._step_counter / self.elapsed_time
+
+ @property
+ def seconds_per_step(self):
+ """Returns the average time between steps."""
+ return self.elapsed_time / self._step_counter
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/ik_based_wrapper_env.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/ik_based_wrapper_env.py
new file mode 100644
index 000000000..da80bf159
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/ik_based_wrapper_env.py
@@ -0,0 +1,109 @@
+"""A wrapped Quadruped with Inverse Kinematics based controller."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import gin
+import gym
+import numpy as np
+from pybullet_envs.minitaur.robots import vision60
+from pybullet_envs.minitaur.robots.utilities import kinematics
+
+ACTION_DIM_PER_LEG = 3
+ACTION_DIM_BASE = 7
+ACTION_DIM_TOTAL = vision60.NUM_LEGS * ACTION_DIM_PER_LEG + ACTION_DIM_BASE
+
+
+@gin.configurable
+class IKBasedWrapperEnv(object):
+ """An env using IK to convert toe positions to joint angles."""
+
+ def __init__(self,
+ gym_env,
+ toe_indices=(3, 7, 11, 15),
+ abduction_motor_ids=(0, 3, 6, 9)):
+ """Initialzes the wrapped env.
+
+ Args:
+ gym_env: An instance of LocomotionGymEnv.
+ toe_indices: A list of four pybullet joint indices for the four toes. [3,
+ 7, 11, 15] for the vision60.
+ abduction_motor_ids: A list of four pybullet joint indices for the four
+ abuduction motors. [0, 3, 6, 9] for the vision60.
+ """
+ lower_bound = np.array([-1.0] * ACTION_DIM_TOTAL)
+ upper_bound = np.array([1.0] * ACTION_DIM_TOTAL)
+ self._gym_env = gym_env
+ self.action_space = gym.spaces.Box(lower_bound, upper_bound)
+ self._toe_ids = toe_indices
+ self._abduction_motor_ids = abduction_motor_ids
+
+ def __getattr__(self, attr):
+ return getattr(self._gym_env, attr)
+
+ def get_toe_indices(self):
+ return self._toe_ids
+
+ def _joint_angles_from_toe_positions_and_base_pose(self, ik_actions):
+ """Uses Inverse Kinematics to calculate jont angles.
+
+ Args:
+ ik_actions: The action should be local (x, y, z) for each toe. action for
+ each leg [x, y, z] in a local frame. This local frame is transformed
+ relative to the COM frame using a given translation, and rotation. The
+ total action space would be 3 + 4 + 3 * ACTION_DIM_PER_LEG = 16.
+
+ Returns:
+ A list of joint angles.
+ """
+ assert len(ik_actions) == ACTION_DIM_TOTAL
+
+ base_translation_index = vision60.NUM_LEGS * ACTION_DIM_PER_LEG
+ base_rotation_index = vision60.NUM_LEGS * ACTION_DIM_PER_LEG + 3
+
+ base_translation = ik_actions[
+ base_translation_index:base_translation_index + 3]
+ base_rotation = ik_actions[base_rotation_index:base_rotation_index + 4]
+ desired_joint_angles = []
+ for i in range(vision60.NUM_LEGS):
+ local_toe_pos = ik_actions[i * ACTION_DIM_PER_LEG:i * ACTION_DIM_PER_LEG +
+ ACTION_DIM_PER_LEG]
+ leg_joint_ids = [
+ self._abduction_motor_ids[i], self._abduction_motor_ids[i] + 1,
+ self._abduction_motor_ids[i] + 2
+ ]
+
+ desired_joint_angles.extend(
+ kinematics.joint_angles_from_link_position(
+ robot=self._gym_env.robot,
+ link_position=local_toe_pos,
+ link_id=self._toe_ids[i],
+ joint_ids=leg_joint_ids,
+ base_translation=base_translation,
+ base_rotation=base_rotation))
+
+ return desired_joint_angles
+
+ def step(self, action):
+ """Steps the wrapped environment.
+
+ Args:
+ action: Numpy array. The input action from an NN agent.
+
+ Returns:
+ The tuple containing the modified observation, the reward, the epsiode end
+ indicator.
+
+ Raises:
+ ValueError if input action is None.
+
+ """
+ if action is None:
+ raise ValueError('Action cannot be None')
+
+ desired_joint_angles = self._joint_angles_from_toe_positions_and_base_pose(
+ ik_actions=action)
+ observation, reward, done, _ = self._gym_env.step(desired_joint_angles)
+
+ return observation, reward, done, _
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/imitation_wrapper_env.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/imitation_wrapper_env.py
new file mode 100644
index 000000000..bbe14ea83
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/imitation_wrapper_env.py
@@ -0,0 +1,101 @@
+"""A wrapper for motion imitation environment."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import gin
+import gym
+import numpy as np
+
+
+@gin.configurable
+class ImitationWrapperEnv(object):
+ """An env using for training policy with motion imitation."""
+
+ def __init__(self, gym_env):
+ """Initialzes the wrapped env.
+
+ Args:
+ gym_env: An instance of LocomotionGymEnv.
+ """
+ self._gym_env = gym_env
+ self.observation_space = self._build_observation_space()
+
+ self.seed()
+ return
+
+ def __getattr__(self, attr):
+ return getattr(self._gym_env, attr)
+
+ def step(self, action):
+ """Steps the wrapped environment.
+
+ Args:
+ action: Numpy array. The input action from an NN agent.
+
+ Returns:
+ The tuple containing the modified observation, the reward, the epsiode end
+ indicator.
+
+ Raises:
+ ValueError if input action is None.
+
+ """
+ original_observation, reward, done, _ = self._gym_env.step(action)
+ observation = self._modify_observation(original_observation)
+
+ return observation, reward, done, _
+
+ @gin.configurable('imitation_wrapper_env.ImitationWrapperEnv.reset')
+ def reset(self, initial_motor_angles=None, reset_duration=1.0):
+ """Resets the robot's position in the world or rebuild the sim world.
+
+ The simulation world will be rebuilt if self._hard_reset is True.
+
+ Args:
+ initial_motor_angles: A list of Floats. The desired joint angles after
+ reset. If None, the robot will use its built-in value.
+ reset_duration: Float. The time (in seconds) needed to rotate all motors
+ to the desired initial values.
+
+ Returns:
+ A numpy array contains the initial observation after reset.
+ """
+ original_observation = self._gym_env.reset(initial_motor_angles, reset_duration)
+ observation = self._modify_observation(original_observation)
+ return observation
+
+ def _modify_observation(self, original_observation):
+ """Appends target observations from the reference motion to the observations.
+
+ Args:
+ original_observation: A numpy array containing the original observations.
+
+ Returns:
+ A numpy array contains the initial original concatenated with target
+ observations from the reference motion.
+ """
+ target_observation = self._task.build_target_obs()
+ observation = np.concatenate([original_observation, target_observation], axis=-1)
+ return observation
+
+ def _build_observation_space(self):
+ """Constructs the observation space, including target observations from
+ the reference motion.
+
+ Returns:
+ Observation space representing the concatenations of the original
+ observations and target observations.
+ """
+ obs_space0 = self._gym_env.observation_space
+ low0 = obs_space0.low
+ high0 = obs_space0.high
+
+ task_low, task_high = self._task.get_target_obs_bounds()
+ low = np.concatenate([low0, task_low], axis=-1)
+ high = np.concatenate([high0, task_high], axis=-1)
+
+ obs_space = gym.spaces.Box(low, high)
+
+ return obs_space
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/mpc_locomotion_wrapper.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/mpc_locomotion_wrapper.py
new file mode 100644
index 000000000..faebcdfea
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/mpc_locomotion_wrapper.py
@@ -0,0 +1,794 @@
+"""An env that uses MPC-based motion controller to realize higher level footstep planning."""
+
+import copy
+import enum
+from typing import Sequence
+
+import dataclasses
+import gin
+import gym
+import numpy as np
+
+from pybullet_envs.minitaur.agents.baseline_controller import com_height_estimator
+from pybullet_envs.minitaur.agents.baseline_controller import gait_generator as gait_generator_lib
+from pybullet_envs.minitaur.agents.baseline_controller import imu_based_com_velocity_estimator
+from pybullet_envs.minitaur.agents.baseline_controller import multi_state_estimator
+from pybullet_envs.minitaur.agents.baseline_controller import openloop_gait_generator
+from pybullet_envs.minitaur.agents.baseline_controller import torque_stance_leg_controller
+from pybullet_envs.minitaur.robots import laikago_kinematic_constants
+
+_UNIT_QUATERNION = (0, 0, 0, 1)
+_NUM_LEGS = laikago_kinematic_constants.NUM_LEGS
+_MOTORS_PER_LEG = 3
+_DEFAULT_BODY_HEIGHT = 0.45
+_DEFAULT_BASE_SPEED = (0.0, 0.0)
+_DEFAULT_BASE_TWIST_SPEED = 0.0
+_DEFAULT_ROLL_PITCH = (0.0, 0.0)
+_DEFAULT_SWING_TARGET = (0.0, 0.0, 0.0)
+_DEFAULT_SWING_CLERANCE = 0.05
+_MOTOR_KP = [220.0] * 12
+_MOTOR_KD = [0.3, 2.0, 2.0] * 4
+_BASE_VELOCITY_ACTION_RANGE = ((-1.0, -0.1), (1.0, 0.1))
+_BASE_TWIST_SPEED_ACTION_RANGE = (-0.5, 0.5)
+_BASE_HEIGHT_ACTION_RANGE = (0.3, 0.5)
+# Action bound for the swing target in local x, y, z direction.
+_SWING_TARGET_ACTION_RANGE = ((-0.3, -0.1, -0.25), (0.3, 0.1, 0.25))
+_PITCH_ROLL_ACTION_RANGE = (-0.35, 0.35)
+_SWING_CLEARANCE_ACTION_RANGE = (0.05, 0.3)
+_SWING_TARGET_DELTA_ACTION_RANGE = ((-0.02, -0.01, -0.02), (0.02, 0.01, 0.02))
+_SWING_CLEARANCE_DELTA_RANGE = (-0.02, 0.02)
+
+
+@gin.configurable
+@dataclasses.dataclass
+class BaseTargetHorizontalComVelocityHeuristic(object):
+ """A class for mapping swing foot targets to a heuristic target com velocity."""
+ horizontal_com_velocity_heuristic: np.ndarray = np.zeros(2)
+
+ def update_horizontal_com_velocity_heuristic(self, hip_relative_swing_targets,
+ com_velocity, swing_durations):
+ del hip_relative_swing_targets, com_velocity, swing_durations
+ pass
+
+ def reset(self):
+ self.horizontal_com_velocity_heuristic = np.zeros(2)
+
+
+# TODO(magicmelon): Add a one-pager to explain the inverse raibert heuristics.
+@gin.configurable
+class InverseRaibertTargetHorizontalComVelocityHeuristic(
+ BaseTargetHorizontalComVelocityHeuristic):
+ """A class for mapping swing foot targets to a target com velocity with Raibert Heuristics."""
+
+ def __init__(self, gains=(-0.25, -0.1)):
+ self._gains = np.array(gains)
+
+ def update_horizontal_com_velocity_heuristic(self, hip_relative_swing_targets,
+ com_velocity, swing_durations):
+ assert len(hip_relative_swing_targets) == len(swing_durations)
+ target_com_horizontal_velocities = []
+ for i in range(len(hip_relative_swing_targets)):
+ target_com_horizontal_velocity = (
+ com_velocity / 2.0 * swing_durations[i] -
+ hip_relative_swing_targets[i]) / self._gains + com_velocity
+ target_com_horizontal_velocities.append(target_com_horizontal_velocity)
+ if target_com_horizontal_velocities:
+ self.horizontal_com_velocity_heuristic = np.mean(
+ target_com_horizontal_velocities, axis=0)
+
+
+@gin.constants_from_enum
+class Gait(enum.Enum):
+ """The possible gaits."""
+ WALK = 0
+ TROT = 1
+
+
+def _select_gait(gait_type=Gait.WALK):
+ """Selects a gait pattern.
+
+ Args:
+ gait_type: which gait to use.
+
+ Returns:
+ A tuple of (stance_duration, duty_factor, initial_phase)
+ """
+ # Each gait is composed of stance_duration, duty_factor, and
+ # init_phase_full_cycle.
+ if gait_type == Gait.TROT:
+ return [0.3] * 4, [0.6] * 4, [0, 0.5, 0.5, 0]
+ elif gait_type == Gait.WALK:
+ return [0.75] * 4, [0.8] * 4, [0.25, 0.75, 0.5, 0]
+ else:
+ raise NotImplementedError
+
+
+@gin.configurable
+class MPCLocomotionWrapper(object):
+ """An env that uses MPC-based motion controller to realize footstep planning.
+
+ The env takes as input the target position of the swing feet and the target
+ base movements, and internally uses an MPC-based controller to achieve these
+ targets. It assumes that the robot follows a given gait pattern, specified
+ during initialization.
+ Before each foot starts to swing, the env will request from the policy a
+ target swing location and height defined in the local frame w.r.t the
+ default toe position. During the swing of the foot, the policy can adjust
+ the base velocity, height, roll, pitch, and twist. Optionally, the policy
+ can also output a delta to the last target swing location to adjust the
+ swing trajectory during the swing
+ Observations (introduced in this env):
+ gait_phases (4D): Normalized phase within the gait cycle for each foot.
+ feet_states (4D): State of each foot. -1: stance, 1: swing, -2: lose
+ contact, 2: early contact.
+ need_new_swing_target (4D): Whether the foot needs a new swing target at
+ the current step. Set to 1 when the foot switches to swing from a
+ different state. When equals to 0, the corresponding foot target will
+ not have effect.
+ estimated_base_speed (3D): Estimated base velocity.
+ estimated_body_height (1D): Estimated base height.
+ heuristics_com_velocity (3D): Target base velocity calculated from the
+ input step-length using inverse Raibert heuristics. Used when
+ compute_heuristic_com_speed is True.
+ current_toe_target: Immediate tracking targets for the four feet in the
+ local frame.
+ Action components:
+ swing_targets (12D): Used in HL_LL and HL_only mode. Specifies
+ the swing target for each foot w.r.t the default local toe position.
+ swing_clearance (4D): Used in HL_LL and HL_only mode. Specifies
+ the height of the highest point in the swing trajectory.
+ swing_targets_delta (12D): Used if policy_output_swing_action_delta.
+ Specifies the change in the swing target for each foot.
+ swing_clearance_delta (4D): Used if policy_output_swing_action_delta.
+ Specifies the change in the swing clearance for each foot.
+ target_base_velocity (2D): Target base velocity in the horizontal plane.
+ Used when compute_heuristic_com_speed is False.
+ base_twist (1D): Target base twist.
+ base_height (1D): Target base height.
+ base_roll_pitch (2D): Target base roll and pitch.
+
+ Attributes:
+ observation: The current observation of the environment.
+ last_action: The last that was used to step the environment.
+ env_step_counter: The number of control steps that have been elapesed since
+ the environment is reset.
+ action_space: The action space of the environment.
+ """
+
+ def __init__(
+ self,
+ gym_env,
+ swing_target_action_range=_SWING_TARGET_ACTION_RANGE,
+ swing_clearance_action_range=_SWING_CLEARANCE_ACTION_RANGE,
+ pitch_action_range=_PITCH_ROLL_ACTION_RANGE,
+ roll_action_range=_PITCH_ROLL_ACTION_RANGE,
+ base_velocity_action_range=_BASE_VELOCITY_ACTION_RANGE,
+ base_twist_action_range=_BASE_TWIST_SPEED_ACTION_RANGE,
+ base_height_action_range=_BASE_HEIGHT_ACTION_RANGE,
+ policy_output_swing_action_delta=False,
+ swing_target_delta_range=_SWING_TARGET_DELTA_ACTION_RANGE,
+ swing_clearance_delta_range=_SWING_CLEARANCE_DELTA_RANGE,
+ foot_friction_coeff=0.5,
+ contact_detection_force_threshold=0.0,
+ locomotion_gait=Gait.WALK,
+ target_horizontal_com_velocity_heuristic=BaseTargetHorizontalComVelocityHeuristic(
+ ),
+ robot_mass_in_mpc=235.0 / 9.8,
+ control_frequency=20,
+ com_velocity_estimator_class=imu_based_com_velocity_estimator
+ .IMUBasedCOMVelocityEstimator):
+ """Initializes the wrapper.
+
+ Args:
+ gym_env: the wrapped gym environment.
+ swing_target_action_range: range for the swing targets specified before
+ each swing.
+ swing_clearance_action_range: range for the swing clearance.
+ pitch_action_range: range for the target body pitch.
+ roll_action_range: range for the target body roll.
+ base_velocity_action_range: range for the base velocity.
+ base_twist_action_range: range for the base twist.
+ base_height_action_range: range for the base height.
+ policy_output_swing_action_delta: whether to allow the policy to output an
+ adjustment to the last swing target during the swing motion.
+ swing_target_delta_range: range for the adjustment of the swing target.
+ swing_clearance_delta_range: range for the swing clearance adjustments.
+ foot_friction_coeff: friction on the feet.
+ contact_detection_force_threshold: Threshold of the contact sensor for
+ determining whether a foot is in contact. Use 20 for real robot and 0
+ for simulation.
+ locomotion_gait: Gait to be used.
+ target_horizontal_com_velocity_heuristic: .
+ robot_mass_in_mpc: mass of the robot used in MPC.
+ control_frequency: frequency of querying the policy. The internal MPC
+ controller can have higher frequency. Note that the policy outputs a
+ swing target and clearance at each query, however, it is only used by
+ the environment at the beginning of each swing phase (when
+ need_new_swing_target is 1).
+ com_velocity_estimator_class: class of the com velocity estimator. Use
+ IMUBasedCOMVelocityEstimator for estimating velocity from IMU sensor and
+ contact states. Use COMVelocityEstimator for using the ground-truth com
+ velocity (e.g. when mocap is available).
+ """
+ self._gym_env = gym_env
+ self._time_per_control_step = 1.0 / control_frequency
+ self._foot_friction_coeff = foot_friction_coeff
+ self._contact_detection_force_threshold = contact_detection_force_threshold
+ self._locomotion_gait = locomotion_gait
+ self._policy_output_swing_action_delta = policy_output_swing_action_delta
+ self._target_horizontal_com_velocity_heuristic = target_horizontal_com_velocity_heuristic
+
+ self.last_action = None
+
+ self._configure_action_space(
+ swing_target_action_range, swing_clearance_action_range,
+ pitch_action_range, roll_action_range, base_velocity_action_range,
+ base_twist_action_range, base_height_action_range,
+ policy_output_swing_action_delta, swing_target_delta_range,
+ swing_clearance_delta_range)
+
+ self._configure_observation_space()
+
+ # Set up the MPC controller
+ stance_duration, duty_factor, initial_phase = _select_gait(locomotion_gait)
+ self._gait_generator = openloop_gait_generator.OpenloopGaitGenerator(
+ self._gym_env.robot, stance_duration, duty_factor, initial_phase,
+ contact_detection_force_threshold)
+ self._com_velocity_estimator = com_velocity_estimator_class(
+ self._gym_env.robot)
+ self._com_height_estimator = com_height_estimator.COMHeightEstimator(
+ self._gym_env.robot)
+ self._state_estimator = multi_state_estimator.MultiStateEstimator(
+ self._gym_env.robot,
+ state_estimators=[
+ self._com_velocity_estimator, self._com_height_estimator
+ ])
+ self._stance_controller = torque_stance_leg_controller.TorqueStanceLegController(
+ self._gym_env.robot,
+ self._gait_generator,
+ self._state_estimator,
+ desired_speed=np.array(_DEFAULT_BASE_SPEED),
+ desired_twisting_speed=_DEFAULT_BASE_TWIST_SPEED,
+ desired_body_height=_DEFAULT_BODY_HEIGHT,
+ desired_roll_pitch=np.array(_DEFAULT_ROLL_PITCH),
+ body_mass=robot_mass_in_mpc)
+
+ def _configure_observation_space(self):
+ """Configure the observation space."""
+ self.observation_space.spaces["gait_phases"] = gym.spaces.Box(
+ np.array([-1.0] * 4), np.array([1.0] * 4))
+ self.observation_space.spaces["feet_states"] = gym.spaces.Box(
+ np.array([-2.0] * 4), np.array([2.0] * 4))
+ self.observation_space.spaces["need_new_swing_target"] = gym.spaces.Box(
+ np.array([0.0] * 4), np.array([1.0] * 4))
+ self.observation_space.spaces["estimated_base_speed"] = gym.spaces.Box(
+ np.array([-1.0, -1.0, -1.0]), np.array([1.0, 1.0, 1.0]))
+ self.observation_space.spaces["estimated_body_height"] = gym.spaces.Box(
+ np.array([0.35]), np.array([0.5]))
+ self.observation_space.spaces["heuristics_com_velocity"] = gym.spaces.Box(
+ np.array([-1.0] * 2), np.array([1.0] * 2))
+ self.observation_space.spaces["current_toe_target"] = gym.spaces.Box(
+ np.array([-1.0, -1.0, -1.0] * _NUM_LEGS),
+ np.array([1.0, 1.0, 1.0] * _NUM_LEGS))
+
+ # Needed so that LastActionSensor uses the correct action space.
+ for s in self.all_sensors():
+ s.on_reset(self)
+ for sensor in self.all_sensors():
+ if sensor.get_name() not in self._gym_config.ignored_sensor_list:
+ if hasattr(sensor, "observation_space"):
+ self.observation_space.spaces[
+ sensor.get_name()] = sensor.observation_space
+
+ self.task.reset(self)
+ if hasattr(self.task, "observation_space"):
+ self.observation_space.spaces[
+ self.task.get_name()] = self.task.observation_space
+
+ def _configure_action_space(self, swing_target_action_range,
+ swing_clearance_action_range, pitch_action_range,
+ roll_action_range, base_velocity_action_range,
+ base_twist_action_range, base_height_action_range,
+ policy_output_swing_action_delta,
+ swing_target_delta_range,
+ swing_clearance_delta_range):
+ """Configure the action space."""
+ ac_lb = np.array([])
+ ac_ub = np.array([])
+
+ # Index of different part of the actions within the action array.
+ self._action_start_indices = {}
+ self._action_dimensions = {}
+ self._action_names = []
+
+ # Swing targets and swing clearance.
+ for leg_id in range(_NUM_LEGS):
+ self._action_start_indices["swing_targets_" + str(leg_id)] = len(ac_lb)
+ self._action_dimensions["swing_targets_" + str(leg_id)] = len(
+ swing_target_action_range[0])
+ self._action_names.append("swing_targets_" + str(leg_id))
+ ac_lb = np.concatenate([ac_lb, swing_target_action_range[0]])
+ ac_ub = np.concatenate([ac_ub, swing_target_action_range[1]])
+ for leg_id in range(_NUM_LEGS):
+ self._action_start_indices["swing_clearance_" + str(leg_id)] = len(ac_lb)
+ self._action_dimensions["swing_clearance_" + str(leg_id)] = 1
+ self._action_names.append("swing_clearance_" + str(leg_id))
+ ac_lb = np.concatenate([ac_lb, [swing_clearance_action_range[0]]])
+ ac_ub = np.concatenate([ac_ub, [swing_clearance_action_range[1]]])
+
+ # Delta to the swing targets and clearance.
+ if policy_output_swing_action_delta:
+ for leg_id in range(_NUM_LEGS):
+ self._action_start_indices["swing_targets_delta_" +
+ str(leg_id)] = len(ac_lb)
+ self._action_dimensions["swing_targets_delta_" + str(leg_id)] = len(
+ swing_target_delta_range[0])
+ self._action_names.append("swing_targets_delta_" + str(leg_id))
+ ac_lb = np.concatenate([ac_lb, swing_target_delta_range[0]])
+ ac_ub = np.concatenate([ac_ub, swing_target_delta_range[1]])
+ for leg_id in range(_NUM_LEGS):
+ self._action_start_indices["swing_clearance_delta_" +
+ str(leg_id)] = len(ac_lb)
+ self._action_dimensions["swing_clearance_delta_" + str(leg_id)] = 1
+ self._action_names.append("swing_clearance_delta_" + str(leg_id))
+ ac_lb = np.concatenate([ac_lb, [swing_clearance_delta_range[0]]])
+ ac_ub = np.concatenate([ac_ub, [swing_clearance_delta_range[1]]])
+
+ # Desired CoM velocity actions
+ # Do not include the action if bounds are all equal to zero
+ if not np.all(np.array(base_velocity_action_range) == 0):
+ self._action_start_indices["target_base_velocity"] = len(ac_lb)
+ self._action_dimensions["target_base_velocity"] = 2
+ self._action_names.append("target_base_velocity")
+ ac_lb = np.concatenate([
+ ac_lb,
+ [base_velocity_action_range[0][0], base_velocity_action_range[0][1]]
+ ])
+ ac_ub = np.concatenate([
+ ac_ub,
+ [base_velocity_action_range[1][0], base_velocity_action_range[1][1]]
+ ])
+
+ # Base twist speed action
+ self._action_start_indices["base_twist"] = len(ac_lb)
+ self._action_dimensions["base_twist"] = 1
+ self._action_names.append("base_twist")
+ ac_lb = np.concatenate([ac_lb, [base_twist_action_range[0]]])
+ ac_ub = np.concatenate([ac_ub, [base_twist_action_range[1]]])
+
+ # Base height action
+ self._action_start_indices["base_height"] = len(ac_lb)
+ self._action_dimensions["base_height"] = 1
+ self._action_names.append("base_height")
+ ac_lb = np.concatenate([ac_lb, [base_height_action_range[0]]])
+ ac_ub = np.concatenate([ac_ub, [base_height_action_range[1]]])
+
+ # Roll-pitch action
+ self._action_start_indices["base_roll_pitch"] = len(ac_lb)
+ self._action_dimensions["base_roll_pitch"] = 2
+ self._action_names.append("base_roll_pitch")
+ ac_lb = np.concatenate(
+ [ac_lb, [roll_action_range[0], pitch_action_range[0]]])
+ ac_ub = np.concatenate(
+ [ac_ub, [roll_action_range[1], pitch_action_range[1]]])
+
+ self.action_space = gym.spaces.Box(ac_lb, ac_ub)
+
+ def __getattr__(self, attr):
+ return getattr(self._gym_env, attr)
+
+ def _fill_observations(self, obs):
+ """Fill the additional observations from this wrapper."""
+ phase_offset = np.array([
+ 0 if leg_state == gait_generator_lib.LegState.STANCE else 1
+ for leg_state in self._gait_generator.desired_leg_state
+ ])
+ obs["gait_phases"] = self._gait_generator.normalized_phase - phase_offset
+ obs["feet_states"] = []
+ for leg_state in self._gait_generator.desired_leg_state:
+ if leg_state == gait_generator_lib.LegState.STANCE:
+ obs["feet_states"].append(-1)
+ if leg_state == gait_generator_lib.LegState.SWING:
+ obs["feet_states"].append(1)
+ if leg_state == gait_generator_lib.LegState.EARLY_CONTACT:
+ obs["feet_states"].append(2)
+ if leg_state == gait_generator_lib.LegState.LOSE_CONTACT:
+ obs["feet_states"].append(-2)
+ obs["need_new_swing_target"] = np.copy(self._need_new_swing_target)
+ obs["estimated_base_speed"] = self._state_estimator.com_velocity_body_yaw_aligned_frame
+ obs["estimated_body_height"] = [self._state_estimator.estimated_com_height]
+ obs["heuristics_com_velocity"] = np.copy(
+ self._target_horizontal_com_velocity_heuristic
+ .horizontal_com_velocity_heuristic)
+ obs["current_toe_target"] = np.copy(self._current_toe_target)
+
+ def _reset_mpc_controller(self):
+ """Reset the state of the MPC controller."""
+ self._gait_generator.reset(0.0)
+ self._state_estimator.reset(0.0)
+ self._stance_controller.reset(0.0)
+ self._stance_controller.desired_speed = np.array(_DEFAULT_BASE_SPEED)
+ self._stance_controller.desired_twisting_speed = _DEFAULT_BASE_TWIST_SPEED
+ self._stance_controller.desired_body_height = _DEFAULT_BODY_HEIGHT
+ self._stance_controller.desired_roll_pitch = np.array(_DEFAULT_ROLL_PITCH)
+ self._mpc_reset_time = self.robot.GetTimeSinceReset()
+
+ def reset(self, **kwargs):
+ """Reset the environment."""
+ self._gym_env.reset(**kwargs)
+
+ self._reset_mpc_controller()
+
+ self._last_leg_state = copy.copy(self._gait_generator.leg_state)
+
+ self._initial_local_toe_positions = np.array(
+ self._gym_env.robot.foot_positions(position_in_world_frame=False))
+
+ self._current_toe_target = np.reshape(
+ copy.deepcopy(self._initial_local_toe_positions), 3 * _NUM_LEGS)
+
+ # Record last lift off position for each foot for computing swing
+ # trajectory.
+ self._lift_off_positions = copy.deepcopy(self._initial_local_toe_positions)
+
+ # Swing command by policy at the beginning of each swing phase.
+ self._nominal_swing_leg_commands = []
+ for _ in range(_NUM_LEGS):
+ self._nominal_swing_leg_commands.append({
+ "swing_target": np.array(_DEFAULT_SWING_TARGET),
+ "swing_clearance": _DEFAULT_SWING_CLERANCE
+ })
+ # Actual swing leg commands that incorporated potential delta adjustments
+ # from the policy.
+ self._actual_swing_leg_commands = []
+ for _ in range(_NUM_LEGS):
+ self._actual_swing_leg_commands.append({
+ "swing_target": np.array(_DEFAULT_SWING_TARGET),
+ "swing_clearance": _DEFAULT_SWING_CLERANCE
+ })
+
+ self._need_new_swing_target = np.array([0.0] * _NUM_LEGS)
+
+ self._target_horizontal_com_velocity_heuristic.reset()
+
+ self.last_action = []
+ for leg_id in range(_NUM_LEGS):
+ self.last_action.extend(
+ [0.0] * self._action_dimensions["swing_targets_" + str(leg_id)])
+ for leg_id in range(_NUM_LEGS):
+ self.last_action.extend(
+ [0.0] * self._action_dimensions["swing_clearance_" + str(leg_id)])
+ if self._policy_output_swing_action_delta:
+ for leg_id in range(_NUM_LEGS):
+ self.last_action.extend(
+ [0.0] *
+ self._action_dimensions["swing_targets_delta_" + str(leg_id)])
+ for leg_id in range(_NUM_LEGS):
+ self.last_action.extend(
+ [0.0] *
+ self._action_dimensions["swing_clearance_delta_" + str(leg_id)])
+ print("=========", len(self.last_action))
+ if "target_base_velocity" in self._action_names:
+ self.last_action.extend([0.0] *
+ self._action_dimensions["target_base_velocity"])
+ self.last_action.extend([0.0] * self._action_dimensions["base_twist"])
+ self.last_action.extend([0.0] * self._action_dimensions["base_height"])
+ self.last_action.extend([0.0] * self._action_dimensions["base_roll_pitch"])
+
+ # Needed for LastActionSensor to use the correct last_action.
+ for s in self.all_sensors():
+ s.on_reset(self)
+ obs = self._get_observation()
+ self._fill_observations(obs)
+
+ self.task.reset(self)
+ self._step_counter = 0
+
+ # Change feet friction.
+ for link_id in list(
+ self.robot.urdf_loader.get_end_effector_id_dict().values()):
+ self.pybullet_client.changeDynamics(
+ self.robot.robot_id,
+ link_id,
+ lateralFriction=self._foot_friction_coeff)
+ self._observation = obs
+
+ return self._observation
+
+ def _clean_up_action(self, action):
+ """Return a cleaned up action to use previous value or zero for components not used.
+
+ Args:
+ action: Input action from the policy.
+
+ Returns:
+ A cleaned up action where components not used in this step is replaced
+ with zero or value from previous steps.
+ """
+ cleaned_action = np.copy(action)
+ for leg_id in range(_NUM_LEGS):
+ if not self._need_new_swing_target[leg_id]:
+ swing_target_name = "swing_targets_" + str(leg_id)
+ swing_targets_start_index = self._action_start_indices[
+ swing_target_name]
+ swing_targets_end_index = swing_targets_start_index + self._action_dimensions[
+ swing_target_name]
+ cleaned_action[swing_targets_start_index:
+ swing_targets_end_index] = self.last_action[
+ swing_targets_start_index:swing_targets_end_index]
+
+ swing_clearance_name = "swing_clearance_" + str(leg_id)
+ swing_clearance_start_index = self._action_start_indices[
+ swing_clearance_name]
+ cleaned_action[swing_clearance_start_index] = self.last_action[
+ swing_clearance_start_index]
+ if self._policy_output_swing_action_delta:
+ for leg_id in range(_NUM_LEGS):
+ if self._observation["feet_states"][leg_id] != 1:
+ swing_target_delta_name = "swing_targets_delta_" + str(leg_id)
+ swing_targets_delta_start_index = self._action_start_indices[
+ swing_target_delta_name]
+ swing_targets_delta_end_index = swing_targets_delta_start_index + self._action_dimensions[
+ swing_target_delta_name]
+ cleaned_action[
+ swing_targets_delta_start_index:
+ swing_targets_delta_end_index] = self.last_action[
+ swing_targets_delta_start_index:swing_targets_delta_end_index]
+
+ swing_clearance_delta_name = "swing_clearance_delta_" + str(leg_id)
+ swing_clearance_delta_start_index = self._action_start_indices[
+ swing_clearance_delta_name]
+ cleaned_action[swing_clearance_delta_start_index] = self.last_action[
+ swing_clearance_delta_start_index]
+
+ return cleaned_action
+
+ def _get_toe_tracking_target(self, lift_off_position, phase, swing_clearance,
+ landing_position):
+ """Get the tracking target for the toes during the swing phase.
+
+ The swing toe will move 70% of the distance in the first half of the swing.
+ Intuitely, we want to move the swing foot quickly to the target landing
+ location and stay above the ground, in this way the control is more robust
+ to perturbations to the body that may cause the swing foot to drop onto
+ the ground earlier than expected. This is a common practice similar
+ to the MIT cheetah and Marc Raibert's original controllers. After the
+ designated swing motion finishes, we also command the foot to go down
+ for a short ditance (0.03m). This is to mitigate issues when the swing
+ finishes before it touches the ground.
+
+ Args:
+ lift_off_position: Local position when the foot leaves ground.
+ phase: Normalized phase of the foot in the current swing cycle.
+ swing_clearance: Height of the highest point in the swing trajectory.
+ landing_position: Target landing position in the local space.
+
+ Returns:
+ The interpolated foot target for the current step.
+ """
+ # Up vector in the world coordinate (without considering yaw).
+ rotated_up_vec = np.array(
+ self.pybullet_client.multiplyTransforms(
+ (0, 0, 0),
+ self.pybullet_client.getQuaternionFromEuler(
+ (self.robot.base_roll_pitch_yaw[0],
+ self.robot.base_roll_pitch_yaw[1], 0)), (0, 0, 1),
+ _UNIT_QUATERNION)[0])
+
+ # Linearly interpolate the trajectory to get the foot target.
+ keyframe_timings = [0.0, 0.45, 0.9, 0.9001, 1.0]
+ peak_toe_position = 0.3 * lift_off_position + 0.7 * landing_position + rotated_up_vec * swing_clearance
+ # TODO(magicmelon): Update the gait generator to warp the gait and keep
+ # the swing leg going down until it touches the ground.
+ landing_position_pressing_down = landing_position - rotated_up_vec * 0.03
+ keyframe_positions = np.array([
+ lift_off_position, peak_toe_position, landing_position,
+ landing_position_pressing_down, landing_position_pressing_down
+ ])
+
+ target_toe_positions = np.array([
+ np.interp(phase, keyframe_timings, keyframe_positions[:, 0]),
+ np.interp(phase, keyframe_timings, keyframe_positions[:, 1]),
+ np.interp(phase, keyframe_timings, keyframe_positions[:, 2])
+ ])
+ return target_toe_positions
+
+ def step(self, action: Sequence[float]):
+ """Steps the wrapped environment.
+
+ Args:
+ action:
+
+ Returns:
+ The tuple containing the observation, the reward, and the episode
+ end indicator.
+ """
+ self.last_action = self._clean_up_action(action)
+
+ obs, reward, done, info = self._step_motion_controller(self.last_action)
+ self._step_counter += 1
+ self._fill_observations(obs)
+ self._observation = obs
+
+ return obs, reward, done, info
+
+ def _extract_action(self, action, name):
+ return action[self.
+ _action_start_indices[name]:self._action_start_indices[name] +
+ self._action_dimensions[name]]
+
+ def _get_swing_foot_ids(self):
+ swing_foot_ids = []
+ for leg_id in range(_NUM_LEGS):
+ if self._gait_generator.leg_state[
+ leg_id] == gait_generator_lib.LegState.SWING:
+ swing_foot_ids.append(leg_id)
+ return swing_foot_ids
+
+ def _update_gait_states_and_flags(self):
+ """Update gait-related variables and flags."""
+ current_leg_state = self._gait_generator.leg_state
+ for leg_id in range(_NUM_LEGS):
+ if current_leg_state[
+ leg_id] == gait_generator_lib.LegState.SWING and self._last_leg_state[
+ leg_id] != gait_generator_lib.LegState.SWING:
+ self._lift_off_positions[leg_id] = self._gym_env.robot.foot_positions(
+ )[leg_id]
+ self._need_new_swing_target[leg_id] = 1
+ self._last_leg_state = copy.copy(current_leg_state)
+
+ com_estimate_leg_indices = []
+ for leg_id in range(_NUM_LEGS):
+ # Use the ones not swinging to estimate the com height
+ if leg_id not in self._get_swing_foot_ids():
+ com_estimate_leg_indices.append(leg_id)
+ self._com_height_estimator.com_estimate_leg_indices = com_estimate_leg_indices
+ self._state_estimator.update(self.robot.GetTimeSinceReset() -
+ self._mpc_reset_time)
+ self._gait_generator.update(self.robot.GetTimeSinceReset() -
+ self._mpc_reset_time)
+
+ def _process_action(self, action):
+ """Process the action and set relevant variables."""
+ # Extract the swing targets from the input action.
+ for leg_id in range(_NUM_LEGS):
+ if self._need_new_swing_target[leg_id]:
+ self._nominal_swing_leg_commands[leg_id]["swing_target"] = (
+ self._extract_action(action, "swing_targets_" + str(leg_id)))
+ self._nominal_swing_leg_commands[leg_id]["swing_clearance"] = (
+ self._extract_action(action, "swing_clearance_" + str(leg_id)))
+
+ self._actual_swing_leg_commands[leg_id]["swing_target"] = np.copy(
+ self._nominal_swing_leg_commands[leg_id]["swing_target"])
+ self._actual_swing_leg_commands[leg_id][
+ "swing_clearance"] = self._nominal_swing_leg_commands[leg_id][
+ "swing_clearance"]
+
+ # Reset the flags so the next high level commands are not used until the
+ # next swing happens.
+ self._need_new_swing_target = np.zeros(_NUM_LEGS)
+
+ # Extract the delta swing targets from the input action.
+ if self._policy_output_swing_action_delta:
+ for leg_id in self._get_swing_foot_ids():
+ self._actual_swing_leg_commands[leg_id]["swing_target"] = (
+ self._nominal_swing_leg_commands[leg_id]["swing_target"] +
+ self._extract_action(action, "swing_targets_delta_" + str(leg_id)))
+ self._actual_swing_leg_commands[leg_id]["swing_clearance"] = (
+ self._nominal_swing_leg_commands[leg_id]["swing_clearance"] +
+ self._extract_action(action,
+ "swing_clearance_delta_" + str(leg_id)))
+ # Extract the target base movement commands from the input action.
+ if "target_base_velocity" in self._action_names:
+ self._target_base_velocity_from_policy = self._extract_action(
+ action, "target_base_velocity")
+ else:
+ self._target_base_velocity_from_policy = np.zeros(3)
+ desired_twist_speed = self._extract_action(action, "base_twist")
+ desired_body_height = self._extract_action(action, "base_height")
+ desired_roll_pitch = self._extract_action(action, "base_roll_pitch")
+
+ self._stance_controller.desired_twisting_speed = desired_twist_speed
+ self._stance_controller.desired_body_height = desired_body_height
+ self._stance_controller.desired_roll_pitch = desired_roll_pitch
+
+ def _compute_swing_action(self):
+ """Compute actions for the swing legs."""
+ local_toe_positions = np.array(
+ self._gym_env.robot.foot_positions(position_in_world_frame=False))
+ toe_positions_over_time = copy.deepcopy(local_toe_positions)
+
+ for leg_id in self._get_swing_foot_ids():
+ target_toe_positions_local = self._get_toe_tracking_target(
+ self._lift_off_positions[leg_id],
+ self._gait_generator.normalized_phase[leg_id],
+ self._actual_swing_leg_commands[leg_id]["swing_clearance"],
+ self._actual_swing_leg_commands[leg_id]["swing_target"] +
+ self._initial_local_toe_positions[leg_id])
+ toe_positions_over_time[leg_id] = target_toe_positions_local
+ self._current_toe_target = np.reshape(
+ copy.deepcopy(toe_positions_over_time), 3 * _NUM_LEGS)
+ return np.array(
+ self.robot.motor_angles_from_foot_positions(
+ toe_positions_over_time, position_in_world_frame=False)[1])
+
+ def _compute_stance_action(self):
+ """Compute actions for the stance legs."""
+
+ # update target com velocity by combining policy output and heuristics
+ hip_relative_swing_targets = []
+ swing_durations = []
+ com_horizontal_velocity = np.array(
+ self._state_estimator.com_velocity_body_yaw_aligned_frame)[0:2]
+ for swing_id in self._get_swing_foot_ids():
+ hip_relative_swing_targets.append(
+ np.array([
+ self._actual_swing_leg_commands[swing_id]["swing_target"][0],
+ self._actual_swing_leg_commands[swing_id]["swing_target"][1]
+ ]))
+ swing_durations.append(self._gait_generator.swing_duration[swing_id])
+
+ self._target_horizontal_com_velocity_heuristic.update_horizontal_com_velocity_heuristic(
+ hip_relative_swing_targets, com_horizontal_velocity, swing_durations)
+ self._stance_controller.desired_speed = np.array(
+ self._target_base_velocity_from_policy)
+ self._stance_controller.desired_speed[
+ 0:
+ 2] += self._target_horizontal_com_velocity_heuristic.horizontal_com_velocity_heuristic
+
+ # compute actions for the stance leg
+ return self._stance_controller.get_action()
+
+ def _combine_swing_stance_action(self, swing_action, stance_action):
+ """Combine stance and swing leg actions."""
+ feet_contact_states = copy.copy(self._gait_generator.leg_state)
+ actions = []
+ for leg_id in range(_NUM_LEGS):
+ if leg_id in self._get_swing_foot_ids(
+ ) and feet_contact_states[leg_id] == gait_generator_lib.LegState.SWING:
+ for motor_id_in_leg in range(_MOTORS_PER_LEG):
+ actions.extend(
+ (swing_action[leg_id * _MOTORS_PER_LEG + motor_id_in_leg],
+ _MOTOR_KP[leg_id * _MOTORS_PER_LEG + motor_id_in_leg], 0.0,
+ _MOTOR_KD[leg_id * _MOTORS_PER_LEG + motor_id_in_leg], 0.0))
+ else:
+ for motor_id_in_leg in range(_MOTORS_PER_LEG):
+ actions.extend(stance_action[leg_id * _MOTORS_PER_LEG +
+ motor_id_in_leg])
+ return actions
+
+ def _step_motion_controller(self, action):
+ """Run the MPC controller to advance the robot's state."""
+
+ self._process_action(action)
+
+ # run MPC-based control for a certain amount of time
+ total_reward = 0.0
+ start_time = self._gym_env.robot.GetTimeSinceReset()
+ while self._gym_env.robot.GetTimeSinceReset(
+ ) - start_time < self._time_per_control_step:
+ self._update_gait_states_and_flags()
+
+ swing_action = self._compute_swing_action()
+
+ stance_action = self._compute_stance_action()
+
+ actions = self._combine_swing_stance_action(swing_action, stance_action)
+
+ obs, rew, done, info = self._gym_env.step(actions)
+ if self._stance_controller.qp_solver_fail:
+ done = True
+ total_reward += rew
+
+ if done:
+ return obs, 0.0, done, info
+
+ return obs, total_reward, done, info
+
+ @property
+ def observation(self):
+ return self._observation
+
+ @property
+ def env_step_counter(self):
+ return self._step_counter
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/observation_dictionary_to_array_wrapper.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/observation_dictionary_to_array_wrapper.py
new file mode 100644
index 000000000..34e6049cc
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/observation_dictionary_to_array_wrapper.py
@@ -0,0 +1,65 @@
+"""An env wrapper that flattens the observation dictionary to an array."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import gym
+import gin
+from pybullet_envs.minitaur.envs_v2.utilities import env_utils
+
+
+@gin.configurable
+class ObservationDictionaryToArrayWrapper(gym.Env):
+ """An env wrapper that flattens the observation dictionary to an array."""
+
+ def __init__(self, gym_env, observation_excluded=()):
+ """Initializes the wrapper."""
+ self.observation_excluded = observation_excluded
+ self._gym_env = gym_env
+ self.observation_space = self._flatten_observation_spaces(
+ self._gym_env.observation_space)
+ self.action_space = self._gym_env.action_space
+
+ def __getattr__(self, attr):
+ return getattr(self._gym_env, attr)
+
+ def _flatten_observation_spaces(self, observation_spaces):
+ flat_observation_space = env_utils.flatten_observation_spaces(
+ observation_spaces=observation_spaces,
+ observation_excluded=self.observation_excluded)
+ return flat_observation_space
+
+ def _flatten_observation(self, input_observation):
+ """Flatten the dictionary to an array."""
+ return env_utils.flatten_observations(
+ observation_dict=input_observation,
+ observation_excluded=self.observation_excluded)
+
+ def seed(self, seed=None):
+ return self._gym_env.seed(seed)
+
+ def reset(self, initial_motor_angles=None, reset_duration=1.0):
+ observation = self._gym_env.reset(
+ initial_motor_angles=initial_motor_angles,
+ reset_duration=reset_duration)
+ return self._flatten_observation(observation)
+
+ def step(self, action):
+ """Steps the wrapped environment.
+
+ Args:
+ action: Numpy array. The input action from an NN agent.
+
+ Returns:
+ The tuple containing the flattened observation, the reward, the epsiode
+ end indicator.
+ """
+ observation_dict, reward, done, _ = self._gym_env.step(action)
+ return self._flatten_observation(observation_dict), reward, done, _
+
+ def render(self, mode='human'):
+ return self._gym_env.render(mode)
+
+ def close(self):
+ return self._gym_env.close()
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/pmtg_inplace_wrapper_env.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/pmtg_inplace_wrapper_env.py
new file mode 100644
index 000000000..0a274d399
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/pmtg_inplace_wrapper_env.py
@@ -0,0 +1,176 @@
+"""A wrapped MinitaurGymEnv with a built-in controller."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+import attr
+from gym import spaces
+import numpy as np
+import gin
+from pybullet_envs.minitaur.agents.trajectory_generator import tg_inplace
+from pybullet_envs.minitaur.envs.utilities import laikago_pose_utils
+from pybullet_envs.minitaur.envs.utilities import minitaur_pose_utils
+
+_NUM_LEGS = 4
+_LAIKAGO_NUM_ACTIONS = 12
+_FREQ_LOWER_BOUND = 0.0
+_FREQ_UPPER_BOUND = 3
+_DEFAULT_AMPLITUDE_STANCE = -0.02
+_DEFAULT_AMPLITUDE_LIFT = 0.9
+_DEFAULT_CENTER_EXTENSION = 0
+_DEFAULT_STANCE_LIFT_CUTOFF = 2 * np.pi * 0.67
+_DEFAULT_RESIDUAL_RANGE = 0.4
+_LAIKAGO_KNEE_ACTION_INDEXES = [2, 5, 8, 11]
+MINITAUR_INIT_EXTENSION_POS = 2.0
+MINITAUR_INIT_SWING_POS = 0.0
+
+
+@gin.configurable
+class PmtgInplaceWrapperEnv(object):
+ """A wrapped LocomotionGymEnv with a built-in trajectory generator."""
+
+ def __init__(self,
+ gym_env,
+ freq_lower_bound=_FREQ_LOWER_BOUND,
+ freq_upper_bound=_FREQ_UPPER_BOUND,
+ residual_range=_DEFAULT_RESIDUAL_RANGE,
+ amplitude_stance=_DEFAULT_AMPLITUDE_STANCE,
+ amplitude_lift=_DEFAULT_AMPLITUDE_LIFT,
+ center_extension=_DEFAULT_CENTER_EXTENSION,
+ stance_lift_cutoff=_DEFAULT_STANCE_LIFT_CUTOFF):
+ """Initializes the TG inplace wrapper class.
+
+ Args:
+ gym_env: the gym environment to wrap on.
+ freq_lower_bound: minimum frequency that the TGs can be propagated at.
+ freq_upper_bound: maximum frequency that the TGs can be propagated at.
+ residual_range: range of residuals that can be added to tg outputs.
+ amplitude_stance: stance amplitude of TG (see tg_inplace.py for details).
+ amplitude_lift: swing amplitude of TG (see tg_inplace.py for details).
+ center_extension: center extension of TG (see tg_inplace.py for details).
+ stance_lift_cutoff: phase cutoff between stance and lift phase (see
+ tg_inplace.py for details).
+ """
+ self._gym_env = gym_env
+ self._num_actions = gym_env.robot.num_motors
+ self._tg_phases = tg_inplace.reset()
+ self._tg_params = dict(
+ amplitude_stance=amplitude_stance,
+ amplitude_lift=amplitude_lift,
+ center_extension=center_extension,
+ stance_lift_cutoff=stance_lift_cutoff)
+
+ # Add the action boundaries for delta time, one per integrator.
+ action_low = np.hstack(
+ ([-residual_range] * self._num_actions, [freq_lower_bound] * _NUM_LEGS))
+ action_high = np.hstack(
+ ([residual_range] * self._num_actions, [freq_upper_bound] * _NUM_LEGS))
+ self.action_space = spaces.Box(action_low, action_high)
+
+ # Set the observation space and boundaries.
+ if hasattr(self._gym_env.observation_space, "spaces"):
+ self.observation_space = self._gym_env.observation_space
+ self.observation_space.spaces["pmtg_inplace"] = spaces.Box(
+ -1 * np.ones(2 * _NUM_LEGS), np.ones(2 * _NUM_LEGS))
+ else:
+ lower_bound = self._gym_env.observation_space.low
+ upper_bound = self._gym_env.observation_space.high
+ lower_bound = np.hstack((lower_bound, [-1.] * 2 * _NUM_LEGS))
+ upper_bound = np.hstack((upper_bound, [1.] * 2 * _NUM_LEGS))
+ self.observation_space = spaces.Box(lower_bound, upper_bound)
+
+ def __getattr__(self, attrb):
+ return getattr(self._gym_env, attrb)
+
+ def _modify_observation(self, observation):
+ if isinstance(observation, dict):
+ observation["tg_inplace"] = np.hstack(
+ (np.cos(self._tg_phases), np.sin(self._tg_phases)))
+ return observation
+ else:
+ return np.hstack(
+ (observation, np.cos(self._tg_phases), np.sin(self._tg_phases)))
+
+ def reset(self, initial_motor_angles=None, reset_duration=1.0):
+ """Resets the environment as well as trajectory generators."""
+ self._last_real_time = 0
+ self._num_step = 0
+ self._tg_phases = tg_inplace.reset()
+ if self._num_actions == _LAIKAGO_NUM_ACTIONS:
+ # Use laikago's init pose as zero action.
+ init_pose = np.array(
+ attr.astuple(
+ laikago_pose_utils.LaikagoPose(
+ abduction_angle_0=laikago_pose_utils
+ .LAIKAGO_DEFAULT_ABDUCTION_ANGLE,
+ hip_angle_0=laikago_pose_utils.LAIKAGO_DEFAULT_HIP_ANGLE,
+ knee_angle_0=laikago_pose_utils.LAIKAGO_DEFAULT_KNEE_ANGLE,
+ abduction_angle_1=laikago_pose_utils
+ .LAIKAGO_DEFAULT_ABDUCTION_ANGLE,
+ hip_angle_1=laikago_pose_utils.LAIKAGO_DEFAULT_HIP_ANGLE,
+ knee_angle_1=laikago_pose_utils.LAIKAGO_DEFAULT_KNEE_ANGLE,
+ abduction_angle_2=laikago_pose_utils
+ .LAIKAGO_DEFAULT_ABDUCTION_ANGLE,
+ hip_angle_2=laikago_pose_utils.LAIKAGO_DEFAULT_HIP_ANGLE,
+ knee_angle_2=laikago_pose_utils.LAIKAGO_DEFAULT_KNEE_ANGLE,
+ abduction_angle_3=laikago_pose_utils
+ .LAIKAGO_DEFAULT_ABDUCTION_ANGLE,
+ hip_angle_3=laikago_pose_utils.LAIKAGO_DEFAULT_HIP_ANGLE,
+ knee_angle_3=laikago_pose_utils.LAIKAGO_DEFAULT_KNEE_ANGLE)))
+ self._init_pose = init_pose
+ observation = self._gym_env.reset(init_pose, reset_duration)
+ else:
+ # Use minitaur's init pose as zero action.
+ init_pose = np.array(
+ attr.astuple(
+ minitaur_pose_utils.MinitaurPose(
+ swing_angle_0=MINITAUR_INIT_SWING_POS,
+ swing_angle_1=MINITAUR_INIT_SWING_POS,
+ swing_angle_2=MINITAUR_INIT_SWING_POS,
+ swing_angle_3=MINITAUR_INIT_SWING_POS,
+ extension_angle_0=MINITAUR_INIT_EXTENSION_POS,
+ extension_angle_1=MINITAUR_INIT_EXTENSION_POS,
+ extension_angle_2=MINITAUR_INIT_EXTENSION_POS,
+ extension_angle_3=MINITAUR_INIT_EXTENSION_POS)))
+ initial_motor_angles = minitaur_pose_utils.leg_pose_to_motor_angles(
+ init_pose)
+ observation = self._gym_env.reset(initial_motor_angles, reset_duration)
+ return self._modify_observation(observation)
+
+ def step(self, action):
+ """Steps the wrapped PMTG inplace environment."""
+ time = self._gym_env.get_time_since_reset()
+
+ # Convert the policy's residual actions to motor space.
+ if self._num_actions == _LAIKAGO_NUM_ACTIONS:
+ action_residual = np.array(
+ attr.astuple(
+ laikago_pose_utils.LaikagoPose(*(action[0:self._num_actions]))))
+ else:
+ action_residual = minitaur_pose_utils.leg_pose_to_motor_angles(
+ action[0:self._num_actions])
+
+ self._last_real_time = time
+ self._tg_phases, tg_extensions = tg_inplace.step(
+ self._tg_phases, action[-_NUM_LEGS:], self._gym_env.env_time_step,
+ self._tg_params)
+ # Convert TG's actions to motor space depending on the robot type.
+ if self._num_actions == _LAIKAGO_NUM_ACTIONS:
+ # If the legs have 3 DOF, apply extension directly to knee joints
+ action_tg_motor_space = np.zeros(self._num_actions)
+ for tg_idx, knee_idx in zip(
+ range(_NUM_LEGS), _LAIKAGO_KNEE_ACTION_INDEXES):
+ action_tg_motor_space[knee_idx] = tg_extensions[tg_idx]
+ else:
+ # Conversion to motor space for minitaur robot.
+ action_tg_motor_space = []
+ for idx in range(_NUM_LEGS):
+ extend = tg_extensions[idx]
+ action_tg_motor_space.extend(
+ minitaur_pose_utils.swing_extend_to_motor_angles(idx, 0, extend))
+ new_action = action_tg_motor_space + action_residual
+ if self._num_actions == _LAIKAGO_NUM_ACTIONS:
+ new_action += self._init_pose
+ original_observation, reward, done, _ = self._gym_env.step(new_action)
+
+ return self._modify_observation(original_observation), reward, done, _
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/pmtg_wrapper_env.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/pmtg_wrapper_env.py
new file mode 100644
index 000000000..1df14a296
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/pmtg_wrapper_env.py
@@ -0,0 +1,290 @@
+"""A wrapped MinitaurGymEnv with a built-in controller."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+from gym import spaces
+import numpy as np
+import gin
+from pybullet_envs.minitaur.agents.trajectory_generator import tg_simple
+from pybullet_envs.minitaur.envs_v2.utilities import robot_pose_utils
+from pybullet_envs.minitaur.robots.utilities import action_filter
+
+_DELTA_TIME_LOWER_BOUND = 0.0
+_DELTA_TIME_UPPER_BOUND = 3
+
+_GAIT_PHASE_MAP = {
+ "walk": [0, 0.25, 0.5, 0.75],
+ "trot": [0, 0.5, 0.5, 0],
+ "bound": [0, 0.5, 0, 0.5],
+ "pace": [0, 0, 0.5, 0.5],
+ "pronk": [0, 0, 0, 0]
+}
+
+
+@gin.configurable
+class PmtgWrapperEnv(object):
+ """A wrapped LocomotionGymEnv with a built-in trajectory generator."""
+
+ def __init__(self,
+ gym_env,
+ intensity_upper_bound=1.5,
+ min_delta_time=_DELTA_TIME_LOWER_BOUND,
+ max_delta_time=_DELTA_TIME_UPPER_BOUND,
+ integrator_coupling_mode="all coupled",
+ walk_height_coupling_mode="all coupled",
+ variable_swing_stance_ratio=True,
+ swing_stance_ratio=1.0,
+ residual_range=0.15,
+ init_leg_phase_offsets=None,
+ init_gait=None,
+ default_walk_height=0,
+ action_filter_enable=True,
+ action_filter_order=1,
+ action_filter_low_cut=0,
+ action_filter_high_cut=3.0,
+ action_filter_initialize=False,
+ leg_pose_class=None):
+ """Initialzes the wrapped env.
+
+ Args:
+ gym_env: An instance of LocomotionGymEnv.
+ intensity_upper_bound: The upper bound for intensity of the trajectory
+ generator. It can be used to limit the leg movement.
+ min_delta_time: Lower limit for the time in seconds that the trajectory
+ generator can be moved forward at each simulation step. The effective
+ frequency of the gait is based on the delta time multiplied by the
+ internal frequency of the trajectory generator.
+ max_delta_time: Upper limit for the time in seconds that the trajectory
+ generator can be moved forward at each simulation step.
+ integrator_coupling_mode: How the legs should be coupled for integrators.
+ walk_height_coupling_mode: The same coupling mode used for walking walking
+ heights for the legs.
+ variable_swing_stance_ratio: A boolean to indicate if the swing stance
+ ratio can change per time step or not.
+ swing_stance_ratio: Time taken by swing phase vs stance phase. This is
+ only relevant if variable_swing_stance_ratio is False.
+ residual_range: The upper limit for the residual actions that adds to the
+ leg motion. It is 0.15 by default, can be increased for more flexibility
+ or decreased to only to use the trajectory generator's motion.
+ init_leg_phase_offsets: The initial phases of the legs. A list of 4
+ variables within [0,1). The order is front-left, rear-left, front-right
+ and rear-right.
+ init_gait: The initial gait that sets the starting phase difference
+ between the legs. Overrides the arg init_phase_offsets. Has to be
+ "walk", "trot", "bound" or "pronk". Used in vizier search.
+ default_walk_height: Offset for the extension of the legs for the robot.
+ Applied to the legs at every time step. Implicitly affects the walking
+ and standing height of the policy. Zero by default. Units is in
+ extension space (can be considered in radiant since it is a linear
+ transformation to motor angles based on the robot's geometry).
+ action_filter_enable: Use a butterworth filter for the output of the PMTG
+ actions (before conversion to leg swing-extend model). It forces
+ smoother behaviors depending on the parameters used.
+ action_filter_order: The order for the action_filter (1 by default).
+ action_filter_low_cut: The cut for the lower frequencies (0 by default).
+ action_filter_high_cut: The cut for the higher frequencies (3 by default).
+ action_filter_initialize: If the action filter should be initialized when
+ the first action is taken. If enabled, the filter does not affect action
+ value the first time it is called and fills the history with that value.
+ leg_pose_class: A class providing a convert_leg_pose_to_motor_angle
+ instance method or None. If None, robot_pose_utils is used.
+
+ Raises:
+ ValueError if the controller does not implement get_action and
+ get_observation.
+
+ """
+ self._gym_env = gym_env
+ self._num_actions = gym_env.robot.num_motors
+ self._residual_range = residual_range
+ self._min_delta_time = min_delta_time
+ self._max_delta_time = max_delta_time
+ self._leg_pose_util = leg_pose_class() if leg_pose_class else None
+ # If not specified, default leg phase offsets to walking.
+ if init_gait:
+ if init_gait in _GAIT_PHASE_MAP:
+ init_leg_phase_offsets = _GAIT_PHASE_MAP[init_gait]
+ else:
+ raise ValueError("init_gait is not one of the defined gaits.")
+ else:
+ init_leg_phase_offsets = init_leg_phase_offsets or [0, 0.25, 0.5, 0.75]
+ # Create the Trajectory Generator based on the parameters.
+ self._trajectory_generator = tg_simple.TgSimple(
+ intensity_upper_bound=intensity_upper_bound,
+ integrator_coupling_mode=integrator_coupling_mode,
+ walk_height_coupling_mode=walk_height_coupling_mode,
+ variable_swing_stance_ratio=variable_swing_stance_ratio,
+ swing_stance_ratio=swing_stance_ratio,
+ init_leg_phase_offsets=init_leg_phase_offsets)
+
+ action_dim = self._extend_action_space()
+ self._extend_obs_space()
+
+ self._default_walk_height = default_walk_height
+ self._action_filter_enable = action_filter_enable
+ if self._action_filter_enable:
+ self._action_filter_initialize = action_filter_initialize
+ self._action_filter_order = action_filter_order
+ self._action_filter_low_cut = action_filter_low_cut
+ self._action_filter_high_cut = action_filter_high_cut
+ self._action_filter = self._build_action_filter(action_dim)
+
+ def _extend_obs_space(self):
+ """Extend observation space to include pmtg phase variables."""
+ # Set the observation space and boundaries.
+ lower_bound, upper_bound = self._get_observation_bounds()
+ if hasattr(self._gym_env.observation_space, "spaces"):
+ new_obs_space = spaces.Box(np.array(lower_bound), np.array(upper_bound))
+ self.observation_space.spaces.update({"pmtg_phase": new_obs_space})
+ else:
+ lower_bound = np.append(self._gym_env.observation_space.low, lower_bound)
+ upper_bound = np.append(self._gym_env.observation_space.high, upper_bound)
+ self.observation_space = spaces.Box(
+ np.array(lower_bound), np.array(upper_bound), dtype=np.float32)
+
+ def _extend_action_space(self):
+ """Extend the action space to include pmtg parameters."""
+ # Add the action boundaries for delta time, one per integrator.
+ action_low = [-self._residual_range] * self._num_actions
+ action_high = [self._residual_range] * self._num_actions
+ action_low = np.append(action_low, [self._min_delta_time] *
+ self._trajectory_generator.num_integrators)
+ action_high = np.append(action_high, [self._max_delta_time] *
+ self._trajectory_generator.num_integrators)
+
+ # Add the action boundaries for parameters of the trajectory generator.
+ l_bound, u_bound = self._trajectory_generator.get_parameter_bounds()
+ action_low = np.append(action_low, l_bound)
+ action_high = np.append(action_high, u_bound)
+ self.action_space = spaces.Box(
+ np.array(action_low), np.array(action_high), dtype=np.float32)
+ return len(action_high)
+
+ def __getattr__(self, attrb):
+ return getattr(self._gym_env, attrb)
+
+ def _modify_observation(self, observation):
+ if isinstance(observation, dict):
+ observation["pmtg_phase"] = self._trajectory_generator.get_state()
+ return observation
+ else:
+ return np.append(observation, self._trajectory_generator.get_state())
+
+ def reset(self, initial_motor_angles=None, reset_duration=1.0):
+ """Resets the environment as well as the trajectory generator(s).
+
+ Args:
+ initial_motor_angles: Unused for PMTG. Instead, it sets the legs to a pose
+ with the neutral action for the trajectory generator.
+ reset_duration: Float. The time (in seconds) needed to rotate all motors
+ to the desired initial values.
+
+ Returns:
+ A numpy array contains the initial observation after reset.
+ """
+ del initial_motor_angles
+ if self._action_filter_enable:
+ self._reset_action_filter()
+ self._last_real_time = 0
+ self._num_step = 0
+ self._target_speed_coef = 0.0
+ if self._trajectory_generator:
+ self._trajectory_generator.reset()
+ if self._leg_pose_util:
+ initial_motor_angles = self._leg_pose_util.convert_leg_pose_to_motor_angles(
+ [0, 0, 0] * 4)
+ else:
+ initial_motor_angles = robot_pose_utils.convert_leg_pose_to_motor_angles(
+ self._gym_env.robot_class, [0, 0, 0] * 4)
+ observation = self._gym_env.reset(initial_motor_angles, reset_duration)
+ return self._modify_observation(observation)
+
+ def step(self, action):
+ """Steps the wrapped environment.
+
+ Args:
+ action: Numpy array. The input action from an NN agent.
+
+ Returns:
+ The tuple containing the modified observation, the reward, the epsiode end
+ indicator.
+
+ Raises:
+ ValueError if input action is None.
+
+ """
+
+ if action is None:
+ raise ValueError("Action cannot be None")
+
+ if self._action_filter_enable:
+ action = self._filter_action(action)
+
+ time = self._gym_env.get_time_since_reset()
+
+ action_residual = action[0:self._num_actions]
+ # Add the default walking height offset to extension.
+ dimensions = len(action_residual) // 4
+ action_residual[dimensions - 1::dimensions] += self._default_walk_height
+ # Calculate trajectory generator's output based on the rest of the actions.
+ delta_real_time = time - self._last_real_time
+ self._last_real_time = time
+ action_tg = self._trajectory_generator.get_actions(
+ delta_real_time, action[self._num_actions:])
+ # If the residuals have a larger dimension, extend trajectory generator's
+ # actions to include abduction motors.
+ if len(action_tg) == 8 and len(action_residual) == 12:
+ for i in [0, 3, 6, 9]:
+ action_tg.insert(i, 0)
+ # Add TG actions with residual actions (in swing - extend space).
+ action_total = [a + b for a, b in zip(action_tg, action_residual)]
+ # Convert them to motor space based on the robot-specific conversions.
+ if self._leg_pose_util:
+ action_motor_space = self._leg_pose_util.convert_leg_pose_to_motor_angles(
+ action_total)
+ else:
+ action_motor_space = robot_pose_utils.convert_leg_pose_to_motor_angles(
+ self._gym_env.robot_class, action_total)
+ original_observation, reward, done, _ = self._gym_env.step(
+ action_motor_space)
+
+ return self._modify_observation(original_observation), np.float32(
+ reward), done, _
+
+ def _get_observation_bounds(self):
+ """Get the bounds of the observation added from the trajectory generator.
+
+ Returns:
+ lower_bounds: Lower bounds for observations.
+ upper_bounds: Upper bounds for observations.
+ """
+ lower_bounds = self._trajectory_generator.get_state_lower_bounds()
+ upper_bounds = self._trajectory_generator.get_state_upper_bounds()
+ return lower_bounds, upper_bounds
+
+ def _build_action_filter(self, num_joints):
+ order = self._action_filter_order
+ low_cut = self._action_filter_low_cut
+ high_cut = self._action_filter_high_cut
+ sampling_rate = 1 / (0.01)
+ a_filter = action_filter.ActionFilterButter([low_cut], [high_cut],
+ sampling_rate, order,
+ num_joints)
+ return a_filter
+
+ def _reset_action_filter(self):
+ self._action_filter.reset()
+ self._action_filter_empty = True
+ return
+
+ def _filter_action(self, action):
+ if self._action_filter_empty and self._action_filter_initialize:
+ # If initialize is selected and it is the first time filter is called,
+ # fill the buffer with that action so that it starts from that value
+ # instead of zero(s).
+ init_action = np.array(action).reshape(len(action), 1)
+ self._action_filter.reset(init_action)
+ self._action_filter_empty = False
+ filtered_action = self._action_filter.filter(np.array(action))
+ return filtered_action
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/simple_openloop.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/simple_openloop.py
new file mode 100644
index 000000000..65cb6446f
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/simple_openloop.py
@@ -0,0 +1,127 @@
+"""Simple openloop trajectory generators."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import attr
+from gym import spaces
+import numpy as np
+
+import gin
+from pybullet_envs.minitaur.envs_v2.utilities import laikago_pose_utils
+from pybullet_envs.minitaur.envs_v2.utilities import minitaur_pose_utils
+
+
+@gin.configurable
+class MinitaurPoseOffsetGenerator(object):
+ """A trajectory generator that return a constant leg pose."""
+
+ def __init__(self,
+ init_swing=0,
+ init_extension=2.0,
+ init_pose=None,
+ action_scale=1.0,
+ action_limit=0.5):
+ """Initializes the controller.
+
+ Args:
+ init_swing: the swing of the default pose offset
+ init_extension: the extension of the default pose offset
+ init_pose: the default pose offset, which is None by default. If not None,
+ it will define the default pose offset while ignoring init_swing and
+ init_extension.
+ action_scale: changes the magnitudes of actions
+ action_limit: clips actions
+ """
+ if init_pose is None:
+ self._pose = np.array(
+ attr.astuple(
+ minitaur_pose_utils.MinitaurPose(
+ swing_angle_0=init_swing,
+ swing_angle_1=init_swing,
+ swing_angle_2=init_swing,
+ swing_angle_3=init_swing,
+ extension_angle_0=init_extension,
+ extension_angle_1=init_extension,
+ extension_angle_2=init_extension,
+ extension_angle_3=init_extension)))
+ else: # Ignore init_swing and init_extension
+ self._pose = np.array(init_pose)
+ action_high = np.array([action_limit] * minitaur_pose_utils.NUM_MOTORS)
+ self.action_space = spaces.Box(-action_high, action_high, dtype=np.float32)
+ self._action_scale = action_scale
+
+ def reset(self):
+ pass
+
+ def get_action(self, current_time=None, input_action=None):
+ """Computes the trajectory according to input time and action.
+
+ Args:
+ current_time: The time in gym env since reset.
+ input_action: A numpy array. The input leg pose from a NN controller.
+
+ Returns:
+ A numpy array. The desired motor angles.
+ """
+ del current_time
+ return minitaur_pose_utils.leg_pose_to_motor_angles(
+ self._pose + self._action_scale * np.array(input_action))
+
+ def get_observation(self, input_observation):
+ """Get the trajectory generator's observation."""
+
+ return input_observation
+
+
+@gin.configurable
+class LaikagoPoseOffsetGenerator(object):
+ """A trajectory generator that return constant motor angles."""
+
+ def __init__(
+ self,
+ init_abduction=laikago_pose_utils.LAIKAGO_DEFAULT_ABDUCTION_ANGLE,
+ init_hip=laikago_pose_utils.LAIKAGO_DEFAULT_HIP_ANGLE,
+ init_knee=laikago_pose_utils.LAIKAGO_DEFAULT_KNEE_ANGLE,
+ action_limit=0.5,
+ ):
+ """Initializes the controller."""
+ self._pose = np.array(
+ attr.astuple(
+ laikago_pose_utils.LaikagoPose(
+ abduction_angle_0=init_abduction,
+ hip_angle_0=init_hip,
+ knee_angle_0=init_knee,
+ abduction_angle_1=init_abduction,
+ hip_angle_1=init_hip,
+ knee_angle_1=init_knee,
+ abduction_angle_2=init_abduction,
+ hip_angle_2=init_hip,
+ knee_angle_2=init_knee,
+ abduction_angle_3=init_abduction,
+ hip_angle_3=init_hip,
+ knee_angle_3=init_knee)))
+ action_high = np.array([action_limit] * 12)
+ self.action_space = spaces.Box(-action_high, action_high, dtype=np.float32)
+
+ def reset(self):
+ pass
+
+ def get_action(self, current_time=None, input_action=None):
+ """Computes the trajectory according to input time and action.
+
+ Args:
+ current_time: The time in gym env since reset.
+ input_action: A numpy array. The input leg pose from a NN controller.
+
+ Returns:
+ A numpy array. The desired motor angles.
+ """
+ del current_time
+ return self._pose + input_action
+
+ def get_observation(self, input_observation):
+ """Get the trajectory generator's observation."""
+
+ return input_observation
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/state_machine_based_wrapper_env.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/state_machine_based_wrapper_env.py
new file mode 100644
index 000000000..56a92dcd0
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/state_machine_based_wrapper_env.py
@@ -0,0 +1,321 @@
+"""A wrapped Quadruped with State-machine based controller."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import enum
+import gin
+import gym
+import numpy as np
+
+NUM_LEGS = 4
+ACTION_DIM_COM = 2
+ACTION_DIM_TOE = 1
+ACTION_DIM_TOTAL = ACTION_DIM_COM + ACTION_DIM_TOE
+OBSERVATION_DIM_LEG_ID = NUM_LEGS
+OBSERVATION_DIM_TOE_POS = 2 * NUM_LEGS
+OBSERVATION_DIM_TOTAL = OBSERVATION_DIM_TOE_POS + OBSERVATION_DIM_LEG_ID
+
+
+# States of the state machine.
+class GaitStateMachine(enum.IntEnum):
+ """The state machine for quadruped gait."""
+ STEP_LEFT_FRONT_TOE = 0
+ STEP_RIGHT_HIND_TOE = 1
+ STEP_RIGHT_FRONT_TOE = 2
+ STEP_LEFT_HIND_TOE = 3
+ TOTAL_GAIT_STATE_NUM = 4
+
+
+@gin.configurable
+class StateMachineBasedWrapperEnv(object):
+ """An env using IK to convert toe positions to joint angles.
+
+ The state machine consists of 4 states. During each state, the center of
+ mass of the base link is moved first, then one of the legs will take a
+ step by following a planned elliptical trajectory. The legs will move in
+ the order of front left -> hind right -> front right -> hind left.
+ The state transition is determined by elapsed time since last transition.
+ Observation (16 dimensions):
+ [one hot vector of the state id, local toe positions in x and y direction]
+ Action (3 dimensions):
+ [target moving distance of the current moving leg in x direction,
+ target moving distance of base COM in x direction,
+ target moving distance of base COM in y direction]
+ """
+
+ def __init__(self,
+ gym_env,
+ default_local_toe_positions,
+ toe_link_indices=(3, 7, 11, 15),
+ foot_lift_height=0.15,
+ state_duration=2.0,
+ action_lower_bound=(-0.0, -0.25, -0.25),
+ action_upper_bound=(0.3, 0.25, 0.25),
+ state_to_foot_id=(0, 3, 2, 1)):
+ """Initialzes the wrapped env.
+
+ Args:
+ gym_env: An instance of LocomotionGymEnv.
+ default_local_toe_positions: A list of vectors that contains the default
+ local position of each toe.
+ toe_link_indices: A list of indices to the toe link. Used for calculating
+ local toe positions.
+ foot_lift_height: Specifies how high the foot lifts during swing stage.
+ state_duration: Specifies the duration of each state.
+ action_lower_bound: Lower bound for the actions.
+ action_upper_bound: Upper bound for the actions.
+ state_to_foot_id: Mapping from state machine state to foot id.
+ """
+ self._gym_env = gym_env
+
+ assert len(action_lower_bound) == ACTION_DIM_TOTAL
+ assert len(action_upper_bound) == ACTION_DIM_TOTAL
+ self.action_space = gym.spaces.Box(
+ np.array(action_lower_bound), np.array(action_upper_bound))
+ observation_lower_bound = np.array([-1.0] * OBSERVATION_DIM_TOTAL)
+ observation_upper_bound = np.array([1.0] * OBSERVATION_DIM_TOTAL)
+ self.observation_space = gym.spaces.Box(observation_lower_bound,
+ observation_upper_bound)
+ self._default_local_toe_positions = default_local_toe_positions
+ self._toe_link_indices = toe_link_indices
+ self._foot_lift_height = foot_lift_height
+ self._state_to_foot_id = state_to_foot_id
+
+ # Use the largest value for bounding the toe movement
+ self._toe_move_bound = np.max(
+ np.abs(np.concatenate([action_lower_bound, action_upper_bound])))
+
+ # Duration of each state
+ self.state_machine_state_duration = [state_duration] * int(
+ GaitStateMachine.TOTAL_GAIT_STATE_NUM)
+
+ def __getattr__(self, attr):
+ return getattr(self._gym_env, attr)
+
+ def _state_machine_observation(self):
+ """Get the current observation: [state_id, local_toe_positions]."""
+ observation = []
+ # One-hot vector for the current state machine state.
+ one_hot_foot_id = np.zeros(GaitStateMachine.TOTAL_GAIT_STATE_NUM)
+ one_hot_foot_id[self.current_state_machine_state] = 1
+ observation.extend(one_hot_foot_id)
+
+ # Toe positions in X and Y direction in the local space.
+ for toe_index in range(NUM_LEGS):
+ toe_pos_local = self.current_local_toe_positions[toe_index]
+ toe_pos_local_xy = [toe_pos_local[0], toe_pos_local[1]]
+ observation.extend(toe_pos_local_xy)
+
+ return observation
+
+ def _get_constant_accel_interpolation(self, interp_ratio):
+ """Modify an interpolation between 0 and 1 to have constant acceleration."""
+ assert interp_ratio <= 1.0 and interp_ratio >= 0.0
+ if interp_ratio < 0.5:
+ return 0.5 * (2 * interp_ratio)**2
+ else:
+ return -0.5 * (2 * interp_ratio - 2)**2 + 1
+
+ def _move_com(self, target_com_movement, time_since_transition,
+ state_duration):
+ """Get the ik action for moving the COM.
+
+ Args:
+ target_com_movement: Target COM movement relative to the previous COM
+ position in the x-y plane.
+ time_since_transition: Time elapsed since last state machien transition.
+ state_duration: Duration of the state machine. The first half will be used
+ for moving COM and the second half for moving the swing leg.
+
+ Returns:
+ ik_action: ik targets for moving the COM.
+ """
+ com_movement_ratio = np.clip(
+ time_since_transition / ((state_duration - 0.0) / 2.0), 0, 1)
+ com_movement_ratio = self._get_constant_accel_interpolation(
+ com_movement_ratio)
+ current_com_movement = np.array(target_com_movement) * com_movement_ratio
+
+ ik_action = []
+ for toe_index in range(NUM_LEGS):
+ toe_pos_local = np.copy(self.current_local_toe_positions[toe_index])
+ toe_pos_local[2] = self._default_local_toe_positions[toe_index][2]
+ toe_pos_local[0] -= current_com_movement[0]
+ toe_pos_local[1] -= current_com_movement[1]
+ toe_pos_local[0] = np.clip(
+ toe_pos_local[0], self._default_local_toe_positions[toe_index][0] -
+ self._toe_move_bound,
+ self._default_local_toe_positions[toe_index][0] +
+ self._toe_move_bound)
+ toe_pos_local[1] = np.clip(
+ toe_pos_local[1], self._default_local_toe_positions[toe_index][1] -
+ self._toe_move_bound,
+ self._default_local_toe_positions[toe_index][1] +
+ self._toe_move_bound)
+ ik_action.extend(toe_pos_local)
+
+ zero_translation = [0, 0, 0]
+ identity_rotation = [0, 0, 0, 1]
+ ik_action.extend(zero_translation)
+ ik_action.extend(identity_rotation)
+
+ return ik_action
+
+ def _move_leg(self, target_toe_movement, time_since_transition,
+ state_duration):
+ """Get the ik action for moving the swing leg.
+
+ Args:
+ target_toe_movement: Target toe movement relative to the default toe
+ position in the positive x direction.
+ time_since_transition: Time elapsed since last state machien transition.
+ state_duration: Duration of the state machine. The first half will be used
+ for moving COM and the second half for moving the swing leg.
+
+ Returns:
+ ik_action: ik targets for moving the swing leg.
+ """
+
+ # The target toe position at the end of the movement.
+ target_toe_local_position = np.array(self._default_local_toe_positions[
+ self._state_to_foot_id[self.current_state_machine_state]])
+ target_toe_local_position[0] += target_toe_movement
+
+ # Toe position at the beginning of the movement.
+ initial_toe_local_position = np.array(self.current_local_toe_positions[
+ self._state_to_foot_id[self.current_state_machine_state]])
+
+ # Auxiliary variables for computing the interpolation between the current
+ # toe position and the target toe position.
+ toe_circle_radius = 0.5 * np.linalg.norm(
+ np.array(target_toe_local_position) - initial_toe_local_position)
+ toe_moving_direction = np.array(
+ target_toe_local_position) - initial_toe_local_position
+ toe_moving_direction /= np.max([np.linalg.norm(toe_moving_direction), 1e-5])
+ toe_traj_scale_ratio = self._foot_lift_height / np.max(
+ [toe_circle_radius, 1e-5])
+
+ # Current percentage of time into the state machine.
+ toe_movement_ratio = np.clip(
+ (time_since_transition - state_duration / 2.0) /
+ ((state_duration - 0.0) / 2.0), 0, 1)
+ toe_movement_ratio = self._get_constant_accel_interpolation(
+ toe_movement_ratio)
+ toe_circle_moved_angle = np.pi * toe_movement_ratio
+
+ # Target horizontal movement from the previous local toe position.
+ target_toe_horizontal_movement = toe_circle_radius - np.cos(
+ toe_circle_moved_angle) * toe_circle_radius
+ current_toe_target = np.array([
+ target_toe_horizontal_movement * toe_moving_direction[0],
+ target_toe_horizontal_movement * toe_moving_direction[1],
+ np.sin(toe_circle_moved_angle) * toe_circle_radius *
+ toe_traj_scale_ratio
+ ]) + initial_toe_local_position
+
+ ik_action = []
+ for toe_index in range(NUM_LEGS):
+ toe_pos_local = np.copy(self.current_local_toe_positions[toe_index])
+ toe_pos_local[2] = self._default_local_toe_positions[toe_index][2]
+ if toe_index == self._state_to_foot_id[self.current_state_machine_state]:
+ toe_pos_local[0] = current_toe_target[0]
+ toe_pos_local[1] = current_toe_target[1]
+ toe_pos_local[2] = current_toe_target[2]
+
+ ik_action.extend(toe_pos_local)
+
+ zero_translation = [0, 0, 0]
+ identity_rotation = [0, 0, 0, 1]
+ ik_action.extend(zero_translation)
+ ik_action.extend(identity_rotation)
+ return ik_action
+
+ def _update_state_machine_transition(self):
+ """Update the state machine state if the duration has been reached."""
+ self.current_state_machine_state = (self.current_state_machine_state + 1
+ ) % GaitStateMachine.TOTAL_GAIT_STATE_NUM
+ self.last_state_transition_time = self.robot.GetTimeSinceReset()
+
+ def _update_local_toe_positions(self):
+ """Update the local position of the toes."""
+ identity_orientation = [0, 0, 0, 1]
+ base_position = self.robot.GetBasePosition()
+ base_orientation = self.robot.GetBaseOrientation()
+ inv_base_position, inv_base_orientation = self.pybullet_client.invertTransform(
+ base_position, base_orientation)
+ for toe_index in range(NUM_LEGS):
+ toe_pose_world = self.pybullet_client.getLinkState(
+ self.robot.quadruped, self._toe_link_indices[toe_index])[0]
+ toe_pos_local, _ = self.pybullet_client.multiplyTransforms(
+ inv_base_position, inv_base_orientation, toe_pose_world,
+ identity_orientation)
+ self.current_local_toe_positions[toe_index] = np.array(toe_pos_local)
+
+ def step(self, action):
+ """Steps the wrapped environment.
+
+ Args:
+ action: Numpy array. The input action from an NN agent.
+
+ Returns:
+ The tuple containing the modified observation, the reward, the epsiode end
+ indicator.
+
+ Raises:
+ ValueError if input action is None.
+
+ """
+ if action is None:
+ raise ValueError("Action cannot be None")
+ action = np.clip(action, self.action_space.low, self.action_space.high)
+ sum_reward = 0
+ step_num = 0
+ state_duration = self.state_machine_state_duration[
+ self.current_state_machine_state]
+ time_since_transition = self.robot.GetTimeSinceReset(
+ ) - self.last_state_transition_time
+
+ # Move COM
+ while time_since_transition < state_duration / 2.0:
+ ik_actions = self._move_com(action[1:3], time_since_transition,
+ state_duration)
+ _, reward, done, _ = self._gym_env.step(ik_actions)
+ sum_reward += reward
+ step_num += 1
+ time_since_transition = self.robot.GetTimeSinceReset(
+ ) - self.last_state_transition_time
+ if done:
+ break
+ self._update_local_toe_positions()
+ # Move Leg
+ while time_since_transition < state_duration:
+ ik_actions = self._move_leg(action[0], time_since_transition,
+ state_duration)
+ _, reward, done, _ = self._gym_env.step(ik_actions)
+ sum_reward += reward
+ step_num += 1
+ time_since_transition = self.robot.GetTimeSinceReset(
+ ) - self.last_state_transition_time
+ if done:
+ break
+ self._update_local_toe_positions()
+ self._update_state_machine_transition()
+
+ state_machine_observation = self._state_machine_observation()
+
+ return state_machine_observation, sum_reward, done, _
+
+ def reset(self):
+ """Reset the simulation and state machine states."""
+ self.current_state_machine_state = GaitStateMachine.STEP_LEFT_FRONT_TOE
+ self.last_state_transition_time = 0
+ self.current_local_toe_positions = np.copy(
+ self._default_local_toe_positions)
+
+ self._gym_env.reset()
+
+ state_machine_observation = self._state_machine_observation()
+
+ return state_machine_observation
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/step_based_curriculum_wrapper_env.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/step_based_curriculum_wrapper_env.py
new file mode 100644
index 000000000..e6296cd80
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/step_based_curriculum_wrapper_env.py
@@ -0,0 +1,231 @@
+"""A wrapped LocomotionGymEnv with functions that change the world and task."""
+
+import numbers
+from typing import Optional, Sequence, Text, Tuple, Union
+
+import numpy as np
+
+import gin
+
+
+@gin.configurable
+class CurriculumParameter(object):
+ """Definition of a env parameter tuned by a curriculum."""
+
+ def __init__(self,
+ name: Text,
+ init_val: numbers.Real,
+ bounds: Tuple[numbers.Real, numbers.Real]):
+ """A parameter to tune throughout the curriculum.
+
+ Args:
+ name: The name of the curriculum parameter. This must be the name of
+ attribute of the scene class.
+ init_val: The value to use at the start of the curriculum.
+ bounds: A tuple of [lower_bound, upper_bound] defining the minimum and
+ maximum values of the parameter.
+ """
+ self.name = name
+
+ if not isinstance(bounds[0], type(bounds[1])):
+ raise ValueError("All elements in [bounds] must be of the same type.")
+
+ if (init_val < min(bounds)) or (init_val > max(bounds)):
+ raise ValueError("Initial parameter value must lie in range defined"
+ " by [bounds].")
+
+ if not isinstance(init_val, type(bounds[0])):
+ raise ValueError("[init_val] type must match the type of the elements"
+ " in [bounds].")
+
+ self.init_val = init_val
+ self.bounds = bounds
+ self.dtype = type(init_val)
+
+
+@gin.configurable
+class RandomSamplingCurriculumParameter(CurriculumParameter):
+ """Env parameter whose value is sampled randomly without curriculum."""
+
+ def __init__(self, name: Text, bounds: Tuple[numbers.Real, numbers.Real]):
+ """A parameter whose value to sample randomly.
+
+ Args:
+ name: The name of the parameter.
+ bounds: A tuple of [lower_bound, upper_bound] defining the minimum and
+ maximum values of the parameter.
+ """
+ super(RandomSamplingCurriculumParameter, self).__init__(
+ name=name, init_val=bounds[0], bounds=bounds)
+
+ def sample(
+ self, step: numbers.Real,
+ curriculum_steps: Optional[numbers.Real] = None) -> Union[int, float]:
+ del step, curriculum_steps
+ sampled_val = np.random.uniform(*self.bounds)
+
+ if not isinstance(self.bounds[0], float):
+ sampled_val = int(round(sampled_val))
+
+ return sampled_val
+
+ def __call__(
+ self, step: numbers.Real,
+ curriculum_steps: Optional[numbers.Real] = None) -> Union[int, float]:
+ return self.sample(step, curriculum_steps)
+
+
+@gin.configurable
+class LinearStepBasedCurriculumParameter(CurriculumParameter):
+ """Definition of a env parameter tuned by a linear time-based curriculum."""
+
+ def __init__(self,
+ name: Text,
+ init_val: numbers.Real,
+ bounds: Tuple[numbers.Real, numbers.Real],
+ curriculum_steps: Optional[numbers.Real] = None):
+ """A parameter to tune throughout the curriculum.
+
+ Args:
+ name: The name of the curriculum parameter. This must be the name of
+ attribute of the scene class.
+ init_val: The value to use at the start of the curriculum.
+ bounds: A tuple of [lower_bound, upper_bound] defining the minimum and
+ maximum values of the parameter.
+ curriculum_steps: Integer defining the number of steps to take when
+ varying the curriculum parameter value from the init_val to either
+ bound. If None is specified, then the curriculum must provide the
+ curriculum_steps when sampling.
+ """
+ super(LinearStepBasedCurriculumParameter, self).__init__(
+ name=name, init_val=init_val, bounds=bounds)
+ self.curriculum_steps = curriculum_steps
+
+ def get_bounds_at_step(
+ self, step: numbers.Real,
+ curriculum_steps: Optional[numbers.Real] = None
+ ) -> Tuple[Union[int, float], Union[int, float]]:
+ """Compute the bounds of the parameter at the current step.
+
+ Args:
+ step: An integer defining the current timestep.
+ curriculum_steps: Optional curriculum steps. Must be passed if not passed
+ during initialization.
+ Returns:
+ A tuple containing the lower bound and upper bound at the current step.
+ """
+
+ if not self.curriculum_steps and not curriculum_steps:
+ raise ValueError("curriculum_steps not defined. Must be passed upon"
+ " initialization or must specified by curriculum"
+ " wrapper env on method call.")
+
+ if self.curriculum_steps:
+ curriculum_steps = self.curriculum_steps
+
+ prog = min(float(step) / curriculum_steps, 1.0)
+ curr_lower_bound = self.init_val - prog * (self.init_val - self.bounds[0])
+ curr_upper_bound = self.init_val + prog * (self.bounds[1] - self.init_val)
+
+ if not isinstance(self.bounds[0], float):
+ curr_lower_bound = int(round(curr_lower_bound))
+ curr_upper_bound = int(round(curr_upper_bound))
+
+ return (curr_lower_bound, curr_upper_bound)
+
+ def sample(
+ self, step: numbers.Real,
+ curriculum_steps: Optional[numbers.Real] = None) -> Union[int, float]:
+ sampled_val = np.random.uniform(*self.get_bounds_at_step(
+ step, curriculum_steps))
+
+ if not isinstance(self.bounds[0], float):
+ sampled_val = int(round(sampled_val))
+
+ return sampled_val
+
+ def __call__(
+ self, step: numbers.Real,
+ curriculum_steps: Optional[numbers.Real] = None) -> Union[int, float]:
+ return self.sample(step, curriculum_steps)
+
+
+@gin.configurable
+class Task(object):
+ """Defines a single task in the environment and its corresponding params."""
+
+ def __init__(self, name: Text,
+ curriculum_parameters: Sequence[CurriculumParameter]):
+ """Initialize the task.
+
+ Args:
+ name: The name of the task.
+ curriculum_parameters: A list of CurriculumParameter instances which
+ define the parameters that this task makes use of.
+ """
+ self.name = name
+ self.curriculum_parameters = curriculum_parameters
+
+
+@gin.configurable
+class StepBasedCurriculumWrapperEnv(object):
+ """A wrapper to tune the scene parameters linearly with the steps taken."""
+
+ def __init__(self, env, tasks: Sequence[Task],
+ default_curriculum_steps: Optional[numbers.Real] = None,
+ reset_total_step_count_val: numbers.Real = -1,
+ steps_before_curriculum_start: numbers.Real = 0):
+ """Initializes the linear curriculum wrapper env.
+
+ Args:
+ env: An instance of a (potentially previously wrapped) LocomotionGymEnv.
+ tasks: Various tasks to shuffle through throughout the curriculum.
+ default_curriculum_steps: Optional default value for curriculum steps.
+ reset_total_step_count_val: Step at which to reset the total step count.
+ The internal total_step_count is reset to 0 once this value is reached.
+ steps_before_curriculum_start: Steps to take in environment before the
+ curriculum begins.
+ """
+ self._gym_env = env
+ self._tasks = tasks
+ self._default_curriculum_steps = default_curriculum_steps
+ self._reset_total_step_count_val = reset_total_step_count_val
+ self._steps_before_curriculum_start = steps_before_curriculum_start
+
+ # Total number of environment steps.
+ self._total_step_count = 0
+
+ def __getattr__(self, attr):
+ return getattr(self._gym_env, attr)
+
+ def set_scene_params(self):
+ # Choose a task at random.
+ curr_task = np.random.choice(self._tasks)
+
+ # Cycle through the task's parameters and set them in the scene.
+ self._gym_env.scene.reset_scene_params()
+ self._gym_env.scene.scene_type = curr_task.name
+ for curriculum_parameter in curr_task.curriculum_parameters:
+ setattr(
+ self._gym_env.scene, curriculum_parameter.name,
+ curriculum_parameter(
+ self._total_step_count - self._steps_before_curriculum_start,
+ self._default_curriculum_steps))
+
+ def reset(self, *args, **kwargs):
+ """Reset and adjust the environment."""
+
+ # Update the total step count.
+ self._total_step_count += self._gym_env.env_step_counter
+ if self._reset_total_step_count_val >= 0:
+ if self._total_step_count >= self._reset_total_step_count_val:
+ self._total_step_count = 0
+
+ if self._total_step_count < self._steps_before_curriculum_start:
+ return self._get_observation()
+
+ self.set_scene_params()
+ self._gym_env.reset(*args, **kwargs)
+
+ return self._get_observation()
+
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/trajectory_generator_wrapper_env.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/trajectory_generator_wrapper_env.py
new file mode 100644
index 000000000..a2c6f0bf6
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/trajectory_generator_wrapper_env.py
@@ -0,0 +1,81 @@
+"""A wrapped MinitaurGymEnv with a built-in controller."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+
+import gin
+
+
+@gin.configurable
+class TrajectoryGeneratorWrapperEnv(object):
+ """A wrapped LocomotionGymEnv with a built-in trajectory generator."""
+
+ def __init__(self, gym_env, trajectory_generator):
+ """Initialzes the wrapped env.
+
+ Args:
+ gym_env: An instance of LocomotionGymEnv.
+ trajectory_generator: A trajectory_generator that can potentially modify
+ the action and observation. Typticall generators includes the PMTG and
+ openloop signals. Expected to have get_action and get_observation
+ interfaces.
+
+ Raises:
+ ValueError if the controller does not implement get_action and
+ get_observation.
+
+ """
+ self._gym_env = gym_env
+ if not hasattr(trajectory_generator, 'get_action') or not hasattr(
+ trajectory_generator, 'get_observation'):
+ raise ValueError(
+ 'The controller does not have the necessary interface(s) implemented.'
+ )
+
+ self._trajectory_generator = trajectory_generator
+
+ # The trajectory generator can subsume the action/observation space.
+ if hasattr(trajectory_generator, 'observation_space'):
+ self.observation_space = self._trajectory_generator.observation_space
+
+ if hasattr(trajectory_generator, 'action_space'):
+ self.action_space = self._trajectory_generator.action_space
+
+ def __getattr__(self, attr):
+ return getattr(self._gym_env, attr)
+
+ def _modify_observation(self, observation):
+ return self._trajectory_generator.get_observation(observation)
+
+ def reset(self, initial_motor_angles=None, reset_duration=1.0):
+ if getattr(self._trajectory_generator, 'reset'):
+ self._trajectory_generator.reset()
+ observation = self._gym_env.reset(initial_motor_angles, reset_duration)
+ return self._modify_observation(observation)
+
+ def step(self, action):
+ """Steps the wrapped environment.
+
+ Args:
+ action: Numpy array. The input action from an NN agent.
+
+ Returns:
+ The tuple containing the modified observation, the reward, the epsiode end
+ indicator.
+
+ Raises:
+ ValueError if input action is None.
+
+ """
+
+ if action is None:
+ raise ValueError('Action cannot be None')
+
+ new_action = self._trajectory_generator.get_action(
+ self._gym_env.robot.GetTimeSinceReset(), action)
+
+ original_observation, reward, done, _ = self._gym_env.step(new_action)
+
+ return self._modify_observation(original_observation), reward, done, _
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/walking_wrapper.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/walking_wrapper.py
new file mode 100644
index 000000000..6235a8536
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/walking_wrapper.py
@@ -0,0 +1,134 @@
+"""Wraps a quadruped walking controller for navigation control."""
+
+from typing import Any
+import gin
+from gym import spaces
+import numpy as np
+
+from pybullet_envs.minitaur.agents.baseline_controller import com_velocity_estimator
+from pybullet_envs.minitaur.agents.baseline_controller import locomotion_controller
+from pybullet_envs.minitaur.agents.baseline_controller import openloop_gait_generator
+from pybullet_envs.minitaur.agents.baseline_controller import raibert_swing_leg_controller
+from pybullet_envs.minitaur.agents.baseline_controller import torque_stance_leg_controller
+
+_N_LEGS = 4
+_STANCE_DURATION_SECONDS = [
+ 0.25
+] * _N_LEGS # The stance phase duration for each leg.
+_DUTY_FACTOR = [
+ 0.6
+] * _N_LEGS # Percentage of the leg in the stance phase within the cycle.
+_BODY_HEIGHT = 0.45
+_INIT_PHASE_FULL_CYCLE = [0, 0.5, 0.5, 0]
+
+
+def _setup_controller(robot: Any) -> locomotion_controller.LocomotionController:
+ """Creates the controller."""
+ desired_speed = (0, 0)
+ desired_twisting_speed = 0
+
+ gait_generator = openloop_gait_generator.OpenloopGaitGenerator(
+ robot,
+ stance_duration=_STANCE_DURATION_SECONDS,
+ duty_factor=_DUTY_FACTOR,
+ initial_leg_phase=_INIT_PHASE_FULL_CYCLE)
+ state_estimator = com_velocity_estimator.COMVelocityEstimator(robot)
+ sw_controller = raibert_swing_leg_controller.RaibertSwingLegController(
+ robot,
+ gait_generator,
+ state_estimator,
+ desired_speed=desired_speed,
+ desired_twisting_speed=desired_twisting_speed,
+ desired_height=_BODY_HEIGHT,
+ )
+ st_controller = torque_stance_leg_controller.TorqueStanceLegController(
+ robot,
+ gait_generator,
+ state_estimator,
+ desired_speed=desired_speed,
+ desired_twisting_speed=desired_twisting_speed,
+ desired_body_height=_BODY_HEIGHT,
+ body_mass=215 / 9.8,
+ body_inertia=(0.07335, 0, 0, 0, 0.25068, 0, 0, 0, 0.25447),
+ )
+
+ controller = locomotion_controller.LocomotionController(
+ robot=robot,
+ gait_generator=gait_generator,
+ state_estimator=state_estimator,
+ swing_leg_controller=sw_controller,
+ stance_leg_controller=st_controller,
+ clock=robot.GetTimeSinceReset)
+ return controller
+
+
+def _update_controller_params(
+ controller: locomotion_controller.LocomotionController,
+ lin_speed: np.ndarray, ang_speed: float):
+ """Apply the desired speed and twisting speed."""
+ controller.swing_leg_controller.desired_speed = lin_speed
+ controller.swing_leg_controller.desired_twisting_speed = ang_speed
+ controller.stance_leg_controller.desired_speed = lin_speed
+ controller.stance_leg_controller.desired_twisting_speed = ang_speed
+
+
+@gin.configurable
+class WalkingWrapper(object):
+ """Wraps a baseline walking controller for Laikago/Vision60."""
+
+ def __init__(self,
+ gym_env: Any,
+ action_repeat=20,
+ speed_bound=(-0.3, 0.3),
+ angular_speed_bound=(-0.3, 0.3)):
+ """Initialzes the wrapped env.
+
+ Args:
+ gym_env: An instance of LocomotionGymEnv.
+ action_repeat: Number of control steps to run low level controller with
+ the high level inputs per step().
+ speed_bound: The min/max of the input speed.
+ angular_speed_bound: The min/max of the twisting speed.
+ """
+ self._gym_env = gym_env
+ self._controller = _setup_controller(self._gym_env.robot)
+ self._action_repeat = action_repeat
+
+ action_low = np.array(speed_bound)
+ action_high = np.array(angular_speed_bound)
+
+ # Overwrite the action space.
+ self.action_space = spaces.Box(action_low, action_high)
+
+ def reset(self, *args, **kwargs) -> Any:
+ obs = self._gym_env.reset(*args, **kwargs)
+ # The robot instance might have been replaced if hard_reset is called. We
+ # just recreate the controller.
+ self._controller = _setup_controller(self._gym_env.robot)
+ self._controller.reset()
+ return obs
+
+ def __getattr__(self, attr):
+ return getattr(self._gym_env, attr)
+
+ def step(self, action) -> Any:
+ """Steps the wrapped env with high level commands.
+
+ Args:
+ action: A high level command containing the desired linear and angular
+ speed of the robot base. The speed can be adjusted at any time.
+
+ Returns:
+ The gym env observation, reward, termination, and additional info.
+
+ """
+ lin_speed = np.array((action[0], 0, 0))
+ ang_speed = action[1]
+ _update_controller_params(self._controller, lin_speed, ang_speed)
+ for _ in range(self._action_repeat):
+ self._controller.update()
+ hybrid_action = self._controller.get_action()
+ obs, reward, done, info = self._gym_env.step(hybrid_action)
+ if done:
+ break
+ return obs, reward, done, info
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/evaluation/__init__.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/evaluation/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/evaluation/metric.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/evaluation/metric.py
new file mode 100644
index 000000000..00dea1800
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/evaluation/metric.py
@@ -0,0 +1,125 @@
+"""The metric reporting system used in our env.
+
+In the gym environment, there are many variables or quantities that can help
+researchers to debug, evaluate policy performance. Such quantities may
+include: Motor torques for quadrupeds, even when they are controlled in
+POSITION mode; Distance to walls while a wheeled robot is navigating the
+indoor environment. Often, these metrics are private variables (or can only be
+computed from private variables). To expose the user interested metrics
+outside the environment's observations, we designed this Metric system that
+can be invoked in any modules (robot, tasks, sensors) inside the gym env.
+"""
+
+import enum
+import logging
+from typing import Any, Callable, Dict, Sequence, Text
+import gin
+
+
+@gin.constants_from_enum
+class MetricScope(enum.Enum):
+ """The supported scope of metrics."""
+ # The performance metrics.
+ PERFORMANCE = 1,
+
+ # The debug metrics for diagnostic purposes.
+ DEBUG = 2,
+
+ # The safety metrics.
+ SAFETY = 3,
+
+ # The statistics of episodes in metric format.
+ STATISTIC = 4,
+
+
+class MetricCore(object):
+ """Aggregates values of interest to compute statistics."""
+
+ def __init__(
+ self,
+ name: Text,
+ scope: MetricScope,
+ single_ep_aggregator: Callable[[Sequence[Any]], Any],
+ multi_ep_aggregator: Callable[[Sequence[Any]], Dict[Text, Any]],
+ ):
+ """Initializes the class.
+
+ Args:
+ name: The name of the metric, for example "motor_torques",
+ "distance_to_wall", etc. The full name of the metric will have scope
+ name in the prefix, i.e. "scope/name".
+ scope: The scope of this metric. Most metric should be for DEBUG purpose.
+ The scope name will be added to the final name of metric in this way:
+ "scope/name", which is standarded format for Tensorboard to group
+ named variables.
+ single_ep_aggregator: The function to process all aggregated metric
+ values. The derived MetricReporter (see below) will implements
+ reset_episode() which clears the episode data, and will be called during
+ env.reset().
+ multi_ep_aggregator: The functions to process multi-episode metric values.
+ We assume the inputs to the functions is a list of per episode metric
+ values, i.e. each element of the list is the output from the
+ single_ep_aggregator.
+ """
+ self._name = scope.name + "/" + name
+ self._single_ep_aggregator = single_ep_aggregator
+ self._multi_ep_aggregator = multi_ep_aggregator
+ self._episode_data = []
+
+ def report(self, metric_value: Any):
+ """Stores the reported metric in the internal buffer.
+
+ Args:
+ metric_value: The metric we are interested to report.
+ """
+ self._episode_data.append(metric_value)
+
+
+class MetricReporter(MetricCore):
+ """Reports the metric using the provided aggregator functions."""
+
+ def get_episode_metric(self) -> Dict[Text, Sequence[Any]]:
+ """Processes and returns episode metric values.
+
+ Returns:
+ Aggregated metrics for the current episode.
+ """
+ if self._episode_data:
+ return {self._name: self._single_ep_aggregator(self._episode_data)}
+ else:
+ return {}
+
+ def get_multi_ep_metric(
+ self, episodic_metric: Dict[Text, Sequence[Any]]) -> Dict[Text, Any]:
+ """Processes the aggregated metrics over many episodes.
+
+ Will not be affected by reset_episode, since we take multi-episode data as
+ inputs.
+
+ Args:
+ episodic_metric: The per episode metrics. We expect the inputs to contain
+ the same key as self._name, and that the value is a list of metric
+ values computed using self.get_episode_metrc().
+
+ Returns:
+ The processed multi-episode metrics.
+ """
+ if self._name not in episodic_metric:
+ logging.warning(
+ "The inputs does not contain the key for the current metric: %s",
+ self._name)
+ return {}
+ outputs = {}
+ for key, val in self._multi_ep_aggregator(
+ episodic_metric[self._name]).items():
+ outputs[self._name + "_" + key] = val
+ return outputs
+
+ def reset_episode(self):
+ """Clears the episode data stored.
+
+ Will be invoked during env.reset(). This will effect how get_episode_metric
+ gets computed.
+
+ """
+ self._episode_data = []
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/evaluation/metric_logger.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/evaluation/metric_logger.py
new file mode 100644
index 000000000..05d24b4e1
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/evaluation/metric_logger.py
@@ -0,0 +1,155 @@
+# Lint as: python3
+"""The system to log and manage all metrics."""
+
+import logging
+from typing import Any, Callable, Dict, Sequence, Text, Union
+import numpy as np
+
+from pybullet_envs.minitaur.envs_v2.evaluation import metric as metric_lib
+
+
+def _merge_dict_throw_on_duplicates(
+ dict_1: Dict[Text, Any],
+ dict_2: Dict[Text, Any],
+):
+ """Merge the contents of dict_2 to dict_1.
+
+ Args:
+ dict_1: The dictionary to merge into.
+ dict_2: The dictionary to merge from.
+
+ Raises:
+ KeyError: if duplicated keys are found.
+ """
+ for key, val in dict_2.items():
+ if key in dict_1:
+ raise KeyError(f"Duplicate key: {key} found in both "
+ f"dictionaries: {dict_1} {dict_2}.")
+ dict_1[key] = val
+
+
+def common_stats(x: Sequence[Union[float, Sequence[float]]],
+ flatten_array=False):
+ out = x
+ if flatten_array:
+ # Deals with array of arrays.
+ out = np.concatenate(x).flatten()
+ return {
+ "mean": np.mean(out),
+ "max": np.max(out),
+ "min": np.min(out),
+ "std": np.std(out),
+ }
+
+
+class MetricLogger(object):
+ """The central system to manage all metrics."""
+
+ def __init__(self, ignore_duplicate_metrics: bool = True):
+ """Initializes the system.
+
+ Args:
+ ignore_duplicate_metrics: Don't throw error when users want to recreate
+ the same metric.
+ """
+ self._metric_reporters = {}
+ self._ignore_duplicate_metrics = ignore_duplicate_metrics
+
+ def reset_episode(self):
+ """Resets all metric reporters's internal buffer.
+
+ Will be called by the gym env during reset.
+ """
+ for metric in self._metric_reporters.values():
+ metric.reset_episode()
+
+ def create_metric(
+ self,
+ name: Text,
+ scope: metric_lib.MetricScope,
+ single_ep_aggregator: Callable[[Sequence[Any]], Any],
+ multi_ep_aggregator: Callable[[Sequence[Any]], Dict[Text, Any]],
+ ) -> metric_lib.MetricCore:
+ """Creates a new metric.
+
+ Args:
+ name: The name of the metric, for example "motor_torques",
+ "distance_to_wall", etc. The full name of the metric will have scope
+ name in the prefix, i.e. "scope/name".
+ scope: The scope of this metric. Most metric should be for DEBUG purpose.
+ The scope name will be added to the final name of metric in this way:
+ "scope/name", which is standarded format for Tensorboard to group
+ named variables.
+ single_ep_aggregator: The function to process all aggregated metric
+ values. The derived MetricReporter (see below) will implements
+ reset_episode() which clears the episode data, and will be called during
+ env.reset().
+ multi_ep_aggregator: The function to process multi-episode metric values.
+ We assume the inputs to the function is a list of per episode metric
+ values, i.e. each element of the list is the output from the
+ single_ep_aggregator.
+
+ Returns:
+ A MetricCore which can be used to report values of interest.
+ """
+ if name in self._metric_reporters:
+ if self._ignore_duplicate_metrics:
+ logging.warning("Trying to create an existing metric: %s", name)
+ name = self._get_valid_duplicate_name(name)
+ logging.warning("Changed duplicate metric to new name: %s", name)
+ else:
+ raise ValueError(f"Duplicated metrics found: {name}")
+
+ self._metric_reporters[name] = metric_lib.MetricReporter(
+ name, scope, single_ep_aggregator, multi_ep_aggregator)
+ return self._metric_reporters[name]
+
+ def create_scalar_metric(
+ self,
+ name: Text,
+ scope: metric_lib.MetricScope,
+ single_ep_aggregator: Callable[[Sequence[Any]], Any],
+ ) -> metric_lib.MetricCore:
+ """Shortcut to create a metric for scalar variables."""
+ return self.create_metric(
+ name,
+ scope,
+ single_ep_aggregator,
+ multi_ep_aggregator=common_stats,
+ )
+
+ def _get_valid_duplicate_name(self, original_name: Text) -> Text:
+ counter = 1
+ test_name = "{}_duplicate_{}".format(original_name, str(counter))
+ while test_name in self._metric_reporters:
+ counter += 1
+ test_name = "{}_duplicate_{}".format(original_name, str(counter))
+ return test_name
+
+ def get_episode_metrics(self) -> Dict[Text, Any]:
+ """Return all metrics registered in the logger for the current episode."""
+ ep_stats = {}
+ for metric in self._metric_reporters.values():
+ _merge_dict_throw_on_duplicates(ep_stats, metric.get_episode_metric())
+ return ep_stats
+
+ def get_multi_episode_metrics(
+ self, episodic_metrics: Dict[Text, Sequence[Any]]) -> Dict[Text, Any]:
+ """Processes the aggregated metrics over many episodes.
+
+ Will not be affected by reset_episode, since we take multi-episode data as
+ inputs.
+
+ Args:
+ episodic_metrics: The per episode metrics. For each key in the inputs, we
+ expect at least one metric reporter can process the corresponding value,
+ which contains a list of episodic metrics.
+
+ Returns:
+ The processed multi-episode metrics.
+ """
+ stats = {}
+ for metric in self._metric_reporters.values():
+ _merge_dict_throw_on_duplicates(
+ stats, metric.get_multi_ep_metric(episodic_metrics))
+ return stats
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/evaluation/metric_utils.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/evaluation/metric_utils.py
new file mode 100644
index 000000000..1defba977
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/evaluation/metric_utils.py
@@ -0,0 +1,29 @@
+"""Helper class and functions to make computing meteric statistics easier."""
+from typing import Any, Sequence
+
+import numpy as np
+
+
+class MetricStats(object):
+ """Helper class to make computing meteric statistics easier to manage."""
+
+ def __init__(self, data: Sequence[Any]):
+ if None or not list(data):
+ raise ValueError("Input data for ComputeMetricStats cannot be empty.")
+ self._data = np.asarray(data).flatten()
+
+ @property
+ def avg(self):
+ return np.mean(self._data)
+
+ @property
+ def min(self):
+ return np.min(self._data)
+
+ @property
+ def max(self):
+ return np.max(self._data)
+
+ @property
+ def sum(self):
+ return np.sum(self._data)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/examples/__init__.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/examples/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/examples/laikago_mpc_wrapper_example.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/examples/laikago_mpc_wrapper_example.py
new file mode 100644
index 000000000..ad41f5633
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/examples/laikago_mpc_wrapper_example.py
@@ -0,0 +1,96 @@
+# Lint as: python3
+r"""An example where Laikago walks forward using mpc controller.
+
+This script illustrates an example of MPCLocomotionWrapper class for
+controlling a simulated Laikago robot to walk on a flat ground. The wrapped
+environment takes as input the target local foothold locations and the desired
+base pose. An MPC-based controller is executed internally to compute the
+required forces to achieve the desired foothold position and base movement.
+
+
+blaze run -c opt \
+//robotics/reinforcement_learning/minitaur/envs_v2/examples\
+:laikago_mpc_wrapper_example
+"""
+import os
+import tempfile
+
+from absl import app
+from absl import flags
+import gin
+import numpy as np
+import pybullet_data as pd
+
+from pybullet_envs.minitaur.envs_v2 import env_loader
+
+FLAGS = flags.FLAGS
+flags.DEFINE_string("video_file", None, "The filename for saving the videos.")
+
+CONFIG_FILE_SIM = (pd.getDataPath()+"/configs/laikago_mpc_example_flat.gin")
+
+NUM_STEPS = 100
+ENABLE_RENDERING = True # Will be disabled for tests
+ENV_RANDOM_SEED = 100
+DEFAULT_TARGET_FOOTHOLD = (0.05, 0.0, -0.01)
+DEFAULT_BASE_VELOCITY = (0.0, 0.0)
+DEFAULT_TWIST_SPEED = 0.0
+DEFAULT_BODY_HEIGHT = 0.45
+DEFAULT_ROLL_PITCH = (0.0, 0.0)
+DEFAULT_SWING_HEIGHT = 0.07
+
+
+def _build_env():
+ """Builds the environment for the Laikago robot.
+
+ Returns:
+ The OpenAI gym environment.
+ """
+ gin.parse_config_file(CONFIG_FILE_SIM)
+ gin.bind_parameter("SimulationParameters.enable_rendering", ENABLE_RENDERING)
+ env = env_loader.load()
+ env.seed(ENV_RANDOM_SEED)
+
+ return env
+
+
+def _run_example():
+ """An example that Laikago moves with a constant speed and predicts foothold.
+
+ Returns:
+ env: the environment after the simulation
+ """
+
+ env = _build_env()
+
+ env.reset()
+ if FLAGS.video_file is not None:
+ pybullet = env.pybullet_client
+ pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0)
+ log_id = pybullet.startStateLogging(pybullet.STATE_LOGGING_VIDEO_MP4,
+ FLAGS.video_file)
+
+ try:
+ max_step = NUM_STEPS
+ for _ in range(max_step):
+ target_foothold = np.array(DEFAULT_TARGET_FOOTHOLD)
+ action = np.concatenate([
+ target_foothold, target_foothold, target_foothold, target_foothold,
+ [
+ DEFAULT_SWING_HEIGHT, DEFAULT_SWING_HEIGHT, DEFAULT_SWING_HEIGHT,
+ DEFAULT_SWING_HEIGHT
+ ], DEFAULT_BASE_VELOCITY, [DEFAULT_TWIST_SPEED],
+ [DEFAULT_BODY_HEIGHT], DEFAULT_ROLL_PITCH
+ ])
+ _ = env.step(action)
+ finally:
+ if FLAGS.video_dir is not None:
+ pybullet.stopStateLogging(log_id)
+
+
+def main(argv):
+ del argv
+ _run_example()
+
+
+if __name__ == "__main__":
+ app.run(main)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/examples/laikago_pmtg_example.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/examples/laikago_pmtg_example.py
new file mode 100644
index 000000000..c86c10db4
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/examples/laikago_pmtg_example.py
@@ -0,0 +1,84 @@
+r"""An example to run an OpenAI gym environment with laikago and PMTG wrapper.
+
+This is an open-loop controller where we use 0 residuals and only execute the
+trajectory generator. The parameters that are (normally modulated by the policy)
+are fixed (except the intensity) and not optimized. They are hand picked as
+follows:
+ - The gait cycle frequency is 3 Hz.
+ - Walking height is neutral (0).
+ - Swing vs stance ratio is 2 (swing takes half the time vs stance).
+ - Intensity starts from zero and is gradually increased over time.
+
+blaze run -c opt \
+//robotics/reinforcement_learning/minitaur/envs_v2/examples\
+:laikago_pmtg_example
+"""
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import os
+
+import gin
+import tensorflow.compat.v1 as tf
+from pybullet_envs.minitaur.envs_v2 import env_loader
+import pybullet_data as pd
+
+CONFIG_DIR = pd.getDataPath()+"/configs_v2/"
+CONFIG_FILES = [
+ os.path.join(CONFIG_DIR, "base/laikago_with_imu.gin"),
+ os.path.join(CONFIG_DIR, "tasks/fwd_task_no_termination.gin"),
+ os.path.join(CONFIG_DIR, "wrappers/pmtg_wrapper.gin"),
+ os.path.join(CONFIG_DIR, "scenes/simple_scene.gin")
+]
+# Constants used to show example PMTG behavior with zero residual.
+# Since we use the default PMTG configuration, there is only one trajectory
+# generator. So the parameters that can be changed are:
+# - Multiplier for delta time (similar to frequency but per time step).
+# - Intensity of the trajectory generator.
+# - Walking heights used for the legs.
+# - The ratio of the speed of the leg during swing vs stance phase.
+# For more details, check out TgSimple._process_tg_params method.
+_PMTG_DELTA_TIME_MULTIPLIER = 2.0
+_PMTG_INTENSITY_RANGE = (0.0, 1.5)
+_PMTG_INTENSITY_STEP_SIZE = 0.0001
+_PMTG_WALKING_HEIGHT = -0.3
+_PMTG_SWING_VS_STANCE = 2
+_NUM_MOTORS = 12
+
+
+def main(argv):
+ del argv # Unused.
+
+ # Parse the gym config and create the environment.
+ for gin_file in CONFIG_FILES:
+ gin.parse_config_file(gin_file)
+ gin.bind_parameter("SimulationParameters.enable_rendering", True)
+ gin.bind_parameter("terminal_conditions.maxstep_terminal_condition.max_step",
+ 10000)
+ env = env_loader.load()
+ tg_intensity = _PMTG_INTENSITY_RANGE[0]
+ sum_reward = 0
+ env.reset()
+ done = False
+ # Use zero residual, only use the output of the trajectory generator.
+ residual = [0] * _NUM_MOTORS
+ # Since we fix residuals and all the parameters of the TG, this example
+ # is practically an open loop controller. A learned policy would provide
+ # different values for these parameters at every timestep.
+ while not done:
+ # Increase the intensity of the trajectory generator gradually
+ # to illustrate increasingly larger steps.
+ if tg_intensity < _PMTG_INTENSITY_RANGE[1]:
+ tg_intensity += _PMTG_INTENSITY_STEP_SIZE
+ tg_params = [
+ _PMTG_DELTA_TIME_MULTIPLIER, tg_intensity, _PMTG_WALKING_HEIGHT,
+ _PMTG_SWING_VS_STANCE
+ ]
+ action = residual + tg_params
+ _, reward, done, _ = env.step(action)
+ sum_reward += reward
+
+
+if __name__ == "__main__":
+ tf.app.run(main)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/examples/laikago_static_gait_example.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/examples/laikago_static_gait_example.py
new file mode 100644
index 000000000..13e2387cc
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/examples/laikago_static_gait_example.py
@@ -0,0 +1,52 @@
+# Lint as: python3
+r"""An example that the Laikago walks forward using a static gait.
+
+blaze run -c opt //robotics/reinforcement_learning/minitaur/envs_v2/examples:\
+laikago_static_gait_example
+"""
+from absl import app
+from absl import flags
+import gin
+from pybullet_envs.minitaur.agents.baseline_controller import static_gait_controller
+from pybullet_envs.minitaur.envs_v2 import env_loader
+import pybullet_data as pd
+
+flags.DEFINE_bool("render", True, "Whether to render the example.")
+
+FLAGS = flags.FLAGS
+_CONFIG_FILE = pd.getDataPath()+"/configs/laikago_walk_static_gait.gin"
+_NUM_STEPS = 10000
+_ENV_RANDOM_SEED = 13
+
+
+def _load_config(render=False):
+ gin.parse_config_file(_CONFIG_FILE)
+ gin.bind_parameter("SimulationParameters.enable_rendering", render)
+
+
+def run_example(num_max_steps=_NUM_STEPS):
+ """Runs the example.
+
+ Args:
+ num_max_steps: Maximum number of steps this example should run for.
+ """
+ env = env_loader.load()
+
+ env.seed(_ENV_RANDOM_SEED)
+ observation = env.reset()
+ policy = static_gait_controller.StaticGaitController(env.robot)
+
+ for _ in range(num_max_steps):
+ action = policy.act(observation)
+ _, _, done, _ = env.step(action)
+ if done:
+ break
+
+
+def main(_):
+ _load_config(FLAGS.render)
+ run_example()
+
+
+if __name__ == "__main__":
+ app.run(main)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/locomotion_gym_config.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/locomotion_gym_config.py
new file mode 100644
index 000000000..7b9ad955a
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/locomotion_gym_config.py
@@ -0,0 +1,58 @@
+"""A gin-config class for locomotion_gym_env.
+
+This should be identical to locomotion_gym_config.proto.
+"""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+from typing import Sequence, Text
+
+import attr
+import gin
+
+
+@gin.configurable
+@attr.s
+class SimulationParameters(object):
+ """Parameters specific for the pyBullet simulation."""
+ sim_time_step_s = attr.ib(type=float, default=0.002)
+ num_action_repeat = attr.ib(type=int, default=5)
+ enable_hard_reset = attr.ib(type=bool, default=False)
+ enable_rendering = attr.ib(type=bool, default=False)
+ enable_rendering_gui = attr.ib(type=bool, default=True)
+ robot_on_rack = attr.ib(type=bool, default=False)
+ camera_target = attr.ib(type=Sequence[float], default=None)
+ camera_distance = attr.ib(type=float, default=1.0)
+ camera_yaw = attr.ib(type=float, default=0)
+ camera_pitch = attr.ib(type=float, default=-30)
+ render_width = attr.ib(type=int, default=480)
+ render_height = attr.ib(type=int, default=360)
+ egl_rendering = attr.ib(type=bool, default=False)
+
+
+@gin.configurable
+@attr.s
+class ScalarField(object):
+ """A named scalar space with bounds."""
+ # TODO(sehoonha) extension to vector fields.
+ name = attr.ib(type=str)
+ upper_bound = attr.ib(type=float)
+ lower_bound = attr.ib(type=float)
+
+
+@gin.configurable
+@attr.s
+class LocomotionGymConfig(object):
+ """Grouped Config Parameters for LocomotionGym."""
+ simulation_parameters = attr.ib(type=SimulationParameters)
+ # TODO(sehoonha) implement attr validators for the list
+ actions = attr.ib(type=list, default=None) # pylint: disable=g-bare-generic
+ log_path = attr.ib(type=Text, default=None)
+ data_dir = attr.ib(
+ type=Text,
+ default='robotics/reinforcement_learning/minitaur/data/')
+ profiling_path = attr.ib(type=Text, default=None)
+ seed = attr.ib(type=int, default=None)
+ ignored_sensor_list = attr.ib(type=Sequence[Text], default=())
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/locomotion_gym_config_test.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/locomotion_gym_config_test.py
new file mode 100644
index 000000000..fc53f02e7
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/locomotion_gym_config_test.py
@@ -0,0 +1,75 @@
+"""Tests for pybullet_envs.minitaur.envs.locomotion_gym_config."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import gin
+from pybullet_envs.minitaur.envs_v2 import locomotion_gym_config
+import tensorflow.compat.v1 as tf
+from absl.testing import parameterized
+
+
+
+class LocomotionGymConfigTest(tf.test.TestCase, parameterized.TestCase):
+
+ def testSimulationParametersFromGinString(self):
+ config_text = (
+ 'import pybullet_envs.minitaur'
+ '.envs_v2.locomotion_gym_config\n'
+ 'locomotion_gym_config.SimulationParameters.sim_time_step_s = 0.005\n'
+ 'locomotion_gym_config.SimulationParameters.camera_distance = 5.0\n'
+ 'locomotion_gym_config.SimulationParameters.camera_yaw = 10\n'
+ 'locomotion_gym_config.SimulationParameters.camera_pitch = -50\n'
+ )
+ gin.parse_config(config_text)
+
+ cfg = locomotion_gym_config.SimulationParameters()
+ self.assertEqual(cfg.sim_time_step_s, 0.005)
+ self.assertFalse(cfg.enable_hard_reset)
+ self.assertEqual(cfg.camera_distance, 5.0)
+ self.assertEqual(cfg.camera_yaw, 10)
+ self.assertEqual(cfg.camera_pitch, -50)
+
+ def testScalarFieldFromGinString(self):
+ config_text = (
+ 'import pybullet_envs.minitaur'
+ '.envs_v2.locomotion_gym_config\n'
+ 'locomotion_gym_config.ScalarField.name = "MotorUpperLimit"\n'
+ 'locomotion_gym_config.ScalarField.upper_bound = 1.0\n'
+ 'locomotion_gym_config.ScalarField.lower_bound = -1.0\n'
+ )
+ gin.parse_config(config_text)
+
+ cfg = locomotion_gym_config.ScalarField()
+ self.assertEqual(cfg.name, 'MotorUpperLimit')
+ self.assertEqual(cfg.upper_bound, 1.0)
+ self.assertEqual(cfg.lower_bound, -1.0)
+
+ def testLocomotionGymConfigFromGinString(self):
+ config_text = (
+ 'import pybullet_envs.minitaur'
+ '.envs_v2.locomotion_gym_config\n'
+ # SimulationParameters
+ 'locomotion_gym_config.SimulationParameters.sim_time_step_s = 0.005\n'
+ # Actions
+ 'Motor/locomotion_gym_config.ScalarField.name = "MotorUpperLimit"\n'
+ 'Motor/locomotion_gym_config.ScalarField.upper_bound = 1.0\n'
+ 'Motor/locomotion_gym_config.ScalarField.lower_bound = -1.0\n'
+ # LocomotionGymConfigs
+ 'locomotion_gym_config.LocomotionGymConfig.simulation_parameters = '
+ '@locomotion_gym_config.SimulationParameters()\n'
+ 'locomotion_gym_config.LocomotionGymConfig.actions = ['
+ '@Motor/locomotion_gym_config.ScalarField()]\n'
+ 'locomotion_gym_config.LocomotionGymConfig.ignored_sensor_list = ['
+ '"Collisions"]\n')
+ gin.parse_config(config_text)
+
+ cfg = locomotion_gym_config.LocomotionGymConfig()
+ self.assertEqual(cfg.simulation_parameters.sim_time_step_s, 0.005)
+ self.assertEqual(cfg.actions[0].upper_bound, 1.0)
+ self.assertEqual(cfg.ignored_sensor_list, ['Collisions'])
+
+
+if __name__ == '__main__':
+ tf.test.main()
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/locomotion_gym_env.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/locomotion_gym_env.py
new file mode 100644
index 000000000..0c30e5b8e
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/locomotion_gym_env.py
@@ -0,0 +1,731 @@
+# Lint as: python3
+"""This file implements the locomotion gym env."""
+
+import atexit
+import collections
+import time
+from typing import Any, Callable, Sequence, Text, Union
+import gin
+import gym
+import numpy as np
+
+from pybullet_envs.minitaur.envs_v2 import base_client
+from pybullet_utils import bullet_client
+import pybullet_data
+import pybullet
+from pybullet_envs.minitaur.envs import minitaur_logging
+from pybullet_envs.minitaur.envs import minitaur_logging_pb2
+#from pybullet_envs.minitaur.envs import minitaur_logging
+#from pybullet_envs.minitaur.envs import minitaur_logging_pb2
+from pybullet_envs.minitaur.envs_v2.evaluation import metric_logger
+from pybullet_envs.minitaur.envs_v2.scenes import scene_base
+from pybullet_envs.minitaur.envs_v2.sensors import sensor
+from pybullet_envs.minitaur.envs_v2.sensors import space_utils
+from pybullet_envs.minitaur.envs_v2.utilities import rendering_utils
+from pybullet_envs.minitaur.robots import autonomous_object
+from pybullet_envs.minitaur.robots import robot_base
+
+_ACTION_EPS = 0.01
+_NUM_SIMULATION_ITERATION_STEPS = 300
+_LOG_BUFFER_LENGTH = 5000
+
+SIM_CLOCK = 'SIM_CLOCK'
+
+# Exports this symbol so we can use it in the config file.
+gin.constant('locomotion_gym_env.SIM_CLOCK', SIM_CLOCK)
+
+# This allows us to bind @time.time in the gin configuration.
+gin.external_configurable(time.time, module='time')
+
+
+# TODO(b/122048194): Enable position/torque/hybrid control mode.
+@gin.configurable
+class LocomotionGymEnv(gym.Env):
+ """The gym environment for the locomotion tasks."""
+ metadata = {
+ 'render.modes': ['human', 'rgb_array', 'topdown'],
+ 'video.frames_per_second': 100
+ }
+
+ def __init__(self,
+ gym_config,
+ clock: Union[Callable[..., float], Text] = 'SIM_CLOCK',
+ robot_class: Any = None,
+ scene: scene_base.SceneBase = None,
+ sensors: Sequence[sensor.Sensor] = None,
+ task: Any = None,
+ env_randomizers: Any = None):
+ """Initializes the locomotion gym environment.
+
+ Args:
+ gym_config: An instance of LocomotionGymConfig.
+ clock: The clock source to be used for the gym env. The clock should
+ return a timestamp in seconds. Setting clock == "SIM_CLOCK" will enable
+ the built-in simulation clock. For real robot experiments, we can use
+ time.time or other clock wall clock sources.
+ robot_class: A class of a robot. We provide a class rather than an
+ instance due to hard_reset functionality. Parameters are expected to be
+ configured with gin.
+ scene: An object for managing the robot's surroundings.
+ sensors: A list of environmental sensors for observation.
+ task: A callable function/class to calculate the reward and termination
+ condition. Takes the gym env as the argument when calling.
+ env_randomizers: A list of EnvRandomizer(s). An EnvRandomizer may
+ randomize the physical property of minitaur, change the terrrain during
+ reset(), or add perturbation forces during step().
+ client_factory: A function to create a simulation client, it can be a
+ pybullet client.
+
+ Raises:
+ ValueError: If the num_action_repeat is less than 1.
+
+ """
+ self._pybullet_client = None
+ self._metric_logger = metric_logger.MetricLogger()
+ # TODO(sehoonha) split observation and full-state sensors (b/129858214)
+
+ # Makes sure that close() is always called to flush out the logs to the
+ # disk.
+ atexit.register(self.close)
+ self.seed()
+ self._gym_config = gym_config
+ if robot_class is None:
+ raise ValueError('robot_class cannot be None.')
+ self._robot_class = robot_class
+ if issubclass(self._robot_class, robot_base.RobotBase):
+ self._use_new_robot_class = True
+ else:
+ self._use_new_robot_class = False
+ self._robot = None
+
+ self._scene = scene or scene_base.SceneBase()
+
+ # TODO(sehoonha) change the data structure to dictionary
+ self._env_sensors = list(sensors) if sensors is not None else list()
+
+ # TODO(b/152161457): Make logging a standalone module.
+ self._log_path = gym_config.log_path
+ self._logging = minitaur_logging.MinitaurLogging(self._log_path)
+ self._episode_proto = minitaur_logging_pb2.MinitaurEpisode()
+ self._data_dir = gym_config.data_dir
+
+ self._task = task
+
+ self._env_randomizers = env_randomizers if env_randomizers else []
+
+ # Simulation related parameters.
+ self._num_action_repeat = gym_config.simulation_parameters.num_action_repeat
+ self._on_rack = gym_config.simulation_parameters.robot_on_rack
+ if self._num_action_repeat < 1:
+ raise ValueError('number of action repeats should be at least 1.')
+ # TODO(b/73829334): Fix the value of self._num_bullet_solver_iterations.
+ self._num_bullet_solver_iterations = int(_NUM_SIMULATION_ITERATION_STEPS /
+ self._num_action_repeat)
+
+ self._sim_time_step = gym_config.simulation_parameters.sim_time_step_s
+ # The sim step counter is an internal varialbe to count the number of
+ # pybullet stepSimulation() has been called since last reset.
+ self._sim_step_counter = 0
+
+ self._env_time_step = self._num_action_repeat * self._sim_time_step
+ # The env step counter accounts for how many times env.step has been
+ # called since reset.
+ self._env_step_counter = 0
+
+ if clock == SIM_CLOCK:
+ self._clock = self._get_sim_time
+ else:
+ self._clock = clock
+
+ # Creates the bullet client.
+ self._is_render = gym_config.simulation_parameters.enable_rendering
+ # The wall-clock time at which the last frame is rendered.
+ self._last_frame_time = 0.0
+
+ if gym_config.simulation_parameters.enable_rendering:
+ self._pybullet_client = bullet_client.BulletClient(connection_mode=pybullet.GUI)
+ self._pybullet_client.configureDebugVisualizer(
+ pybullet.COV_ENABLE_GUI,
+ gym_config.simulation_parameters.enable_rendering_gui)
+ else:
+ self._pybullet_client = bullet_client.BulletClient()
+ if gym_config.simulation_parameters.egl_rendering:
+ self._pybullet_client.loadPlugin('eglRendererPlugin')
+
+ self._pybullet_client.setAdditionalSearchPath(
+ pybullet_data.getDataPath())
+
+ # If enabled, save the performance profile to profiling_path
+ # Use Google Chrome about://tracing to open the file
+ if gym_config.profiling_path is not None:
+ self._profiling_slot = self._pybullet_client.startStateLogging(
+ self._pybullet_client.STATE_LOGGING_PROFILE_TIMINGS,
+ gym_config.profiling_path)
+ self._profiling_counter = 10
+ else:
+ self._profiling_slot = -1
+
+ # Set the default render options. TODO(b/152161124): Make rendering a
+ # standalone module.
+ self._camera_target = gym_config.simulation_parameters.camera_target
+ self._camera_dist = gym_config.simulation_parameters.camera_distance
+ self._camera_yaw = gym_config.simulation_parameters.camera_yaw
+ self._camera_pitch = gym_config.simulation_parameters.camera_pitch
+ self._render_width = gym_config.simulation_parameters.render_width
+ self._render_height = gym_config.simulation_parameters.render_height
+
+ # Loads the environment and robot. Actions space will be created as well.
+ self._hard_reset = True
+ self._observation_dict = {}
+ self.reset()
+ self._hard_reset = gym_config.simulation_parameters.enable_hard_reset
+
+ # Construct the observation space from the list of sensors.
+ self.observation_space = (
+ space_utils.convert_sensors_to_gym_space_dictionary([
+ sensor for sensor in self.all_sensors()
+ if sensor.get_name() not in self._gym_config.ignored_sensor_list
+ ]))
+
+ def __del__(self):
+ self.close()
+
+ def _load_old_robot_class(self):
+ self._robot = self._robot_class(
+ pybullet_client=self._pybullet_client, on_rack=self._on_rack)
+ self._action_list = []
+ action_upper_bound = []
+ action_lower_bound = []
+ for action in self._gym_config.actions:
+ self._action_list.append(action.name)
+ action_upper_bound.append(action.upper_bound)
+ action_lower_bound.append(action.lower_bound)
+ self.action_space = gym.spaces.Box(
+ np.array(action_lower_bound),
+ np.array(action_upper_bound),
+ dtype=np.float32)
+
+ def _load_new_robot_class(self):
+ self._robot = self._robot_class(
+ pybullet_client=self._pybullet_client, clock=self._clock)
+ self.action_space = self._robot.action_space
+
+ def _load(self):
+ self._pybullet_client.resetSimulation()
+ self._pybullet_client.setPhysicsEngineParameter(
+ numSolverIterations=self._num_bullet_solver_iterations)
+ self._pybullet_client.setTimeStep(self._sim_time_step)
+ self._pybullet_client.setGravity(0, 0, -10)
+ self._pybullet_client.setPhysicsEngineParameter(enableConeFriction=0)
+
+ # Disable rendering during scene loading will speed up simulation.
+ if self._is_render:
+ self._pybullet_client.configureDebugVisualizer(
+ self._pybullet_client.COV_ENABLE_RENDERING, 0)
+
+ # Rebuild the scene.
+ self._scene.build_scene(self._pybullet_client)
+
+ # TODO(b/151975607): Deprecate old robot support.
+ if self._use_new_robot_class:
+ self._load_new_robot_class()
+ else:
+ self._load_old_robot_class()
+
+ # Check action space.
+ if (isinstance(self.action_space, gym.spaces.Box) and
+ not np.all(self.action_space.low < self.action_space.high)):
+ raise ValueError(f'Action space contains invalid dimensions, '
+ f'action space low = {self.action_space.low}, '
+ f'action space high = {self.action_space.high}')
+
+ for an_object in self._dynamic_objects():
+ an_object.set_clock(self._clock)
+
+ # Enable rendering after loading finishes.
+ if self._is_render:
+ self._pybullet_client.configureDebugVisualizer(
+ self._pybullet_client.COV_ENABLE_RENDERING, 1)
+
+ def close(self):
+ atexit.unregister(self.close)
+ #if self._pybullet_client:
+
+ if self._log_path is not None:
+ self._logging.save_episode(self._episode_proto)
+ for sensor_ in self.all_sensors():
+ sensor_.on_terminate(self)
+ if self._robot:
+ if self._use_new_robot_class:
+ self._robot.terminate()
+ else:
+ self._robot.Terminate()
+ if self._pybullet_client:
+ self._pybullet_client.disconnect()
+ self._pybullet_client = None
+
+ def seed(self, seed=None):
+ self.np_random, self.np_random_seed = gym.utils.seeding.np_random(seed)
+ return [self.np_random_seed]
+
+ def _dynamic_objects(self):
+ """Returns the python objects controlling moving obstacles."""
+ if self._scene:
+ return self._scene.dynamic_objects
+ else:
+ return []
+
+ def all_sensors(self):
+ """Returns all robot, environmental and dynamic objects sensors."""
+ if self._use_new_robot_class:
+ all_sensors = list(self._env_sensors)
+ if self._robot:
+ all_sensors.extend(list(self._robot.sensors))
+ for obj in self._dynamic_objects():
+ all_sensors.extend(obj.sensors)
+
+ # The new way of adding task specific sensors to the sensor lists.
+ if hasattr(self._task, 'sensors'):
+ all_sensors.extend(self._task.sensors)
+ return all_sensors
+ else:
+ # This is a workaround due to the issue in b/130128505#comment5
+ task_sensor = ([self._task]
+ if isinstance(self._task, sensor.Sensor) else [])
+ robot_sensors = []
+ if self._robot:
+ robot_sensors = self._robot.GetAllSensors()
+ return robot_sensors + self._env_sensors + task_sensor
+
+ def sensor_by_name(self, name):
+ """Returns the sensor with the given name, or None if not exist."""
+ # TODO(b/154162104): Store sensors as dictionary.
+ for sensor_ in self.all_sensors():
+ if sensor_.get_name() == name:
+ return sensor_
+ return None
+
+ @gin.configurable('locomotion_gym_env.LocomotionGymEnv.reset')
+ def reset(
+ self,
+ initial_motor_angles=None,
+ reset_duration=1.0,
+ reset_visualization_camera=True,
+ ):
+ """Resets the robot's position in the world or rebuild the sim world.
+
+ The simulation world will be rebuilt if self._hard_reset is True.
+
+ Args:
+ initial_motor_angles: A list of Floats. The desired joint angles after
+ reset. If None, the robot will use its built-in value.
+ reset_duration: Float. The time (in seconds) needed to rotate all motors
+ to the desired initial values.
+ reset_visualization_camera: Whether to reset debug visualization camera on
+ reset.
+
+ Returns:
+ A numpy array contains the initial observation after reset.
+ """
+
+
+ self._env_step_counter = 0
+ self._sim_step_counter = 0
+ self._last_reset_time = self._clock()
+ self._metric_logger.reset_episode()
+
+ # Clear the simulation world and rebuild the robot interface.
+ if self._hard_reset:
+ self._load()
+
+ # Resets the scene
+ self._scene.reset()
+
+ # Resets the robot with the provided init parameters.
+ if self._use_new_robot_class:
+ self._robot.reset()
+ else:
+ self._robot.Reset(
+ reload_urdf=False,
+ default_motor_angles=initial_motor_angles,
+ reset_time=reset_duration)
+
+ # Flush the logs to disc and reinitialize the logging system.
+ if self._log_path is not None:
+ self._logging.save_episode(self._episode_proto)
+ self._episode_proto = minitaur_logging_pb2.MinitaurEpisode()
+ minitaur_logging.preallocate_episode_proto(self._episode_proto,
+ _LOG_BUFFER_LENGTH,
+ self._robot)
+
+ # TODO(b/152161124): Move this part to the renderer module.
+ if reset_visualization_camera:
+ self._pybullet_client.resetDebugVisualizerCamera(self._camera_dist,
+ self._camera_yaw,
+ self._camera_pitch,
+ [0, 0, 0])
+
+ # Create an example last action based on the type of action space.
+ self._last_action = space_utils.create_constant_action(self.action_space)
+
+ for s in self.all_sensors():
+ s.on_reset(self)
+
+ if self._task and hasattr(self._task, 'reset'):
+ self._task.reset(self)
+
+ # Loop over all env randomizers.
+ for env_randomizer in self._env_randomizers:
+ env_randomizer.randomize_env(self)
+
+ for obj in self._dynamic_objects():
+ obj.reset()
+
+ # Initialize the robot base position.
+ if self._use_new_robot_class:
+ self._last_base_position = self._robot.base_position
+ else:
+ self._last_base_position = self._robot.GetBasePosition()
+
+ # Resets the observations again, since randomizers might change the env.
+ for s in self.all_sensors():
+ s.on_reset(self)
+
+
+ self._last_reset_time = self._clock()
+ return self._get_observation()
+
+ def _wait_for_rendering(self):
+ # Sleep, otherwise the computation takes less time than real time,
+ # which will make the visualization like a fast-forward video.
+ time_spent = time.time() - self._last_frame_time
+ self._last_frame_time = time.time()
+ time_to_sleep = self._env_time_step - time_spent
+ if time_to_sleep > 0:
+ time.sleep(time_to_sleep)
+
+ # Also keep the previous orientation of the camera set by the user.
+ [yaw, pitch, dist] = self._pybullet_client.getDebugVisualizerCamera()[8:11]
+ self._pybullet_client.resetDebugVisualizerCamera(dist, yaw, pitch,
+ self._last_base_position)
+
+ def _step_old_robot_class(self, action):
+ self._last_base_position = self._robot.GetBasePosition()
+ self._last_action = action
+
+ if self._is_render:
+ self._wait_for_rendering()
+
+ for env_randomizer in self._env_randomizers:
+ env_randomizer.randomize_step(self)
+
+ self._robot.Step(action)
+
+ if self._profiling_slot >= 0:
+ self._profiling_counter -= 1
+ if self._profiling_counter == 0:
+ self._pybullet_client.stopStateLogging(self._profiling_slot)
+
+ if self._log_path is not None:
+ minitaur_logging.update_episode_proto(self._episode_proto, self._robot,
+ action, self._env_step_counter)
+ reward = self._reward()
+
+ for s in self.all_sensors():
+ s.on_step(self)
+
+ if self._task and hasattr(self._task, 'update'):
+ self._task.update(self) # TODO(b/154635313): resolve API mismatch
+
+ done = self._termination()
+ self._env_step_counter += 1
+ # TODO(b/161941829): terminate removed for now, change terminate to other
+ # names.
+ return self._get_observation(), reward, done, {}
+
+ def _step_new_robot_class(self, action):
+ self._last_base_position = self._robot.base_position
+ self._last_action = action
+
+ if self._is_render:
+ self._wait_for_rendering()
+
+ for env_randomizer in self._env_randomizers:
+ env_randomizer.randomize_step(self)
+
+ action = self._robot.pre_control_step(action, self._env_time_step)
+ for obj in self._dynamic_objects():
+ obj.pre_control_step(autonomous_object.AUTONOMOUS_ACTION)
+ for _ in range(self._num_action_repeat):
+ self._robot.apply_action(action)
+ for obj in self._dynamic_objects():
+ obj.update(self.get_time_since_reset(), self._observation_dict)
+ obj.apply_action(autonomous_object.AUTONOMOUS_ACTION)
+
+ self._pybullet_client.stepSimulation()
+ self._sim_step_counter += 1
+
+ self._robot.receive_observation()
+ for obj in self._dynamic_objects():
+ obj.receive_observation()
+
+ for s in self.all_sensors():
+ s.on_new_observation()
+
+ self._robot.post_control_step()
+ for obj in self._dynamic_objects():
+ obj.post_control_step()
+
+ if self._profiling_slot >= 0:
+ self._profiling_counter -= 1
+ if self._profiling_counter == 0:
+ self._pybullet_client.stopStateLogging(self._profiling_slot)
+
+ if self._log_path is not None:
+ minitaur_logging.update_episode_proto(self._episode_proto, self._robot,
+ action, self._env_step_counter)
+ reward = self._reward()
+
+ for s in self.all_sensors():
+ s.on_step(self)
+
+ if self._task and hasattr(self._task, 'update'):
+ self._task.update(self) # TODO(b/154635313): resolve API mismatch
+
+ done = self._termination()
+ self._env_step_counter += 1
+ # TODO(b/161941829): terminate removed for now, change terminate to other
+ # names.
+ return self._get_observation(), reward, done, {}
+
+ def step(self, action):
+ """Step forward the simulation, given the action.
+
+ Args:
+ action: Can be a list of desired motor angles for all motors when the
+ robot is in position control mode; A list of desired motor torques. Or a
+ list of tuples (q, qdot, kp, kd, tau) for hybrid control mode. The
+ action must be compatible with the robot's motor control mode. Also, we
+ are not going to use the leg space (swing/extension) definition at the
+ gym level, since they are specific to Minitaur.
+
+ Returns:
+ observations: The observation dictionary. The keys are the sensor names
+ and the values are the sensor readings.
+ reward: The reward for the current state-action pair.
+ done: Whether the episode has ended.
+ info: A dictionary that stores diagnostic information.
+
+ Raises:
+ ValueError: The action dimension is not the same as the number of motors.
+ ValueError: The magnitude of actions is out of bounds.
+ """
+ # TODO(b/151975607): Finish the migration and remove old robot class
+ # support.
+ if self._use_new_robot_class:
+ return self._step_new_robot_class(action)
+ else:
+ return self._step_old_robot_class(action)
+
+ @gin.configurable('locomotion_gym_env.LocomotionGymEnv.render')
+ def render(self, mode='rgb_array'):
+
+ if mode == 'topdown':
+ # Provide ground height if we know it. Otherwise leave it as gin
+ # configurable.
+ if hasattr(self.scene, 'ground_height'):
+ return rendering_utils.render_topdown(
+ self._pybullet_client, ground_height=self.scene.ground_height)
+ else:
+ return rendering_utils.render_topdown(self._pybullet_client)
+
+ if mode != 'rgb_array':
+ raise ValueError('Unsupported render mode:{}'.format(mode))
+
+ if self._camera_target is not None:
+ target_position = self._camera_target
+ else:
+ target_position = self._last_base_position
+ view_matrix = self._pybullet_client.computeViewMatrixFromYawPitchRoll(
+ cameraTargetPosition=target_position,
+ distance=self._camera_dist,
+ yaw=self._camera_yaw,
+ pitch=self._camera_pitch,
+ roll=0,
+ upAxisIndex=2)
+ proj_matrix = self._pybullet_client.computeProjectionMatrixFOV(
+ fov=60,
+ aspect=float(self._render_width) / self._render_height,
+ nearVal=0.1,
+ farVal=100.0)
+ return rendering_utils.render_image(self._pybullet_client,
+ self._render_width, self._render_height,
+ view_matrix, proj_matrix)
+
+ @property
+ def scene(self):
+ return self._scene
+
+ @property
+ def rendering_enabled(self):
+ return self._is_render
+
+ @property
+ def env_randomizers(self):
+ return self._env_randomizers
+
+ @property
+ def last_base_position(self):
+ return self._last_base_position
+
+ @property
+ def gym_config(self):
+ return self._gym_config
+
+ def _termination(self):
+ if not self._robot.is_safe:
+ return True
+
+ if self._task and hasattr(self._task, 'done'):
+ return self._task.done(self) # TODO(b/154635313): resolve API mismatch
+
+ return False
+
+ def _reward(self):
+ if self._task:
+ return self._task.reward(self) # TODO(b/154635313): resolve API mismatch
+ return 0
+
+ def _get_observation(self):
+ """Get observation of this environment from a list of sensors.
+
+ Returns:
+ observations: dictionary of sensory observation with sensor name as key
+ and corresponding observation in numpy array as value.
+ """
+ sensors_dict = {}
+ for s in self.all_sensors():
+ if s.get_name() in self._gym_config.ignored_sensor_list:
+ continue
+
+ obs = s.get_observation()
+ if isinstance(obs, dict):
+ sensors_dict.update(obs)
+ else:
+ sensors_dict[s.get_name()] = obs
+
+ self._observation_dict = collections.OrderedDict(
+ sorted(sensors_dict.items()))
+ return self._observation_dict
+
+ def set_time_step(self, num_action_repeat, sim_step=0.001):
+ """Sets the time step of the environment.
+
+ Args:
+ num_action_repeat: The number of simulation steps/action repeats to be
+ executed when calling env.step().
+ sim_step: The simulation time step in PyBullet. By default, the simulation
+ step is 0.001s, which is a good trade-off between simulation speed and
+ accuracy.
+
+ Raises:
+ ValueError: If the num_action_repeat is less than 1.
+ """
+ if num_action_repeat < 1:
+ raise ValueError('number of action repeats should be at least 1.')
+ self._sim_time_step = sim_step
+ self._num_action_repeat = num_action_repeat
+ self._env_time_step = sim_step * num_action_repeat
+ self._num_bullet_solver_iterations = int(
+ _NUM_SIMULATION_ITERATION_STEPS / self._num_action_repeat)
+ self._pybullet_client.setPhysicsEngineParameter(
+ numSolverIterations=self._num_bullet_solver_iterations)
+ self._pybullet_client.setTimeStep(self._sim_time_step)
+ if not self._use_new_robot_class:
+ self._robot.SetTimeSteps(self._num_action_repeat, self._sim_time_step)
+
+ def _get_sim_time(self):
+ """Returns the simulation time since the sim resets."""
+ return self._sim_step_counter * self._sim_time_step
+
+ def get_time_since_reset(self):
+ """Get the time passed (in seconds) since the last reset.
+
+ Returns:
+ Time in seconds since the last reset.
+ """
+ if self._use_new_robot_class:
+ return self._clock() - self._last_reset_time
+ else:
+ return self._robot.GetTimeSinceReset()
+
+ def get_time(self):
+ """Gets the time reading from the clock source."""
+ return self._clock()
+
+ @property
+ def observation(self):
+ return self._observation_dict
+
+ @property
+ def pybullet_client(self):
+ return self._pybullet_client
+
+ @property
+ def robot(self):
+ return self._robot
+
+ @property
+ def num_action_repeat(self):
+ return self._num_action_repeat
+
+ @property
+ def sim_time_step(self):
+ return self._sim_time_step
+
+ @property
+ def env_step_counter(self):
+ return self._env_step_counter
+
+ @property
+ def hard_reset(self):
+ return self._hard_reset
+
+ @property
+ def last_action(self):
+ return self._last_action
+
+ @property
+ def env_time_step(self):
+ return self._env_time_step
+
+ @property
+ def data_dir(self):
+ return self._data_dir
+
+ @property
+ def task(self):
+ return self._task
+
+ @property
+ def robot_class(self):
+ return self._robot_class
+
+ @property
+ def action_names(self):
+ """Name of each action in the action space.
+
+ By default this returns the actions the robot executes (e.g.
+ "VELOCITY_elbow_joint"), but env wrappers may override this if they change
+ the action space (e.g. if they convert twist to motor commands).
+
+ Returns:
+ Tuple of strings, the action names.
+ """
+ if self._use_new_robot_class:
+ return self._robot.action_names
+ return self._action_list
+
+ @property
+ def metric_logger(self):
+ return self._metric_logger
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/locomotion_gym_env_test.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/locomotion_gym_env_test.py
new file mode 100644
index 000000000..51f3c9ae6
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/locomotion_gym_env_test.py
@@ -0,0 +1,241 @@
+# Lint as: python3
+r"""Tests for locomotion_gym_env.
+
+
+"""
+
+import math
+import random
+
+import gin
+import mock
+import numpy as np
+import tensorflow.compat.v1 as tf
+from absl.testing import parameterized
+
+from pybullet_envs.minitaur.envs_v2 import locomotion_gym_env
+from pybullet_envs.minitaur.envs_v2.evaluation import metric as metric_lib
+from pybullet_envs.minitaur.envs_v2.scenes import scene_base
+from pybullet_envs.minitaur.envs_v2.scenes import simple_scene
+from pybullet_envs.minitaur.envs_v2.tasks import task_interface
+from pybullet_envs.minitaur.envs_v2.utilities import env_utils
+from pybullet_envs.minitaur.robots import autonomous_object
+from pybullet_envs.minitaur.robots import minitaur_v2
+import pybullet_data as pd
+
+import unittest
+
+
+_POSITION_GAIN = 1.0
+_VELOCITY_GAIN = 0.015
+_CONTROL_LATENCY = 0.015
+_CONFIG_FILE = (pd.getDataPath()+'/configs/minitaur_gym_env.gin')
+_CONFIG_FILE_NEW_ROBOT = (pd.getDataPath()+'/configs_v2/base/laikago_reactive.gin')
+
+
+class TestTask(task_interface.Task):
+ """A step counter task for test purpose."""
+
+ def __init__(self):
+ self._counter = 0
+
+ def reset(self, env):
+ del env
+ self._counter = 0
+
+ def reward(self, env):
+ del env # TODO(b/154635313): resolve the API mismatch
+ self._counter += 1
+ return self._counter
+
+ def update(self, env):
+ del env # TODO(b/154635313): resolve the API mismatch
+
+ def done(self, env):
+ del env # TODO(b/154635313): resolve the API mismatch
+ return False
+
+
+class LocomotionGymEnvTest(tf.test.TestCase, parameterized.TestCase):
+
+ def setUp(self):
+ super().setUp()
+ gin.clear_config()
+
+ def test_env_from_gin(self):
+ # TODO(sehoonha) rename locomotion_gym_*test.gin to locomotion_gym_*.gin
+ gin.parse_config_file(_CONFIG_FILE)
+ env = locomotion_gym_env.LocomotionGymEnv()
+ self.assertIsInstance(env.robot, minitaur_v2.Minitaur)
+ # The robot will stand on the ground.
+ self.assertNear(env.robot.base_position[2], 0.25, 5e-2)
+
+ def test_reset_gym(self):
+ gin.parse_config_file(_CONFIG_FILE)
+ env = locomotion_gym_env.LocomotionGymEnv(task=None)
+
+ desired_init_motor_angle = math.pi / 2
+ action_dim = len(env.action_space.high)
+ observations = env.reset(initial_motor_angles=[desired_init_motor_angle] *
+ action_dim)
+ observations = env_utils.flatten_observations(observations)
+ self.assertEqual(observations.size, 12)
+ self.assertNear(observations[0], 0, 1e-2)
+ self.assertNear(observations[4], desired_init_motor_angle, 2e-1)
+
+ def test_step_gym(self):
+ gin.parse_config_file(_CONFIG_FILE)
+ env = locomotion_gym_env.LocomotionGymEnv(task=TestTask())
+
+ desired_motor_angle = math.pi / 3
+ steps = 1000
+ action_dim = len(env.action_space.high)
+ for _ in range(steps):
+ observations, reward, done, _ = env.step([desired_motor_angle] *
+ action_dim)
+ observations = env_utils.flatten_observations(observations)
+
+ self.assertFalse(done)
+ self.assertEqual(reward, steps)
+ self.assertEqual(observations.size, 12)
+ self.assertNear(observations[0], 0, 1e-2)
+ self.assertNear(observations[4], desired_motor_angle, 2e-1)
+ np.testing.assert_allclose(env._last_action,
+ [desired_motor_angle] * action_dim, 2e-1)
+
+ def test_scene(self):
+ gin.parse_config_file(_CONFIG_FILE)
+ data_root = 'urdf/'
+ env = locomotion_gym_env.LocomotionGymEnv(
+ task=TestTask(), scene=simple_scene.SimpleScene(data_root=data_root))
+ desired_motor_angle = math.pi / 3
+ steps = 2
+ action_dim = len(env.action_space.high)
+ for _ in range(steps):
+ _, reward, _, _ = env.step([desired_motor_angle] * action_dim)
+ self.assertEqual(reward, steps)
+
+ def test_except_on_invalid_config(self):
+ gin.parse_config_file(_CONFIG_FILE)
+ gin.bind_parameter('SimulationParameters.num_action_repeat', 0)
+ with self.assertRaises(ValueError):
+ locomotion_gym_env.LocomotionGymEnv(task=None)
+
+ def test_no_scene(self):
+ gin.parse_config_file(_CONFIG_FILE)
+ env = locomotion_gym_env.LocomotionGymEnv(task=None, scene=None)
+
+ # The robot will free fall.
+ self.assertNear(env.robot.base_position[2], 0.15, 5e-2)
+
+ def test_seed_draw_with_np(self):
+ gin.parse_config_file(_CONFIG_FILE)
+ env = locomotion_gym_env.LocomotionGymEnv(task=None)
+ # first draw
+ env.seed(42)
+ nums1 = []
+ for _ in range(3):
+ nums1.append(env.np_random.randint(2**31 - 1))
+ # second draw
+ env.seed(42)
+ nums2 = []
+ for _ in range(3):
+ nums2.append(env.np_random.randint(2**31 - 1))
+ self.assertListEqual(nums1, nums2)
+
+ def test_get_observations(self):
+ gin.parse_config_file(_CONFIG_FILE)
+ env = locomotion_gym_env.LocomotionGymEnv(task=None)
+ desired_init_motor_angle = math.pi / 2
+ action_dim = len(env.action_space.high)
+ observations = env.reset(initial_motor_angles=[desired_init_motor_angle] *
+ action_dim)
+ self.assertLen(observations, 2)
+ self.assertLen(observations['IMU'], 4)
+ self.assertNear(observations['IMU'][0], 0, 1e-2)
+ self.assertLen(observations['MotorAngle'], 8)
+ self.assertNear(observations['MotorAngle'][0], desired_init_motor_angle,
+ 2e-1)
+
+
+
+
+
+ def test_step_with_dynamic_objects(self):
+ gin.parse_config_file(_CONFIG_FILE_NEW_ROBOT)
+
+ gin.parse_config([
+ 'AutonomousObject.urdf_file = "urdf/mug.urdf"',
+ 'SceneBase.dynamic_objects = [@AutonomousObject(), @AutonomousObject()]',
+ 'LocomotionGymEnv.scene = @SceneBase()',
+ ])
+ env = locomotion_gym_env.LocomotionGymEnv()
+
+ self.assertLen(env.scene.dynamic_objects, 2)
+ for obj in env.scene.dynamic_objects:
+ self.assertIsInstance(obj, autonomous_object.AutonomousObject)
+
+ # Replace dynamic objects with mocks for step tests.
+ mock_objects = [
+ mock.create_autospec(autonomous_object.AutonomousObject),
+ mock.create_autospec(autonomous_object.AutonomousObject)
+ ]
+ env.scene._type_to_objects_dict[
+ scene_base.ObjectType.DYNAMIC_OBJECT] = mock_objects
+ env.step(env.robot.action_space.sample())
+
+ expected_update_calls = [
+ mock.call(i * env.sim_time_step, mock.ANY)
+ for i in range(env.num_action_repeat)
+ ]
+ expected_apply_action_calls = [
+ mock.call(autonomous_object.AUTONOMOUS_ACTION)
+ for i in range(env.num_action_repeat)
+ ]
+ expected_receive_observation_calls = [
+ mock.call() for i in range(env.num_action_repeat)
+ ]
+
+ for mock_obj in mock_objects:
+ mock_obj.pre_control_step.assert_called_once_with(
+ autonomous_object.AUTONOMOUS_ACTION)
+ self.assertEqual(mock_obj.update.call_args_list, expected_update_calls)
+ self.assertEqual(mock_obj.apply_action.call_args_list,
+ expected_apply_action_calls)
+ self.assertEqual(mock_obj.receive_observation.call_args_list,
+ expected_receive_observation_calls)
+ mock_obj.post_control_step.assert_called_once_with()
+
+
+ def test_env_metrics(self):
+ gin.parse_config_file(_CONFIG_FILE_NEW_ROBOT)
+ env = locomotion_gym_env.LocomotionGymEnv()
+ metric_logger = env.metric_logger
+ test_metric_1 = metric_logger.create_scalar_metric(
+ 'test_metric_1',
+ scope=metric_lib.MetricScope.DEBUG,
+ single_ep_aggregator=np.mean)
+ test_metric_1.report(22)
+
+ test_metric_2 = metric_logger.create_scalar_metric(
+ 'test_metric_2',
+ scope=metric_lib.MetricScope.PERFORMANCE,
+ single_ep_aggregator=np.max)
+ test_metric_2.report(15)
+ test_metric_2.report(16)
+
+ all_metrics = metric_logger.get_episode_metrics()
+
+ self.assertDictEqual(all_metrics, {
+ 'DEBUG/test_metric_1': 22,
+ 'PERFORMANCE/test_metric_2': 16
+ })
+
+ env.reset()
+
+ all_metrics = metric_logger.get_episode_metrics()
+ self.assertDictEqual(all_metrics, {})
+
+
+if __name__ == '__main__':
+ tf.test.main()
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/multiagent_mobility_gym_env.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/multiagent_mobility_gym_env.py
new file mode 100644
index 000000000..c716d367b
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/multiagent_mobility_gym_env.py
@@ -0,0 +1,485 @@
+"""This file implements the locomotion gym env."""
+# pylint: disable=dangerous-default-value
+
+import atexit
+import collections
+import time
+
+import gin
+from gym import spaces
+import numpy as np
+
+from pybullet_utils import bullet_client
+from pybullet_envs.minitaur.envs import minitaur_logging
+from pybullet_envs.minitaur.envs import minitaur_logging_pb2
+from pybullet_envs.minitaur.envs_v2 import locomotion_gym_env
+from pybullet_envs.minitaur.envs_v2.scenes import scene_base
+from pybullet_envs.minitaur.envs_v2.sensors import sensor
+from pybullet_envs.minitaur.envs_v2.sensors import space_utils
+import pybullet
+
+_ACTION_EPS = 0.01
+_NUM_SIMULATION_ITERATION_STEPS = 300
+_LOG_BUFFER_LENGTH = 5000
+
+
+@gin.configurable
+class MultiagentMobilityGymEnv(locomotion_gym_env.LocomotionGymEnv):
+ """The gym environment for the locomotion tasks."""
+ metadata = {
+ 'render.modes': ['human', 'rgb_array'],
+ 'video.frames_per_second': 100
+ }
+
+ def __init__(self,
+ gym_config,
+ robot_classes,
+ scene: scene_base.SceneBase = scene_base.SceneBase(),
+ sensors=None,
+ tasks=[],
+ global_task=None,
+ single_reward=False,
+ env_randomizers=None):
+ """Initializes the locomotion gym environment.
+
+ Args:
+ gym_config: An instance of LocomotionGymConfig.
+ robot_classes: A list of robot classes. We provide a class rather than an
+ instance due to hard_reset functionality. Parameters are expected to be
+ configured with gin.
+ scene: An object for managing the robot's surroundings.
+ sensors: A list of environmental sensors for observation. This does not
+ include on-robot sensors.
+ tasks: A list of callable function/class to calculate the reward and
+ termination condition. Takes the gym env as the argument when calling.
+ global_task: A callable function/class to calculate the reward and
+ termination condition for all robots. Takes the gym env as the argument
+ when calling.
+ single_reward: Whether the environment returns a single reward for all
+ agents or a dictionary.
+ env_randomizers: A list of EnvRandomizer(s). An EnvRandomizer may
+ randomize the physical property of minitaur, change the terrrain during
+ reset(), or add perturbation forces during step().
+
+ Raises:
+ ValueError: If the num_action_repeat is less than 1, or if number of
+ unique robot names do not match the number of robot classes.
+
+ """
+ # TODO(sehoonha) split observation and full-state sensors (b/129858214)
+
+ # Makes sure that close() is always called to flush out the logs to the
+ # disk.
+ atexit.register(self.close)
+ self.seed()
+ self._gym_config = gym_config
+ self._robot_classes = robot_classes
+ # Checking uniqueness of names and number of names
+ self._scene = scene
+ # TODO(sehoonha) change the data structure to dictionary
+ # TODO(b/144521291) make sure sensors have their own robot names
+ self._sensors = sensors if sensors is not None else list()
+ self._log_path = gym_config.log_path
+ self._logging = minitaur_logging.MinitaurLogging(self._log_path)
+ self._episode_proto = minitaur_logging_pb2.MinitaurEpisode()
+ self._data_dir = gym_config.data_dir
+
+ # A dictionary containing the objects in the world other than the robot.
+ self._tasks = tasks
+ self._global_task = global_task
+ self._single_reward = single_reward
+
+ self._env_randomizers = env_randomizers if env_randomizers else []
+
+ # This is a workaround due to the issue in b/130128505#comment5
+ for task in self._tasks:
+ if isinstance(task, sensor.Sensor):
+ self._sensors.append(task)
+ if global_task and isinstance(global_task, sensor.Sensor):
+ self._sensors.append(global_task)
+
+ # Simulation related parameters.
+ self._num_action_repeat = gym_config.simulation_parameters.num_action_repeat
+ self._on_rack = gym_config.simulation_parameters.robot_on_rack
+ if self._num_action_repeat < 1:
+ raise ValueError('number of action repeats should be at least 1.')
+ self._sim_time_step = gym_config.simulation_parameters.sim_time_step_s
+ self._env_time_step = self._num_action_repeat * self._sim_time_step
+ self._env_step_counter = 0
+
+ # TODO(b/73829334): Fix the value of self._num_bullet_solver_iterations.
+ self._num_bullet_solver_iterations = int(_NUM_SIMULATION_ITERATION_STEPS /
+ self._num_action_repeat)
+ self._is_render = gym_config.simulation_parameters.enable_rendering
+
+ # The wall-clock time at which the last frame is rendered.
+ self._last_frame_time = 0.0
+ if self._is_render:
+ self._pybullet_client = bullet_client.BulletClient(
+ connection_mode=pybullet.GUI)
+ else:
+ self._pybullet_client = bullet_client.BulletClient()
+
+ if gym_config.simulation_parameters.egl_rendering:
+ self._pybullet_client.loadPlugin('eglRendererPlugin')
+ self._pybullet_client.enable_cns()
+
+ # If enabled, save the performance profile to profiling_path
+ # Use Google Chrome about://tracing to open the file
+ if gym_config.profiling_path is not None:
+ self._profiling_slot = self._pybullet_client.startStateLogging(
+ self._pybullet_client.STATE_LOGGING_PROFILE_TIMINGS,
+ gym_config.profiling_path)
+ self._profiling_counter = 10
+ else:
+ self._profiling_slot = -1
+ # Build the action space. The action space must be compatible with the
+ # robot configuration.
+
+ # The action list contains the name of all actions.
+ # TODO(b/144479707): Allow robots to set the action space automatically.
+
+ action_space = collections.OrderedDict()
+ for robot_name, action in gym_config.actions.items():
+ action_lower_bound = []
+ action_upper_bound = []
+ for action_scalar in action:
+ action_upper_bound.append(action_scalar.upper_bound)
+ action_lower_bound.append(action_scalar.lower_bound)
+ action_space[robot_name] = spaces.Box(
+ np.asarray(action_lower_bound),
+ np.asarray(action_upper_bound),
+ dtype=np.float32)
+ self.action_space = spaces.Dict(action_space)
+
+ # Set the default render options.
+ self._camera_dist = gym_config.simulation_parameters.camera_distance
+ self._camera_yaw = gym_config.simulation_parameters.camera_yaw
+ self._camera_pitch = gym_config.simulation_parameters.camera_pitch
+ self._render_width = gym_config.simulation_parameters.render_width
+ self._render_height = gym_config.simulation_parameters.render_height
+
+ self._hard_reset = True
+ self.reset()
+
+ self._hard_reset = gym_config.simulation_parameters.enable_hard_reset
+
+ # Construct the observation space from the list of sensors. Note that we
+ # will reconstruct the observation_space after the robot is created.
+ self.observation_space = (
+ space_utils.convert_sensors_to_gym_space_dictionary(self.all_sensors()))
+
+ def close(self):
+ if self._log_path is not None:
+ self._logging.save_episode(self._episode_proto)
+
+ for robot in self._robots:
+ robot.Terminate()
+
+ def all_sensors(self):
+ """Returns all robot and environmental sensors."""
+ robot_sensors = []
+ for robot in self._robots:
+ robot_sensors += robot.GetAllSensors()
+ return robot_sensors + self._sensors
+
+ @gin.configurable('multiagent_mobility_gym_env.MultiagentMobilityGymEnv.reset'
+ )
+ def reset(self,
+ initial_motor_angles=None,
+ reset_duration=1.0,
+ reset_visualization_camera=True):
+ """Resets the robot's position in the world or rebuild the sim world.
+
+ The simulation world will be rebuilt if self._hard_reset is True.
+
+ Args:
+ initial_motor_angles: A list of Floats. The desired joint angles after
+ reset. If None, the robot will use its built-in value.
+ reset_duration: Float. The time (in seconds) needed to rotate all motors
+ to the desired initial values.
+ reset_visualization_camera: Whether to reset debug visualization camera on
+ reset.
+
+ Returns:
+ A numpy array contains the initial observation after reset.
+ """
+ if self._is_render:
+ self._pybullet_client.configureDebugVisualizer(
+ self._pybullet_client.COV_ENABLE_RENDERING, 0)
+
+ # Clear the simulation world and rebuild the robot interface.
+ if self._hard_reset:
+ self._pybullet_client.resetSimulation()
+ self._pybullet_client.setPhysicsEngineParameter(
+ numSolverIterations=self._num_bullet_solver_iterations)
+ self._pybullet_client.setTimeStep(self._sim_time_step)
+ self._pybullet_client.setGravity(0, 0, -10)
+
+ # Rebuild the world.
+ self._scene.build_scene(self._pybullet_client)
+
+ # Rebuild the robots
+ # TODO(b/144545080): Make this scale to more than two agents
+ # Have multiple robot classes as a list.
+ self._robots = []
+ for robot_class in self._robot_classes:
+
+ self._robots.append(
+ robot_class(
+ pybullet_client=self._pybullet_client,
+ # TODO(rosewang): Remove on rack in multiagent acase
+ on_rack=self._on_rack))
+
+ # Reset the pose of the robot.
+ for robot in self._robots:
+ robot.Reset(
+ reload_urdf=False,
+ default_motor_angles=initial_motor_angles,
+ reset_time=reset_duration)
+
+ self._env_step_counter = 0
+ self._pybullet_client.resetDebugVisualizerCamera(self._camera_dist,
+ self._camera_yaw,
+ self._camera_pitch,
+ [0, 0, 0])
+
+ # Flush the logs to disc and reinitialize the logging system.
+ if self._log_path is not None:
+ self._logging.save_episode(self._episode_proto)
+ self._episode_proto = minitaur_logging_pb2.MinitaurEpisode()
+ minitaur_logging.preallocate_episode_proto(self._episode_proto,
+ _LOG_BUFFER_LENGTH,
+ self._robots[0])
+ self._pybullet_client.setPhysicsEngineParameter(enableConeFriction=0)
+ self._env_step_counter = 0
+ if reset_visualization_camera:
+ self._pybullet_client.resetDebugVisualizerCamera(self._camera_dist,
+ self._camera_yaw,
+ self._camera_pitch,
+ [0, 0, 0])
+
+ self._last_action = {
+ robot_name: np.zeros(space.shape)
+ for robot_name, space in self.action_space.spaces.items()
+ }
+
+ if self._is_render:
+ self._pybullet_client.configureDebugVisualizer(
+ self._pybullet_client.COV_ENABLE_RENDERING, 1)
+
+ for s in self.all_sensors():
+ # set name
+ if any([r.name in s.get_name() for r in self.robots]):
+ robot = [r for r in self.robots if r.name in s.get_name()][0]
+ s.set_robot(robot)
+
+ for task in self._tasks:
+ if hasattr(task, 'reset'):
+ task.reset(self)
+ if self._global_task and hasattr(self._global_task, 'reset'):
+ self._global_task.reset(self)
+
+ # Loop over all env randomizers.
+ for env_randomizer in self._env_randomizers:
+ env_randomizer.randomize_env(self)
+
+ for s in self.all_sensors():
+ s.on_reset(self)
+
+ return self._get_observation()
+
+ def get_robot(self, name):
+ for robot in self.robots:
+ if robot.name == name:
+ return robot
+
+ def _reward(self):
+ """Returns a list of rewards.
+
+ Returns:
+ A list of rewards corresponding to each robot and their task.
+ """
+ global_reward = 0
+ if self._global_task:
+ global_reward = self._global_task(self)
+ if self._single_reward: # Needed for tfagents compatibility.
+ if self._tasks:
+ return min([task(self) + global_reward for task in self._tasks])
+ return 0
+ else:
+ if self._tasks:
+ return [task(self) + global_reward for task in self._tasks]
+ return [0 for _ in self.robots]
+
+ def step(self, actions):
+ """Step forward the simulation, given the actions.
+
+ Args:
+ actions: A dictionary of actions for all robots. The action for each robot
+ can be joint angles for legged platforms like Laikago, and base
+ velocity/steering for kinematic robots such like Fetch.
+
+ Returns:
+ observations: The observation dictionary. The keys are the sensor names
+ and the values are the sensor readings.
+ reward: The reward for the current state-action pair.
+ done: Whether the episode has ended.
+ info: A dictionary that stores diagnostic information.
+
+ Raises:
+ ValueError: The action dimension is not the same as the number of motors.
+ ValueError: The magnitude of actions is out of bounds.
+ """
+ self._last_base_position = [
+ robot.GetBasePosition() for robot in self._robots
+ ]
+ self._last_action = actions
+
+ if self._is_render:
+ # Sleep, otherwise the computation takes less time than real time,
+ # which will make the visualization like a fast-forward video.
+ time_spent = time.time() - self._last_frame_time
+ self._last_frame_time = time.time()
+ time_to_sleep = self._env_time_step - time_spent
+ if time_to_sleep > 0:
+ time.sleep(time_to_sleep)
+ camera_target = np.mean(
+ [robot.GetBasePosition() for robot in self._robots], axis=0)
+
+ # Also keep the previous orientation of the camera set by the user.
+ [yaw, pitch,
+ dist] = self._pybullet_client.getDebugVisualizerCamera()[8:11]
+ self._pybullet_client.resetDebugVisualizerCamera(dist, yaw, pitch,
+ camera_target)
+
+ for env_randomizer in self._env_randomizers:
+ env_randomizer.randomize_step(self)
+
+ # Stepping broken down into their parts
+ for robot in self._robots:
+ robot.PreStepPerStep(actions)
+
+ for _ in range(self._num_action_repeat):
+ for robot in self._robots:
+ robot.PreStepPerActionRepeat(actions)
+
+ self._pybullet_client.stepSimulation()
+
+ for robot in self._robots:
+ robot.PostStepPerActionRepeat()
+
+ for robot in self._robots:
+ robot.PostStepPerStep()
+
+ if self._profiling_slot >= 0:
+ self._profiling_counter -= 1
+ if self._profiling_counter == 0:
+ self._pybullet_client.stopStateLogging(self._profiling_slot)
+ if self._log_path is not None:
+ minitaur_logging.update_episode_proto(self._episode_proto,
+ self._robots[0], actions,
+ self._env_step_counter)
+ reward = self._reward()
+
+ for s in self.all_sensors():
+ s.on_step(self)
+
+ for task in self._tasks:
+ if hasattr(task, 'update'):
+ task.update(self)
+ if self._global_task and hasattr(self._global_task, 'update'):
+ self._global_task.update(self)
+
+ done = self._termination()
+ self._env_step_counter += 1
+ if done:
+ for robot in self._robots:
+ robot.Terminate()
+ return self._get_observation(), reward, done, {}
+
+ def render(self, mode='rgb_array'):
+ if mode != 'rgb_array':
+ raise ValueError('Unsupported render mode:{}'.format(mode))
+ base_pos = np.mean([robot.GetBasePosition() for robot in self._robots],
+ axis=0)
+ view_matrix = self._pybullet_client.computeViewMatrixFromYawPitchRoll(
+ cameraTargetPosition=base_pos,
+ distance=self._camera_dist,
+ yaw=self._camera_yaw,
+ pitch=self._camera_pitch,
+ roll=0,
+ upAxisIndex=2)
+ proj_matrix = self._pybullet_client.computeProjectionMatrixFOV(
+ fov=60,
+ aspect=float(self._render_width) / self._render_height,
+ nearVal=0.1,
+ farVal=100.0)
+ (_, _, px, _, _) = self._pybullet_client.getCameraImage(
+ width=self._render_width,
+ height=self._render_height,
+ renderer=self._pybullet_client.ER_BULLET_HARDWARE_OPENGL,
+ viewMatrix=view_matrix,
+ projectionMatrix=proj_matrix)
+ rgb_array = np.array(px)
+ rgb_array = rgb_array[:, :, :3]
+ return rgb_array
+
+ def _termination(self):
+ if not all([robot.is_safe for robot in self._robots]):
+ return True
+
+ if self._tasks:
+ return (self._global_task and self._global_task.done(self)) or any(
+ [task.done(self) for task in self._tasks])
+
+ for s in self.all_sensors():
+ s.on_terminate(self)
+
+ return False
+
+ def set_time_step(self, num_action_repeat, sim_step=0.001):
+ """Sets the time step of the environment.
+
+ Args:
+ num_action_repeat: The number of simulation steps/action repeats to be
+ executed when calling env.step().
+ sim_step: The simulation time step in PyBullet. By default, the simulation
+ step is 0.001s, which is a good trade-off between simulation speed and
+ accuracy.
+
+ Raises:
+ ValueError: If the num_action_repeat is less than 1.
+ """
+ if num_action_repeat < 1:
+ raise ValueError('number of action repeats should be at least 1.')
+ self._sim_time_step = sim_step
+ self._num_action_repeat = num_action_repeat
+ self._env_time_step = sim_step * num_action_repeat
+ self._num_bullet_solver_iterations = (
+ _NUM_SIMULATION_ITERATION_STEPS / self._num_action_repeat)
+ self._pybullet_client.setPhysicsEngineParameter(
+ numSolverIterations=self._num_bullet_solver_iterations)
+ self._pybullet_client.setTimeStep(self._sim_time_step)
+ for robot in self._robots:
+ robot.SetTimeSteps(self._num_action_repeat, self._sim_time_step)
+
+ def get_time_since_reset(self):
+ """Get the time passed (in seconds) since the last reset.
+
+ Returns:
+ List of time in seconds since the last reset for each robot.
+ """
+ return self._robots[0].GetTimeSinceReset()
+
+ @property
+ def tasks(self):
+ return self._tasks
+
+ @property
+ def robots(self):
+ return self._robots
+
+ @property
+ def num_robots(self):
+ return len(self._robots)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/scenes/__init__.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/scenes/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/scenes/scene_base.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/scenes/scene_base.py
new file mode 100644
index 000000000..c52ce10f6
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/scenes/scene_base.py
@@ -0,0 +1,256 @@
+# Lint as: python3
+"""Class for loading and managing a scene in pybullet."""
+
+import enum
+from typing import Any, Dict, List, Optional, Sequence, Text
+import gin
+import numpy as np
+
+from pybullet_envs.minitaur.envs_v2.scenes import world_asset_pb2
+from pybullet_envs.minitaur.envs_v2 import base_client
+from pybullet_utils import bullet_client
+from pybullet_envs.minitaur.robots import autonomous_object
+
+
+
+# The 2D coordinates of the corners of a polygon. The corners are specified in
+# counterclock-wise direction.
+Polygon = Sequence[Sequence[float]]
+
+
+class ObjectType(enum.Enum):
+ """Categories of objects that may be found in a scene."""
+ OTHER = 0
+ GROUND = 1
+ OBSTACLE = 2
+ GOAL = 3
+ DYNAMIC_OBJECT = 4
+
+
+@gin.configurable
+class SceneBase(object):
+ """Class for loading and managing a scene."""
+
+ def __init__(
+ self,
+ data_root: Text = None,
+ dynamic_objects: Sequence[autonomous_object.AutonomousObject] = ()):
+ """Initializes SceneBase.
+
+ Args:
+ data_root: Root directory for finding object models.
+ dynamic_objects: Dynamic objects to be added into the scene.
+ crowd_builders: Builders of crowds formed by autonomous objects.
+ """
+ self._pybullet_client = None
+ self._data_root = data_root
+ temp_dynamic_objects = list(dynamic_objects)
+
+ self._dynamic_objects = tuple(temp_dynamic_objects)
+ # Dictionaries and world_asset are declared outside init to make sure they
+ # are all reset in _reset_scene_tracking().
+ self._reset_scene_tracking()
+
+ def _reset_scene_tracking(self):
+ """Clears all scene dicts. Used when the simulation is reset."""
+ self._type_to_ids_dict = {object_type: [] for object_type in ObjectType}
+ self._type_to_objects_dict = {object_type: [] for object_type in ObjectType}
+ self._id_to_type_dict = {}
+ self._id_to_object_dict = {}
+ self._world_asset = None
+
+ def destroy_scene(self):
+ """Destroys contents of scene to get ready for another build_scene call."""
+ id_to_remove = list(self._id_to_object_dict.keys())
+ for object_id in id_to_remove:
+ self.remove_object(object_id)
+ self._reset_scene_tracking()
+
+ def build_scene(self, pybullet_client: bullet_client.BulletClient):
+ """Loads and positions all scene objects in pybullet.
+
+ Override this function in subclass to implement customized scene. The
+ overriding function must call base function first.
+
+ Args:
+ pybullet_client: A pybullet client in which the scene will be built.
+ """
+ self._reset_scene_tracking()
+ self._pybullet_client = pybullet_client
+ self._init_dynamic_objects()
+
+ def reset(self):
+ """The soft reset of scene.
+
+ Unlike "build_scene", this is called at each env.reset() before robot
+ resetting. Typically we use this API to do some soft resetting like putting
+ objects back to its place. Howevever, for special cases such as P2P multimap
+ training, we can reload a different mesh scene once a while.
+ """
+ pass
+
+ def _init_dynamic_objects(self):
+ """Adds dynamic objects to scene."""
+ for an_object in self._dynamic_objects:
+ an_object.set_sim_client(self._pybullet_client)
+ self.add_object(an_object.sim_object_id, ObjectType.DYNAMIC_OBJECT,
+ an_object)
+
+ @property
+ def pybullet_client(self) -> bullet_client.BulletClient:
+ if self._pybullet_client is None:
+ raise ValueError("pybullet_client is None; did you call build_scene()?")
+ return self._pybullet_client
+
+ @property
+ def ground_height(self) -> float:
+ """Returns ground height of the scene."""
+ return 0.0
+
+ @property
+ def ground_ids(self) -> List[int]:
+ """Returns the pybullet ids of the ground."""
+ return self._type_to_ids_dict[ObjectType.GROUND]
+
+ @property
+ def obstacle_ids(self) -> List[int]:
+ """Returns the pybullet ids of all obstacles in the scene."""
+ return self._type_to_ids_dict[ObjectType.OBSTACLE]
+
+ @property
+ def goal_ids(self) -> List[int]:
+ """Returns the pybullet ids of any goals in the scene."""
+ return self._type_to_ids_dict[ObjectType.GOAL]
+
+ @property
+ def dynamic_object_ids(self) -> List[int]:
+ """Returns the pybullet ids of dynamic objects."""
+ return self._type_to_ids_dict[ObjectType.DYNAMIC_OBJECT]
+
+ @property
+ def dynamic_objects(self) -> List[autonomous_object.AutonomousObject]:
+ """Returns the dynamic objects python object (AutonomousObject)."""
+ return self._type_to_objects_dict[ObjectType.DYNAMIC_OBJECT]
+
+ @property
+ def world_asset(self) -> world_asset_pb2.WorldAsset:
+ """Returns a proto describing the semantics of the scene.
+
+ If the scene keeps a WorldAsset, then mutating this proto will mutate it for
+ everyone. If the scene generates a WorldAsset from _type_to_ids_dict, then
+ this is not an issue.
+ """
+ if self._world_asset:
+ return self._world_asset
+ return self._dict_to_world_asset(self._type_to_ids_dict)
+
+ def add_object(self,
+ object_id: int,
+ class_label: ObjectType,
+ python_object: Optional[Any] = None):
+ """Adds an object to be tracked.
+
+ Does not load anything into pybullet.
+
+ Args:
+ object_id: objectUniqueId from pybullet.
+ class_label: What type to consider the new object.
+ python_object: Associated python object for the pybullet object of
+ objectUniqueId == object_id. Environment uses the python object to
+ control object in pybullet in these cases. One example is python objects
+ of class label DYNAMIC_OBJECT: they are associated with python objects
+ of type AutonomousObject.
+ """
+ if python_object is not None:
+ if (isinstance(python_object, autonomous_object.AutonomousObject) and
+ python_object.sim_object_id != object_id):
+ raise ValueError(
+ f"Mismatch object ids, object_id = {object_id}, sim_object_id = "
+ f"{python_object.sim_object_id}")
+ self._type_to_objects_dict[class_label].append(python_object)
+ self._type_to_ids_dict[class_label].append(object_id)
+ self._id_to_type_dict[object_id] = class_label
+ self._id_to_object_dict[object_id] = python_object
+
+ def remove_object(self, object_id: int):
+ """Removes an object from tracking and from pybullet.
+
+ Args:
+ object_id: objectUniqueID from pybullet.
+
+ Raises:
+ KeyError: if object_id does not exist in the record.
+ """
+ if object_id not in self._id_to_type_dict.keys():
+ raise KeyError(
+ f"Object with object_id = {object_id} does not exist in the record.")
+
+ self.pybullet_client.removeBody(object_id)
+ object_type = self._id_to_type_dict[object_id]
+ self._type_to_ids_dict[object_type].remove(object_id)
+
+ object_to_remove = self.id_to_object(object_id)
+ if object_to_remove is not None:
+ # Removes item by identity comparison and avoid slow down due to objects
+ # with complex equality comparison function. list.remove() compares
+ # equality instead of identity.
+ for i, an_object in enumerate(self._type_to_objects_dict[object_type]):
+ if an_object is object_to_remove:
+ del self._type_to_objects_dict[object_type][i]
+ break
+ del self._id_to_type_dict[object_id]
+ del self._id_to_object_dict[object_id]
+
+ def id_to_object(self, object_id: int) -> Any:
+ """Returns underlying python object from sim object id.
+
+ Args:
+ object_id: objectUniqueID from pybullet.
+
+ Returns:
+ None is returned if the sim object does not have a corresponding python
+ object.
+ """
+ return self._id_to_object_dict[object_id]
+
+ def _dict_to_world_asset(
+ self, type_to_ids_dict: Dict[ObjectType,
+ List[int]]) -> world_asset_pb2.WorldAsset:
+ """Converts a dictionary to a WorldAsset.
+
+ Args:
+ type_to_ids_dict: Dictionary that describes the scene. Keys are
+ ObjectTypes and values are lists of integers, where each integer is a
+ pybullet id for an object of a given type.
+
+ Returns:
+ A WorldAsset proto with the types, locations and bounding boxes of all
+ objects in the scene.
+ """
+ world_asset = world_asset_pb2.WorldAsset()
+ for object_type in type_to_ids_dict.keys():
+ for obj_id in type_to_ids_dict[object_type]:
+ bbox = np.array(self.pybullet_client.getAABB(obj_id))
+ bbox_center = np.mean(bbox, axis=0)
+ bbox_dimensions = bbox[1] - bbox[0]
+
+ obj = world_asset_pb2.Object()
+ obj.id = str(obj_id)
+ obj.label = str(object_type)
+ obj.bounding_box.center.x = bbox_center[0]
+ obj.bounding_box.center.y = bbox_center[1]
+ obj.bounding_box.center.z = bbox_center[2]
+ obj.bounding_box.dimensions.x = bbox_dimensions[0]
+ obj.bounding_box.dimensions.y = bbox_dimensions[1]
+ obj.bounding_box.dimensions.z = bbox_dimensions[2]
+ world_asset.objects.append(obj)
+ return world_asset
+
+ def close(self):
+ """Closes the scene at the end of life cycle of the environment."""
+ pass
+
+ @property
+ def vectorized_map(self) -> Sequence[Polygon]:
+ """Returns vectorized map containing a list of polygon obstacles."""
+ raise NotImplementedError("vectorized_map is not implemented by default.")
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/scenes/simple_scene.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/scenes/simple_scene.py
new file mode 100644
index 000000000..0099e9e96
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/scenes/simple_scene.py
@@ -0,0 +1,35 @@
+# Lint as: python3
+"""A scene containing only a planar floor."""
+
+from typing import Sequence
+
+import gin
+from pybullet_envs.minitaur.envs_v2 import base_client
+from pybullet_envs.minitaur.envs_v2.scenes import scene_base
+
+_PLANE_URDF = (
+ "plane.urdf")
+
+
+@gin.configurable
+class SimpleScene(scene_base.SceneBase):
+ """A scene containing only a planar floor."""
+
+ def build_scene(self, pybullet_client):
+ super().build_scene(pybullet_client)
+
+ visual_shape_id = self._pybullet_client.createVisualShape(
+ shapeType=self._pybullet_client.GEOM_PLANE)
+ collision_shape_id = self._pybullet_client.createCollisionShape(
+ shapeType=self._pybullet_client.GEOM_PLANE)
+ ground_id = self._pybullet_client.createMultiBody(
+ baseMass=0,
+ baseCollisionShapeIndex=collision_shape_id,
+ baseVisualShapeIndex=visual_shape_id)
+ self._pybullet_client.changeDynamics(ground_id, -1, lateralFriction=1.0)
+ self.add_object(ground_id, scene_base.ObjectType.GROUND)
+
+ @property
+ def vectorized_map(self) -> Sequence[scene_base.Polygon]:
+ """Returns vectorized map containing a list of polygon obstacles."""
+ return []
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/scenes/world_asset.proto b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/scenes/world_asset.proto
new file mode 100644
index 000000000..0e0b97cc4
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/scenes/world_asset.proto
@@ -0,0 +1,86 @@
+// A set of protocol buffer definitions for representing 'Worlds' for
+// used by simulation engines.
+syntax = "proto3";
+
+package robotics.messages;
+option cc_enable_arenas = true;
+
+
+// A single precision quaternion.
+message QQuaternionf {
+ // The x-component.
+ float x = 1;
+ // The y-component.
+ float y = 2;
+ // The z-component.
+ float z = 3;
+ // The w-component.
+ float w = 4;
+}
+
+
+
+
+// A three-dimensional single precision vector.
+message VVector3f {
+ float x = 1;
+ float y = 2;
+ float z = 3;
+}
+
+
+
+message Labels {
+ map label = 1;
+}
+
+// A general bounding box is specified by its center, dimensions and
+// orientation. If the orientation field is not specified, then the bounding box
+// is aligned with the axes of the world coordinate system.
+message GeneralBox3f {
+ VVector3f center = 1;
+ VVector3f dimensions = 2;
+ QQuaternionf orientation = 3;
+}
+
+// An object is specified by its label, bounding box, and a path to an obj
+// file containing its mesh. Not all to be specified.
+message Object {
+ string id = 1;
+ string label = 2;
+ GeneralBox3f bounding_box = 3;
+ string mesh_path = 4;
+}
+
+message Polygon3f {
+ repeated VVector3f vertex = 1;
+}
+
+// A region is specified by its label, spatial extent, and a path to an obj
+// file containing its mesh. Not all to be specified.
+message Region {
+ string id = 1;
+ string label = 2;
+ oneof spatial_extent {
+ GeneralBox3f bounding_box = 3;
+ Polygon3f polygon = 5;
+ }
+ string mesh_path = 6;
+}
+
+// A world consist of a mesh, and potentially a set of object and regions.
+message WorldAsset {
+ // A set of labels for objects. This the space of all object labels.
+ Labels object_labels = 1;
+ repeated Object objects = 2;
+
+ // A set of labels for regions. This the space of all region labels.
+ Labels region_labels = 3;
+ repeated Region regions = 4;
+
+ // A path to a mesh (as obj file) containing the geometry of the world.
+ string mesh_path = 5;
+
+ // A path to a mesh (as obj file) containing the semantic labels of the world.
+ string segmentation_mesh_path = 6;
+}
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/scenes/world_asset_pb2.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/scenes/world_asset_pb2.py
new file mode 100644
index 000000000..a7d99baea
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/scenes/world_asset_pb2.py
@@ -0,0 +1,555 @@
+# -*- coding: utf-8 -*-
+# Generated by the protocol buffer compiler. DO NOT EDIT!
+# source: world_asset.proto
+"""Generated protocol buffer code."""
+from google.protobuf import descriptor as _descriptor
+from google.protobuf import message as _message
+from google.protobuf import reflection as _reflection
+from google.protobuf import symbol_database as _symbol_database
+# @@protoc_insertion_point(imports)
+
+_sym_db = _symbol_database.Default()
+
+
+
+
+DESCRIPTOR = _descriptor.FileDescriptor(
+ name='world_asset.proto',
+ package='robotics.messages',
+ syntax='proto3',
+ serialized_options=b'\370\001\001',
+ create_key=_descriptor._internal_create_key,
+ serialized_pb=b'\n\x11world_asset.proto\x12\x11robotics.messages\":\n\x0cQQuaternionf\x12\t\n\x01x\x18\x01 \x01(\x02\x12\t\n\x01y\x18\x02 \x01(\x02\x12\t\n\x01z\x18\x03 \x01(\x02\x12\t\n\x01w\x18\x04 \x01(\x02\",\n\tVVector3f\x12\t\n\x01x\x18\x01 \x01(\x02\x12\t\n\x01y\x18\x02 \x01(\x02\x12\t\n\x01z\x18\x03 \x01(\x02\"k\n\x06Labels\x12\x33\n\x05label\x18\x01 \x03(\x0b\x32$.robotics.messages.Labels.LabelEntry\x1a,\n\nLabelEntry\x12\x0b\n\x03key\x18\x01 \x01(\t\x12\r\n\x05value\x18\x02 \x01(\t:\x02\x38\x01\"\xa4\x01\n\x0cGeneralBox3f\x12,\n\x06\x63\x65nter\x18\x01 \x01(\x0b\x32\x1c.robotics.messages.VVector3f\x12\x30\n\ndimensions\x18\x02 \x01(\x0b\x32\x1c.robotics.messages.VVector3f\x12\x34\n\x0borientation\x18\x03 \x01(\x0b\x32\x1f.robotics.messages.QQuaternionf\"m\n\x06Object\x12\n\n\x02id\x18\x01 \x01(\t\x12\r\n\x05label\x18\x02 \x01(\t\x12\x35\n\x0c\x62ounding_box\x18\x03 \x01(\x0b\x32\x1f.robotics.messages.GeneralBox3f\x12\x11\n\tmesh_path\x18\x04 \x01(\t\"9\n\tPolygon3f\x12,\n\x06vertex\x18\x01 \x03(\x0b\x32\x1c.robotics.messages.VVector3f\"\xb2\x01\n\x06Region\x12\n\n\x02id\x18\x01 \x01(\t\x12\r\n\x05label\x18\x02 \x01(\t\x12\x37\n\x0c\x62ounding_box\x18\x03 \x01(\x0b\x32\x1f.robotics.messages.GeneralBox3fH\x00\x12/\n\x07polygon\x18\x05 \x01(\x0b\x32\x1c.robotics.messages.Polygon3fH\x00\x12\x11\n\tmesh_path\x18\x06 \x01(\tB\x10\n\x0espatial_extent\"\xfb\x01\n\nWorldAsset\x12\x30\n\robject_labels\x18\x01 \x01(\x0b\x32\x19.robotics.messages.Labels\x12*\n\x07objects\x18\x02 \x03(\x0b\x32\x19.robotics.messages.Object\x12\x30\n\rregion_labels\x18\x03 \x01(\x0b\x32\x19.robotics.messages.Labels\x12*\n\x07regions\x18\x04 \x03(\x0b\x32\x19.robotics.messages.Region\x12\x11\n\tmesh_path\x18\x05 \x01(\t\x12\x1e\n\x16segmentation_mesh_path\x18\x06 \x01(\tB\x03\xf8\x01\x01\x62\x06proto3'
+)
+
+
+
+
+_QQUATERNIONF = _descriptor.Descriptor(
+ name='QQuaternionf',
+ full_name='robotics.messages.QQuaternionf',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='x', full_name='robotics.messages.QQuaternionf.x', index=0,
+ number=1, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='y', full_name='robotics.messages.QQuaternionf.y', index=1,
+ number=2, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='z', full_name='robotics.messages.QQuaternionf.z', index=2,
+ number=3, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='w', full_name='robotics.messages.QQuaternionf.w', index=3,
+ number=4, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=40,
+ serialized_end=98,
+)
+
+
+_VVECTOR3F = _descriptor.Descriptor(
+ name='VVector3f',
+ full_name='robotics.messages.VVector3f',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='x', full_name='robotics.messages.VVector3f.x', index=0,
+ number=1, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='y', full_name='robotics.messages.VVector3f.y', index=1,
+ number=2, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='z', full_name='robotics.messages.VVector3f.z', index=2,
+ number=3, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=100,
+ serialized_end=144,
+)
+
+
+_LABELS_LABELENTRY = _descriptor.Descriptor(
+ name='LabelEntry',
+ full_name='robotics.messages.Labels.LabelEntry',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='key', full_name='robotics.messages.Labels.LabelEntry.key', index=0,
+ number=1, type=9, cpp_type=9, label=1,
+ has_default_value=False, default_value=b"".decode('utf-8'),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='value', full_name='robotics.messages.Labels.LabelEntry.value', index=1,
+ number=2, type=9, cpp_type=9, label=1,
+ has_default_value=False, default_value=b"".decode('utf-8'),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=b'8\001',
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=209,
+ serialized_end=253,
+)
+
+_LABELS = _descriptor.Descriptor(
+ name='Labels',
+ full_name='robotics.messages.Labels',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='label', full_name='robotics.messages.Labels.label', index=0,
+ number=1, type=11, cpp_type=10, label=3,
+ has_default_value=False, default_value=[],
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[_LABELS_LABELENTRY, ],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=146,
+ serialized_end=253,
+)
+
+
+_GENERALBOX3F = _descriptor.Descriptor(
+ name='GeneralBox3f',
+ full_name='robotics.messages.GeneralBox3f',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='center', full_name='robotics.messages.GeneralBox3f.center', index=0,
+ number=1, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='dimensions', full_name='robotics.messages.GeneralBox3f.dimensions', index=1,
+ number=2, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='orientation', full_name='robotics.messages.GeneralBox3f.orientation', index=2,
+ number=3, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=256,
+ serialized_end=420,
+)
+
+
+_OBJECT = _descriptor.Descriptor(
+ name='Object',
+ full_name='robotics.messages.Object',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='id', full_name='robotics.messages.Object.id', index=0,
+ number=1, type=9, cpp_type=9, label=1,
+ has_default_value=False, default_value=b"".decode('utf-8'),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='label', full_name='robotics.messages.Object.label', index=1,
+ number=2, type=9, cpp_type=9, label=1,
+ has_default_value=False, default_value=b"".decode('utf-8'),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='bounding_box', full_name='robotics.messages.Object.bounding_box', index=2,
+ number=3, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='mesh_path', full_name='robotics.messages.Object.mesh_path', index=3,
+ number=4, type=9, cpp_type=9, label=1,
+ has_default_value=False, default_value=b"".decode('utf-8'),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=422,
+ serialized_end=531,
+)
+
+
+_POLYGON3F = _descriptor.Descriptor(
+ name='Polygon3f',
+ full_name='robotics.messages.Polygon3f',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='vertex', full_name='robotics.messages.Polygon3f.vertex', index=0,
+ number=1, type=11, cpp_type=10, label=3,
+ has_default_value=False, default_value=[],
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=533,
+ serialized_end=590,
+)
+
+
+_REGION = _descriptor.Descriptor(
+ name='Region',
+ full_name='robotics.messages.Region',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='id', full_name='robotics.messages.Region.id', index=0,
+ number=1, type=9, cpp_type=9, label=1,
+ has_default_value=False, default_value=b"".decode('utf-8'),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='label', full_name='robotics.messages.Region.label', index=1,
+ number=2, type=9, cpp_type=9, label=1,
+ has_default_value=False, default_value=b"".decode('utf-8'),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='bounding_box', full_name='robotics.messages.Region.bounding_box', index=2,
+ number=3, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='polygon', full_name='robotics.messages.Region.polygon', index=3,
+ number=5, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='mesh_path', full_name='robotics.messages.Region.mesh_path', index=4,
+ number=6, type=9, cpp_type=9, label=1,
+ has_default_value=False, default_value=b"".decode('utf-8'),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ _descriptor.OneofDescriptor(
+ name='spatial_extent', full_name='robotics.messages.Region.spatial_extent',
+ index=0, containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[]),
+ ],
+ serialized_start=593,
+ serialized_end=771,
+)
+
+
+_WORLDASSET = _descriptor.Descriptor(
+ name='WorldAsset',
+ full_name='robotics.messages.WorldAsset',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='object_labels', full_name='robotics.messages.WorldAsset.object_labels', index=0,
+ number=1, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='objects', full_name='robotics.messages.WorldAsset.objects', index=1,
+ number=2, type=11, cpp_type=10, label=3,
+ has_default_value=False, default_value=[],
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='region_labels', full_name='robotics.messages.WorldAsset.region_labels', index=2,
+ number=3, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='regions', full_name='robotics.messages.WorldAsset.regions', index=3,
+ number=4, type=11, cpp_type=10, label=3,
+ has_default_value=False, default_value=[],
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='mesh_path', full_name='robotics.messages.WorldAsset.mesh_path', index=4,
+ number=5, type=9, cpp_type=9, label=1,
+ has_default_value=False, default_value=b"".decode('utf-8'),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='segmentation_mesh_path', full_name='robotics.messages.WorldAsset.segmentation_mesh_path', index=5,
+ number=6, type=9, cpp_type=9, label=1,
+ has_default_value=False, default_value=b"".decode('utf-8'),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=774,
+ serialized_end=1025,
+)
+
+_LABELS_LABELENTRY.containing_type = _LABELS
+_LABELS.fields_by_name['label'].message_type = _LABELS_LABELENTRY
+_GENERALBOX3F.fields_by_name['center'].message_type = _VVECTOR3F
+_GENERALBOX3F.fields_by_name['dimensions'].message_type = _VVECTOR3F
+_GENERALBOX3F.fields_by_name['orientation'].message_type = _QQUATERNIONF
+_OBJECT.fields_by_name['bounding_box'].message_type = _GENERALBOX3F
+_POLYGON3F.fields_by_name['vertex'].message_type = _VVECTOR3F
+_REGION.fields_by_name['bounding_box'].message_type = _GENERALBOX3F
+_REGION.fields_by_name['polygon'].message_type = _POLYGON3F
+_REGION.oneofs_by_name['spatial_extent'].fields.append(
+ _REGION.fields_by_name['bounding_box'])
+_REGION.fields_by_name['bounding_box'].containing_oneof = _REGION.oneofs_by_name['spatial_extent']
+_REGION.oneofs_by_name['spatial_extent'].fields.append(
+ _REGION.fields_by_name['polygon'])
+_REGION.fields_by_name['polygon'].containing_oneof = _REGION.oneofs_by_name['spatial_extent']
+_WORLDASSET.fields_by_name['object_labels'].message_type = _LABELS
+_WORLDASSET.fields_by_name['objects'].message_type = _OBJECT
+_WORLDASSET.fields_by_name['region_labels'].message_type = _LABELS
+_WORLDASSET.fields_by_name['regions'].message_type = _REGION
+DESCRIPTOR.message_types_by_name['QQuaternionf'] = _QQUATERNIONF
+DESCRIPTOR.message_types_by_name['VVector3f'] = _VVECTOR3F
+DESCRIPTOR.message_types_by_name['Labels'] = _LABELS
+DESCRIPTOR.message_types_by_name['GeneralBox3f'] = _GENERALBOX3F
+DESCRIPTOR.message_types_by_name['Object'] = _OBJECT
+DESCRIPTOR.message_types_by_name['Polygon3f'] = _POLYGON3F
+DESCRIPTOR.message_types_by_name['Region'] = _REGION
+DESCRIPTOR.message_types_by_name['WorldAsset'] = _WORLDASSET
+_sym_db.RegisterFileDescriptor(DESCRIPTOR)
+
+QQuaternionf = _reflection.GeneratedProtocolMessageType('QQuaternionf', (_message.Message,), {
+ 'DESCRIPTOR' : _QQUATERNIONF,
+ '__module__' : 'world_asset_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.QQuaternionf)
+ })
+_sym_db.RegisterMessage(QQuaternionf)
+
+VVector3f = _reflection.GeneratedProtocolMessageType('VVector3f', (_message.Message,), {
+ 'DESCRIPTOR' : _VVECTOR3F,
+ '__module__' : 'world_asset_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.VVector3f)
+ })
+_sym_db.RegisterMessage(VVector3f)
+
+Labels = _reflection.GeneratedProtocolMessageType('Labels', (_message.Message,), {
+
+ 'LabelEntry' : _reflection.GeneratedProtocolMessageType('LabelEntry', (_message.Message,), {
+ 'DESCRIPTOR' : _LABELS_LABELENTRY,
+ '__module__' : 'world_asset_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.Labels.LabelEntry)
+ })
+ ,
+ 'DESCRIPTOR' : _LABELS,
+ '__module__' : 'world_asset_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.Labels)
+ })
+_sym_db.RegisterMessage(Labels)
+_sym_db.RegisterMessage(Labels.LabelEntry)
+
+GeneralBox3f = _reflection.GeneratedProtocolMessageType('GeneralBox3f', (_message.Message,), {
+ 'DESCRIPTOR' : _GENERALBOX3F,
+ '__module__' : 'world_asset_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.GeneralBox3f)
+ })
+_sym_db.RegisterMessage(GeneralBox3f)
+
+Object = _reflection.GeneratedProtocolMessageType('Object', (_message.Message,), {
+ 'DESCRIPTOR' : _OBJECT,
+ '__module__' : 'world_asset_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.Object)
+ })
+_sym_db.RegisterMessage(Object)
+
+Polygon3f = _reflection.GeneratedProtocolMessageType('Polygon3f', (_message.Message,), {
+ 'DESCRIPTOR' : _POLYGON3F,
+ '__module__' : 'world_asset_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.Polygon3f)
+ })
+_sym_db.RegisterMessage(Polygon3f)
+
+Region = _reflection.GeneratedProtocolMessageType('Region', (_message.Message,), {
+ 'DESCRIPTOR' : _REGION,
+ '__module__' : 'world_asset_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.Region)
+ })
+_sym_db.RegisterMessage(Region)
+
+WorldAsset = _reflection.GeneratedProtocolMessageType('WorldAsset', (_message.Message,), {
+ 'DESCRIPTOR' : _WORLDASSET,
+ '__module__' : 'world_asset_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.WorldAsset)
+ })
+_sym_db.RegisterMessage(WorldAsset)
+
+
+DESCRIPTOR._options = None
+_LABELS_LABELENTRY._options = None
+# @@protoc_insertion_point(module_scope)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/__init__.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/accelerometer_sensor.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/accelerometer_sensor.py
new file mode 100644
index 000000000..954e8ecb3
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/accelerometer_sensor.py
@@ -0,0 +1,86 @@
+# Lint as: python3
+"""A sensor that measures the acceleration of the robot base."""
+
+from typing import Any, Callable, Sequence, Type, Text, Union
+
+import gin
+import gym
+import numpy as np
+
+from pybullet_envs.minitaur.envs_v2.sensors import sensor
+from pybullet_envs.minitaur.envs_v2.utilities import noise_generators
+
+_ACCELEROMETER_DIM = 3
+_DEFAULT_ACCELEROMETER_LOWER_BOUND = (-1, -1, -1)
+_DEFAULT_ACCELEROMETER_UPPER_BOUND = (1, 1, 1)
+
+
+@gin.configurable
+class AccelerometerSensor(sensor.Sensor):
+ """An Accelerometer sensor."""
+
+ def __init__(
+ self,
+ name: Text = "Accelerometer",
+ dtype: Type[Any] = np.float64,
+ lower_bound: Sequence[float] = _DEFAULT_ACCELEROMETER_LOWER_BOUND,
+ upper_bound: Sequence[float] = _DEFAULT_ACCELEROMETER_UPPER_BOUND,
+ noise_generator: Union[Callable[..., Any],
+ noise_generators.NoiseGenerator] = None,
+ sensor_latency: Union[float, Sequence[float]] = 0.0,
+ ):
+ """Constructs AccelerometerSensor.
+
+ Generates separate IMU value channels as per configuration.
+
+ Args:
+ name: the name of the sensor.
+ dtype: data type of sensor value.
+ lower_bound: The lower bounds of the sensor reading.
+ upper_bound: The upper bounds of the sensor reading.
+ noise_generator: Used to add noise to the readings.
+ sensor_latency: There are two ways to use this expected sensor latency.
+ For both methods, the latency should be in the same unit as the sensor
+ data timestamp. 1. As a single float number, the observation will be a
+ 1D array. For real robots, this should be set to 0.0. 2. As a array of
+ floats, the observation will be a 2D array based on how long the history
+ need to be. Thus, [0.0, 0.1, 0.2] is a history length of 3.
+
+ """
+ super().__init__(
+ name=name,
+ sensor_latency=sensor_latency,
+ interpolator_fn=sensor.linear_obs_blender)
+
+ self._noise_generator = noise_generator
+ self._dtype = dtype
+
+ if lower_bound is None or upper_bound is None:
+ raise ValueError("Must provides bounds for the Accelerometer readings.")
+
+ if len(lower_bound) != _ACCELEROMETER_DIM or len(
+ upper_bound) != _ACCELEROMETER_DIM:
+ raise ValueError(
+ "Bounds must be {} dimensions.".format(_ACCELEROMETER_DIM))
+
+ lower_bound = np.array(lower_bound, dtype=dtype)
+ upper_bound = np.array(upper_bound, dtype=dtype)
+
+ self._observation_space = self._stack_space(
+ gym.spaces.Box(
+ low=np.array(lower_bound, dtype=self._dtype),
+ high=np.array(upper_bound, dtype=self._dtype),
+ dtype=self._dtype))
+
+ def _get_original_observation(self):
+ return self._robot.timestamp, np.array(
+ self._robot.base_acceleration_accelerometer, dtype=self._dtype)
+
+ def get_observation(self) -> np.ndarray:
+ delayed_observation = super().get_observation()
+ if self._noise_generator:
+ if callable(self._noise_generator):
+ return self._noise_generator(delayed_observation)
+ else:
+ return self._noise_generator.add_noise(delayed_observation)
+ return delayed_observation
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/camera_sensor.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/camera_sensor.py
new file mode 100644
index 000000000..d1c6bde8c
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/camera_sensor.py
@@ -0,0 +1,184 @@
+# Lint as: python3
+"""A sensor for robot-mounted 1D lidar (laser scan)."""
+
+from typing import Any, Iterable, Sequence, Type, Union
+
+import gin
+import gym
+import numpy as np
+
+from pybullet_envs.minitaur.envs_v2.sensors import sensor
+from pybullet_envs.minitaur.vision import point_cloud_utils
+from pybullet_envs.minitaur.vision import sim_camera
+
+_MODE_TO_NUM_CHANNELS_DICT = {
+ sim_camera.CameraMode.DEPTH: 1,
+ sim_camera.CameraMode.RGB: 3,
+ sim_camera.CameraMode.RGBD: 4,
+ sim_camera.CameraMode.POINTCLOUD_WORLD_FRAME: 3,
+ sim_camera.CameraMode.POINTCLOUD_ROBOT_FRAME: 3,
+}
+
+
+@gin.configurable
+class CameraSensor(sensor.Sensor):
+ """A robot-mounted sensor that returns RGBD images.
+
+ Attributes:
+ resolution: A 2-tuple (width, height) that represents the resolution of the
+ camera.
+ fov_degree: A floating point value that represents the field of view of the
+ camera in the vertical direction. The unit is degree.
+ """
+
+ def __init__(self,
+ camera_translation_from_base,
+ camera_rotation_from_base,
+ parent_link_id=-1,
+ camera_mode=sim_camera.CameraMode.DEPTH,
+ camera_update_frequency_hz=10,
+ camera_stabilized=False,
+ fov_degree=60,
+ resolution=(32, 32),
+ lower_bound: Union[float, Iterable[float]] = 0.0,
+ upper_bound: Union[float, Iterable[float]] = 255.0,
+ sensor_latency: Union[float, Sequence[float]] = 0.0,
+ dtype: Type[Any] = np.float64,
+ name="vision"):
+ """Initializes the CameraSensor.
+
+ Args:
+ camera_translation_from_base: A 3-vector translation from the center of
+ the specified link.
+ camera_rotation_from_base: A 4-vector quaternion represents the rotation
+ of the camera relative to the specified link.
+ parent_link_id: The pybullet link id, where the camera is mounted on.
+ camera_mode: An enum that specifies the mode that the camera operates. See
+ sim_camera.CameraMode for more details.
+ camera_update_frequency_hz: The frequency at which the camera will capture
+ a frame.
+ camera_stabilized: Whether the camera is stabilized. See
+ sim_camera.MountedCamera for more details.
+ fov_degree: The vertical field of view of the camera (in degree).
+ resolution: A 2-tuple that represents the width and the height of the
+ camera image.
+ lower_bound: The lower bound of values of the camera output. It could be a
+ single float or an array of floats with shape (height, width, channels).
+ upper_bound: The upper bound of values of the camera output. It could be a
+ single float or an array of floats with shape (height, width,
+ channels)..
+ sensor_latency: See base class.
+ dtype: See base class.
+ name: The name of the sensor.
+ """
+ super().__init__(
+ name=name,
+ sensor_latency=sensor_latency,
+ interpolator_fn=sensor.closest_obs_blender)
+ self._parent_link_id = parent_link_id
+ self._camera_mode = camera_mode
+ self._camera_translation_from_base = camera_translation_from_base
+ self._camera_rotation_from_base = camera_rotation_from_base
+ self.camera_update_frequency_hz = camera_update_frequency_hz
+ self._time_interval_every_camera_update = (1.0 /
+ self.camera_update_frequency_hz)
+ self._camera_stabilized = camera_stabilized
+ self._fov_degree = fov_degree
+ self._resolution = resolution
+ num_channels = _MODE_TO_NUM_CHANNELS_DICT[self._camera_mode]
+ self._camera = None
+ self._camera_image = None
+ self._dtype = dtype
+ if isinstance(lower_bound, list):
+ lower_bound = np.array(lower_bound, dtype=self._dtype)
+ else:
+ lower_bound = lower_bound * np.ones(
+ shape=(resolution[1], resolution[0], num_channels), dtype=self._dtype)
+ if isinstance(upper_bound, list):
+ upper_bound = np.array(upper_bound, dtype=self._dtype)
+ else:
+ upper_bound = upper_bound * np.ones(
+ shape=(resolution[1], resolution[0], num_channels), dtype=self._dtype)
+ self._observation_space = gym.spaces.Box(
+ low=lower_bound, high=upper_bound, dtype=self._dtype)
+ self._last_camera_image_timestamp = None
+
+ def change_mounting_point(
+ self,
+ camera_translation_from_link: Sequence[float] = (0, 0, 0),
+ camera_rotation_from_link: Sequence[float] = (0, 0, 0, 1),
+ parent_link_id: int = -1):
+ """Changes mounting point. Must be called before calls to set_robot().
+
+ Args:
+ camera_translation_from_link: A 3-vector translation from the center of
+ the specified link.
+ camera_rotation_from_link: A 4-vector quaternion represents the rotation
+ of the camera relative to the specified link.
+ parent_link_id: The pybullet link id, where the camera is mounted on.
+ """
+ self._parent_link_id = parent_link_id
+ self._camera_translation_from_base = camera_translation_from_link
+ self._camera_rotation_from_base = camera_rotation_from_link
+
+ def set_robot(self, robot):
+ super().set_robot(robot)
+
+ self._camera = sim_camera.MountedCamera(
+ pybullet_client=robot.pybullet_client,
+ body_id=robot.robot_id,
+ parent_link_id=self._parent_link_id,
+ relative_translation=self._camera_translation_from_base,
+ relative_rotation=self._camera_rotation_from_base,
+ stabilized=self._camera_stabilized,
+ camera_mode=self._camera_mode,
+ fov_degree=self._fov_degree,
+ resolution=self._resolution)
+
+ def on_reset(self, env):
+ self._env = env
+ self._last_camera_image_timestamp = None
+ super().on_reset(env)
+
+ def _get_original_observation(self):
+ if self._last_camera_image_timestamp is None or (
+ self._robot.timestamp >= self._last_camera_image_timestamp +
+ self._time_interval_every_camera_update):
+ self._camera_image = self._camera.render_image().astype(self._dtype)
+ self._last_camera_image_timestamp = self._robot.timestamp
+ return self._robot.timestamp, self._camera_image
+
+ def project_depth_map_to_point_cloud(self, depth_map, use_world_frame=True):
+ """Convert the depth map into a 3D point cloud.
+
+ Args:
+ depth_map: A 2D numpy array with shape (height, width) which represents
+ the depth map.
+ use_world_frame: Whether converts the depth map into a point cloud in the
+ world frame. If False, the point cloud is in the robot's local frame. If
+ True, the point cloud is in the world frame if the robot's base
+ position/orientation can be measured (e.g. in sim, using SLAM or mocap).
+
+ Returns:
+ A point cloud represented by a numpy array of shape (height, width, 3).
+ """
+ point_cloud = point_cloud_utils.distance_map_to_point_cloud(
+ np.squeeze(depth_map), self.fov_degree / 180.0 * np.pi,
+ depth_map.shape[1], depth_map.shape[0])
+ if use_world_frame:
+ point_cloud = (
+ self._camera.transform_point_cloud_from_camera_to_world_frame(
+ point_cloud))
+ else:
+ point_cloud = (
+ self._camera.transform_point_cloud_from_camera_to_robot_frame(
+ point_cloud))
+ return point_cloud
+
+ @property
+ def resolution(self):
+ return self._camera.resolution
+
+ @property
+ def fov_degree(self):
+ return self._camera.fov_degree
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/imu_sensor.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/imu_sensor.py
new file mode 100644
index 000000000..a26cf491d
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/imu_sensor.py
@@ -0,0 +1,133 @@
+# Lint as: python3
+"""The on robot sensor classes."""
+
+import enum
+from typing import Any, Callable, Iterable, Sequence, Type, Text, Union
+
+import gin
+import gym
+import numpy as np
+
+from pybullet_envs.minitaur.envs_v2.sensors import sensor
+from pybullet_envs.minitaur.envs_v2.utilities import noise_generators
+
+
+@gin.constants_from_enum
+class IMUChannel(enum.Enum):
+ ROLL = 1,
+ PITCH = 2,
+ YAW = 3,
+ ROLL_RATE = 4,
+ PITCH_RATE = 5,
+ YAW_RATE = 6,
+
+
+@gin.configurable
+class IMUSensor(sensor.Sensor):
+ """An IMU sensor."""
+
+ def __init__(
+ self,
+ name: Text = "IMU",
+ dtype: Type[Any] = np.float64,
+ channels: Sequence[IMUChannel] = None,
+ lower_bound: Union[float, Iterable[float]] = None,
+ upper_bound: Union[float, Iterable[float]] = None,
+ noise_generator: Union[Callable[..., Any],
+ noise_generators.NoiseGenerator] = None,
+ sensor_latency: Union[float, Sequence[float]] = 0.0,
+ ):
+ """Constructs IMUSensor.
+
+ Generates separate IMU value channels as per configuration.
+
+ Args:
+ name: the name of the sensor.
+ dtype: data type of sensor value.
+ channels: value channels wants to subscribe. Must be members of the
+ IMUChannel class.
+ lower_bound: The lower bounds of the sensor reading.
+ upper_bound: The upper bounds of the sensor reading.
+ noise_generator: Used to add noise to the readings.
+ sensor_latency: There are two ways to use this expected sensor latency.
+ For both methods, the latency should be in the same unit as the sensor
+ data timestamp. 1. As a single float number, the observation will be a
+ 1D array. For real robots, this should be set to 0.0. 2. As a array of
+ floats, the observation will be a 2D array based on how long the history
+ need to be. Thus, [0.0, 0.1, 0.2] is a history length of 3.
+
+ Raises:
+ ValueError: If no IMU channel is provided and no bounds for the channels.
+ """
+ super().__init__(
+ name=name,
+ sensor_latency=sensor_latency,
+ interpolator_fn=sensor.linear_obs_blender)
+ if channels is None:
+ raise ValueError("IMU channels are not provided.")
+ self._channels = channels
+ self._num_channels = len(self._channels)
+ self._noise_generator = noise_generator
+ self._dtype = dtype
+
+ if lower_bound is None or upper_bound is None:
+ raise ValueError("Must provides bounds for the IMU readings.")
+
+ if isinstance(lower_bound, (float, int)):
+ lower_bound = np.full(self._num_channels, lower_bound, dtype=dtype)
+ else:
+ lower_bound = np.array(lower_bound, dtype=dtype)
+
+ if len(lower_bound) != self._num_channels:
+ raise ValueError("length of sensor lower bound {lower_bound} does not"
+ " match the number of channels.")
+
+ if isinstance(upper_bound, (float, int)):
+ upper_bound = np.full(self._num_channels, upper_bound, dtype=dtype)
+ else:
+ upper_bound = np.array(upper_bound, dtype=dtype)
+
+ if len(upper_bound) != self._num_channels:
+ raise ValueError("length of sensor upper bound {upper_bound} does not"
+ " match the number of channels.")
+
+ self._observation_space = self._stack_space(
+ gym.spaces.Box(
+ low=np.array(lower_bound, dtype=self._dtype),
+ high=np.array(upper_bound, dtype=self._dtype),
+ dtype=self._dtype))
+
+ def get_channels(self) -> Sequence[IMUChannel]:
+ return self._channels
+
+ def get_num_channels(self) -> int:
+ return self._num_channels
+
+ def _get_original_observation(self):
+ rpy = self._robot.base_roll_pitch_yaw
+ observations = np.zeros(self._num_channels)
+ for i, channel in enumerate(self._channels):
+ if channel == IMUChannel.ROLL:
+ observations[i] = rpy[0]
+ elif channel == IMUChannel.PITCH:
+ observations[i] = rpy[1]
+ elif channel == IMUChannel.YAW:
+ observations[i] = rpy[2]
+ elif channel == IMUChannel.ROLL_RATE:
+ observations[i] = self._robot.base_roll_pitch_yaw_rate[0]
+ elif channel == IMUChannel.PITCH_RATE:
+ observations[i] = self._robot.base_roll_pitch_yaw_rate[1]
+ elif channel == IMUChannel.YAW_RATE:
+ observations[i] = self._robot.base_roll_pitch_yaw_rate[2]
+
+ return self._robot.timestamp, np.array(observations, dtype=self._dtype)
+
+ def get_observation(self) -> np.ndarray:
+ delayed_observation = super().get_observation()
+ if self._noise_generator:
+ if callable(self._noise_generator):
+ return self._noise_generator(delayed_observation)
+ else:
+ return self._noise_generator.add_noise(delayed_observation)
+
+ return delayed_observation
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/last_action_sensor.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/last_action_sensor.py
new file mode 100644
index 000000000..e61299f02
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/last_action_sensor.py
@@ -0,0 +1,60 @@
+# Lint as: python3
+"""A sensor that returns the last action(s) sent to the environment."""
+
+from typing import Any, Dict, Sequence, Text, Type, Tuple, Union
+import gin
+import gym
+import numpy as np
+
+from pybullet_envs.minitaur.envs_v2.sensors import sensor
+from pybullet_envs.minitaur.envs_v2.sensors import space_utils
+
+
+@gin.configurable
+class LastActionSensor(sensor.Sensor):
+ """A sensor that reports the last action taken."""
+
+ def __init__(self,
+ name: Text = "LastAction",
+ dtype: Type[Any] = np.float64,
+ sensor_latency: Union[float, Sequence[float]] = 0):
+ """Constructs LastActionSensor.
+
+ We do not provide a robot instance during __init__, as robot instances may
+ be reloaded/recreated during the simulation.
+
+ Args:
+ name: the name of the sensor
+ dtype: data type of sensor value.
+ sensor_latency: There are two ways to use this expected sensor latency.
+ For both methods, the latency should be in the same unit as the sensor
+ data timestamp. 1. As a single float number, the observation will be a
+ 1D array. For real robots, this should be set to 0.0. 2. As a array of
+ floats, the observation will be a 2D array based on how long the history
+ need to be. Thus, [0.0, 0.1, 0.2] is a history length of 3.
+ """
+ super().__init__(name=name,
+ sensor_latency=sensor_latency,
+ # We generally don't interpolate actions.
+ interpolator_fn=sensor.older_obs_blender)
+
+ self._dtype = dtype
+ self._env = None
+
+ def on_reset(self, env: gym.Env):
+ """From the callback, the sensor remembers the environment.
+
+ Args:
+ env: the environment who invokes this callback function.
+ """
+ # Constructs the observation space using the env's action space.
+ self._observation_space = self._stack_space(
+ env.action_space, dtype=self._dtype)
+
+ # Call the super class methods to initialize the buffers
+ super().on_reset(env)
+
+ def _get_original_observation(
+ self) -> Tuple[float, Union[Dict[Text, np.ndarray], np.ndarray]]:
+ return self._env.get_time(), space_utils.action_astype(
+ self._env.last_action, self._dtype)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/motor_angle_sensor.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/motor_angle_sensor.py
new file mode 100644
index 000000000..ce1754547
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/motor_angle_sensor.py
@@ -0,0 +1,122 @@
+# Lint as: python3
+"""The on robot sensor classes."""
+
+from typing import Any, Callable, Iterable, Optional, Sequence, Type, Text, Tuple, Union
+
+import gin
+import gym
+import numpy as np
+
+from pybullet_envs.minitaur.envs_v2.sensors import sensor
+
+from pybullet_envs.minitaur.envs_v2.utilities import noise_generators
+
+
+def _convert_to_np_array(inputs: Union[float, Tuple[float], np.ndarray], dim):
+ """Converts the inputs to a numpy array.
+
+ Args:
+ inputs: The input scalar or array.
+ dim: The dimension of the converted numpy array.
+
+ Returns:
+ The converted numpy array.
+
+ Raises:
+ ValueError: If the inputs is an array whose dimension does not match the
+ provided dimension.
+ """
+ outputs = None
+ if isinstance(inputs, (tuple, np.ndarray)):
+ outputs = np.array(inputs)
+ else:
+ outputs = np.full(dim, inputs)
+
+ if len(outputs) != dim:
+ raise ValueError("The inputs array has a different dimension {}"
+ " than provided, which is {}.".format(len(outputs), dim))
+
+ return outputs
+
+
+@gin.configurable
+class MotorAngleSensor(sensor.Sensor):
+ """A sensor that reads motor angles from the robot."""
+
+ def __init__(self,
+ name: Text = "MotorAngle",
+ dtype: Type[Any] = np.float64,
+ lower_bound: Optional[Union[float, Iterable[float]]] = None,
+ upper_bound: Optional[Union[float, Iterable[float]]] = None,
+ noise_generator: Union[Callable[..., Any],
+ noise_generators.NoiseGenerator] = None,
+ sensor_latency: Union[float, Sequence[float]] = 0.0,
+ observe_sine_cosine: bool = False):
+ """Initializes the class.
+
+ Args:
+ name: The name of the sensor.
+ dtype: The datatype of this sensor.
+ lower_bound: The optional lower bounds of the sensor reading. If not
+ provided, will extract from the motor limits of the robot class.
+ upper_bound: The optional upper bounds of the sensor reading. If not
+ provided, will extract from the motor limits of the robot class.
+ noise_generator: Adds noise to the sensor readings.
+ sensor_latency: There are two ways to use this expected sensor latency.
+ For both methods, the latency should be in the same unit as the sensor
+ data timestamp. 1. As a single float number, the observation will be a
+ 1D array. For real robots, this should be set to 0.0. 2. As a array of
+ floats, the observation will be a 2D array based on how long the history
+ need to be. Thus, [0.0, 0.1, 0.2] is a history length of 3.
+ observe_sine_cosine: whether to observe motor angles as sine and cosine
+ values.
+ """
+ super().__init__(
+ name=name,
+ sensor_latency=sensor_latency,
+ interpolator_fn=sensor.linear_obs_blender)
+ self._noise_generator = noise_generator
+ self._dtype = dtype
+ self._lower_bound = lower_bound
+ self._upper_bound = upper_bound
+ self._observe_sine_cosine = observe_sine_cosine
+
+ def set_robot(self, robot):
+ self._robot = robot
+ # Creates the observation space based on the robot motor limitations.
+ if self._observe_sine_cosine:
+ lower_bound = _convert_to_np_array(-1, 2 * self._robot.num_motors)
+ elif self._lower_bound:
+ lower_bound = _convert_to_np_array(self._lower_bound,
+ self._robot.num_motors)
+ else:
+ lower_bound = _convert_to_np_array(
+ self._robot.motor_limits.angle_lower_limits, self._robot.num_motors)
+ if self._observe_sine_cosine:
+ upper_bound = _convert_to_np_array(1, 2 * self._robot.num_motors)
+ elif self._upper_bound:
+ upper_bound = _convert_to_np_array(self._upper_bound,
+ self._robot.num_motors)
+ else:
+ upper_bound = _convert_to_np_array(
+ self._robot.motor_limits.angle_upper_limits, self._robot.num_motors)
+
+ self._observation_space = self._stack_space(
+ gym.spaces.Box(low=lower_bound, high=upper_bound, dtype=self._dtype))
+
+ def _get_original_observation(self):
+ if self._observe_sine_cosine:
+ return self._robot.timestamp, np.hstack(
+ (np.cos(self._robot.motor_angles), np.sin(self._robot.motor_angles)))
+ else:
+ return self._robot.timestamp, self._robot.motor_angles
+
+ def get_observation(self) -> np.ndarray:
+ delayed_observation = super().get_observation()
+ if self._noise_generator:
+ if callable(self._noise_generator):
+ return self._noise_generator(delayed_observation)
+ else:
+ return self._noise_generator.add_noise(delayed_observation)
+
+ return delayed_observation
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/sensor.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/sensor.py
new file mode 100644
index 000000000..e3208cdc3
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/sensor.py
@@ -0,0 +1,451 @@
+# Lint as: python3
+"""A sensor prototype class.
+
+The concept is explained in: go/minitaur-gym-redesign-1.1
+"""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+from typing import Any, Iterable, Optional, Sequence, Text, Tuple, Union
+
+import gin
+import gym
+import numpy as np
+
+from pybullet_envs.minitaur.robots import robot_base
+from pybullet_envs.minitaur.robots import time_ordered_buffer
+
+_ARRAY = Sequence[float]
+_FloatOrArray = Union[float, _ARRAY]
+_DataTypeList = Iterable[Any]
+
+# For sensor with multiput outputs, key of the main observation in output dict.
+MAIN_OBS_KEY = ""
+
+# This allows referencing np.float32 in gin config files. For example:
+# lidar_sensor.LidarSensor.dtype = @np.float32
+gin.external_configurable(np.float32, module="np")
+gin.external_configurable(np.float64, module="np")
+gin.external_configurable(np.uint8, module="np")
+
+
+# Observation blenders take a pair of low/high values. The low/high is measured
+# by the latency of the observation. So the low value is actually newer in time
+# and high value older. The coeff [0, 1] can be thinked as the distance between
+# the low and high value value, with 0 being 100% low value and 1 as 100% high
+# value.
+def linear_obs_blender(low_value: Any, high_value: Any, coeff: float):
+ """Linear interpolation of low/high values based on coefficient value."""
+ return low_value * (1 - coeff) + high_value * coeff
+
+
+def closest_obs_blender(low_value: Any, high_value: Any, coeff: float):
+ """Choosing the high or low value based on coefficient value."""
+ return low_value if coeff < 0.5 else high_value
+
+
+def newer_obs_blender(low_value: Any, unused_high_value: Any,
+ unused_coeff: float):
+ """Always choosing low value, which is the newer value between low/high."""
+ return low_value
+
+
+def older_obs_blender(unused_low_value: Any, high_value: Any,
+ unused_coeff: float):
+ """Always choosing the high value, which is the older value between low/high."""
+ return high_value
+
+
+@gin.configurable
+class Sensor(object):
+ """A prototype class of sensors."""
+
+ def __init__(
+ self,
+ name: Text,
+ sensor_latency: _FloatOrArray,
+ interpolator_fn: Any,
+ enable_debug_visualization: bool = False,
+ ):
+ """A basic constructor of the sensor.
+
+ We do not provide a robot instance during __init__, as robot instances may
+ be reloaded/recreated during the simulation.
+
+ Args:
+ name: the name of the sensor
+ sensor_latency: There are two ways to use this expected sensor latency.
+ For both methods, the latency should be in the same unit as the sensor
+ data timestamp. 1. As a single float number, the observation will be a
+ 1D array. For real robots, this should be set to 0.0. 2. As an array of
+ floats, the observation will be a 2D array based on how long the history
+ need to be. Thus, [0.0, 0.1, 0.2] is a history length of 3. Observations
+ are stacked on a new axis appended after existing axes.
+ interpolator_fn: Function that controls how to interpolate the two values
+ that is returned from the time ordered buffer.
+ enable_debug_visualization: Whether to draw debugging visualization.
+ """
+ self._robot = None
+ self._name = name
+ # Observation space will be implemented by derived classes.
+ self._observation_space = None
+ self._sensor_latency = sensor_latency
+ self._single_latency = True if isinstance(sensor_latency,
+ (float, int)) else False
+ self._enable_debug_visualization = enable_debug_visualization
+ if not self._is_valid_latency():
+ raise ValueError("sensor_latency is expected to be a non-negative number "
+ "or a non-empty list of non-negative numbers.")
+ self._interpolator_fn = interpolator_fn or newer_obs_blender
+ self._axis = -1
+ timespan = sensor_latency if self._single_latency else max(sensor_latency)
+ self._observation_buffer = time_ordered_buffer.TimeOrderedBuffer(
+ max_buffer_timespan=timespan)
+
+ def _is_valid_latency(self):
+ if self._single_latency:
+ return self._sensor_latency >= 0
+ if self._sensor_latency:
+ return all(value >= 0 for value in self._sensor_latency)
+ return False
+
+ def get_name(self) -> Text:
+ return self._name
+
+ @property
+ def is_single_latency(self) -> bool:
+ return self._single_latency
+
+ @property
+ def observation_space(self) -> gym.spaces.Space:
+ return self._observation_space
+
+ @property
+ def enable_debug_visualization(self):
+ return self._enable_debug_visualization
+
+ @enable_debug_visualization.setter
+ def enable_debug_visualization(self, enable):
+ self._enable_debug_visualization = enable
+
+ def get_observation_datatype(self):
+ """Returns the data type for the numpy structured array.
+
+ It is recommended to define a list of tuples: (name, datatype, shape)
+ Reference: https://docs.scipy.org/doc/numpy-1.15.0/user/basics.rec.html
+ Ex:
+ return [('motor_angles', np.float64, (8, ))] # motor angle sensor
+ return [('IMU_x', np.float64), ('IMU_z', np.float64), ] # IMU
+ Will be deprecated (b/150818246) in favor of observation_space.
+
+ Returns:
+ datatype: a list of data types.
+ """
+ raise NotImplementedError("Deprecated. Are you using the old robot class?")
+
+ def get_lower_bound(self):
+ """Returns the lower bound of the observation.
+
+ Will be deprecated (b/150818246) in favor of observation_space.
+
+ Returns:
+ lower_bound: the lower bound of sensor values in np.array format
+ """
+ raise NotImplementedError("Deprecated. Are you using the old robot class?")
+
+ def get_upper_bound(self):
+ """Returns the upper bound of the observation.
+
+ Will be deprecated (b/150818246) in favor of observation_space.
+
+ Returns:
+ upper_bound: the upper bound of sensor values in np.array format
+ """
+ raise NotImplementedError("Deprecated. Are you using the old robot class?")
+
+ def _get_original_observation(self) -> Tuple[float, Any]:
+ """Gets the non-modified observation.
+
+ Different from the get_observation, which can pollute and sensor data with
+ noise and latency, this method shall return the best effort measurements of
+ the sensor. For simulated robots, this will return the clean data. For reals
+ robots, just return the measurements as is. All inherited class shall
+ implement this method.
+
+ Returns:
+ The timestamp and the original sensor measurements.
+
+ Raises:
+ NotImplementedError for the base class.
+
+ """
+ raise NotImplementedError("Not implemented for base class." "")
+
+ def get_observation(self):
+ """Returns the observation data.
+
+ Returns:
+ observation: the observed sensor values in np.array format
+ """
+ obs = self._observation_buffer.get_delayed_value(self._sensor_latency)
+
+ if self._single_latency:
+ if isinstance(self._observation_space, gym.spaces.Dict):
+ return self._interpolator_fn(obs.value_0, obs.value_1, obs.coeff)
+ else:
+ return np.asarray(
+ self._interpolator_fn(obs.value_0, obs.value_1, obs.coeff))
+ else:
+ if isinstance(self._observation_space, gym.spaces.Dict):
+ # interpolate individual sub observation
+ interpolated = [
+ self._interpolator_fn(data.value_0, data.value_1, data.coeff)
+ for data in obs
+ ]
+
+ stacked_per_sub_obs = {}
+ for k in interpolated[0]:
+ stacked_per_sub_obs[k] = np.stack(
+ np.asarray([d[k] for d in interpolated]), axis=self._axis)
+ return stacked_per_sub_obs
+ else:
+ obs = np.asarray([
+ self._interpolator_fn(data.value_0, data.value_1, data.coeff)
+ for data in obs
+ ])
+ return np.stack(obs, axis=self._axis)
+
+ def set_robot(self, robot: robot_base.RobotBase):
+ """Set a robot instance."""
+ self._robot = robot
+
+ def get_robot(self):
+ """Returns the robot instance."""
+ return self._robot
+
+ def on_reset(self, env):
+ """A callback function for the reset event.
+
+ Args:
+ env: the environment who invokes this callback function.
+ """
+ self._env = env
+ self._observation_buffer.reset()
+ self.on_new_observation()
+
+ def on_step(self, env):
+ """A callback function for the control step event.
+
+ Args:
+ env: the environment who invokes this callback function.
+ """
+ pass
+
+ def visualize(self):
+ """Visualizes the sensor information."""
+ pass
+
+ def on_new_observation(self):
+ """A callback for each observation received.
+
+ To be differentiated from on_step, which will be called only once per
+ control step (i.e. env.step), this API will be called everytime in the
+ substep/action repeat loop, when new observations are expected. Each derived
+ sensor class should implement this API by implementing:
+
+ my_obs = call env/robot api to get the observation
+ self._observation_buffer.add(my_obs)
+ """
+ timestamp, obs = self._get_original_observation()
+ if self._enable_debug_visualization:
+ self.visualize()
+ self._observation_buffer.add(timestamp, obs)
+
+ def on_terminate(self, env):
+ """A callback function for the terminate event.
+
+ Args:
+ env: the environment who invokes this callback function.
+ """
+ pass
+
+ def _stack_space(self,
+ space: Union[gym.spaces.Box, gym.spaces.Dict],
+ dtype: np.dtype = None) -> Any:
+ """Returns stacked version of observation space.
+
+ This stacks a gym.spaces.Box or gym.spaces.Dict action space based on the
+ length of the sensor latency and the axis for stacking specified in the
+ sensor. A gym.spaces.Box is just stacked, but a gym.spaces.Dict is
+ recursively stacked, preserving its dictionary structure while stacking
+ any gym.spaces.Box contained within. For example, the input action space:
+
+ gym.spaces.Dict({
+ 'space_1': gym.spaces.Box(low=0, high=10, shape=(1,)),
+ 'space_2': gym.spaces.Dict({
+ 'space_3': gym.spaces.Box(low=0, high=10, shape=(2,)),
+ }),
+ }))
+
+ would be converted to the following if sensor latency was [0, 1]:
+
+ gym.spaces.Dict({
+ 'space_1': gym.spaces.Box(low=0, high=10, shape=(1, 2)),
+ 'space_2': gym.spaces.Dict({
+ 'space_3': gym.spaces.Box(low=0, high=10, shape=(2, 2)),
+ }),
+ }))
+
+ Args:
+ space: A gym.spaces.Dict or gym.spaces.Box to be stacked.
+ dtype: Datatype for the stacking.
+
+ Returns:
+ stacked_space: A stacked version of the action space.
+ """
+ if self._single_latency:
+ return space
+
+ # Allow sensors such as last_action_sensor to override the dtype.
+ dtype = dtype or space.dtype
+
+ if isinstance(space, gym.spaces.Box):
+ return self._stack_space_box(space, dtype)
+ elif isinstance(space, gym.spaces.Dict):
+ return self._stack_space_dict(space, dtype)
+ else:
+ raise ValueError(f"Space {space} is an unsupported type.")
+
+ def _stack_space_box(self, space: gym.spaces.Box,
+ dtype: np.dtype) -> gym.spaces.Box:
+ """Returns stacked version of a box observation space.
+
+ This stacks a gym.spaces.Box action space based on the length of the sensor
+ latency and the axis for stacking specified in the sensor.
+
+ Args:
+ space: A gym.spaces.Box to be stacked.
+ dtype: Datatype for the stacking
+
+ Returns:
+ stacked_space: A stacked version of the gym.spaces.Box action space.
+ """
+ length = len(self._sensor_latency)
+ stacked_space = gym.spaces.Box(
+ low=np.repeat(
+ np.expand_dims(space.low, axis=self._axis), length,
+ axis=self._axis),
+ high=np.repeat(
+ np.expand_dims(space.high, axis=self._axis),
+ length,
+ axis=self._axis),
+ dtype=dtype)
+
+ return stacked_space
+
+ def _stack_space_dict(self, space: gym.spaces.Dict,
+ dtype: np.dtype) -> gym.spaces.Dict:
+ """Returns stacked version of a dict observation space.
+
+ This stacks a gym.spaces.Dict action space based on the length of the sensor
+ latency and the recursive structure of the gym.spaces.Dict itself.
+
+ Args:
+ space: A gym.spaces.Dict to be stacked.
+ dtype: Datatype for the stacking.
+
+ Returns:
+ stacked_space: A stacked version of the dictionary action space.
+ """
+ return gym.spaces.Dict([
+ (k, self._stack_space(v, dtype)) for k, v in space.spaces.items()
+ ])
+
+ def _encode_obs_dict_keys(self, obs_dict):
+ """Encodes sub obs keys of observation dict or observsation space dict."""
+ return {encode_sub_obs_key(self, k): v for k, v in obs_dict.items()}
+
+
+class BoxSpaceSensor(Sensor):
+ """A prototype class of sensors with Box shapes."""
+
+ def __init__(self,
+ name: Text,
+ shape: Tuple[int, ...],
+ lower_bound: _FloatOrArray = -np.pi,
+ upper_bound: _FloatOrArray = np.pi,
+ dtype=np.float64) -> None:
+ """Constructs a box type sensor.
+
+ Will be deprecated (b/150818246) once we switch to gym spaces.
+ Args:
+ name: the name of the sensor
+ shape: the shape of the sensor values
+ lower_bound: the lower_bound of sensor value, in float or np.array.
+ upper_bound: the upper_bound of sensor value, in float or np.array.
+ dtype: data type of sensor value
+ """
+ super(BoxSpaceSensor, self).__init__(
+ name=name, sensor_latency=0.0, interpolator_fn=newer_obs_blender)
+ self._shape = shape
+ self._dtype = dtype
+
+ if isinstance(lower_bound, float):
+ self._lower_bound = np.full(shape, lower_bound, dtype=dtype)
+ else:
+ self._lower_bound = np.array(lower_bound)
+
+ if isinstance(upper_bound, float):
+ self._upper_bound = np.full(shape, upper_bound, dtype=dtype)
+ else:
+ self._upper_bound = np.array(upper_bound)
+
+ def set_robot(self, robot):
+ # Since all old robot class do not inherit from RobotBase, we can enforce
+ # the checking here.
+ if isinstance(robot, robot_base.RobotBase):
+ raise ValueError(
+ "Cannot use new robot interface RobotBase with old sensor calss.")
+ self._robot = robot
+
+ def get_shape(self) -> Tuple[int, ...]:
+ return self._shape
+
+ def get_dimension(self) -> int:
+ return len(self._shape)
+
+ def get_dtype(self):
+ return self._dtype
+
+ def get_observation_datatype(self) -> _DataTypeList:
+ """Returns box-shape data type."""
+ return [(self._name, self._dtype, self._shape)]
+
+ def get_lower_bound(self) -> _ARRAY:
+ """Returns the computed lower bound."""
+ return self._lower_bound
+
+ def get_upper_bound(self) -> _ARRAY:
+ """Returns the computed upper bound."""
+ return self._upper_bound
+
+ def get_observation(self) -> np.ndarray:
+ return np.asarray(self._get_observation(), dtype=self._dtype)
+
+ def _get_original_observation(self) -> Tuple[float, Any]:
+ # Maintains compatibility with the new sensor classes."""
+ raise NotImplementedError("Not implemented for this class.")
+
+ def on_new_observation(self):
+ # Maintains compatibility with the new sensor classes."""
+ pass
+
+
+def encode_sub_obs_key(s: Sensor, sub_obs_name: Optional[Text]):
+ """Returns a sub observation key for use in observation dictionary."""
+ if sub_obs_name == MAIN_OBS_KEY:
+ return s.get_name()
+ else:
+ return f"{s.get_name()}/{sub_obs_name}"
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/space_utils.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/space_utils.py
new file mode 100644
index 000000000..270bee2f1
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/space_utils.py
@@ -0,0 +1,137 @@
+# Lint as: python3
+"""Converts a list of sensors to gym space."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+from typing import List
+import gin
+import gym
+from gym import spaces
+import numpy as np
+
+from pybullet_envs.minitaur.envs_v2.sensors import sensor
+
+
+class UnsupportedConversionError(NotImplementedError):
+ """An exception when the function cannot convert sensors to the gym space."""
+
+
+class AmbiguousDataTypeError(TypeError):
+ """An exception when the function cannot determine the data type."""
+
+
+@gin.configurable
+def convert_sensors_to_gym_space(sensors: List[sensor.Sensor]) -> gym.Space:
+ """Convert a list of sensors to the corresponding gym space.
+
+ Args:
+ sensors: a list of the current sensors
+
+ Returns:
+ space: the converted gym space
+
+ Raises:
+ UnsupportedConversionError: raises when the function cannot convert the
+ given list of sensors.
+ """
+
+ if all([
+ isinstance(s, sensor.BoxSpaceSensor) and s.get_dimension() == 1
+ for s in sensors
+ ]):
+ return convert_1d_box_sensors_to_gym_space(sensors)
+ raise UnsupportedConversionError('sensors = ' + str(sensors))
+
+
+@gin.configurable
+def convert_1d_box_sensors_to_gym_space(
+ sensors: List[sensor.Sensor]) -> gym.Space:
+ """Convert a list of 1D BoxSpaceSensors to the corresponding gym space.
+
+ Args:
+ sensors: a list of the current sensors
+
+ Returns:
+ space: the converted gym space
+
+ Raises:
+ UnsupportedConversionError: raises when the function cannot convert the
+ given list of sensors.
+ AmbiguousDataTypeError: raises when the function cannot determine the
+ data types because they are not uniform.
+ """
+ # Check if all sensors are 1D BoxSpaceSensors
+ if not all([
+ isinstance(s, sensor.BoxSpaceSensor) and s.get_dimension() == 1
+ for s in sensors
+ ]):
+ raise UnsupportedConversionError('sensors = ' + str(sensors))
+
+ # Check if all sensors have the same data type
+ sensor_dtypes = [s.get_dtype() for s in sensors]
+ if sensor_dtypes.count(sensor_dtypes[0]) != len(sensor_dtypes):
+ raise AmbiguousDataTypeError('sensor datatypes are inhomogeneous')
+
+ lower_bound = np.concatenate([s.get_lower_bound() for s in sensors])
+ upper_bound = np.concatenate([s.get_upper_bound() for s in sensors])
+ observation_space = spaces.Box(
+ np.array(lower_bound), np.array(upper_bound), dtype=np.float32)
+ return observation_space
+
+
+@gin.configurable
+def convert_sensors_to_gym_space_dictionary(
+ sensors: List[sensor.Sensor]) -> gym.Space:
+ """Convert a list of sensors to the corresponding gym space dictionary.
+
+ Args:
+ sensors: a list of the current sensors
+
+ Returns:
+ space: the converted gym space dictionary
+
+ Raises:
+ UnsupportedConversionError: raises when the function cannot convert the
+ given list of sensors.
+ """
+ gym_space_dict = {}
+ for s in sensors:
+ if isinstance(s, sensor.BoxSpaceSensor):
+ gym_space_dict[s.get_name()] = spaces.Box(
+ np.array(s.get_lower_bound()),
+ np.array(s.get_upper_bound()),
+ dtype=np.float32)
+ elif isinstance(s, sensor.Sensor):
+ if isinstance(s.observation_space, spaces.Box):
+ gym_space_dict[s.get_name()] = s.observation_space
+ elif isinstance(s.observation_space, spaces.Dict):
+ gym_space_dict.update(s.observation_space.spaces)
+ else:
+ raise UnsupportedConversionError(
+ f'Unsupported space type {type(s.observation_space)}, '
+ f'must be Box or Dict. sensor = {s}')
+ else:
+ raise UnsupportedConversionError('sensors = ' + str(sensors))
+ return spaces.Dict(gym_space_dict)
+
+
+def create_constant_action(action_space, action_value=0):
+ """Create an uniform value action based on the type of action space."""
+ if isinstance(action_space, gym.spaces.Dict):
+ # gym.spaces.Dict has a shape of None, so construct action over subspaces.
+ return {
+ sub_name: create_constant_action(sub_space, action_value)
+ for sub_name, sub_space in action_space.spaces.items()
+ }
+ else: # Presumably gym.spaces.Box, but in case it is not ...
+ return np.full(action_space.shape, action_value)
+
+
+def action_astype(action, dtype):
+ """Transform an action to a different datatype."""
+ if isinstance(action, dict):
+ return {key: action_astype(value, dtype) for key, value in action.items()}
+ else:
+ return np.array(action, dtype=dtype)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/toe_position_sensor.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/toe_position_sensor.py
new file mode 100644
index 000000000..5e875045a
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/toe_position_sensor.py
@@ -0,0 +1,100 @@
+# Lint as: python3
+"""Quadruped toe position sensor."""
+
+from typing import Any, Callable, Sequence, Text, Tuple, Type, Union
+
+import gin
+import gym
+import numpy as np
+
+from pybullet_envs.minitaur.envs_v2.sensors import sensor
+from pybullet_envs.minitaur.envs_v2.utilities import noise_generators
+
+
+def _convert_to_np_array(inputs: Union[float, Tuple[float], np.ndarray], dim):
+ """Converts the inputs to a numpy array.
+
+ Args:
+ inputs: The input scalar or array.
+ dim: The dimension of the converted numpy array.
+
+ Returns:
+ The converted numpy array.
+
+ Raises:
+ ValueError: If the inputs is an array whose dimension does not match the
+ provided dimension.
+ """
+ outputs = None
+ if isinstance(inputs, (tuple, np.ndarray)):
+ outputs = np.array(inputs)
+ else:
+ outputs = np.full(dim, inputs)
+
+ if len(outputs) != dim:
+ raise ValueError("The inputs array has a different dimension {}"
+ " than provided, which is {}.".format(len(outputs), dim))
+
+ return outputs
+
+
+@gin.configurable
+class ToePositionSensor(sensor.Sensor):
+ """A sensor that outputs the toe positions of attached robots or objects."""
+
+ def __init__(
+ self,
+ name: Text = "toe_position",
+ dtype: Type[Any] = np.float64,
+ lower_bound: Union[float, Sequence[float]] = -1.0,
+ upper_bound: Union[float, Sequence[float]] = 1.0,
+ noise_generator: Union[Callable[..., Any],
+ noise_generators.NoiseGenerator] = None,
+ sensor_latency: Union[float, Sequence[float]] = 0.0,
+ ):
+ """Constructor.
+
+ Args:
+ name: Name of the sensor.
+ dtype: Data type of sensor value.
+ lower_bound: The optional lower bounds of the sensor reading.
+ upper_bound: The optional upper bounds of the sensor reading.
+ noise_generator: Used to add noise to the readings.
+ sensor_latency: Single or multiple latency in seconds. See sensor.Sensor
+ docstring for details.
+ """
+ super().__init__(
+ name=name,
+ sensor_latency=sensor_latency,
+ interpolator_fn=sensor.linear_obs_blender)
+ self._dtype = dtype
+ self._lower_bound = lower_bound
+ self._upper_bound = upper_bound
+ self._noise_generator = noise_generator
+
+ def set_robot(self, robot):
+ self._robot = robot
+ num_legs = len(robot.urdf_loader.get_end_effector_id_dict().values())
+ lower_bound = _convert_to_np_array(self._lower_bound, num_legs * 3)
+
+ upper_bound = _convert_to_np_array(self._upper_bound, num_legs * 3)
+
+ self._observation_space = self._stack_space(
+ gym.spaces.Box(low=lower_bound, high=upper_bound, dtype=self._dtype))
+
+ def _get_original_observation(self) -> Tuple[float, np.ndarray]:
+ """Returns raw observation with timestamp."""
+ toe_position = np.array(
+ self._robot.foot_positions(), dtype=self._dtype).flatten()
+
+ return self._robot.timestamp, toe_position
+
+ def get_observation(self) -> np.ndarray:
+ delayed_observation = super().get_observation()
+ if self._noise_generator:
+ if callable(self._noise_generator):
+ return self._noise_generator(delayed_observation)
+ else:
+ return self._noise_generator.add_noise(delayed_observation)
+
+ return delayed_observation
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/tasks/__init__.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/tasks/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/tasks/simple_locomotion_task.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/tasks/simple_locomotion_task.py
new file mode 100644
index 000000000..9046f9d8d
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/tasks/simple_locomotion_task.py
@@ -0,0 +1,125 @@
+"""A simple locomotion taskand termination condition."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import numpy as np
+
+import gin
+from pybullet_envs.minitaur.envs_v2.tasks import task_interface
+from pybullet_envs.minitaur.envs_v2.tasks import task_utils
+from pybullet_envs.minitaur.envs_v2.tasks import terminal_conditions
+from pybullet_envs.minitaur.envs_v2.utilities import env_utils_v2 as env_utils
+
+
+@gin.configurable
+class SimpleForwardTask(task_interface.Task):
+ """A basic "move forward" task."""
+
+ def __init__(self,
+ weight=1.0,
+ terminal_condition=terminal_conditions
+ .default_terminal_condition_for_minitaur,
+ divide_with_dt=False,
+ clip_velocity=None,
+ energy_penalty_coef=0.0,
+ min_com_height=None,
+ weight_action_accel=None):
+ """Initializes the task.
+
+ Args:
+ weight: Float. The scaling factor for the reward.
+ terminal_condition: Callable object or function. Determines if the task is
+ done.
+ divide_with_dt: if True, we divide the velocity reward with dt.
+ clip_velocity: if not None, we will clip the velocity with this value.
+ energy_penalty_coef: Coefficient for the energy penalty that will be added
+ to the reward. 0 by default.
+ min_com_height: Minimum height for the center of mass of the robot that
+ will be used to terminate the task. This is used to obtain task specific
+ gaits and set by the config or gin files based on the task and robot.
+ weight_action_accel: if not None, penalize the action acceleration.
+
+ Raises:
+ ValueError: The energey coefficient is smaller than zero.
+ """
+ self._weight = weight
+ self._terminal_condition = terminal_condition
+ self._last_base_position = None
+ self._divide_with_dt = divide_with_dt
+ self._clip_velocity = clip_velocity
+ self._weight_action_accel = weight_action_accel
+ self._action_history_sensor = None
+ self._min_com_height = min_com_height
+ self._energy_penalty_coef = energy_penalty_coef
+ self._env = None
+ self._step_count = 0
+ if energy_penalty_coef < 0:
+ raise ValueError("Energy Penalty Coefficient should be >= 0")
+
+ def __call__(self, env):
+ return self.reward(env)
+
+ def reset(self, env):
+ self._env = env
+ self._last_base_position = env_utils.get_robot_base_position(
+ self._env.robot)
+
+ if self._weight_action_accel is not None:
+ sensor_name = "LastAction"
+ self._action_history_sensor = env.sensor_by_name(sensor_name)
+
+ @property
+ def step_count(self):
+ return self._step_count
+
+ def update(self, env):
+ """Updates the internal state of the task."""
+ del env
+ self._last_base_position = env_utils.get_robot_base_position(
+ self._env.robot)
+
+ def reward(self, env):
+ """Get the reward without side effects."""
+ del env
+
+ self._step_count += 1
+ env = self._env
+ current_base_position = env_utils.get_robot_base_position(self._env.robot)
+ velocity = current_base_position[0] - self._last_base_position[0]
+ if self._divide_with_dt:
+ velocity /= env.env_time_step
+ if self._clip_velocity is not None:
+ limit = float(self._clip_velocity)
+ velocity = np.clip(velocity, -limit, limit)
+
+ if self._weight_action_accel is None:
+ action_acceleration_penalty = 0.0
+ else:
+ past_actions = self._action_history_sensor.get_observation().T
+ action = past_actions[0]
+ prev_action = past_actions[1]
+ prev_prev_action = past_actions[2]
+ acc = action - 2 * prev_action + prev_prev_action
+ action_acceleration_penalty = (
+ float(self._weight_action_accel) * np.mean(np.abs(acc)))
+
+ reward = velocity
+ reward -= action_acceleration_penalty
+
+ # Energy
+ if self._energy_penalty_coef > 0:
+ energy_reward = -task_utils.calculate_estimated_energy_consumption(
+ self._env.robot.motor_torques, self._env.robot.motor_velocities,
+ self._env.sim_time_step, self._env.num_action_repeat)
+ reward += energy_reward * self._energy_penalty_coef
+
+ return reward * self._weight
+
+ def done(self, env):
+ del env
+ position = env_utils.get_robot_base_position(self._env.robot)
+ if self._min_com_height and position[2] < self._min_com_height:
+ return True
+ return self._terminal_condition(self._env)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/tasks/task_interface.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/tasks/task_interface.py
new file mode 100644
index 000000000..720d45010
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/tasks/task_interface.py
@@ -0,0 +1,37 @@
+# Lint as: python3
+"""Interface that specifies tasks."""
+# TODO(tingnan): Def. proper task interface - see TODO(b/154635313) in comments.
+
+import abc
+from typing import Sequence
+
+import gym
+
+from pybullet_envs.minitaur.envs_v2.sensors import sensor
+
+
+class Task(metaclass=abc.ABCMeta):
+ """Base class for tasks."""
+
+ # TODO(b/154635313): Deprecate this method. Consolidate it into update().
+ @abc.abstractmethod
+ def reward(self, env: gym.Env) -> float:
+ """Returns the reward for the current state of the environment."""
+
+ @abc.abstractmethod
+ def reset(self, env: gym.Env) -> None:
+ """Resets the task."""
+
+ @abc.abstractmethod
+ def update(self, env: gym.Env) -> None:
+ """Updates the internal state of the task."""
+
+ # TODO(b/154635313): Deprecate this method. Consolidate it into update().
+ @abc.abstractmethod
+ def done(self, env: gym.Env) -> bool:
+ """Determines whether the task is done."""
+
+ @property
+ def sensors(self) -> Sequence[sensor.Sensor]:
+ """Returns sensors used by task."""
+ return []
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/tasks/task_utils.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/tasks/task_utils.py
new file mode 100644
index 000000000..90ac2814a
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/tasks/task_utils.py
@@ -0,0 +1,108 @@
+"""Common tools and functionalities used in different tasks."""
+import numpy as np
+
+
+def calculate_target_speed_at_timestep(speed_stages):
+ """Interpolates the speed based on a speed profile and simulation steps.
+
+ Args:
+ speed_stages: The list of timesteps (in increasing order) and speed (float
+ or a list of floats for x and y components) at that specific timestep.
+ Example formats can be found on task_utils_test.py and at the header of
+ the speed_reward_task.py.
+
+ Returns:
+ Target speed for the specific step (meters per simulation step).
+ Raises:
+ ValueError if the input is not in the expected format.
+ """
+ if len(speed_stages) != 2 or len(speed_stages[0]) != len(speed_stages[1]):
+ raise ValueError('Speed stages for the task is not in correct format!')
+ steps = np.array(speed_stages[0])
+ speeds = np.array(speed_stages[1])
+ num_steps = steps[-1]
+ if len(speeds.shape) == 1:
+ return np.interp(range(num_steps), steps, speeds)
+ speed_at_timestep = np.interp(range(num_steps), steps, speeds[:, 0]).reshape(
+ (num_steps, 1))
+ speed_at_timestep = speed_at_timestep.reshape((num_steps, 1))
+ if speeds.shape[1] == 2:
+ speed_y = np.interp(range(num_steps), steps, speeds[:, 1]).reshape(
+ (num_steps, 1))
+ speed_at_timestep = np.concatenate((speed_at_timestep, speed_y), axis=1)
+ else:
+ speed_at_timestep = np.concatenate(
+ (speed_at_timestep, np.zeros((num_steps, 1))), axis=1)
+ return speed_at_timestep
+
+
+def calculate_distance(vector_1, vector_2):
+ """Calculates the distance between 2 vectors.
+
+ This is used to calculate distance between 2 points in 3D space as well as
+ distances between two orientation vectors.
+
+ Args:
+ vector_1: First vector that will be used for comparison with the other.
+ vector_2: Second vector used for comparison.
+
+ Returns:
+ Distance between the two vectors represented by a float.
+ """
+ return np.linalg.norm(np.array(vector_1) - np.array(vector_2))
+
+
+def turn_angle(new_vector, reference_vector):
+ """Calculates the change in orientation of the two vectors.
+
+ This is used to calculate the relative angle perception for the
+ robot.
+
+ Args:
+ new_vector: The front vector of the robot at current timestep.
+ reference_vector: The front vector of the robot at previous timestep.
+
+ Returns:
+ Angle representing the change in orientation of the robot projected to
+ x-y plane.
+ """
+ # Project the vectors to x-y plane
+ v1 = np.resize(new_vector, 3)
+ v2 = np.resize(reference_vector, 3)
+ v1[2] = 0
+ v2[2] = 0
+ # Calculate the right hand rotation between two vectors using z vector
+ # (0,0,-1) as reference cross product vector.
+ # Compatible with the yaw rotation of pyBullet.
+ return np.arctan2(np.dot(np.cross(v2, v1), (0, 0, -1)), np.dot(v1, v2))
+
+
+def front_vector(pybullet_client, orientation):
+ """Calculates the front vector of the robot on x-y plane.
+
+ Args:
+ pybullet_client: Pybullet client instantiation.
+ orientation: Orientation of the robot in quaternion form.
+
+ Returns:
+ 3D vector where z component is set to 0.
+ """
+ rot_matrix = pybullet_client.getMatrixFromQuaternion(orientation)
+ return [rot_matrix[0], -rot_matrix[1], 0]
+
+
+def calculate_estimated_energy_consumption(motor_torques, motor_velocities,
+ sim_time_step, num_action_repeat):
+ """Calculates energy consumption based on the args listed.
+
+ Args:
+ motor_torques: Torques of all the motors
+ motor_velocities: Velocities of all the motors.
+ sim_time_step: Simulation time step length (seconds).
+ num_action_repeat: How many steps the simulation repeats the same action.
+
+ Returns:
+ Total energy consumption of all the motors (watts).
+ """
+ return np.abs(np.dot(motor_torques,
+ motor_velocities)) * sim_time_step * num_action_repeat
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/tasks/terminal_conditions.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/tasks/terminal_conditions.py
new file mode 100644
index 000000000..9fe512057
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/tasks/terminal_conditions.py
@@ -0,0 +1,301 @@
+"""Contains the terminal conditions for locomotion tasks."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import gin
+import numpy as np
+
+from pybullet_envs.minitaur.envs_v2.utilities import minitaur_pose_utils
+from pybullet_envs.minitaur.envs_v2.utilities import env_utils_v2 as env_utils
+from pybullet_envs.minitaur.envs_v2.utilities import termination_reason as tr
+
+
+@gin.configurable
+def default_terminal_condition_for_minitaur(env):
+ """A default terminal condition for Minitaur.
+
+ Minitaur is considered as fallen if the base position is too low or the base
+ tilts/rolls too much.
+
+ Args:
+ env: An instance of MinitaurGymEnv
+
+ Returns:
+ A boolean indicating if Minitaur is fallen.
+ """
+ orientation = env_utils.get_robot_base_orientation(env.robot)
+ rot_mat = env.pybullet_client.getMatrixFromQuaternion(orientation)
+ local_up = rot_mat[6:]
+ pos = env_utils.get_robot_base_position(env.robot)
+ return (np.dot(np.asarray([0, 0, 1]), np.asarray(local_up)) < 0.85 or
+ pos[2] < 0.13)
+
+
+@gin.configurable
+def terminal_condition_for_minitaur_extended_env(env):
+ """Returns a bool indicating that the extended env is terminated.
+
+ This predicate checks whether 1) the legs are bent inward too much or
+ 2) the body is tilted too much.
+
+ Args:
+ env: An instance of MinitaurGymEnv
+ """
+ motor_angles = env.robot.motor_angles
+ leg_pose = minitaur_pose_utils.motor_angles_to_leg_pose(motor_angles)
+
+ swing_threshold = np.radians(35.0)
+ if (leg_pose[0] > swing_threshold or leg_pose[2] > swing_threshold or # Front
+ leg_pose[1] < -swing_threshold or leg_pose[3] < -swing_threshold): # Rear
+ return True
+ roll, _, _ = env.robot.base_roll_pitch_yaw
+ if abs(roll) > np.radians(30.0):
+ return True
+
+ return False
+
+
+@gin.configurable
+def default_terminal_condition_for_laikago(env):
+ """A default terminal condition for Laikago.
+
+ Minitaur is considered as fallen if the base position is too low or the base
+ tilts/rolls too much.
+
+ Args:
+ env: An instance of MinitaurGymEnv
+
+ Returns:
+ A boolean indicating if Minitaur is fallen.
+ """
+ roll, pitch, _ = env.robot.base_roll_pitch_yaw
+ pos = env_utils.get_robot_base_position(env.robot)
+ return abs(roll) > 0.2 or abs(pitch) > 0.2 or pos[2] < 0.35
+
+
+@gin.configurable
+def default_terminal_condition_for_laikago_v2(
+ env,
+ max_roll: float = 1.2,
+ max_pitch: float = 1.2,
+ min_height: float = 0.15,
+ enforce_foot_contacts: bool = False):
+ """A default terminal condition for Laikago_v2.
+
+ Laikago is considered as fallen if the base position is too low or the base
+ tilts/rolls too much.
+
+ Args:
+ env: An instance of MinitaurGymEnv
+ max_roll: Max roll before the episode terminates.
+ max_pitch: Max pitch before the episode terminates.
+ min_height: Min height before the episode terminates.
+ enforce_foot_contacts: Ensure that contacts are established with the feet.
+
+ Returns:
+ A boolean indicating if Minitaur is fallen.
+ """
+ # Make sure that contacts are only made with the robot's feet.
+ unwanted_collision = False
+ if enforce_foot_contacts:
+ # Get list of foot and knee link ids. Sometimes, the simulation will
+ # register a contact as having been made with the knee link, even though it
+ # was actually the foot that made the contact. Checking both the foot and
+ # knee links for contact accounts for that.
+ foot_link_ids = list(
+ env.robot.urdf_loader.get_end_effector_id_dict().values())
+ knee_link_ids = [foot_link_id - 1 for foot_link_id in foot_link_ids]
+ contacts = env.pybullet_client.getContactPoints(bodyA=env.robot.robot_id)
+ for contact in contacts:
+ # Two different bodies made contact (i.e. not a self-collision).
+ if contact[1] != contact[2]:
+ foot_contact = (contact[3] in foot_link_ids) or (
+ contact[3] in knee_link_ids)
+ unwanted_collision = unwanted_collision or not foot_contact
+
+ roll, pitch, _ = env.robot.base_roll_pitch_yaw
+ pos = env.robot.base_position
+ return (abs(roll) > max_roll or abs(pitch) > max_pitch or
+ pos[2] < min_height or unwanted_collision)
+
+
+@gin.configurable
+def default_terminal_condition_for_agility(env,
+ max_roll: float = 1.8,
+ max_pitch: float = 1.8,
+ min_height: float = 0.0,
+ enforce_foot_contacts: bool = False):
+ """A default terminal condition for more agile tasks (i.e. jumping).
+
+ The robot is considered as fallen if the base position is too low, the base
+ tilts/rolls too much or parts of the body other than the feet touch the
+ ground.
+
+ Args:
+ env: An instance of the gym env.
+ max_roll: Max roll before the episode terminates.
+ max_pitch: Max pitch before the episode terminates.
+ min_height: Min height before the episode terminates.
+ enforce_foot_contacts: Ensure that contacts are established with the feet.
+
+ Returns:
+ A boolean indicating if the episode should be terminated.
+ """
+
+ # Make sure that contacts are only made with the robot's feet.
+ unwanted_collision = False
+ if enforce_foot_contacts:
+ knee_link_ids = [2, 5, 8, 11]
+ contacts = env.pybullet_client.getContactPoints(bodyA=env.robot.robot_id)
+ for contact in contacts:
+ if contact[1] != contact[2]:
+ foot_contact = contact[3] in knee_link_ids
+ unwanted_collision = unwanted_collision or not foot_contact
+
+ roll, pitch, _ = env.robot.base_roll_pitch_yaw
+ pos = env.robot.base_position
+ return (abs(roll) > max_roll or abs(pitch) > max_pitch or
+ pos[2] < min_height or unwanted_collision)
+
+
+@gin.configurable
+def get_terminal_reason(collisions, task):
+ termination_reason = None
+ # Checking collision termination
+ if collisions:
+ termination_reason = tr.TerminationReason.AGENT_COLLISION
+ if task.is_task_success():
+ termination_reason = tr.TerminationReason.GOAL_REACHED
+ return termination_reason
+
+
+@gin.configurable
+def maxstep_terminal_condition(env, max_step=500):
+ """A terminal condition based on the time step.
+
+ Args:
+ env: An instance of MinitaurGymEnv
+ max_step: The maximum time step allowed for the environment
+
+ Returns:
+ A boolean indicating if the env.step exceeds the given limit
+ """
+ return env.env_step_counter > max_step
+
+
+@gin.configurable
+class MaxTimeTerminalCondition(object):
+ """Terminal condition based on time, independent of step length."""
+
+ def __init__(self, max_time_s: float):
+ """Initializes the MaxTimeTerminalCondition.
+
+ Args:
+ max_time_s: Time limit in seconds. In sim, this is the simulation time,
+ not wall time.
+ """
+ if max_time_s <= 0:
+ raise ValueError("Max time for MaxTimeTerminalCondition cannot be zero "
+ "or less. Input value: %s" % max_time_s)
+ self._max_time_s = max_time_s
+
+ def __call__(self, env):
+ if self._max_time_s is None:
+ return False, None
+ is_done = env.get_time_since_reset() >= self._max_time_s
+ term_reason = tr.TerminationReason.RUN_TIME_LIMIT if is_done else None
+ return is_done, term_reason
+
+
+@gin.configurable
+class MovementDetectorTerminalCondition(object):
+ """Terminal condition for not moving past a distance in specified time."""
+
+ def __init__(self,
+ max_time_s: float = None,
+ min_travel_distance_m: float = 1.0):
+ """Initializes the MovementDetectorTerminalCondition.
+
+ Args:
+ max_time_s: Time limit in seconds. In sim, this is the simulation time,
+ not wall time.
+ min_travel_distance_m: Minimum distance in meters to travel in time limit.
+ """
+ if max_time_s is not None and max_time_s <= 0:
+ raise ValueError("Max time for MovementDetectorTerminalCondition cannot "
+ "be zero or less. Input value: %s" % max_time_s)
+ if min_travel_distance_m is not None and min_travel_distance_m < 0:
+ raise ValueError(
+ "Minimum travel distance for MovementDetectorTerminalCondition "
+ "cannot be less than zero. Input value: %s" % min_travel_distance_m)
+ self._max_time_s = max_time_s
+ self._min_travel_distance_m = min_travel_distance_m
+ self._not_advancing_limit = None
+ self._reference_position = None
+
+ def _update_limit_time(self, env):
+ self._not_advancing_limit = env.get_time_since_reset() + self._max_time_s
+
+ def _current_position(self, env):
+ return np.asarray(env.robot.base_position[:2])
+
+ def _exceed_time_limit(self, env):
+ return self._not_advancing_limit < env.get_time_since_reset()
+
+ def __call__(self, env):
+ is_done = False
+ term_reason = None
+
+ if self._max_time_s is None:
+ return is_done, term_reason
+
+ if self._not_advancing_limit is None or self._reference_position is None:
+ self._update_limit_time(env)
+ self._reference_position = self._current_position(env)
+
+ distance_shifted = np.linalg.norm(
+ self._current_position(env) - self._reference_position)
+ if distance_shifted >= self._min_travel_distance_m:
+ self._update_limit_time(env)
+ self._reference_position = self._current_position(env)
+
+ if self._exceed_time_limit(env):
+ is_done = True
+ term_reason = tr.TerminationReason.NOT_ADVANCING_LIMIT
+
+ return is_done, term_reason
+
+
+
+@gin.configurable
+def bad_front_leg_terminal_condition(env, max_angle=0.8):
+ """A terminal condition for checking whether front legs are bent backward.
+
+ Args:
+ env: An instance of MinitaurGymEnv
+ max_angle: The maximum angle allowed for front legs
+
+ Returns:
+ A boolean indicating if front legs are bent backward or not
+ """
+ motor_angles = env.robot.motor_angles
+ leg_pose = minitaur_pose_utils.motor_angles_to_leg_pose(motor_angles)
+ swing0 = leg_pose[0]
+ swing1 = leg_pose[2]
+ return swing0 > max_angle or swing1 > max_angle
+
+
+@gin.configurable
+def logical_any_terminal_condition(env, conditions):
+ """A logical "Any" operator for terminal conditions.
+
+ Args:
+ env: An instance of MinitaurGymEnv
+ conditions: a list of terminal conditions
+
+ Returns:
+ A boolean indicating if any of terminal conditions is satisfied
+ """
+ return any([cond(env) for cond in conditions])
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/__init__.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/env_utils.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/env_utils.py
new file mode 100644
index 000000000..f82283bf3
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/env_utils.py
@@ -0,0 +1,175 @@
+"""Utility functions to manipulate environment."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import collections
+import gin
+from gym import spaces
+import numpy as np
+
+import tensorflow.compat.v1 as tf
+
+
+
+
+def flatten_observations(observation_dict, observation_excluded=()):
+ """Flattens the observation dictionary to an array.
+
+ If observation_excluded is passed in, it will still return a dictionary,
+ which includes all the (key, observation_dict[key]) in observation_excluded,
+ and ('other': the flattened array).
+
+ Args:
+ observation_dict: A dictionary of all the observations.
+ observation_excluded: A list/tuple of all the keys of the observations to be
+ ignored during flattening.
+
+ Returns:
+ An array or a dictionary of observations based on whether
+ observation_excluded is empty.
+ """
+ if not isinstance(observation_excluded, (list, tuple)):
+ observation_excluded = [observation_excluded]
+ observations = []
+ for key, value in observation_dict.items():
+ if key not in observation_excluded:
+ observations.append(np.asarray(value).flatten())
+ flat_observations = np.float32(np.concatenate(observations))
+ if not observation_excluded:
+ return flat_observations
+ else:
+ observation_dict_after_flatten = {"other": flat_observations}
+ for key in observation_excluded:
+ if key in observation_dict:
+ observation_dict_after_flatten[key] = observation_dict[key]
+ return collections.OrderedDict(
+ sorted(list(observation_dict_after_flatten.items())))
+
+
+def flatten_observation_dim(observation_dim, observation_excluded=()):
+ """Flattens the observation dimensions to an array.
+
+ If observation_excluded is passed in, it will still return a dictionary,
+ which includes all the (key, observation_dict[key]) in observation_excluded,
+ and ('other': the flattened array).
+
+ Args:
+ observation_dim: A dictionary of all the observation dimensions.
+ observation_excluded: A list/tuple of all the keys of the observations to be
+ ignored during flattening.
+
+ Returns:
+ An array or a dictionary of observation dimensions based on whether
+ observation_excluded is empty.
+ """
+ if not isinstance(observation_excluded, (list, tuple)):
+ observation_excluded = [observation_excluded]
+ observation_dims = 0
+ for key, value in observation_dim.items():
+ if key not in observation_excluded:
+ observation_dims += value
+ if not observation_excluded:
+ return observation_dims
+ else:
+ dim_dict_after_flatten = {"other": observation_dims}
+ for key in observation_excluded:
+ if key in observation_dim:
+ dim_dict_after_flatten[key] = observation_dim[key]
+ return collections.OrderedDict(sorted(list(dim_dict_after_flatten.items())))
+
+
+def flatten_observation_spaces(observation_spaces, observation_excluded=()):
+ """Flattens the dictionary observation spaces to gym.spaces.Box.
+
+ If observation_excluded is passed in, it will still return a dictionary,
+ which includes all the (key, observation_spaces[key]) in observation_excluded,
+ and ('other': the flattened Box space).
+
+ Args:
+ observation_spaces: A dictionary of all the observation spaces.
+ observation_excluded: A list/tuple of all the keys of the observations to be
+ ignored during flattening.
+
+ Returns:
+ A box space or a dictionary of observation spaces based on whether
+ observation_excluded is empty.
+ """
+ if not isinstance(observation_excluded, (list, tuple)):
+ observation_excluded = [observation_excluded]
+ lower_bound = []
+ upper_bound = []
+ for key, value in observation_spaces.spaces.items():
+ if key not in observation_excluded:
+ lower_bound.append(np.asarray(value.low).flatten())
+ upper_bound.append(np.asarray(value.high).flatten())
+ lower_bound = np.concatenate(lower_bound)
+ upper_bound = np.concatenate(upper_bound)
+ observation_space = spaces.Box(
+ np.array(lower_bound), np.array(upper_bound), dtype=np.float32)
+ if not observation_excluded:
+ return observation_space
+ else:
+ observation_spaces_after_flatten = {"other": observation_space}
+ for key in observation_excluded:
+ if key in observation_spaces.spaces:
+ observation_spaces_after_flatten[key] = observation_spaces[key]
+ return spaces.Dict(observation_spaces_after_flatten)
+
+
+
+@gin.configurable
+def get_action_spec(action_spec):
+ """Get action spec for one agent from the environment specs."""
+ return list(action_spec.values())[0]
+
+
+
+@gin.configurable
+def get_get_actions_fn(agent_name_to_index):
+ """Get function which returns other agents' actions."""
+
+ def get_actions(action):
+ """Returns a list of actions.
+
+ Args:
+ action: A dictionary of action tensors with keys matching agent names
+
+ Returns:
+ critic_actions: A list of action tensors for (N-1) other agents.
+ Shape: (B x N, N-1, D)
+ """
+ critic_actions = []
+ for agent_name in sorted(agent_name_to_index.keys()):
+ other_agent_actions = []
+ for other_agent_name in sorted(agent_name_to_index.keys()):
+ if other_agent_name != agent_name:
+ other_agent_actions.append(action[other_agent_name])
+ critic_actions.append(other_agent_actions)
+ print([tf.shape(critic_action) for critic_action in critic_actions])
+ # Shape goes from (N, N-1, B, D) to (B x N, N-1, D)
+ critic_actions = tf.transpose(tf.concat(critic_actions, axis=1), (1, 0, 2))
+ return critic_actions
+
+ return get_actions
+
+
+
+
+def get_robot_base_position(robot):
+ """Gets the base position of robot."""
+ # TODO(b/151975607): Clean this after robot interface migration.
+ if hasattr(robot, "GetBasePosition"):
+ return robot.GetBasePosition()
+ else:
+ return robot.base_position
+
+
+def get_robot_base_orientation(robot):
+ """Gets the base orientation of robot."""
+ # TODO(b/151975607): Clean this after robot interface migration.
+ if hasattr(robot, "GetBaseOrientation"):
+ return robot.GetBaseOrientation()
+ else:
+ return robot.base_orientation_quaternion
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/env_utils_v2.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/env_utils_v2.py
new file mode 100644
index 000000000..02d2eeaef
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/env_utils_v2.py
@@ -0,0 +1,22 @@
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+
+def get_robot_base_position(robot):
+ """Gets the base position of robot."""
+ # TODO(b/151975607): Clean this after robot interface migration.
+ if hasattr(robot, "GetBasePosition"):
+ return robot.GetBasePosition()
+ else:
+ return robot.base_position
+
+
+def get_robot_base_orientation(robot):
+ """Gets the base orientation of robot."""
+ # TODO(b/151975607): Clean this after robot interface migration.
+ if hasattr(robot, "GetBaseOrientation"):
+ return robot.GetBaseOrientation()
+ else:
+ return robot.base_orientation_quaternion
\ No newline at end of file
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/laikago_pose_utils.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/laikago_pose_utils.py
new file mode 100644
index 000000000..f328c3418
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/laikago_pose_utils.py
@@ -0,0 +1,50 @@
+# coding=utf-8
+# Copyright 2020 The Google Research Authors.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Utility functions to calculate Laikago's pose and motor angles."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import attr
+
+LAIKAGO_DEFAULT_ABDUCTION_ANGLE = 0
+LAIKAGO_DEFAULT_HIP_ANGLE = 0.67
+LAIKAGO_DEFAULT_KNEE_ANGLE = -1.25
+
+
+@attr.s
+class LaikagoPose(object):
+ """Default pose of the Laikago.
+
+ Leg order:
+ 0 -> Front Right.
+ 1 -> Front Left.
+ 2 -> Rear Right.
+ 3 -> Rear Left.
+ """
+ abduction_angle_0 = attr.ib(type=float, default=0)
+ hip_angle_0 = attr.ib(type=float, default=0)
+ knee_angle_0 = attr.ib(type=float, default=0)
+ abduction_angle_1 = attr.ib(type=float, default=0)
+ hip_angle_1 = attr.ib(type=float, default=0)
+ knee_angle_1 = attr.ib(type=float, default=0)
+ abduction_angle_2 = attr.ib(type=float, default=0)
+ hip_angle_2 = attr.ib(type=float, default=0)
+ knee_angle_2 = attr.ib(type=float, default=0)
+ abduction_angle_3 = attr.ib(type=float, default=0)
+ hip_angle_3 = attr.ib(type=float, default=0)
+ knee_angle_3 = attr.ib(type=float, default=0)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/mini_cheetah_pose_utils.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/mini_cheetah_pose_utils.py
new file mode 100644
index 000000000..37d12fe95
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/mini_cheetah_pose_utils.py
@@ -0,0 +1,35 @@
+"""Utility functions to calculate Mini Cheetah's pose and motor angles."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import attr
+
+MINI_CHEETAH_DEFAULT_ABDUCTION_ANGLE = 0
+MINI_CHEETAH_DEFAULT_HIP_ANGLE = -1.2
+MINI_CHEETAH_DEFAULT_KNEE_ANGLE = 2.1
+
+
+@attr.s
+class MiniCheetahPose(object):
+ """Default pose of the Laikago.
+
+ Leg order:
+ 0 -> Front Right.
+ 1 -> Front Left.
+ 2 -> Rear Right.
+ 3 -> Rear Left.
+ """
+ abduction_angle_0 = attr.ib(type=float, default=0)
+ hip_angle_0 = attr.ib(type=float, default=0)
+ knee_angle_0 = attr.ib(type=float, default=0)
+ abduction_angle_1 = attr.ib(type=float, default=0)
+ hip_angle_1 = attr.ib(type=float, default=0)
+ knee_angle_1 = attr.ib(type=float, default=0)
+ abduction_angle_2 = attr.ib(type=float, default=0)
+ hip_angle_2 = attr.ib(type=float, default=0)
+ knee_angle_2 = attr.ib(type=float, default=0)
+ abduction_angle_3 = attr.ib(type=float, default=0)
+ hip_angle_3 = attr.ib(type=float, default=0)
+ knee_angle_3 = attr.ib(type=float, default=0)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/minitaur_pose_utils.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/minitaur_pose_utils.py
new file mode 100644
index 000000000..34ad9da01
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/minitaur_pose_utils.py
@@ -0,0 +1,187 @@
+# coding=utf-8
+# Copyright 2020 The Google Research Authors.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Utility functions to calculate Minitaur's pose and motor angles."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import math
+import attr
+import numpy as np
+
+NUM_MOTORS = 8
+NUM_LEGS = 4
+MOTOR_SIGNS = (1, 1, -1, -1)
+# Constants for the function swing_extend_to_motor_angles
+EPS = 0.1
+# Range of motion for the legs (does not allow pointing towards the body).
+LEG_SWING_LIMIT_LOW = -math.pi / 2 + EPS
+LEG_SWING_LIMIT_HIGH = 3 * math.pi / 2 - EPS
+# Range of gap between motors for feasibility.
+MOTORS_GAP_LIMIT_HIGH = 2 * math.pi - EPS
+MOTORS_GAP_LIMIT_LOW = EPS
+
+
+@attr.s
+class MinitaurPose(object):
+ """Default pose of the Minitaur."""
+ swing_angle_0 = attr.ib(type=float, default=0)
+ swing_angle_1 = attr.ib(type=float, default=0)
+ swing_angle_2 = attr.ib(type=float, default=0)
+ swing_angle_3 = attr.ib(type=float, default=0)
+ extension_angle_0 = attr.ib(type=float, default=0)
+ extension_angle_1 = attr.ib(type=float, default=0)
+ extension_angle_2 = attr.ib(type=float, default=0)
+ extension_angle_3 = attr.ib(type=float, default=0)
+
+
+def motor_angles_to_leg_pose(motor_angles):
+ """Convert motor angles to the leg pose.
+
+ A single leg pose is a tuple (swing, extension). The definition can be find
+ in:
+ Sim-to-Real: Learning Agile Locomotion For Quadruped Robot
+
+ Args:
+ motor_angles: A numpy array. Contains all eight motor angles for Minitaur.
+
+ Returns:
+ A numpy array. Contains the leg pose for all four legs: [swing_0, swing_1,
+ swing_2, swing_3, extension_0, extension_1, extension_2, extension_3]
+
+ """
+ motor_angles = np.array(motor_angles)
+
+ swings = 0.5 * np.multiply(
+ np.array(MOTOR_SIGNS), (motor_angles[1::2] - motor_angles[::2]))
+ extensions = 0.5 * (motor_angles[::2] + motor_angles[1::2])
+
+ return np.concatenate((swings, extensions), axis=None)
+
+
+def leg_pose_to_motor_angles(leg_pose):
+ """Converts the leg pose to the motor angles.
+
+ Args:
+ leg_pose: A numpy array. Contains the leg pose for all four legs: [swing_0,
+ swing_1, swing_2, swing_3, extension_0, extension_1, extension_2,
+ extension_3]
+
+ Returns:
+ A numpy array. All eight motor angles.
+ """
+ leg_pose = np.array(leg_pose)
+
+ # swings multiplied with the sign array.
+ signed_swings = np.multiply(np.array(MOTOR_SIGNS), leg_pose[0:NUM_LEGS])
+ extensions = leg_pose[NUM_LEGS:]
+
+ motor_angles = np.zeros(NUM_MOTORS)
+ motor_angles[1::2] = signed_swings + extensions
+ motor_angles[::2] = extensions - signed_swings
+ return motor_angles
+
+
+# This method also does the same conversion, but 0 swing and 0 extension maps
+# to a neutral standing still motor positions with motors at + or - pi. It also
+# contains a safety layer so that the legs don't swing or extend too much to hit
+# the body of the robot.
+def leg_pose_to_motor_angles_with_half_pi_offset_and_safety(leg_pose):
+ """Converts the swing extension poses to the motor angles with safety limits.
+
+ Args:
+ leg_pose: A numpy array. Contains the leg pose for all four legs: [swing_0,
+ extension_0, swing_1, extension_1, swing_2, extension_2, swing_3,
+ extension_3]
+
+ Returns:
+ A numpy array. All eight motor angles.
+ """
+
+ motor_angles = []
+ for idx in range(4):
+ swing = leg_pose[idx * 2]
+ extend = leg_pose[idx * 2 + 1]
+ motor_angles.extend(swing_extend_to_motor_angles(idx, swing, extend))
+ return motor_angles
+
+
+def swing_extend_to_motor_angles(leg_id, swing, extension, noise_stdev=0):
+ """Swing - extension based leg model for minitaur.
+
+ Swing extension leg model calculates motor positions using 2 separate motions:
+ swing and extension. Swing rotates the whole leg by rotating both motors
+ equally towards same direction. Extension increases or decreases the length
+ of the leg by turning both motors equally in opposite direction.
+
+ This method also does the same conversion as leg_pose_to_motor_angles, but 0
+ swing and 0 extension maps to a neutral standing still motor positions with
+ motors at + or - pi.
+ Args:
+ leg_id: The id of the leg that the conversion is made for (0, 1, 2, 3).
+ swing: Swing degree for the leg (in radians). 0 means perpendicular to the
+ body).
+ extension: Extension level (length) of the leg, limited to [-1, 1].
+ noise_stdev: Standard deviation of the introduced noise at the motor
+ position level. Noise is turned off by default.
+
+ Returns:
+ motor0: Position for the first motor for that leg.
+ motor1: Position for the second motor for that leg.
+ Raises:
+ ValueError: In case calculated positions are outside the allowed boundaries.
+ """
+ # Check if the leg_id is in valid range
+ if not 0 <= leg_id <= 3:
+ raise ValueError('leg {} does not exist for a quadruped.'.format(leg_id))
+
+ # Front legs can not swing too much towards the body.
+ if leg_id % 2 == 0:
+ swing = np.clip(swing, LEG_SWING_LIMIT_LOW, LEG_SWING_LIMIT_HIGH)
+ # Back legs can not swing too much towards the body (opposite direction).
+ else:
+ swing = np.clip(swing, -LEG_SWING_LIMIT_HIGH, -LEG_SWING_LIMIT_LOW)
+
+ # Check if the motors are too close or too far away to make it impossible
+ # for the physical robot.
+ gap = math.pi - 2 * extension
+ if gap < MOTORS_GAP_LIMIT_LOW or gap > MOTORS_GAP_LIMIT_HIGH:
+ top_extension = (math.pi - MOTORS_GAP_LIMIT_LOW) / 2.0
+ least_extension = (math.pi - MOTORS_GAP_LIMIT_HIGH) / 2.0
+ extension = np.clip(extension, least_extension, top_extension)
+
+ # Initialization to neutral standing position where both motors point to
+ # opposite directions
+ motor0 = math.pi / 2
+ motor1 = math.pi / 2
+ # Rotational move
+ if leg_id in (0, 1):
+ motor0 += swing
+ motor1 -= swing
+ elif leg_id in (2, 3):
+ motor0 -= swing
+ motor1 += swing
+ # Extension
+ motor0 += extension
+ motor1 += extension
+
+ # Add noise if requested.
+ if noise_stdev > 0:
+ motor0 += np.random.normal(0, noise_stdev)
+ motor1 += np.random.normal(0, noise_stdev)
+
+ return motor0, motor1
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/noise_generators.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/noise_generators.py
new file mode 100644
index 000000000..012a93421
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/noise_generators.py
@@ -0,0 +1,255 @@
+"""Noise generators to simulate noise in sensor / actuator classes."""
+
+import abc
+import gin
+import numpy as np
+
+
+class NoiseGenerator(metaclass=abc.ABCMeta):
+ """Base class for noise generators."""
+
+ @abc.abstractmethod
+ def _get_noise(self, shape, dtype=None):
+ """Gets noise as a numpy array in the specified shape and dtype.
+
+ Tensorflow requires the shape and dtype of noise to be correctly specified,
+ so the generator needs to know this to produce data of the correct type.
+
+ Args:
+ shape: Shape of the returned array.
+ dtype: Datatype of returned array (None for default).
+ """
+
+ @abc.abstractmethod
+ def add_noise(self, data):
+ """Adds noise generated by _get_noise to the given data with clipping.
+
+ Args:
+ data: Numpy array of data to be modified with noise.
+ """
+
+
+@gin.configurable
+class BiasNoise(NoiseGenerator):
+ """Adds bias to the data, possibly with clipping."""
+
+ def __init__(self,
+ bias=0.0,
+ clipping_lower_bound=-np.inf,
+ clipping_upper_bound=np.inf):
+ """Create a bias noise generator.
+
+ Args:
+ bias: Absolute magnitude of bias applied to input.
+ clipping_lower_bound: lower bound of add_noise (use -np.inf to ignore).
+ clipping_upper_bound: Upper bound of add_noise (use np.inf to ignore).
+ """
+ self._bias = bias
+ self._clipping_lower_bound = clipping_lower_bound
+ self._clipping_upper_bound = clipping_upper_bound
+
+ def _get_noise(self, shape, dtype=None):
+ """Create bias noise of the given direction and datatype."""
+ return np.full(shape, self._bias, dtype)
+
+ def add_noise(self, data):
+ """Add bias noise to the given data, clipping to the given range."""
+ noise = self._get_noise(data.shape, data.dtype)
+ return np.clip(data + noise, self._clipping_lower_bound,
+ self._clipping_upper_bound)
+
+
+@gin.configurable
+class NormalNoise(BiasNoise):
+ """Adds Gaussian noise to the data, possibly with clipping."""
+
+ def __init__(self, scale, **kwargs):
+ """Create a normal noise generator.
+
+ Args:
+ scale: Absolute magnitude of standard deviation of Gaussian noise. Note
+ numpy will throw an error if scale < 0.
+ **kwargs: Arguments passed to BiasNoise (e.g. bias and clipping).
+ """
+ super(NormalNoise, self).__init__(**kwargs)
+ self._scale = scale
+
+ def _get_noise(self, shape, dtype=None):
+ """Create normal noise of the given direction and datatype."""
+ return np.random.normal(self._bias, self._scale, shape).astype(dtype)
+
+
+@gin.configurable
+class UniformNoise(NoiseGenerator):
+ """Generates uniform noise in the given range."""
+
+ def __init__(self,
+ low,
+ high,
+ clipping_lower_bound=-np.inf,
+ clipping_upper_bound=np.inf):
+ """Creates a uniform noise generator.
+
+ Args:
+ low: the lower bound of the noise.
+ high: the higher bound of the noise.
+ clipping_lower_bound: lower bound of add_noise (use -np.inf to ignore).
+ clipping_upper_bound: Upper bound of add_noise (use np.inf to ignore).
+ """
+ super().__init__()
+ self._low = low
+ self._high = high
+ self._clipping_lower_bound = clipping_lower_bound
+ self._clipping_upper_bound = clipping_upper_bound
+
+ def _get_noise(self, shape, dtype=None):
+ """Generates a noise using the given shape and data type."""
+ return np.random.uniform(self._low, self._high, shape).astype(dtype)
+
+ def add_noise(self, data):
+ """Adds noise to the given data, clipping to the given bound."""
+ noise = self._get_noise(data.shape, data.dtype)
+ return np.clip(data + noise, self._clipping_lower_bound,
+ self._clipping_upper_bound)
+
+
+@gin.configurable
+class RangeNoise(NormalNoise):
+ """Add normally distributed noise in m, applied to hit fractions in (0, 1).
+
+ This enables us to specify range noise in terms of meters of Gaussian noise
+ between a maximum and minimum range, but the add_noise is applied as above
+ to values expected to be in a hit fraction range of (0, 1) as needed for the
+ SimLidarSensor API. Separate methods return noise or noisify data in meters.
+ """
+
+ def __init__(self, range_noise_m, max_range_m, min_range_m=0.0, **kwargs):
+ """Create a normal noise generator suitable for use in a range scanner.
+
+ Args:
+ range_noise_m: Absolute magnitude of standard deviation of Gaussian noise,
+ applied to range observation readngs, measured in meters.
+ max_range_m: Maximum range in meters of the data, used for clipping.
+ min_range_m: Minimum range in meters of the data, used for clipping.
+ **kwargs: Other arguments passed to NormalNoise (principally bias).
+ """
+ # Validate range values.
+ if range_noise_m < 0.0:
+ raise ValueError("Range noise should not be negative: %r" % range_noise_m)
+ if min_range_m >= max_range_m:
+ raise ValueError("min_range_m %s must be less than max_range_m %s" %
+ (min_range_m, max_range_m))
+
+ self._range_noise_m = range_noise_m
+ self._max_range_m = max_range_m
+ self._min_range_m = min_range_m
+ self._total_range = max_range_m - min_range_m
+ super(RangeNoise, self).__init__(
+ scale=range_noise_m / self._total_range,
+ clipping_lower_bound=0.0,
+ clipping_upper_bound=1.0,
+ **kwargs)
+
+ def _get_noise_m(self, shape, dtype=None):
+ """Create normal noise of the given direction and datatype, in meters."""
+ return self.range_to_m(self._get_noise(shape=shape, dtype=dtype))
+
+ def add_noise_m(self, data):
+ """Add normal noise to the given data, scaled in meters."""
+ return self.range_to_m(self.add_noise(self.m_to_range(data)))
+
+ def m_to_range(self, data):
+ """Scale data in meters to a range of (0, 1)."""
+ return (data - self._min_range_m) / self._total_range
+
+ def range_to_m(self, data):
+ """Scale data in range of (0, 1) to meters."""
+ return data * self._total_range + self._min_range_m
+
+
+@gin.configurable
+class TwistNoise(object):
+ """Add normally distributed noise to twist actions.
+
+ Note this is a simplified noise model in action space designed for parity
+ with DriveWorld's r/s/e/drive_models/twist_drive.py;rcl=307540784;l=161.
+ This assumes that the TwistNoise will be applied to a twist action which is
+ then clipped, as currently done in wheeled_robot_base.py:
+
+ robotics/reinforcement_learning/minitaur/robots/wheeled_robot_base.py;l=533
+ # We assume that the velocity clipping would be absorbed in this API.
+ if self._action_filter:
+ action = self._action_filter.filter(action)
+
+ where action is a linear_velocity, angular_velocity pair, which is clipped
+ to limits subsequently by the _compute_kinematic_base_velocity method.
+ """
+
+ def __init__(self,
+ linear_velocity_noise_stdev_mps: float,
+ linear_velocity_noise_max_stdevs: float,
+ angular_velocity_noise_stdev_rps: float,
+ angular_velocity_noise_max_stdevs: float,
+ noise_scaling_cutoff_mps: float = 0.0):
+ """Create a normal noise generator suitable for use in a range scanner.
+
+ Supports the API specified in the DriveWorld TwistDrive class:
+ robotics/simulation/environments/drive_models/twist_drive.py;l=54
+
+ Args:
+ linear_velocity_noise_stdev_mps: One standard deviation of normal noise
+ for linear velocity in meters per second.
+ linear_velocity_noise_max_stdevs: Max stdevs for linear velocity noise.
+ This ensures that the noise values do not spike too crazy.
+ angular_velocity_noise_stdev_rps: One standard deviation of normal noise
+ for angular velocity in radians per second.
+ angular_velocity_noise_max_stdevs: Max stdevs for angular velocity noise.
+ noise_scaling_cutoff_mps: If linear velocity is less than this cutoff,
+ linear and angular noise are scaled so that zero velocity produces zero
+ noise. This enables a robot at rest to remain at rest, while still
+ applying reasonable noise values to finite velocities. Angular velocity
+ does not contribute to this computation to keep the model simple.
+ """
+ # Validate range values.
+ if linear_velocity_noise_stdev_mps < 0.0:
+ raise ValueError("Linear action noise should not be negative: %r" %
+ linear_velocity_noise_stdev_mps)
+ if linear_velocity_noise_max_stdevs < 0.0:
+ raise ValueError("Maximum linear noise should not be negative: %r" %
+ linear_velocity_noise_max_stdevs)
+ if angular_velocity_noise_stdev_rps < 0.0:
+ raise ValueError("Angular action noise should not be negative: %r" %
+ angular_velocity_noise_stdev_rps)
+ if angular_velocity_noise_max_stdevs < 0.0:
+ raise ValueError("Maximum action noise should not be negative: %r" %
+ angular_velocity_noise_max_stdevs)
+ if noise_scaling_cutoff_mps < 0.0:
+ raise ValueError("Noise scaling cutoff should not be negative: %r" %
+ noise_scaling_cutoff_mps)
+
+ # Save the values to create our noise later.
+ self._noise_shape = [
+ linear_velocity_noise_stdev_mps, angular_velocity_noise_stdev_rps
+ ]
+ # The noise clipping is performed using one standard deviation as the unit.
+ self._noise_lower_bound = np.array([
+ -linear_velocity_noise_max_stdevs * linear_velocity_noise_stdev_mps,
+ -angular_velocity_noise_max_stdevs * angular_velocity_noise_stdev_rps
+ ])
+ self._noise_upper_bound = -self._noise_lower_bound
+ self._noise_scaling_cutoff_mps = noise_scaling_cutoff_mps
+
+ def filter(self, action):
+ """Filter the linear and angular velocity by adding noise."""
+ linear_velocity, angular_velocity = action
+ linear_noise, angular_noise = np.clip(
+ np.random.normal(0, self._noise_shape, 2), self._noise_lower_bound,
+ self._noise_upper_bound)
+ if self._noise_scaling_cutoff_mps:
+ clipped_velocity = min(abs(linear_velocity),
+ self._noise_scaling_cutoff_mps)
+ scaling_factor = clipped_velocity / self._noise_scaling_cutoff_mps
+ linear_noise *= scaling_factor
+ angular_noise *= scaling_factor
+
+ return (linear_velocity + linear_noise, angular_velocity + angular_noise)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/rendering_utils.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/rendering_utils.py
new file mode 100644
index 000000000..99568108b
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/rendering_utils.py
@@ -0,0 +1,272 @@
+"""Rendering utilities."""
+
+import enum
+import math
+import os
+from typing import Any, Callable, Dict, Iterable, Optional, Text
+from absl import logging
+import gin
+import numpy as np
+
+
+# These matrices will change by any call to render() and should be saved
+# immediately to the local object.
+last_used_view_matrix = None
+last_used_proj_matrix = None
+last_rendered_image_size = None
+
+# Bounds of plane loaded with GEOM_PLANE.
+_INFINITY = 1.0e30
+# In case of infinite plane, use this scene bounding box.
+_DEFAULT_BOUNDING_BOX = ((-15, -15, -10), (15, 15, 0))
+
+
+def render_image(pybullet_client, width, height, view_matrix, proj_matrix):
+ """Renders image as numpy array given view and projection matrices."""
+ global last_used_view_matrix, last_used_proj_matrix, last_rendered_image_size
+ last_used_view_matrix = view_matrix
+ last_used_proj_matrix = proj_matrix
+
+ (_, _, px, _, _) = pybullet_client.getCameraImage(
+ width=width,
+ height=height,
+ renderer=pybullet_client.ER_BULLET_HARDWARE_OPENGL,
+ viewMatrix=view_matrix,
+ projectionMatrix=proj_matrix)
+ rgb_array = np.array(px)
+ image = rgb_array[:, :, :3]
+ last_rendered_image_size = (image.shape[1], image.shape[0])
+ return image
+
+
+def project_world_to_image(points,
+ view_matrix=None,
+ proj_matrix=None,
+ image_size=None,
+ include_z_coord=False):
+ """Projects 3D world-space points to 2D or 3D image-space points.
+
+ If no projection matrices or image_size are given, the last ones are used.
+ This means that you can render an image with `render_image` function (or other
+ functions that use this one) and then project points to that image immediately
+ after that call. Otherwise, you can save `last_used_view_matrix` and
+ `last_used_proj_matrix` variables and pass them in later.
+
+ Args:
+ points: Sequence of 3D points.
+ view_matrix: Optional view matrix (column-major).
+ proj_matrix: Optional projection matrix (column-major).
+ image_size: Optional image size for resulting coords.
+ include_z_coord: Whether to include Z coordinate in the resulting
+ projection. Z coordinate goes from 0 (far plane) to 1 (near plane).
+
+ Returns:
+ A Numpy array of shape (points_count, 2) or (points_count, 3) when
+ `include_z_coord` is True, with dtype of np.float32.
+ """
+ # Note that these matrices are column-major.
+ view_matrix = np.array(
+ view_matrix or last_used_view_matrix, dtype=np.float32).reshape((4, 4))
+ proj_matrix = np.array(
+ proj_matrix or last_used_proj_matrix, dtype=np.float32).reshape((4, 4))
+ mvp_matrix = np.matmul(view_matrix, proj_matrix)
+
+ # Add w component equal to 1 (for perspective projection).
+ points = np.asarray(points, dtype=np.float32)
+ points_4d = np.concatenate(
+ [points, np.ones((points.shape[0], 1), dtype=np.float32)], axis=-1)
+ points_proj = np.matmul(points_4d, mvp_matrix)
+
+ if include_z_coord:
+ # Perspective divide (only keep X, Y, and Z, discard W).
+ points_proj_3d = points_proj[:, 0:3] / np.expand_dims(points_proj[:, 3], -1)
+ # Shift origin to bottom left and rescale range to [0,1]. This assumes
+ # OpenGL projection space.
+ points_proj_3d = (points_proj_3d + 1) * 0.5
+ # Invert y-axis to have a coordinate with (0,0) on the top left.
+ points_proj_3d[:, 1] = 1 - points_proj_3d[:, 1]
+ # Scale projection to image size, ignore Z.
+ image_size = np.asarray(
+ image_size or last_rendered_image_size, dtype=np.float32)
+ image_size = np.append(image_size, 1)
+ return points_proj_3d * image_size
+ else:
+ # Perspective divide (only keep X and Y, discard Z and W).
+ points_proj_2d = points_proj[:, 0:2] / np.expand_dims(points_proj[:, 3], -1)
+ # Shift origin to bottom left and rescale range to [0,1]. This assumes
+ # OpenGL projection space.
+ points_proj_2d = (points_proj_2d + 1) * 0.5
+ # Invert y-axis to have a coordinate with (0,0) on the top left.
+ points_proj_2d[:, 1] = 1 - points_proj_2d[:, 1]
+ # Scale projection to image size.
+ image_size = np.asarray(
+ image_size or last_rendered_image_size, dtype=np.float32)
+ return points_proj_2d * image_size
+
+
+def get_scene_bounding_box(pybullet_client, scene=None):
+ """Computes scene axis-aligned bounding box.
+
+ Args:
+ pybullet_client: PyBullet client.
+ scene: Scene instance for filtering the bounding box of camera.
+
+ Returns:
+ A tuple of min and max 3D coordinates. Returns (None, None) if the scene
+ is empty.
+ """
+ aabb_min = None
+ aabb_max = None
+
+ for i in range(pybullet_client.getNumBodies()):
+ body_id = pybullet_client.getBodyUniqueId(i)
+
+ # If a scene has been provided, only count bodes which are in
+ # either the ground or obstacle id lists.
+ if scene is not None:
+ if body_id not in scene.ground_ids and body_id not in scene.obstacle_ids:
+ continue
+
+ aabb = pybullet_client.getAABB(body_id)
+ if np.any(np.abs(aabb) >= _INFINITY):
+ aabb = _DEFAULT_BOUNDING_BOX
+ if aabb_min is None:
+ aabb_min = aabb[0]
+ aabb_max = aabb[1]
+ else:
+ aabb_min = np.minimum(aabb_min, aabb[0])
+ aabb_max = np.maximum(aabb_max, aabb[1])
+
+ return aabb_min, aabb_max
+
+
+@gin.configurable
+def render_topdown(
+ pybullet_client,
+ result_size=(1280, 720),
+ scale_px_per_meter=None,
+ camera_height=50,
+ ground_height=None,
+ low_render_height_from_ground=None,
+ high_render_height_from_ground=None,
+ scene=None,
+ use_y_as_up_axis=False,
+ rendered_origin_and_size=None,
+):
+ """Renders top-down image of the environment.
+
+ Args:
+ pybullet_client: PyBullet client.
+ result_size: Resulting image size.
+ scale_px_per_meter: Resulting image scale in pixels per meter. This
+ overrides `result_size`.
+ camera_height: Height of the camera above the environment. This is not very
+ significant, the lower the height, the larger perspective distortion.
+ ground_height: Ground height for following two parameters.
+ low_render_height_from_ground: If set, rendering is cut below this distance
+ from ground.
+ high_render_height_from_ground: If set, rendering is cut above this
+ distance from ground.
+ scene: Scene instance for filtering the bounding box of camera.
+ use_y_as_up_axis: Whether to consider Y axis as world's "up" instead of Z.
+ rendered_origin_and_size: If set, sets rendered origin (bounding box min)
+ and size (bounding box size) instead of computing that from the scene.
+ Expected format: two tuples of [x, y z] coords containing origin and size
+ of rendered bounding box.
+
+ Returns:
+ RGB image as 3D numpy array.
+ """
+ # Get bounds of the current environment.
+ if rendered_origin_and_size is not None:
+ aabb_min, aabb_size = rendered_origin_and_size
+ if len(aabb_min) != 3:
+ raise ValueError(
+ "Invalid render origin, expected [x, y, z]: {}".format(aabb_min))
+ if len(aabb_size) != 3:
+ raise ValueError(
+ "Invalid render size, expected [x, y, z]: {}".format(aabb_size))
+ aabb_max = tuple(m + s for m, s in zip(aabb_min, aabb_size))
+ else:
+ aabb_min, aabb_max = get_scene_bounding_box(pybullet_client, scene)
+
+ if use_y_as_up_axis:
+ width = aabb_max[0] - aabb_min[0]
+ height = aabb_max[2] - aabb_min[2]
+ z_size = aabb_max[1] - aabb_min[1]
+ z_max = aabb_max[1]
+ else:
+ width = aabb_max[0] - aabb_min[0]
+ height = aabb_max[1] - aabb_min[1]
+ z_size = aabb_max[2] - aabb_min[2]
+ z_max = aabb_max[2]
+
+ if scale_px_per_meter is not None:
+ result_size = (int(width * scale_px_per_meter),
+ int(height * scale_px_per_meter))
+ else:
+ # Adjust scene size to fit inside of the given result size.
+ if len(result_size) != 2 or result_size[0] <= 0 or result_size[1] <= 0:
+ raise ValueError("Invalid result size: {}".format(result_size))
+ img_width, img_height = result_size
+ if img_width / width < img_height / height:
+ # Width is limiting. Adjust height to match.
+ adjusted_height = img_height * width / img_width
+ assert adjusted_height >= height, (adjusted_height, height)
+ height = adjusted_height
+ else:
+ # Height is limiting. Adjust width to match.
+ adjusted_width = img_width * height / img_height
+ assert adjusted_width >= width, (adjusted_width, width)
+ width = adjusted_width
+
+ # Compute view and projection matrices.
+ if use_y_as_up_axis:
+ center_x = (aabb_min[0] + aabb_max[0]) / 2.0
+ center_z = (aabb_min[2] + aabb_max[2]) / 2.0
+ view_matrix = pybullet_client.computeViewMatrix(
+ cameraEyePosition=(center_x, aabb_max[1] + camera_height, center_z),
+ cameraTargetPosition=(center_x, aabb_max[1], center_z),
+ cameraUpVector=(0, 0, 1))
+ else:
+ center_x = (aabb_min[0] + aabb_max[0]) / 2.0
+ center_y = (aabb_min[1] + aabb_max[1]) / 2.0
+ view_matrix = pybullet_client.computeViewMatrix(
+ cameraEyePosition=(center_x, center_y, aabb_max[2] + camera_height),
+ cameraTargetPosition=(center_x, center_y, aabb_max[2]),
+ cameraUpVector=(0, 1, 0))
+
+ near_plane = camera_height
+ far_plane = camera_height + z_size
+ if ground_height is not None:
+ if low_render_height_from_ground is not None:
+ far_plane = (
+ camera_height + z_max - ground_height - low_render_height_from_ground)
+ if high_render_height_from_ground is not None:
+ near_plane = (
+ camera_height + z_max - ground_height -
+ high_render_height_from_ground)
+ else:
+ if (low_render_height_from_ground is not None or
+ high_render_height_from_ground is not None):
+ raise ValueError(
+ "The `low_render_height_from_ground` or "
+ "`high_render_height_from_ground` were specified but no reference "
+ "ground height was given.")
+ vertical_fov = 2 * math.atan2(height / 2, camera_height)
+ proj_matrix = pybullet_client.computeProjectionMatrixFOV(
+ fov=math.degrees(vertical_fov),
+ aspect=width / height,
+ nearVal=near_plane,
+ farVal=far_plane)
+
+ # Render and return image.
+ return render_image(
+ pybullet_client,
+ result_size[0],
+ result_size[1],
+ view_matrix,
+ proj_matrix,
+ )
+
+
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/robot_pose_utils.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/robot_pose_utils.py
new file mode 100644
index 000000000..6e0e92243
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/robot_pose_utils.py
@@ -0,0 +1,189 @@
+"""This file implements the robot specific pose tools."""
+import math
+
+import attr
+import numpy as np
+
+from pybullet_envs.minitaur.envs_v2.utilities import laikago_pose_utils
+from pybullet_envs.minitaur.envs_v2.utilities import mini_cheetah_pose_utils
+from pybullet_envs.minitaur.envs_v2.utilities import minitaur_pose_utils
+from pybullet_envs.minitaur.robots import laikago
+from pybullet_envs.minitaur.robots import laikago_v2
+from pybullet_envs.minitaur.robots import mini_cheetah
+from pybullet_envs.minitaur.robots import minitaur_v2
+
+_ABDUCTION_ACTION_INDEXES = [0, 3, 6, 9]
+
+# The default values used to give a neutral pose for minitaur.
+_MINITAUR_DEFAULT_EXTENSION_POS = math.pi / 2
+_MINITAUR_DEFAULT_SWING_POS = 0
+
+_LAIKAGO_NEUTRAL_POSE_HIP_ANGLE = math.pi / 4
+_LAIKAGO_NEUTRAL_POSE_KNEE_ANGLE = -math.pi / 2
+_LAIKAGO_EXTENSION_CONVERSION_MULTIPLIER = 1.0
+_LAIKAGO_SWING_CONVERSION_MULTIPLIER = -1.0
+
+_MINI_CHEETAH_NEUTRAL_POSE_HIP_ANGLE = -math.pi / 4
+_MINI_CHEETAH_NEUTRAL_POSE_KNEE_ANGLE = math.pi / 2
+_MINI_CHEETAH_EXTENSION_CONVERSION_MULTIPLIER = -1.0
+_MINI_CHEETAH_SWING_CONVERSION_MULTIPLIER = 1.0
+
+
+def get_neutral_motor_angles(robot_class):
+ """Return a neutral (standing) pose for a given robot type.
+
+ Args:
+ robot_class: This returns the class (not the instance) for the robot.
+ Currently it supports minitaur, laikago and mini-cheetah.
+
+ Returns:
+ Pose object for the given robot. It's either MinitaurPose, LaikagoPose or
+ MiniCheetahPose.
+
+ Raises:
+ ValueError: If the given robot_class is different than the supported robots.
+ """
+ if str(robot_class) in [
+ str(minitaur_v2.Minitaur)
+ ]:
+ init_pose = minitaur_pose_utils.leg_pose_to_motor_angles(
+ np.array(
+ attr.astuple(
+ minitaur_pose_utils.MinitaurPose(
+ swing_angle_0=_MINITAUR_DEFAULT_SWING_POS,
+ swing_angle_1=_MINITAUR_DEFAULT_SWING_POS,
+ swing_angle_2=_MINITAUR_DEFAULT_SWING_POS,
+ swing_angle_3=_MINITAUR_DEFAULT_SWING_POS,
+ extension_angle_0=_MINITAUR_DEFAULT_EXTENSION_POS,
+ extension_angle_1=_MINITAUR_DEFAULT_EXTENSION_POS,
+ extension_angle_2=_MINITAUR_DEFAULT_EXTENSION_POS,
+ extension_angle_3=_MINITAUR_DEFAULT_EXTENSION_POS))))
+ elif str(robot_class) in [
+ str(laikago.Laikago),
+ str(laikago_v2.Laikago),
+ ]:
+ init_pose = np.array(
+ attr.astuple(
+ laikago_pose_utils.LaikagoPose(
+ abduction_angle_0=0,
+ hip_angle_0=_LAIKAGO_NEUTRAL_POSE_HIP_ANGLE,
+ knee_angle_0=_LAIKAGO_NEUTRAL_POSE_KNEE_ANGLE,
+ abduction_angle_1=0,
+ hip_angle_1=_LAIKAGO_NEUTRAL_POSE_HIP_ANGLE,
+ knee_angle_1=_LAIKAGO_NEUTRAL_POSE_KNEE_ANGLE,
+ abduction_angle_2=0,
+ hip_angle_2=_LAIKAGO_NEUTRAL_POSE_HIP_ANGLE,
+ knee_angle_2=_LAIKAGO_NEUTRAL_POSE_KNEE_ANGLE,
+ abduction_angle_3=0,
+ hip_angle_3=_LAIKAGO_NEUTRAL_POSE_HIP_ANGLE,
+ knee_angle_3=_LAIKAGO_NEUTRAL_POSE_KNEE_ANGLE)))
+ elif str(robot_class) == str(mini_cheetah.MiniCheetah):
+ init_pose = np.array(
+ attr.astuple(
+ mini_cheetah_pose_utils.MiniCheetahPose(
+ abduction_angle_0=0,
+ hip_angle_0=_MINI_CHEETAH_NEUTRAL_POSE_HIP_ANGLE,
+ knee_angle_0=_MINI_CHEETAH_NEUTRAL_POSE_KNEE_ANGLE,
+ abduction_angle_1=0,
+ hip_angle_1=_MINI_CHEETAH_NEUTRAL_POSE_HIP_ANGLE,
+ knee_angle_1=_MINI_CHEETAH_NEUTRAL_POSE_KNEE_ANGLE,
+ abduction_angle_2=0,
+ hip_angle_2=_MINI_CHEETAH_NEUTRAL_POSE_HIP_ANGLE,
+ knee_angle_2=_MINI_CHEETAH_NEUTRAL_POSE_KNEE_ANGLE,
+ abduction_angle_3=0,
+ hip_angle_3=_MINI_CHEETAH_NEUTRAL_POSE_HIP_ANGLE,
+ knee_angle_3=_MINI_CHEETAH_NEUTRAL_POSE_KNEE_ANGLE)))
+ else:
+ init_pose = robot_class.get_neutral_motor_angles()
+ return init_pose
+
+
+def convert_leg_pose_to_motor_angles(robot_class, leg_poses):
+ """Convert swing-extend coordinate space to motor angles for a robot type.
+
+ Args:
+ robot_class: This returns the class (not the instance) for the robot.
+ Currently it supports minitaur, laikago and mini-cheetah.
+ leg_poses: A list of leg poses in [swing,extend] or [abduction, swing,
+ extend] space for all 4 legs. The order is [abd_0, swing_0, extend_0,
+ abd_1, swing_1, extend_1, ...] or [swing_0, extend_0, swing_1, extend_1,
+ ...]. Zero swing and zero extend gives a neutral standing pose for all the
+ robots. For minitaur, the conversion is fully accurate, for laikago and
+ mini-cheetah the conversion is approximate where swing is reflected to hip
+ and extend is reflected to both knee and the hip.
+
+ Returns:
+ List of motor positions for the selected robot. The list include 8 or 12
+ motor angles depending on the given robot type as an argument. Currently
+ laikago and mini-cheetah has motors for abduction which does not exist for
+ minitaur robot.
+
+ Raises:
+ ValueError: Conversion fails due to wrong inputs.
+ """
+ default_leg_order = ["front_left", "back_left", "front_right", "back_right"]
+ leg_order = default_leg_order
+ if len(leg_poses) not in [8, 12]:
+ raise ValueError("Dimension of the leg pose provided is not 8 or 12.")
+ neutral_motor_angles = get_neutral_motor_angles(robot_class)
+ motor_angles = leg_poses
+ # If it is a robot with 12 motors but the provided leg pose does not contain
+ # abduction, extend the pose to include abduction.
+ if len(neutral_motor_angles) == 12 and len(leg_poses) == 8:
+ for i in _ABDUCTION_ACTION_INDEXES:
+ motor_angles.insert(i, 0)
+ # If the robot does not have abduction (minitaur) but the input contains them,
+ # ignore the abduction angles for the conversion.
+ elif len(neutral_motor_angles) == 8 and len(leg_poses) == 12:
+ del leg_poses[::3]
+ # Minitaur specific conversion calculations using minitaur-specific safety
+ # limits.
+ if str(robot_class) in [
+
+ str(minitaur_v2.Minitaur)
+ ]:
+ motor_angles = minitaur_pose_utils.leg_pose_to_motor_angles_with_half_pi_offset_and_safety(
+ leg_poses)
+ # Laikago and mini-cheetah specific conversion calculations.
+ elif str(robot_class) in [
+ str(mini_cheetah.MiniCheetah),
+ str(laikago.Laikago),
+ str(laikago_v2.Laikago),
+
+ ]:
+ swing_scale = 1.0
+ extension_scale = 1.0
+ # Laikago specific conversion multipliers.
+ if str(robot_class) in [
+ str(laikago.Laikago),
+ str(laikago_v2.Laikago),
+
+ ]:
+ swing_scale = _LAIKAGO_SWING_CONVERSION_MULTIPLIER
+ extension_scale = _LAIKAGO_EXTENSION_CONVERSION_MULTIPLIER
+ leg_order = ["front_right", "front_left", "back_right", "back_left"]
+ # Mini-cheetah specific multipliers.
+ elif str(robot_class) in [str(mini_cheetah.MiniCheetah)]:
+ swing_scale = _MINI_CHEETAH_SWING_CONVERSION_MULTIPLIER
+ extension_scale = _MINI_CHEETAH_EXTENSION_CONVERSION_MULTIPLIER
+ # In this approximate conversion for mini-cheetah and laikago we set hip
+ # angle swing + half of the extend and knee angle to extend as rotation.
+ # We also scale swing and extend based on some hand-tuned constants.
+ multipliers = np.array([1.0, swing_scale, extension_scale] * 4)
+ swing_extend_scaled = leg_poses * multipliers
+ # Swing is (swing - half of the extension) due to the geometry of the leg.
+ extra_swing = swing_extend_scaled * ([0, 0, -0.5] * 4)
+ swing_extend_scaled += np.roll(extra_swing, -1)
+ motor_angles = list(swing_extend_scaled)
+ motor_angles = neutral_motor_angles + motor_angles
+ # Change the order of the legs if it is different for the specific robot.
+ if leg_order != default_leg_order:
+ leg_order = [default_leg_order.index(leg) for leg in leg_order]
+ ordered_motor_angles = []
+ for i in leg_order:
+ ordered_motor_angles.extend(motor_angles[3 * i:3 * i + 3])
+ motor_angles = ordered_motor_angles
+ else:
+ motor_angles = robot_class.convert_leg_pose_to_motor_angles(leg_poses)
+
+ return motor_angles
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/termination_reason.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/termination_reason.py
new file mode 100644
index 000000000..5a943d9e4
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/termination_reason.py
@@ -0,0 +1,40 @@
+"""Enum for classifying the termination reason of an episode."""
+
+import enum
+import gin
+
+
+@gin.constants_from_enum
+class TerminationReason(enum.IntEnum):
+ """Enum that identifies termination reasons of an episode.
+
+ For any new termination reason added here, please update the corresponding
+ termination reward files to make sure it is used properly.
+ """
+ UNKNOWN = 0
+ STEP_LIMIT = 1
+ WALL_COLLISION = 2
+ BAD_LOCATION = 3
+ AGENT_COLLISION = 4
+ GOAL_REACHED = 5
+ INVALID_STEP_REVERT_AND_CONTINUE = 6
+ INVALID_EPISODE = 7
+ RUN_TIME_LIMIT = 8
+ NOT_ADVANCING_LIMIT = 9
+ NOT_LOCALIZED = 10
+
+
+COLORMAP = {
+ TerminationReason.UNKNOWN: (64, 64, 64), # Dark gray.
+ TerminationReason.STEP_LIMIT: (128, 64, 192), # Purple.
+ TerminationReason.WALL_COLLISION: (255, 64, 128), # Bright red.
+ TerminationReason.BAD_LOCATION: (255, 0, 192), # Magenta.
+ TerminationReason.AGENT_COLLISION: (255, 128, 0), # Orange.
+ TerminationReason.GOAL_REACHED: (96, 255, 96), # Bright green.
+ TerminationReason.INVALID_STEP_REVERT_AND_CONTINUE: (255, 255,
+ 255), # White.
+ TerminationReason.INVALID_EPISODE: (0, 0, 0), # Black.
+ TerminationReason.RUN_TIME_LIMIT: (128, 64, 192), # Purple.
+ TerminationReason.NOT_ADVANCING_LIMIT: (128, 64, 192), # Purple.
+ TerminationReason.NOT_LOCALIZED: (255, 0, 192), # Magenta.
+}
\ No newline at end of file
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/__init__.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/autonomous_object.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/autonomous_object.py
new file mode 100644
index 000000000..44c9d65ec
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/autonomous_object.py
@@ -0,0 +1,286 @@
+# Lint as: python3
+"""A module that defines autonomous object class and related functions."""
+from typing import Any, Callable, Dict, Optional, Sequence, Text, Union
+
+from absl import logging
+import gin
+import numpy as np
+
+from pybullet_utils import bullet_client
+from pybullet_envs.minitaur.envs_v2 import base_client
+from pybullet_envs.minitaur.envs_v2.sensors import sensor
+from pybullet_envs.minitaur.robots import object_controller
+from pybullet_envs.minitaur.robots import robot_base
+
+# The action value to pass into AutonomousObject pre_control_step() and
+# apply_action().
+AUTONOMOUS_ACTION = None
+
+# Maximum force used in constraint based actuation.
+_MAX_FORCE = 1000
+
+
+# TODO(b/155124699): find a better way actuate object than using constraint or
+# modifying URDF.
+@gin.configurable
+class AutonomousObject(robot_base.RobotBase):
+ """Autonomous object that moves/acts in simulation guided by a controller."""
+
+ def __init__(self,
+ urdf_file: Text,
+ sensors: Sequence[sensor.Sensor] = (),
+ controller: object_controller.ControllerBase = None,
+ actuate_by_reset: bool = False):
+ """Constructor.
+
+ Args:
+ urdf_file: The path to urdf file of the object.
+ sensors: A list of sensor objects to attach to autonomous object.
+ controller: A controller object that governs autonomous object's motion.
+ If not specified, StationaryController is used.
+ actuate_by_reset: Use pybullet resetBasePositionAndOrientation to actuate
+ the object. Default is False, which means actuate by constraint. In the
+ actuate by constrained mode, be extra cautious when the position or
+ orientation control is based on position or orientation sensor reading
+ of the same object. This loop-back condition is known to be problematic
+ and causes slower than expected motion.
+ """
+ self._urdf_file = urdf_file
+ self._controller = controller or object_controller.StationaryController()
+ self._actuate_by_reset = actuate_by_reset
+ self._actuate_function = (
+ self._actuate_base_pose
+ if not actuate_by_reset else self._reset_base_pose)
+
+ self._sensors = list(sensors)
+ self._object_id = -1
+ self._constraint_id = -1
+
+ self._pybullet_client = None # will be initialized in set_sim_client()
+ self._clock = None # will be initialized in set_clock()
+ self._init_internal_states()
+
+ def _init_internal_states(self) -> None:
+ self._observations_time_since_reset = 0
+ self._observations = {}
+ self._position = np.zeros(3)
+ self._orientation = np.array([0, 0, 0, 1])
+
+ def set_sim_client(self, pybullet_client: bullet_client.BulletClient) -> None:
+ """Sets new simulation client and reload assets."""
+ self._pybullet_client = pybullet_client
+ self._init_internal_states()
+ self.load()
+
+ def set_clock(self, clock: Callable[[], float]) -> None:
+ """Sets monotonic clock when adding into simulation environment."""
+ self._clock = clock
+
+ @property
+ def sim_object_id(self):
+ return self._object_id
+
+ def update(self, time_since_reset_sec: float,
+ observations: Dict[Text, Any]) -> None:
+ """Updates simulation time and observations.
+
+ This function should be called before apply_action in each simulation step.
+
+ Args:
+ time_since_reset_sec: Time from start of simulation reset in seconds.
+ observations: A dict of observations.
+ """
+ if time_since_reset_sec < self._observations_time_since_reset:
+ raise ValueError(
+ "Time cannot go backwards. Current t = %f, new t = %f." %
+ (self._observations_time_since_reset, time_since_reset_sec))
+ self._observations_time_since_reset = time_since_reset_sec
+ self._observations = observations
+
+ def _load_urdf(self):
+ """Loads object URDF file."""
+ try:
+ print("loading: ", self._urdf_file)
+ self._object_id = self._pybullet_client.loadURDF(self._urdf_file)
+ except:
+ print("Error: cannot load ", self._urdf_file)
+ import sys
+ sys.exit(0)
+
+ def load(self) -> None:
+ """Reconstructs the robot and resets its states."""
+ self._load_urdf()
+ if not self._actuate_by_reset:
+ self._constraint_id = self._pybullet_client.createConstraint(
+ parentBodyUniqueId=self._object_id,
+ parentLinkIndex=-1,
+ childBodyUniqueId=-1,
+ childLinkIndex=-1,
+ jointType=self._pybullet_client.JOINT_FIXED,
+ jointAxis=(0, 0, 0),
+ parentFramePosition=(0, 0, 0),
+ childFramePosition=(0, 0, 0),
+ childFrameOrientation=(0, 0, 0, 1))
+
+ for s in self._sensors:
+ s.set_robot(self)
+
+ # Resets the pose and updates the initial observations.
+ self.reset()
+
+ def reset(
+ self,
+ base_position: Optional[Sequence[float]] = None,
+ base_orientation_quaternion: Optional[Sequence[float]] = None,
+ controller: Optional[object_controller.ControllerBase] = None) -> None:
+ """Resets the states (e.g.
+
+ pose and sensor readings) of the robot.
+
+ This is called at the start of each episode by the environment.
+
+ Args:
+ base_position: Robot base position after reset. Must be None.
+ base_orientation_quaternion: Robot base orientation after reset. Must be
+ None.
+ controller: A new controller to replace original controller.
+ """
+ if base_position is not None or base_orientation_quaternion is not None:
+ raise ValueError("Reset position and orientation of AutonomousObject is "
+ "specified in controller.")
+
+ if controller is not None:
+ self._controller = controller
+
+ self._init_internal_states()
+ self._position, self._orientation, _ = self._controller.get_action(
+ object_controller.INIT_TIME, self._observations)
+
+ self._reset_base_pose(self._position, self._orientation)
+ self.receive_observation()
+
+ def terminate(self) -> None:
+ """Shuts down the robot, no-op in simulation."""
+
+ def pre_control_step(self, action: Any) -> Any:
+ """Processes the input action before the action repeat loop.
+
+ Args:
+ action: expect it to be `AUTONOMOUS_ACTION` at present.
+
+ Returns:
+ the action as is.
+ """
+ # Environment should not pass action other than AUTONOMOUS_ACTION.
+ if action is not AUTONOMOUS_ACTION:
+ raise ValueError("AutonomousObject only accept AUTONOMOUS_ACTION as "
+ "action value input.")
+ return action
+
+ def apply_action(self, action: Any) -> None:
+ """Applies the action to the robot."""
+ # Environment should not pass action other than AUTONOMOUS_ACTION.
+ if action is not AUTONOMOUS_ACTION:
+ raise ValueError("AutonomousObject only accept AUTONOMOUS_ACTION as "
+ "action value input.")
+ position, orientation, _ = self._controller.get_action(
+ self._observations_time_since_reset, self._observations)
+
+ self._actuate_function(position, orientation)
+
+ def receive_observation(self) -> None:
+ """Updates the robot sensor readings."""
+ position, orientation = (
+ self._pybullet_client.getBasePositionAndOrientation(self._object_id))
+ self._position = np.array(position)
+ self._orientation = np.array(orientation)
+
+ def post_control_step(self) -> None:
+ """Updates internal variables. Not yet used in AutonomousObject."""
+ pass
+
+ def _reset_base_pose(self,
+ position: Union[Sequence[float], np.ndarray] = None,
+ orientation_quat: Union[Sequence[float],
+ np.ndarray] = None):
+ """Resets the base to the desired position and orientation.
+
+ Args:
+ position: The desired base position. If omitted, current location is used.
+ orientation_quat: The desired base orientation in quaternion. If omitted,
+ current orientation is used.
+ """
+ if position is None:
+ position = self._position
+
+ if orientation_quat is None:
+ orientation_quat = self._orientation
+
+ self._pybullet_client.resetBaseVelocity(self._object_id, (0, 0, 0),
+ (0, 0, 0))
+ self._pybullet_client.resetBasePositionAndOrientation(
+ self._object_id, position, orientation_quat)
+
+ def _actuate_base_pose(self, position: Union[Sequence[float], np.ndarray],
+ orientation_quat: Union[Sequence[float], np.ndarray]):
+ """Actuates the base to the desired position and orientation.
+
+ Difference of this function from _reset_base_pose() is that this function
+ considers dynamics along the path and collisions along the motion path.
+
+ Args:
+ position: The desired base position.
+ orientation_quat: The desired base orientation in quaternion.
+ """
+ self._pybullet_client.changeConstraint(
+ self._constraint_id,
+ position,
+ jointChildFrameOrientation=orientation_quat,
+ maxForce=_MAX_FORCE)
+
+ def _reset_joint_angles(self, joint_angles=None):
+ """Resets the joint angles. Not yet used in AutonomousObject."""
+ del joint_angles
+
+ @property
+ def action_names(self) -> Sequence[Text]:
+ """Returns a sequence of action names. Always () for AutonomousObject."""
+ return ()
+
+ @property
+ def sensors(self) -> Sequence[sensor.Sensor]:
+ """Returns the sensors on this robot."""
+ return self._sensors
+
+ @property
+ def base_orientation_quaternion(self) -> np.ndarray:
+ """Returns the base pose as a quaternion in format (x, y, z, w)."""
+ return self._orientation.copy()
+
+ @property
+ def base_roll_pitch_yaw(self) -> np.ndarray:
+ """Returns the base roll, pitch, and yaw angles in radians."""
+ return np.array(
+ self._pybullet_client.getEulerFromQuaternion(self._orientation))
+
+ @property
+ def base_position(self) -> np.ndarray:
+ """Returns the base cartesian coordinates in meters."""
+ return self._position.copy()
+
+ @property
+ def timestamp(self):
+ """Simulation monotonic time."""
+ if self._clock is None:
+ raise RuntimeError("Must call set_clock() before accessing timestamp.")
+ return self._clock()
+
+ # This is need for CameraSensor.set_robot() to work.
+ @property
+ def pybullet_client(self):
+ return self._pybullet_client
+
+ # This is need for CameraSensor.set_robot() to work.
+ @property
+ def robot_id(self) -> int:
+ return self._object_id
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/crowd_controller.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/crowd_controller.py
new file mode 100644
index 000000000..409c029cf
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/crowd_controller.py
@@ -0,0 +1,706 @@
+# Lint as: python3
+"""Crowd objects/human controllers module."""
+
+import abc
+import collections
+from typing import Any, Callable, Dict, Iterable, List, Optional, Union, Sequence, Text
+
+from absl import logging
+import dataclasses
+import gin
+import numpy as np
+#import rvo2
+
+from pybullet_envs.minitaur.envs_v2.sensors import base_position_sensor
+from pybullet_envs.minitaur.envs_v2.sensors import sensor as generic_sensor
+from pybullet_envs.minitaur.robots import autonomous_object
+from pybullet_envs.minitaur.robots import object_controller
+
+
+POSITION_SENSOR_POSTFIX = "_pos"
+
+
+@dataclasses.dataclass
+class MovingObjectRecord:
+ position_key: Text
+ agent_id: int
+ radius: float
+ last_position: Optional[np.ndarray] = None
+
+
+@gin.configurable
+def sample_start_target_position(scene,
+ start=None,
+ start_circles=None,
+ target_circles=None,
+ num_sampling_retries=1,
+ min_wall_distance=0.0,
+ min_goal_euclidean_distance=0.0,
+ max_goal_euclidean_distance=np.Inf,
+ min_path_clearance=None):
+ """Sample valid start and target position reachable from start.
+
+ Args:
+ scene: a SceneBase instance implementing get_random_valid_position function.
+ start: a 2-tuple (x, y) of start position. If specified, no start is
+ sampled.
+ start_circles: a list of circle specification. Each circle is specified as
+ a tuple ((x, y), r) of a center (x, y) and radius r. If specified, start
+ position is sampled from within one of the start_circles.
+ target_circles: same as start_circle. If specified, target positions is
+ sampled from within one of the start_circles.
+ num_sampling_retries: a positive int, number of attempts to sample a
+ start, target pair.
+ min_wall_distance: a float, the minimum distance to a wall.
+ min_goal_euclidean_distance: a positive float, the minimum distance between
+ start and target.
+ max_goal_euclidean_distance: a positive float, the maximum distance between
+ start and target.
+ min_path_clearance: float, clearance of shortest path to walls.
+
+ Returns:
+ A 4 tuple (start, target, shortest_path, is_valid). start and target are
+ start and target positions, shortest_path is a list of 2-tuples specifying
+ the shortest path from start to target, is_valid is bool specifying whether
+ the start, target pair is valid. If min_path_clearance is not specified,
+ then shortest_path is None.
+ """
+ if not hasattr(scene, "get_random_valid_position"):
+ raise ValueError(
+ "Incompatible scene {}. Expected to have `get_random_valid_position` "
+ "method.".format(scene))
+
+ def _print_counters(counters):
+ for name, value in counters.items():
+ logging.info(" %s: %d", name, value)
+
+ sampling_counters = collections.defaultdict(lambda: 0)
+ for _ in range(num_sampling_retries):
+ if start is None:
+ start_pos = scene.get_random_valid_position(
+ min_wall_distance, inclusion_circles=start_circles)
+ else:
+ if start_circles is not None:
+ raise ValueError("At most one of the arguments start and start_circles "
+ "can be not None.")
+ start_pos = start
+ target_pos = scene.get_random_valid_position(
+ min_wall_distance, inclusion_circles=target_circles)
+ sampling_counters["attempts"] += 1
+
+ euclidean_distance = np.linalg.norm(target_pos - start_pos)
+ if euclidean_distance < min_goal_euclidean_distance:
+ sampling_counters["min_euclidean"] += 1
+ continue
+ if euclidean_distance > max_goal_euclidean_distance:
+ sampling_counters["max_euclidean"] += 1
+ continue
+
+ # Skip the path computation is no path clearance is provided.
+ if min_path_clearance is None:
+ logging.info("Valid goal with no minimum path clearance checking.")
+ _print_counters(sampling_counters)
+ return start_pos, target_pos, None, True
+
+ # Check the goal clearance along the shortest path
+ if not hasattr(scene, "find_shortest_path"):
+ raise ValueError(
+ f"scene %s missing find_shortest_path method {scene}")
+
+ # This is a slow process.
+ shortest_path = scene.find_shortest_path(
+ start_pos[:2], target_pos[:2], min_path_clearance)
+ # No path exists between current robot position and goal satisfying the
+ # clearance.
+ if shortest_path is None:
+ sampling_counters["path_clearance"] += 1
+ continue
+
+ logging.info("Valid start/target with path clearance checking.")
+ _print_counters(sampling_counters)
+ return start_pos, target_pos, shortest_path, True
+
+ logging.info("No valid start/target found.")
+ _print_counters(sampling_counters)
+ return start_pos, target_pos, None, False
+
+
+class CrowdController(metaclass=abc.ABCMeta):
+ """Crowd controller interface."""
+
+ def __init__(self, names: Iterable[Text],
+ position_key_formatter="%s" + POSITION_SENSOR_POSTFIX):
+ """Constructor.
+
+ Args:
+ names: Name of instance (dynamic object or human).
+ position_key_formatter: Formatter to convert name to position sensor name.
+ """
+ self._names = list(names)
+ self._position_key_formatter = position_key_formatter
+ self._num_instance = len(self._names)
+
+ self._current_time = 0
+
+ def _validate_instance_id(self, instance_id):
+ if not 0 <= instance_id < self._num_instance:
+ raise ValueError(
+ f"instance_id must be an integer in [0, {self.num_instance}), "
+ f"got {instance_id}.")
+
+ @property
+ def num_instance(self):
+ """Returns the number of crowd instances."""
+ return self._num_instance
+
+ def instance_name(self, instance_id: int) -> Text:
+ """Returns the name of instance."""
+ self._validate_instance_id(instance_id)
+ return self._names[instance_id]
+
+ def instance_controller(
+ self, instance_id: int) -> object_controller.ControllerBase:
+ """Returns the individual controller of certain instance."""
+ self._validate_instance_id(instance_id)
+ return _IndividualController(self, instance_id)
+
+ def instance_get_action(
+ self, instance_id: int, time_sec: float,
+ observations: Dict[Text, Any]) -> object_controller.ControllerOutput:
+ """Returns action of specific instance given observation.
+
+ This method is for _IndividualController.
+
+ Args:
+ instance_id: Identifier of an object in the crowd.
+ time_sec: Time since simulation reset in seconds. If time < 0, returns
+ initial values and ignores observations.
+ observations: A dict of all observations.
+
+ Returns:
+ Position, orientation and an extra info dict for robot joints, human
+ skeletal pose, etc.
+ """
+ if time_sec < 0:
+ self._recalculate_actions(object_controller.INIT_TIME, {})
+ self._current_time = object_controller.INIT_TIME
+ elif time_sec > self._current_time:
+ self._current_time = time_sec
+ self._recalculate_actions(self._current_time, observations)
+
+ self._validate_instance_id(instance_id)
+
+ return self._get_action_of_instance(instance_id)
+
+ @abc.abstractmethod
+ def _recalculate_actions(
+ self, time_sec: float, observations: Dict[Text, Any]) -> None:
+ """Calculates crowd command for all instances in crowd."""
+ raise NotImplementedError(
+ "_recalculate_actions() should be implemented by subclass.")
+
+ @abc.abstractmethod
+ def _get_action_of_instance(
+ self, instance_id: int) -> object_controller.ControllerOutput:
+ """Returns calculated actions of specific instance."""
+ raise NotImplementedError(
+ "_get_action_of_instance() should be implemented by subclass.")
+
+ def set_scene(self, scene) -> None:
+ """Sets the scene for crowd controller to obtain scene information."""
+ del scene
+
+
+class _IndividualController(object_controller.ControllerBase):
+ """A utility class that wraps crowd controller in ControllerBase interface."""
+
+ def __init__(self, crowd_controller: CrowdController, instance_id: int):
+ """Constructor.
+
+ Args:
+ crowd_controller: The controller of crowd to which this instance belong.
+ instance_id: Identifier of a crowd instance.
+ """
+ self._instance_id = instance_id
+ self._crowd_controller = crowd_controller
+
+ def get_action(
+ self, time_sec: float,
+ observations: Dict[Text, Any]) -> object_controller.ControllerOutput:
+ """Returns position, orientation and pose based on time and observations.
+
+ Args:
+ time_sec: Time since simulation reset in seconds. If time < 0, returns
+ initial values and ignores observations.
+ observations: A dict of all observations.
+
+ Returns:
+ Position, orientation and an extra info dict for robot joints, human
+ skeletal pose, etc.
+ """
+ return self._crowd_controller.instance_get_action(
+ self._instance_id, time_sec, observations)
+
+
+@gin.configurable
+class StationaryController(CrowdController):
+ """A crowd controller that places crowd objects at fixed positions."""
+
+ def __init__(
+ self, positions: Sequence[Sequence[float]],
+ orientations: Optional[Sequence[Sequence[float]]] = None, **kwargs):
+ """Constructor.
+
+ Args:
+ positions: Fixed positions (3D points) of crowd instances.
+ orientations: Fixed orientations in quaternion of crowd instances.
+ **kwargs: Keyword arguments to pass on to base class.
+ """
+ super().__init__(**kwargs)
+
+ if orientations is None:
+ orientations = np.array(((0, 0, 0, 1),) * self.num_instance)
+
+ if not len(positions) == len(orientations) == self.num_instance:
+ raise ValueError(
+ f"positions and orientations should all have the same length "
+ f"{self.num_instance}. Got len(positions) = {len(positions)}, "
+ f"len(orientations) = {len(orientations)}.")
+
+ self._positions = positions
+ self._orientations = orientations
+
+ def _recalculate_actions(
+ self, time_sec: float, observations: Dict[Text, Any]) -> None:
+ """Calculates crowd command for all instances in crowd."""
+ del time_sec
+ del observations
+
+ def _get_action_of_instance(
+ self, instance_id: int) -> object_controller.ControllerOutput:
+ """Returns calculated actions of specific instance."""
+ self._validate_instance_id(instance_id)
+ return self._positions[instance_id], self._orientations[instance_id], {}
+
+
+@gin.configurable
+class OrcaController(CrowdController):
+ """A crowd controller that controls crowd instances using ORCA algorithm.
+
+ Crowd instance will be initialized at a specified start position and move
+ towards specified target position in a linear path while avoid collision with
+ each other.
+ """
+
+ _DEFAULT_NEIGHBOR_DISTANCE_M = 5
+ _DEFAULT_MAX_NEIGHBORS = 10
+ _DEFAULT_RADIUS_M = 0.5
+ _DEFAULT_MAX_SPEED_MPS = 2
+ _DEFAULT_TIME_HORIZON_SEC = 1.0
+ _DEFAULT_OBSTACLE_TIME_HORIZON_SEC = 0.3
+
+ def __init__(
+ self,
+ timestep: float,
+ start_positions: Optional[Sequence[Sequence[float]]] = None,
+ target_positions: Optional[Sequence[Sequence[float]]] = None,
+ use_position_generator: Optional[bool] = False,
+ group_sizes: Sequence[int] = None,
+ radius: float = _DEFAULT_RADIUS_M,
+ max_speed_mps: float = _DEFAULT_MAX_SPEED_MPS,
+ time_horizon_sec: float = _DEFAULT_TIME_HORIZON_SEC,
+ obstacle_time_horizon_sec: float = _DEFAULT_OBSTACLE_TIME_HORIZON_SEC,
+ neighbor_distance_m: float = _DEFAULT_NEIGHBOR_DISTANCE_M,
+ max_neighbors: int = _DEFAULT_MAX_NEIGHBORS,
+ workaround_erp_issue: bool = True,
+ moving_objects_pos_key: Sequence[Text] = (),
+ moving_objects_radius: Union[float, Sequence[float]] = _DEFAULT_RADIUS_M,
+ endless_trajectory: bool = True,
+ **kwargs):
+ """Constructor.
+
+ Args:
+ timestep: Timestep of simulation.
+ start_positions: A list of position (x, y, z) for crowd instances as
+ their starting position.
+ target_positions: A list of position (x, y, z) for crowd instances as
+ their target position.
+ use_position_generator: a boolean, if True than the start and end
+ positions are sampled. start_positions and target_positions must be None
+ group_sizes: If set, then crowd is split in groups randomly, whose sizes
+ are picked in random from this group_size list. In this way, the
+ crowd simulator sumulaters clusters of objects moving around.
+ radius: Radius of crowd instances.
+ max_speed_mps: Maximum crowd instance speed.
+ time_horizon_sec: Time horizon in second.
+ obstacle_time_horizon_sec: Time horizon for static obstacle in second.
+ neighbor_distance_m: Neighbor distance in meters. Instances closer than
+ this distance are considered neighbors.
+ max_neighbors: Max number of neighbors.
+ workaround_erp_issue: There is an issue with pybullet constraint that the
+ constraint is solved only 20% per timestep. Need to amplify position
+ delta by 5x to workaround this issue.
+ moving_objects_pos_key: Position observation key of moving objects not
+ controlled by the ORCA controller.
+ moving_objects_radius: Radius of moving objects. Should be a float, which
+ applies to all moving objects, or a sequence of float, which should be
+ of the same length as moving_objects_pos_key.
+ endless_trajectory: Only valid if use_position_generator is True. Agent
+ returns to starting point after reaching goal to achieve endless motion.
+ **kwargs: Keyword arguments to pass on to base class.
+ """
+ super().__init__(**kwargs)
+
+ assert ((start_positions is not None and target_positions is not None) or
+ use_position_generator)
+ if not use_position_generator:
+ if not len(start_positions) == len(target_positions) == self.num_instance:
+ raise ValueError(
+ f"start_positions and target_positions should both have length "
+ f"equals {self.num_instance}: "
+ f"len(start_positions) = {len(start_positions)}, "
+ f"len(target_positions) = {len(target_positions)}.")
+
+ self._timestep = timestep
+ self._radius = radius
+ self._max_speed_mps = max_speed_mps
+ self._time_horizon_sec = time_horizon_sec
+ self._obstacle_time_horizon_sec = obstacle_time_horizon_sec
+ self._neighbor_distance_m = neighbor_distance_m
+ self._max_neighbors = max_neighbors
+ self._use_position_generator = use_position_generator
+ self._endless_trajectory = endless_trajectory
+ self._scene = None
+ if isinstance(moving_objects_radius, float):
+ moving_objects_radius = [
+ moving_objects_radius] * len(moving_objects_pos_key)
+ if len(moving_objects_radius) != len(moving_objects_pos_key):
+ raise ValueError(
+ "moving_objects_radius should be either a float or a sequence of "
+ "float with the same length as moving_objects_pos_key.")
+ self._moving_objects = [
+ MovingObjectRecord(position_key=key, agent_id=-1, radius=radius)
+ for key, radius in zip(moving_objects_pos_key, moving_objects_radius)]
+
+ self._paths = None
+ self._path_indices = None
+ if self._use_position_generator:
+ self._start_positions = None
+ self._target_positions = None
+ else:
+ self._start_positions = np.array(start_positions, dtype=np.float64)
+ self._target_positions = np.array(target_positions, dtype=np.float64)
+ # A guard against multiple initializations. See recalculate_actions below.
+ self._already_initialized = False
+ self._group_sizes = [1] if group_sizes is None else group_sizes
+
+ # The following variables are initialized in _recalculate_actions()
+ self._current_positions = None
+ self._command_positions = None
+ self._command_orientations = None
+
+ #self._orca = rvo2.PyRVOSimulator(
+ # self._timestep, # timestep
+ # self._neighbor_distance_m, # neighborDist
+ # self._max_neighbors, # maxNeighbors
+ # self._time_horizon_sec, # timeHorizon
+ # self._obstacle_time_horizon_sec, # timeHorizonObst
+ # self._radius, # radius
+ # self._max_speed_mps # maxSpeed
+ #)
+ for i in range(self.num_instance):
+ if self._use_position_generator:
+ start_position = (0, 0)
+ else:
+ start_position = self._start_positions[i, :2]
+ agent_id = self._orca.addAgent(
+ tuple(start_position),
+ self._neighbor_distance_m, # neighborDist
+ self._max_neighbors, # maxNeighbors
+ self._time_horizon_sec, # timeHorizon
+ self._obstacle_time_horizon_sec, # timeHorizonObst
+ self._radius, # radius
+ self._max_speed_mps, # maxSpeed
+ (0.0, 0.0)) # velocity
+ assert agent_id == i
+
+ for obj in self._moving_objects:
+ obj.agent_id = self._orca.addAgent(
+ (0.0, 0.0), # position (will adjust after simulation starts)
+ self._neighbor_distance_m, # neighborDist
+ self._max_neighbors, # maxNeighbors
+ self._timestep, # timeHorizon
+ self._timestep, # timeHorizonObst
+ obj.radius, # radius
+ self._max_speed_mps, # maxSpeed
+ (0.0, 0.0)) # velocity
+
+ self._workaround_erp_issue = workaround_erp_issue
+
+ def _subsample_path(self, path, subsample_step=1.0):
+ subsampled_path = [path[0]]
+ traveled_dist = 0.0
+ for i, (s, t) in enumerate(zip(path[:-1], path[1:])):
+ traveled_dist += np.sqrt(
+ np.square(s[0] - t[0]) + np.square(s[1] - t[1]))
+ if traveled_dist > subsample_step or i >= len(path) - 2:
+ subsampled_path.append(t)
+ traveled_dist = 0.0
+ return subsampled_path
+
+ def _generate_start_target_positions(self):
+ """Generates start and target positions using goal generartors."""
+ assert self._scene is not None
+ self._start_positions = np.zeros((self.num_instance, 3), dtype=np.float64)
+ self._target_positions = np.zeros((self.num_instance, 3), dtype=np.float64)
+
+ self._paths = []
+ self._path_indices = []
+ start_circles, target_circles = None, None
+ group_radius = 1.0
+ current_group_size = np.random.choice(self._group_sizes)
+ index_in_current_group = 0
+ for i in range(self._num_instance):
+ start_pos, target_pos, path, is_valid = sample_start_target_position(
+ self._scene,
+ start_circles=start_circles,
+ target_circles=target_circles)
+ if index_in_current_group == current_group_size - 1:
+ start_circles, target_circles = None, None
+ index_in_current_group = 0
+ current_group_size = np.random.choice(self._group_sizes)
+ else:
+ if start_circles is None:
+ start_circles = [(start_pos[:2], group_radius)]
+ target_circles = [(target_pos[:2], group_radius)]
+ else:
+ start_circles += [(start_pos[:2], group_radius)]
+ target_circles += [(target_pos[:2], group_radius)]
+ index_in_current_group += 1
+ if not is_valid:
+ raise ValueError("No valid start/target positions.")
+ self._start_positions[i, :] = start_pos
+ self._target_positions[i, :] = target_pos
+
+ subsampled_path = self._subsample_path(path)
+ self._paths.append(np.array(subsampled_path, dtype=np.float32))
+ self._path_indices.append(0)
+
+ def _recalculate_actions(
+ self, time_sec: float, observations: Dict[Text, Any]) -> None:
+ """Calculates crowd command for all crowd instances."""
+ if self._use_position_generator:
+ if (time_sec == object_controller.INIT_TIME and
+ self._start_positions is None and
+ not self._already_initialized):
+ self._generate_start_target_positions()
+ # Initialize only once per initial time even if recalculate actions
+ # is called multiple times.
+ self._already_initialized = True
+ if time_sec == object_controller.INIT_TIME:
+ # Resets orca simulator.
+ for i in range(len(self._names)):
+ self._orca.setAgentPosition(i, tuple(self._start_positions[i, :2]))
+
+ self._command_positions = self._start_positions.copy()
+ self._current_positions = self._start_positions.copy()
+ self._command_orientations = np.repeat(
+ ((0.0, 0.0, 0.0, 1.0),), len(self._names), axis=0)
+ self._last_target_recalculation_sec = time_sec
+ return
+ else:
+ # The moment we step beyond initial time, we can initialize again.
+ self._already_initialized = False
+
+ if self._use_position_generator:
+ for i in range(self._num_instance):
+ dist = np.linalg.norm(
+ self._current_positions[i, :] - self._target_positions[i, :])
+ if dist < 2.0:
+ _, target_pos, path, is_valid = sample_start_target_position(
+ self._scene, self._current_positions[i, :])
+ if is_valid:
+ self._target_positions[i, :] = target_pos
+ subsampled_path = self._subsample_path(path)
+ self._paths.append(np.array(subsampled_path, dtype=np.float32))
+ self._path_indices.append(0)
+
+ # Sets agent position and preferred velocity based on target.
+ for i, agent_name in enumerate(self._names):
+ position = observations[self._position_key_formatter % agent_name]
+ self._orca.setAgentPosition(
+ i, tuple(position[:2])) # ORCA uses 2D position.
+ self._current_positions[i, :2] = position[:2]
+
+ if self._paths is not None:
+ # Find closest point on the path from start to target, which (1) hasn't
+ # been covered already; (2) is at least max_coverage_distance away from
+ # current position.
+ distances = np.sqrt(np.sum(np.square(
+ self._paths[i] - position[:2]), axis=1))
+ max_coverage_distance = 1.0
+ index = self._path_indices[i]
+ while True:
+ if index >= len(self._paths[i]) - 1:
+ if self._endless_trajectory:
+ self._paths[i] = self._paths[i][::-1]
+ distances = distances[::-1]
+ index = 0
+ break
+ elif distances[index] > max_coverage_distance:
+ break
+ else:
+ index += 1
+ self._path_indices[i] = index
+ target_position = self._paths[i][index, :]
+ else:
+ target_position = self._target_positions[i][:2]
+
+ goal_vector = target_position - position[:2]
+ goal_vector_norm = np.linalg.norm(goal_vector) + np.finfo(np.float32).eps
+ goal_unit_vector = goal_vector / goal_vector_norm
+
+ kv = 1
+ velocity = min(kv * goal_vector_norm,
+ self._DEFAULT_MAX_SPEED_MPS) * goal_unit_vector
+ self._orca.setAgentPrefVelocity(i, tuple(velocity))
+
+ for obj in self._moving_objects:
+ position = observations[obj.position_key]
+ self._orca.setAgentPosition(obj.agent_id, tuple(position[:2]))
+ if obj.last_position is None:
+ self._orca.setAgentPrefVelocity(obj.agent_id, (0.0, 0.0))
+ else:
+ velocity = (position - obj.last_position) / self._timestep
+ self._orca.setAgentPrefVelocity(obj.agent_id, tuple(velocity[:2]))
+ obj.last_position = position.copy()
+
+ # Advances orca simulator.
+ self._orca.doStep()
+
+ # Retrieve agent position and save in buffer.
+ for i in range(len(self._names)):
+ x, y = self._orca.getAgentPosition(i)
+ self._command_positions[i, :2] = (x, y)
+
+ yaw = np.arctan2(y - self._current_positions[i, 1],
+ x - self._current_positions[i, 0])
+ self._command_orientations[i] = (0, 0, np.sin(yaw / 2), np.cos(yaw / 2))
+
+ def _get_action_of_instance(
+ self, instance_id) -> object_controller.ControllerOutput:
+ """Returns calculated actions of specific instance."""
+
+ if self._command_positions is None:
+ raise RuntimeError(
+ "Attempted to get action of instance before _recalculate_actions().")
+
+ self._validate_instance_id(instance_id)
+
+ if self._workaround_erp_issue:
+ k_erp = 1 / 0.2
+ delta_position = (
+ self._command_positions[instance_id] -
+ self._current_positions[instance_id])
+ command_position = (
+ self._current_positions[instance_id] + k_erp * delta_position)
+ else:
+ command_position = self._command_positions[instance_id].copy()
+ return command_position, self._command_orientations[instance_id], {}
+
+ def set_scene(self, scene) -> None:
+ """Sets the scene for crowd controller to obtain scene information."""
+ try:
+ polygons = scene.vectorized_map
+ for polygon in polygons:
+ self._orca.addObstacle([tuple(point) for point in polygon])
+ self._orca.processObstacles()
+ self._scene = scene
+ except NotImplementedError:
+ logging.exception("Scene does not implement vectorized_map property. "
+ "Crowd agent cannot avoid static obstacles.")
+
+
+@gin.configurable
+def uniform_object_factory(
+ instance_id: int,
+ object_factory: Callable[..., autonomous_object.AutonomousObject],
+ *args, **kwargs) -> autonomous_object.AutonomousObject:
+ """A wrapper that removes instance_id in default crowd object factory."""
+ del instance_id
+ return object_factory(*args, **kwargs)
+
+
+@gin.configurable
+def random_object_factory(
+ instance_id: int,
+ object_factories: Iterable[
+ Callable[..., autonomous_object.AutonomousObject]],
+ *args, **kwargs) -> autonomous_object.AutonomousObject:
+ """A wrapper that removes instance_id in default crowd object factory."""
+ del instance_id
+ object_factory = np.random.choice(object_factories)
+ return object_factory(*args, **kwargs)
+
+
+@gin.configurable
+def sensor_factory(instance_id: int, sensor: Callable[...,
+ generic_sensor.Sensor],
+ *args, **kwargs) -> generic_sensor.Sensor:
+ del instance_id
+ return sensor(*args, **kwargs)
+
+
+@gin.configurable
+class CrowdBuilder(object):
+ """A helper class to construct a crowd."""
+
+ def __init__(
+ self,
+ num_instance: int,
+ crowd_controller_factory: Callable[..., CrowdController],
+ object_factory: Callable[..., autonomous_object.AutonomousObject],
+ sensor_factories: Iterable[Callable[..., generic_sensor.Sensor]] = None):
+ """Constructor.
+
+ Args:
+ num_instance: Number of autonomous objects in the crowd.
+ crowd_controller_factory: A callable that returns a crowd controller
+ object.
+ object_factory: Callable that returns an autonomous object.
+ sensor_factories: list of sensor callables.
+ """
+ self._objects = []
+ crowd_id_prefix = "crowd"
+ names = [crowd_id_prefix + "_%d" % i for i in range(num_instance)]
+
+ self._controller = crowd_controller_factory(names=names)
+
+ for i in range(num_instance):
+ position_sensor = base_position_sensor.BasePositionSensor(
+ name=names[i] + POSITION_SENSOR_POSTFIX)
+
+ # Add additional per agent sensors (e.g. camera, occupancy, etc.).
+ add_sensors = []
+ if sensor_factories:
+ for s in sensor_factories:
+ add_sensors.append(
+ sensor_factory(
+ instance_id=i, sensor=s, name=names[i] + "_" + s.__name__))
+
+ an_object = object_factory(
+ instance_id=i,
+ sensors=(position_sensor,) + tuple(add_sensors),
+ controller=self._controller.instance_controller(i))
+
+ self._objects.append(an_object)
+
+ @property
+ def crowd_objects(self) -> List[autonomous_object.AutonomousObject]:
+ """Returns list of AutonomousObjects in the crowd."""
+ return self._objects
+
+ @property
+ def crowd_controller(self) -> CrowdController:
+ """Returns the crowd controller."""
+ return self._controller
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/hybrid_motor_model.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/hybrid_motor_model.py
new file mode 100644
index 000000000..4a15422b9
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/hybrid_motor_model.py
@@ -0,0 +1,278 @@
+# Lint as: python3
+"""A generic PD motor model."""
+
+from typing import Tuple, Union
+import gin
+import numpy as np
+
+from pybullet_envs.minitaur.robots import robot_config
+from pybullet_envs.minitaur.robots import time_ordered_buffer
+
+_DEFAULT_BUFFER_SIZE = 200
+
+_HYBRID_ACTION_LEN = len(robot_config.HybridActionIndex)
+_HYBRID_POS_INDEX = robot_config.HybridActionIndex.POSITION.value
+_HYBRID_KP_INDEX = robot_config.HybridActionIndex.POSITION_GAIN.value
+_HYBRID_VEL_INDEX = robot_config.HybridActionIndex.VELOCITY.value
+_HYBRID_KD_INDEX = robot_config.HybridActionIndex.VELOCITY_GAIN.value
+_HYBRID_TORQUE_INDEX = robot_config.HybridActionIndex.TORQUE.value
+
+
+def _convert_to_np_array(inputs: Union[float, Tuple[float], np.ndarray], dim):
+ """Converts the inputs to a numpy array.
+
+ Args:
+ inputs: The input scalar or array.
+ dim: The dimension of the converted numpy array.
+
+ Returns:
+ The converted numpy array.
+
+ Raises:
+ ValueError: If the inputs is an array whose dimension does not match the
+ provied dimension.
+ """
+ outputs = None
+ if isinstance(inputs, (tuple, np.ndarray)):
+ outputs = np.array(inputs)
+ else:
+ outputs = np.full(dim, inputs)
+
+ if len(outputs) != dim:
+ raise ValueError("The inputs array has a different dimension {}"
+ " than provided, which is {}.".format(len(outputs), dim))
+
+ return outputs
+
+
+@gin.configurable
+class HybridMotorModel(object):
+ """A simple motor model that supports proportional and derivative control.
+
+ When in POSITION mode, the torque is calculated according to the difference
+ between current and desired joint angle, as well as the joint velocity
+ differences. For more information about PD control, please refer to:
+ https://en.wikipedia.org/wiki/PID_controller.
+
+ The model supports a HYBRID mode in which each motor command can be a tuple
+ (desired_motor_angle, position_gain, desired_motor_velocity, velocity_gain,
+ torque).
+ """
+
+ def __init__(
+ self,
+ num_motors: int,
+ pd_latency: float = 0,
+ motor_control_mode=robot_config.MotorControlMode.POSITION,
+ kp: Union[float, Tuple[float], np.ndarray] = 60,
+ kd: Union[float, Tuple[float], np.ndarray] = 1,
+ strength_ratios: Union[float, Tuple[float], np.ndarray] = 1,
+ torque_lower_limits: Union[float, Tuple[float], np.ndarray] = None,
+ torque_upper_limits: Union[float, Tuple[float], np.ndarray] = None,
+ ):
+ """Initializes the class.
+
+ Args:
+ num_motors: The number of motors for parallel computation.
+ pd_latency: Simulates the motor controller's latency in reading motor
+ angles and velocities.
+ motor_control_mode: Can be POSITION, TORQUE, or HYBRID. In POSITION
+ control mode, the PD formula is used to track a desired position and a
+ zero desired velocity. In TORQUE control mode, we assume a pass through
+ of the provided torques. In HYBRID control mode, the users need to
+ provie (desired_position, position_gain, desired_velocity,
+ velocity_gain, feedfoward_torque) for each motor.
+ kp: The default position gains for motors.
+ kd: The default velocity gains for motors.
+ strength_ratios: The scaling ratio for motor torque outputs. This can be
+ useful for quick debugging when sim-to-real gap is observed in the
+ actuator behavior.
+ torque_lower_limits: The lower bounds for torque outputs.
+ torque_upper_limits: The upper bounds for torque outputs. The output
+ torques will be clipped by the lower and upper bounds.
+
+ Raises:
+ ValueError: If the number of motors provided is negative or zero.
+ """
+ if num_motors <= 0:
+ raise ValueError(
+ "Number of motors must be positive, not {}".format(num_motors))
+ self._num_motors = num_motors
+ self._zero_array = np.full(num_motors, 0)
+ self._pd_latency = pd_latency
+ self._hybrid_command_dim = _HYBRID_ACTION_LEN * self._num_motors
+ self.set_motor_gains(kp, kd)
+ self.set_strength_ratios(strength_ratios)
+ self._torque_lower_limits = None
+ if torque_lower_limits:
+ self._torque_lower_limits = _convert_to_np_array(torque_lower_limits,
+ self._num_motors)
+
+ self._torque_upper_limits = None
+ if torque_upper_limits:
+ self._torque_upper_limits = _convert_to_np_array(torque_upper_limits,
+ self._num_motors)
+ self._motor_control_mode = motor_control_mode
+
+ # The history buffer is used to simulate the pd latency effect.
+ # TODO(b/157786642): remove hacks on duplicate timestep once the sim clock
+ # is fixed.
+ self._observation_buffer = time_ordered_buffer.TimeOrderedBuffer(
+ max_buffer_timespan=pd_latency,
+ error_on_duplicate_timestamp=False,
+ replace_value_on_duplicate_timestamp=True)
+
+ def set_strength_ratios(
+ self,
+ strength_ratios: Union[float, Tuple[float], np.ndarray],
+ ):
+ """Sets the strength of each motor relative to the default value.
+
+ Args:
+ strength_ratios: The relative strength of motor output, ranging from [0,
+ 1] inclusive.
+ """
+ self._strength_ratios = np.clip(
+ _convert_to_np_array(strength_ratios, self._num_motors), 0, 1)
+
+ def set_motor_gains(
+ self,
+ kp: Union[float, Tuple[float], np.ndarray],
+ kd: Union[float, Tuple[float], np.ndarray],
+ ):
+ """Sets the gains of all motors.
+
+ These gains are PD gains for motor positional control. kp is the
+ proportional gain and kd is the derivative gain.
+
+ Args:
+ kp: Proportional gain of the motors.
+ kd: Derivative gain of the motors.
+ """
+ self._kp = _convert_to_np_array(kp, self._num_motors)
+ self._kd = _convert_to_np_array(kd, self._num_motors)
+
+ def get_motor_gains(self):
+ """Get the PD gains of all motors.
+
+ Returns:
+ Proportional and derivative gain of the motors.
+ """
+ return self._kp, self._kd
+
+ def reset(self):
+ self._observation_buffer.reset()
+
+ def update(self, timestamp, true_motor_positions: np.ndarray,
+ true_motor_velocities: np.ndarray):
+ # Push these to the buffer
+ self._observation_buffer.add(timestamp,
+ (true_motor_positions, true_motor_velocities))
+
+ def get_motor_torques(
+ self,
+ motor_commands: np.ndarray,
+ motor_control_mode=None) -> Tuple[np.ndarray, np.ndarray]:
+ """Computes the motor torques.
+
+ Args:
+ motor_commands: The desired motor angle if the motor is in position
+ control mode. The pwm signal if the motor is in torque control mode.
+ motor_control_mode: A MotorControlMode enum.
+
+ Returns:
+ observed_torque: The torque observed. This emulates the limitations in
+ torque measurement, which is generally obtained from current estimations.
+ actual_torque: The torque that needs to be applied to the motor.
+
+ Raises:
+ NotImplementedError if the motor_control_mode is not supported.
+
+ """
+ if not motor_control_mode:
+ motor_control_mode = self._motor_control_mode
+
+ motor_torques = None
+
+ if motor_control_mode is robot_config.MotorControlMode.TORQUE:
+ motor_torques = motor_commands
+
+ if motor_control_mode is robot_config.MotorControlMode.POSITION:
+ motor_torques = self._compute_pd_torques(
+ desired_motor_angles=motor_commands,
+ kp=self._kp,
+ desired_motor_velocities=self._zero_array,
+ kd=self._kd)
+
+ if motor_control_mode is robot_config.MotorControlMode.HYBRID:
+ motor_torques = self._compute_hybrid_action_torques(motor_commands)
+
+ if motor_torques is None:
+ raise ValueError(
+ "{} is not a supported motor control mode".format(motor_control_mode))
+
+ # Rescale and clip the motor torques as needed.
+ motor_torques = self._strength_ratios * motor_torques
+ if (self._torque_lower_limits is not None or
+ self._torque_upper_limits is not None):
+ motor_torques = np.clip(motor_torques, self._torque_lower_limits,
+ self._torque_upper_limits)
+
+ return motor_torques, motor_torques
+
+ def get_motor_states(self, latency=None):
+ """Computes observation of motor angle and velocity under latency."""
+ if latency is None:
+ latency = self._pd_latency
+ buffer = self._observation_buffer.get_delayed_value(latency)
+ angle_vel_t0 = buffer.value_0
+ angle_vel_t1 = buffer.value_1
+ coeff = buffer.coeff
+
+ pos_idx = 0
+ motor_angles = angle_vel_t0[pos_idx] * (
+ 1 - coeff) + coeff * angle_vel_t1[pos_idx]
+ vel_idx = 1
+ motor_velocities = angle_vel_t0[vel_idx] * (
+ 1 - coeff) + coeff * angle_vel_t1[vel_idx]
+ return motor_angles, motor_velocities
+
+ def _compute_pd_torques(
+ self,
+ desired_motor_angles: np.ndarray,
+ kp: np.ndarray,
+ desired_motor_velocities,
+ kd: np.ndarray,
+ ) -> Tuple[np.ndarray, np.ndarray]:
+ """Computes the pd torques.
+
+ Args:
+ desired_motor_angles: The motor angles to track.
+ kp: The position gains.
+ desired_motor_velocities: The motor velocities to track.
+ kd: The velocity gains.
+
+ Returns:
+ The computed motor torques.
+ """
+ motor_angles, motor_velocities = self.get_motor_states()
+ motor_torques = -kp * (motor_angles - desired_motor_angles) - kd * (
+ motor_velocities - desired_motor_velocities)
+
+ return motor_torques
+
+ def _compute_hybrid_action_torques(
+ self, motor_commands: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
+ """Computes the pd torques in the HYBRID mode."""
+ assert len(motor_commands) == self._hybrid_command_dim
+ kp = motor_commands[_HYBRID_KP_INDEX::_HYBRID_ACTION_LEN]
+ kd = motor_commands[_HYBRID_KD_INDEX::_HYBRID_ACTION_LEN]
+ desired_motor_angles = motor_commands[_HYBRID_POS_INDEX::_HYBRID_ACTION_LEN]
+ desired_motor_velocities = motor_commands[
+ _HYBRID_VEL_INDEX::_HYBRID_ACTION_LEN]
+ additional_torques = motor_commands[
+ _HYBRID_TORQUE_INDEX::_HYBRID_ACTION_LEN]
+
+ return self._compute_pd_torques(desired_motor_angles, kp,
+ desired_motor_velocities,
+ kd) + additional_torques
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago.py
new file mode 100644
index 000000000..bcfcbdc3e
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago.py
@@ -0,0 +1,319 @@
+"""Pybullet simulation of a Laikago robot."""
+import math
+import os
+import re
+import gin
+import numpy as np
+from pybullet_utils import transformations
+from pybullet_envs.minitaur.envs_v2.utilities import laikago_pose_utils
+from pybullet_envs.minitaur.robots import laikago_constants
+from pybullet_envs.minitaur.robots import laikago_motor
+from pybullet_envs.minitaur.robots import minitaur
+from pybullet_envs.minitaur.robots import robot_config
+
+NUM_MOTORS = 12
+NUM_LEGS = 4
+MOTOR_NAMES = [
+ "FR_hip_motor_2_chassis_joint",
+ "FR_upper_leg_2_hip_motor_joint",
+ "FR_lower_leg_2_upper_leg_joint",
+ "FL_hip_motor_2_chassis_joint",
+ "FL_upper_leg_2_hip_motor_joint",
+ "FL_lower_leg_2_upper_leg_joint",
+ "RR_hip_motor_2_chassis_joint",
+ "RR_upper_leg_2_hip_motor_joint",
+ "RR_lower_leg_2_upper_leg_joint",
+ "RL_hip_motor_2_chassis_joint",
+ "RL_upper_leg_2_hip_motor_joint",
+ "RL_lower_leg_2_upper_leg_joint",
+]
+INIT_RACK_POSITION = [0, 0, 1]
+INIT_POSITION = [0, 0, 0.48]
+JOINT_DIRECTIONS = np.array([-1, 1, 1, 1, 1, 1, -1, 1, 1, 1, 1, 1])
+HIP_JOINT_OFFSET = 0.0
+UPPER_LEG_JOINT_OFFSET = -0.6
+KNEE_JOINT_OFFSET = 0.66
+DOFS_PER_LEG = 3
+JOINT_OFFSETS = np.array(
+ [HIP_JOINT_OFFSET, UPPER_LEG_JOINT_OFFSET, KNEE_JOINT_OFFSET] * 4)
+PI = math.pi
+
+MAX_MOTOR_ANGLE_CHANGE_PER_STEP = 0.12
+_DEFAULT_HIP_POSITIONS = (
+ (0.21, -0.1157, 0),
+ (0.21, 0.1157, 0),
+ (-0.21, -0.1157, 0),
+ (-0.21, 0.1157, 0),
+)
+
+# Bases on the readings from Laikago's default pose.
+INIT_MOTOR_ANGLES = np.array([
+ laikago_pose_utils.LAIKAGO_DEFAULT_ABDUCTION_ANGLE,
+ laikago_pose_utils.LAIKAGO_DEFAULT_HIP_ANGLE,
+ laikago_pose_utils.LAIKAGO_DEFAULT_KNEE_ANGLE
+] * NUM_LEGS)
+
+CHASSIS_NAME_PATTERN = re.compile(r"\w+_chassis_\w+")
+MOTOR_NAME_PATTERN = re.compile(r"\w+_hip_motor_\w+")
+KNEE_NAME_PATTERN = re.compile(r"\w+_lower_leg_\w+")
+TOE_NAME_PATTERN = re.compile(r"jtoe\d*")
+
+URDF_NO_TOES = "laikago.urdf"
+URDF_WITH_TOES = "laikago_toes_zup.urdf"
+
+_BODY_B_FIELD_NUMBER = 2
+_LINK_A_FIELD_NUMBER = 3
+
+
+@gin.configurable
+class Laikago(minitaur.Minitaur):
+ """A simulation for the Laikago robot."""
+
+ def __init__(self, urdf_filename=URDF_WITH_TOES, **kwargs):
+ self._urdf_filename = urdf_filename
+ if "motor_kp" not in kwargs:
+ kwargs["motor_kp"] = 100.0
+ if "motor_kd" not in kwargs:
+ kwargs["motor_kd"] = 2.0
+ if "motor_torque_limits" not in kwargs:
+ kwargs["motor_torque_limits"] = None
+
+ # enable_clip_motor_commands: Boolean indicating if clipping should be
+ # applied to motor commands, which limits the amount of change in joint
+ # pose between timesteps.
+ if "enable_clip_motor_commands" in kwargs:
+ self._enable_clip_motor_commands = kwargs["enable_clip_motor_commands"]
+ del kwargs["enable_clip_motor_commands"]
+ else:
+ self._enable_clip_motor_commands = False
+
+ # The follwing parameters are fixed for the Laikago robot.
+ kwargs["num_motors"] = NUM_MOTORS
+ kwargs["dofs_per_leg"] = DOFS_PER_LEG
+ kwargs["motor_direction"] = JOINT_DIRECTIONS
+ kwargs["motor_offset"] = JOINT_OFFSETS
+ kwargs["motor_overheat_protection"] = False
+ kwargs["motor_model_class"] = laikago_motor.LaikagoMotorModel
+ kwargs["safety_config"] = None
+
+ super(Laikago, self).__init__(**kwargs)
+
+ def _LoadRobotURDF(self):
+ laikago_urdf_path = self.GetURDFFile()
+ if self._self_collision_enabled:
+ self.quadruped = self._pybullet_client.loadURDF(
+ laikago_urdf_path,
+ self._GetDefaultInitPosition(),
+ self._GetDefaultInitOrientation(),
+ flags=self._pybullet_client.URDF_USE_SELF_COLLISION)
+ else:
+ self.quadruped = self._pybullet_client.loadURDF(
+ laikago_urdf_path, self._GetDefaultInitPosition(),
+ self._GetDefaultInitOrientation())
+
+ def _SettleDownForReset(self, default_motor_angles, reset_time):
+ self.ReceiveObservation()
+
+ if reset_time <= 0:
+ return
+
+ for _ in range(500):
+ self._StepInternal(
+ INIT_MOTOR_ANGLES,
+ motor_control_mode=robot_config.MotorControlMode.POSITION)
+ if default_motor_angles is not None:
+ num_steps_to_reset = int(reset_time / self.time_step)
+ for _ in range(num_steps_to_reset):
+ self._StepInternal(
+ default_motor_angles,
+ motor_control_mode=robot_config.MotorControlMode.POSITION)
+
+ def GetHipPositionsInBaseFrame(self):
+ return _DEFAULT_HIP_POSITIONS
+
+ def GetFootContacts(self):
+ all_contacts = self._pybullet_client.getContactPoints(bodyA=self.quadruped)
+
+ contacts = [False, False, False, False]
+ for contact in all_contacts:
+ # Ignore self contacts
+ if contact[_BODY_B_FIELD_NUMBER] == self.quadruped:
+ continue
+ try:
+ toe_link_index = self._foot_link_ids.index(
+ contact[_LINK_A_FIELD_NUMBER])
+ contacts[toe_link_index] = True
+ except ValueError:
+ continue
+ return contacts
+
+ def ComputeJacobian(self, leg_id):
+ """Compute the Jacobian for a given leg."""
+ # Because of the default rotation in the Laikago URDF, we need to reorder
+ # the rows in the Jacobian matrix.
+ if self._urdf_filename == URDF_WITH_TOES:
+ return super(Laikago, self).ComputeJacobian(leg_id)
+ else:
+ return super(Laikago, self).ComputeJacobian(leg_id)[(2, 0, 1), :]
+
+ def ResetPose(self, add_constraint):
+ del add_constraint
+ for name in self._joint_name_to_id:
+ joint_id = self._joint_name_to_id[name]
+ self._pybullet_client.setJointMotorControl2(
+ bodyIndex=self.quadruped,
+ jointIndex=(joint_id),
+ controlMode=self._pybullet_client.VELOCITY_CONTROL,
+ targetVelocity=0,
+ force=0)
+ for name, i in zip(MOTOR_NAMES, range(len(MOTOR_NAMES))):
+ if "hip_motor_2_chassis_joint" in name:
+ angle = INIT_MOTOR_ANGLES[i] + HIP_JOINT_OFFSET
+ elif "upper_leg_2_hip_motor_joint" in name:
+ angle = INIT_MOTOR_ANGLES[i] + UPPER_LEG_JOINT_OFFSET
+ elif "lower_leg_2_upper_leg_joint" in name:
+ angle = INIT_MOTOR_ANGLES[i] + KNEE_JOINT_OFFSET
+ else:
+ raise ValueError("The name %s is not recognized as a motor joint." %
+ name)
+ self._pybullet_client.resetJointState(
+ self.quadruped, self._joint_name_to_id[name], angle, targetVelocity=0)
+
+ def GetURDFFile(self):
+ return os.path.join(self._urdf_root, "laikago/" + self._urdf_filename)
+
+ def _BuildUrdfIds(self):
+ """Build the link Ids from its name in the URDF file.
+
+ Raises:
+ ValueError: Unknown category of the joint name.
+ """
+ num_joints = self._pybullet_client.getNumJoints(self.quadruped)
+ self._chassis_link_ids = [-1]
+ self._leg_link_ids = []
+ self._motor_link_ids = []
+ self._knee_link_ids = []
+ self._foot_link_ids = []
+
+ for i in range(num_joints):
+ joint_info = self._pybullet_client.getJointInfo(self.quadruped, i)
+ joint_name = joint_info[1].decode("UTF-8")
+ joint_id = self._joint_name_to_id[joint_name]
+ if CHASSIS_NAME_PATTERN.match(joint_name):
+ self._chassis_link_ids.append(joint_id)
+ elif MOTOR_NAME_PATTERN.match(joint_name):
+ self._motor_link_ids.append(joint_id)
+ # We either treat the lower leg or the toe as the foot link, depending on
+ # the urdf version used.
+ elif KNEE_NAME_PATTERN.match(joint_name):
+ self._knee_link_ids.append(joint_id)
+ elif TOE_NAME_PATTERN.match(joint_name):
+ assert self._urdf_filename == URDF_WITH_TOES
+ self._foot_link_ids.append(joint_id)
+ else:
+ raise ValueError("Unknown category of joint %s" % joint_name)
+
+ self._leg_link_ids.extend(self._knee_link_ids)
+ self._leg_link_ids.extend(self._foot_link_ids)
+
+ if self._urdf_filename == URDF_NO_TOES:
+ self._foot_link_ids.extend(self._knee_link_ids)
+
+ assert len(self._foot_link_ids) == NUM_LEGS
+ self._chassis_link_ids.sort()
+ self._motor_link_ids.sort()
+ self._knee_link_ids.sort()
+ self._foot_link_ids.sort()
+ self._leg_link_ids.sort()
+
+ return
+
+ def _GetMotorNames(self):
+ return MOTOR_NAMES
+
+ def _GetDefaultInitPosition(self):
+ if self._on_rack:
+ return INIT_RACK_POSITION
+ else:
+ return INIT_POSITION
+
+ def _GetDefaultInitOrientation(self):
+ # The Laikago URDF assumes the initial pose of heading towards z axis,
+ # and belly towards y axis. The following transformation is to transform
+ # the Laikago initial orientation to our commonly used orientation: heading
+ # towards -x direction, and z axis is the up direction.
+ if self._urdf_filename == URDF_WITH_TOES:
+ return [0, 0, 0, 1]
+ else:
+ return transformations.quaternion_from_euler(
+ ai=math.pi / 2.0, aj=0, ak=math.pi / 2.0, axes="sxyz")
+
+ def GetDefaultInitPosition(self):
+ """Get default initial base position."""
+ return self._GetDefaultInitPosition()
+
+ def GetDefaultInitOrientation(self):
+ """Get default initial base orientation."""
+ return self._GetDefaultInitOrientation()
+
+ def GetDefaultInitJointPose(self):
+ """Get default initial joint pose."""
+ joint_pose = (INIT_MOTOR_ANGLES + JOINT_OFFSETS) * JOINT_DIRECTIONS
+ return joint_pose
+
+ def ApplyAction(self, motor_commands, motor_control_mode=None):
+ """Clips and then apply the motor commands using the motor model.
+
+ Args:
+ motor_commands: np.array. Can be motor angles, torques, hybrid commands,
+ or motor pwms (for Minitaur only).N
+ motor_control_mode: A MotorControlMode enum.
+ """
+ if self._enable_clip_motor_commands:
+ motor_commands = self._ClipMotorCommands(motor_commands)
+
+ super(Laikago, self).ApplyAction(motor_commands, motor_control_mode)
+ return
+
+ def _ClipMotorCommands(self, motor_commands):
+ """Clips motor commands.
+
+ Args:
+ motor_commands: np.array. Can be motor angles, torques, hybrid commands,
+ or motor pwms (for Minitaur only).
+
+ Returns:
+ Clipped motor commands.
+ """
+
+ # clamp the motor command by the joint limit, in case weired things happens
+ max_angle_change = MAX_MOTOR_ANGLE_CHANGE_PER_STEP
+ current_motor_angles = self.GetMotorAngles()
+ motor_commands = np.clip(motor_commands,
+ current_motor_angles - max_angle_change,
+ current_motor_angles + max_angle_change)
+ return motor_commands
+
+ @classmethod
+ def GetConstants(cls):
+ del cls
+ return laikago_constants
+
+ # The following functions are added for the migration purpose. Will be removed
+ # after the migration is complete.
+
+ @property
+ def robot_id(self):
+ return self.quadruped
+
+ @property
+ def base_position(self):
+ return self.GetBasePosition()
+
+ @property
+ def base_roll_pitch_yaw(self):
+ return self.GetTrueBaseRollPitchYaw()
+
+ @property
+ def timestamp(self):
+ return self.GetTimeSinceReset()
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago_constants.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago_constants.py
new file mode 100644
index 000000000..d07e73071
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago_constants.py
@@ -0,0 +1,120 @@
+# Lint as: python3
+"""Defines the laikago robot related constants and URDF specs."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import collections
+import gin
+
+URDF_PATH = "laikago/laikago_toes_zup.urdf"
+
+NUM_MOTORS = 12
+NUM_LEGS = 4
+MOTORS_PER_LEG = 3
+
+INIT_RACK_POSITION = [0, 0, 1]
+INIT_POSITION = [0, 0, 0.48]
+
+# Will be default to (0, 0, 0, 1) once the new laikago_toes_zup.urdf checked in.
+INIT_ORIENTATION = [0, 0, 0, 1]
+
+# Can be different from the motors, although for laikago they are the same list.
+JOINT_NAMES = (
+ # front right leg
+ "FR_hip_motor_2_chassis_joint",
+ "FR_upper_leg_2_hip_motor_joint",
+ "FR_lower_leg_2_upper_leg_joint",
+ # front left leg
+ "FL_hip_motor_2_chassis_joint",
+ "FL_upper_leg_2_hip_motor_joint",
+ "FL_lower_leg_2_upper_leg_joint",
+ # rear right leg
+ "RR_hip_motor_2_chassis_joint",
+ "RR_upper_leg_2_hip_motor_joint",
+ "RR_lower_leg_2_upper_leg_joint",
+ # rear left leg
+ "RL_hip_motor_2_chassis_joint",
+ "RL_upper_leg_2_hip_motor_joint",
+ "RL_lower_leg_2_upper_leg_joint",
+)
+
+INIT_ABDUCTION_ANGLE = 0
+INIT_HIP_ANGLE = 0.67
+INIT_KNEE_ANGLE = -1.25
+
+# Note this matches the Laikago SDK/control convention, but is different from
+# URDF's internal joint angles which needs to be computed using the joint
+# offsets and directions. The conversion formula is (sdk_joint_angle + offset) *
+# joint direction.
+INIT_JOINT_ANGLES = collections.OrderedDict(
+ zip(JOINT_NAMES,
+ (INIT_ABDUCTION_ANGLE, INIT_HIP_ANGLE, INIT_KNEE_ANGLE) * NUM_LEGS))
+
+# Used to convert the robot SDK joint angles to URDF joint angles.
+JOINT_DIRECTIONS = collections.OrderedDict(
+ zip(JOINT_NAMES, (-1, 1, 1, 1, 1, 1, -1, 1, 1, 1, 1, 1)))
+
+HIP_JOINT_OFFSET = 0.0
+UPPER_LEG_JOINT_OFFSET = -0.6
+KNEE_JOINT_OFFSET = 0.66
+
+# Used to convert the robot SDK joint angles to URDF joint angles.
+JOINT_OFFSETS = collections.OrderedDict(
+ zip(JOINT_NAMES,
+ [HIP_JOINT_OFFSET, UPPER_LEG_JOINT_OFFSET, KNEE_JOINT_OFFSET] *
+ NUM_LEGS))
+
+LEG_NAMES = (
+ "front_right",
+ "front_left",
+ "rear_right",
+ "rear_left",
+)
+
+LEG_ORDER = (
+ "front_right",
+ "front_left",
+ "back_right",
+ "back_left",
+)
+
+END_EFFECTOR_NAMES = (
+ "jtoeFR",
+ "jtoeFL",
+ "jtoeRR",
+ "jtoeRL",
+)
+
+MOTOR_NAMES = JOINT_NAMES
+MOTOR_GROUP = collections.OrderedDict((
+ (LEG_NAMES[0], JOINT_NAMES[0:3]),
+ (LEG_NAMES[1], JOINT_NAMES[3:6]),
+ (LEG_NAMES[2], JOINT_NAMES[6:9]),
+ (LEG_NAMES[3], JOINT_NAMES[9:12]),
+))
+
+# Regulates the joint angle change when in position control mode.
+MAX_MOTOR_ANGLE_CHANGE_PER_STEP = 0.12
+
+# The hip joint location in the CoM frame.
+HIP_POSITIONS = collections.OrderedDict((
+ (LEG_NAMES[0], (0.21, -0.1157, 0)),
+ (LEG_NAMES[1], (0.21, 0.1157, 0)),
+ (LEG_NAMES[2], (-0.21, -0.1157, 0)),
+ (LEG_NAMES[3], (-0.21, 0.1157, 0)),
+))
+
+# Add the gin constants to be used for gin binding in config. Append "LAIKAGO_"
+# for unique binding names.
+gin.constant("laikago_constants.LAIKAGO_NUM_MOTORS", NUM_MOTORS)
+gin.constant("laikago_constants.LAIKAGO_URDF_PATH", URDF_PATH)
+gin.constant("laikago_constants.LAIKAGO_INIT_POSITION", INIT_POSITION)
+gin.constant("laikago_constants.LAIKAGO_INIT_ORIENTATION", INIT_ORIENTATION)
+gin.constant("laikago_constants.LAIKAGO_INIT_JOINT_ANGLES", INIT_JOINT_ANGLES)
+gin.constant("laikago_constants.LAIKAGO_JOINT_DIRECTIONS", JOINT_DIRECTIONS)
+gin.constant("laikago_constants.LAIKAGO_JOINT_OFFSETS", JOINT_OFFSETS)
+gin.constant("laikago_constants.LAIKAGO_MOTOR_NAMES", MOTOR_NAMES)
+gin.constant("laikago_constants.LAIKAGO_END_EFFECTOR_NAMES", END_EFFECTOR_NAMES)
+gin.constant("laikago_constants.LAIKAGO_MOTOR_GROUP", MOTOR_GROUP)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago_interface.proto b/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago_interface.proto
new file mode 100644
index 000000000..3e2a31a97
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago_interface.proto
@@ -0,0 +1,181 @@
+syntax = "proto3";
+
+package minitaur_fluxworks.control;
+
+import "timestamp.proto";
+import "vector.proto";
+
+// A general motor command.
+message MotorCommand {
+ // The unique motor id.
+ uint32 motor_id = 1;
+
+ // The motor angle.
+ float position = 2;
+
+ float position_gain = 3;
+
+ // The motor velocity.
+ float velocity = 4;
+ float velocity_gain = 5;
+
+ // The feed forward torque.
+ float torque = 6;
+}
+
+// LED command for the foot.
+message Led {
+ uint32 leg_id = 1;
+ uint32 r = 2;
+ uint32 g = 3;
+ uint32 b = 4;
+}
+
+// The message type for Laikago's motor command.
+message LaikagoCommand {
+ google.protobuf.Timestamp timestamp = 1;
+ enum ControlMode {
+ CONTROL_MODE_UNSPECIFIED = 0;
+ CONTROL_MODE_POSITION = 1;
+ CONTROL_MODE_TORQUE = 2;
+ CONTROL_MODE_HYBRID = 3;
+ }
+ ControlMode control_mode = 2;
+ repeated MotorCommand motor_command = 3;
+ repeated Led led = 4;
+}
+
+// Empty message just to request a state from the control server.
+message LaikagoStateRequest {}
+
+message Imu {
+ robotics.messages.Vector4f quaternion = 1;
+
+ // The unit is rad/s
+ robotics.messages.Vector3f gyroscope = 2;
+
+ // The unit is m/s^2
+ robotics.messages.Vector3f acceleration = 3;
+
+ // The unit is rad
+ robotics.messages.Vector3f rpy = 4;
+
+ // The IMU temperature.
+ float temperature = 5;
+}
+
+message MotorState {
+ uint32 motor_id = 1;
+ uint32 mode = 2;
+
+ float position = 3;
+ // Position/Velocity gains cannot be read from the motor. We just save the
+ // last used value.
+ float position_gain = 4;
+ float velocity = 5;
+ float velocity_gain = 6;
+ float torque = 7;
+ float temperature = 8;
+}
+
+message ContactState {
+ uint32 leg_id = 1;
+
+ // Contact force is measured in one dimension for Laikago.
+ float force = 2;
+
+ // The contact force measurement direction.
+ robotics.messages.Vector3f axis = 3;
+}
+
+// The message type for Laikago's low level state.
+message LaikagoState {
+ google.protobuf.Timestamp timestamp = 1;
+ uint32 control_level = 2;
+ Imu imu = 3;
+ repeated MotorState motor_state = 4;
+ repeated ContactState contact_state = 5;
+ // The microcontroller_time is millis.
+ uint32 microcontroller_time_millis = 6;
+ bytes wireless_remote = 7;
+ uint32 crc = 8;
+}
+
+message LaikagoCommandState {
+ LaikagoCommand command = 1;
+ LaikagoState state = 2;
+}
+
+// The optional gRPC interface for Laikago control.
+service LaikagoControlGrpcInterface {
+ // Sends the low level control command and receives a state.
+ rpc SendCommand(LaikagoCommand) returns (LaikagoState) {}
+
+ // Receives a robot state without sending motor commands.
+ rpc GetState(LaikagoStateRequest) returns (LaikagoState) {}
+}
+
+// Reserved for Laikago's high level command.
+message LaikagoHighLevelCommand {
+ google.protobuf.Timestamp timestamp = 1;
+ uint32 control_level = 2;
+
+ // 1 for standing and 2 for walking.
+ uint32 control_mode = 3;
+
+ // The normalized speed tuple (x, y, \omega_z)
+ robotics.messages.Vector3f walk_speed = 4;
+
+ float body_height = 5;
+ float foot_clearance_height = 6;
+
+ // The target roll, pitch, yaw of the body in the stand mode.
+ robotics.messages.Vector3f rpy = 7;
+}
+
+// Reserved for Laikago's high level status.
+message LaikagoHighLevelState {
+ google.protobuf.Timestamp timestamp = 1;
+ uint32 control_level = 2;
+
+ // 1 for standing and 2 for walking.
+ uint32 control_mode = 3;
+ Imu imu = 4;
+
+ // The normalized speed tuple (x, y, \omega_z)
+ robotics.messages.Vector3f walk_speed = 5;
+
+ // In stand mode.
+ float body_height = 8;
+ float up_down_speed = 9;
+
+ // The com position estimation. Will drift in x-y plane.
+ robotics.messages.Vector3f com_position = 10;
+ repeated robotics.messages.Vector3f foot_position_to_com = 11;
+ repeated robotics.messages.Vector3f foot_velocity_to_com = 12;
+ repeated ContactState contact_state = 13;
+ // The microcontroller_time is millis.
+ uint32 microcontroller_time_millis = 14;
+ // Bytes 4-7: slider_lx (side step speed); Bytes 8-11: slider_rx (twisting
+ // speed); Bytes 12-15: -slider_ry. Bytes 16-19: (slider_r + 1) / 2; Bytes
+ // 20-23: -slider_ly (forward/backward speed). Each float number (4 bytes) are
+ // packed using big endian convention.
+ bytes wireless_remote = 15;
+ uint32 crc = 16;
+}
+
+message LaikagoHighLevelStateRequest {}
+
+message LaikagoHighLevelCommandState {
+ LaikagoHighLevelCommand command = 1;
+ LaikagoHighLevelState state = 2;
+}
+
+// The optional gRPC interface for Laikago control.
+service LaikagoHighLevelControlGrpcInterface {
+ // Sends the high level control command and receives a state.
+ rpc SendCommand(LaikagoHighLevelCommand) returns (LaikagoHighLevelState) {}
+
+ // Requests a state without sending commands.
+ rpc GetState(LaikagoHighLevelStateRequest) returns (LaikagoHighLevelState) {}
+}
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago_interface_pb2.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago_interface_pb2.py
new file mode 100644
index 000000000..7d67f55cc
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago_interface_pb2.py
@@ -0,0 +1,1040 @@
+# -*- coding: utf-8 -*-
+# Generated by the protocol buffer compiler. DO NOT EDIT!
+# source: laikago_interface.proto
+"""Generated protocol buffer code."""
+from google.protobuf import descriptor as _descriptor
+from google.protobuf import message as _message
+from google.protobuf import reflection as _reflection
+from google.protobuf import symbol_database as _symbol_database
+# @@protoc_insertion_point(imports)
+
+_sym_db = _symbol_database.Default()
+
+
+from pybullet_envs.minitaur.robots import timestamp_pb2 as timestamp__pb2
+from pybullet_envs.minitaur.robots import vector_pb2 as vector__pb2
+
+
+
+DESCRIPTOR = _descriptor.FileDescriptor(
+ name='laikago_interface.proto',
+ package='minitaur_fluxworks.control',
+ syntax='proto3',
+ serialized_options=None,
+ create_key=_descriptor._internal_create_key,
+ serialized_pb=b'\n\x17laikago_interface.proto\x12\x1aminitaur_fluxworks.control\x1a\x0ftimestamp.proto\x1a\x0cvector.proto\"\x82\x01\n\x0cMotorCommand\x12\x10\n\x08motor_id\x18\x01 \x01(\r\x12\x10\n\x08position\x18\x02 \x01(\x02\x12\x15\n\rposition_gain\x18\x03 \x01(\x02\x12\x10\n\x08velocity\x18\x04 \x01(\x02\x12\x15\n\rvelocity_gain\x18\x05 \x01(\x02\x12\x0e\n\x06torque\x18\x06 \x01(\x02\"6\n\x03Led\x12\x0e\n\x06leg_id\x18\x01 \x01(\r\x12\t\n\x01r\x18\x02 \x01(\r\x12\t\n\x01g\x18\x03 \x01(\r\x12\t\n\x01\x62\x18\x04 \x01(\r\"\xf6\x02\n\x0eLaikagoCommand\x12-\n\ttimestamp\x18\x01 \x01(\x0b\x32\x1a.google.protobuf.Timestamp\x12L\n\x0c\x63ontrol_mode\x18\x02 \x01(\x0e\x32\x36.minitaur_fluxworks.control.LaikagoCommand.ControlMode\x12?\n\rmotor_command\x18\x03 \x03(\x0b\x32(.minitaur_fluxworks.control.MotorCommand\x12,\n\x03led\x18\x04 \x03(\x0b\x32\x1f.minitaur_fluxworks.control.Led\"x\n\x0b\x43ontrolMode\x12\x1c\n\x18\x43ONTROL_MODE_UNSPECIFIED\x10\x00\x12\x19\n\x15\x43ONTROL_MODE_POSITION\x10\x01\x12\x17\n\x13\x43ONTROL_MODE_TORQUE\x10\x02\x12\x17\n\x13\x43ONTROL_MODE_HYBRID\x10\x03\"\x15\n\x13LaikagoStateRequest\"\xd8\x01\n\x03Imu\x12/\n\nquaternion\x18\x01 \x01(\x0b\x32\x1b.robotics.messages.Vector4f\x12.\n\tgyroscope\x18\x02 \x01(\x0b\x32\x1b.robotics.messages.Vector3f\x12\x31\n\x0c\x61\x63\x63\x65leration\x18\x03 \x01(\x0b\x32\x1b.robotics.messages.Vector3f\x12(\n\x03rpy\x18\x04 \x01(\x0b\x32\x1b.robotics.messages.Vector3f\x12\x13\n\x0btemperature\x18\x05 \x01(\x02\"\xa3\x01\n\nMotorState\x12\x10\n\x08motor_id\x18\x01 \x01(\r\x12\x0c\n\x04mode\x18\x02 \x01(\r\x12\x10\n\x08position\x18\x03 \x01(\x02\x12\x15\n\rposition_gain\x18\x04 \x01(\x02\x12\x10\n\x08velocity\x18\x05 \x01(\x02\x12\x15\n\rvelocity_gain\x18\x06 \x01(\x02\x12\x0e\n\x06torque\x18\x07 \x01(\x02\x12\x13\n\x0btemperature\x18\x08 \x01(\x02\"X\n\x0c\x43ontactState\x12\x0e\n\x06leg_id\x18\x01 \x01(\r\x12\r\n\x05\x66orce\x18\x02 \x01(\x02\x12)\n\x04\x61xis\x18\x03 \x01(\x0b\x32\x1b.robotics.messages.Vector3f\"\xcb\x02\n\x0cLaikagoState\x12-\n\ttimestamp\x18\x01 \x01(\x0b\x32\x1a.google.protobuf.Timestamp\x12\x15\n\rcontrol_level\x18\x02 \x01(\r\x12,\n\x03imu\x18\x03 \x01(\x0b\x32\x1f.minitaur_fluxworks.control.Imu\x12;\n\x0bmotor_state\x18\x04 \x03(\x0b\x32&.minitaur_fluxworks.control.MotorState\x12?\n\rcontact_state\x18\x05 \x03(\x0b\x32(.minitaur_fluxworks.control.ContactState\x12#\n\x1bmicrocontroller_time_millis\x18\x06 \x01(\r\x12\x17\n\x0fwireless_remote\x18\x07 \x01(\x0c\x12\x0b\n\x03\x63rc\x18\x08 \x01(\r\"\x8b\x01\n\x13LaikagoCommandState\x12;\n\x07\x63ommand\x18\x01 \x01(\x0b\x32*.minitaur_fluxworks.control.LaikagoCommand\x12\x37\n\x05state\x18\x02 \x01(\x0b\x32(.minitaur_fluxworks.control.LaikagoState\"\x84\x02\n\x17LaikagoHighLevelCommand\x12-\n\ttimestamp\x18\x01 \x01(\x0b\x32\x1a.google.protobuf.Timestamp\x12\x15\n\rcontrol_level\x18\x02 \x01(\r\x12\x14\n\x0c\x63ontrol_mode\x18\x03 \x01(\r\x12/\n\nwalk_speed\x18\x04 \x01(\x0b\x32\x1b.robotics.messages.Vector3f\x12\x13\n\x0b\x62ody_height\x18\x05 \x01(\x02\x12\x1d\n\x15\x66oot_clearance_height\x18\x06 \x01(\x02\x12(\n\x03rpy\x18\x07 \x01(\x0b\x32\x1b.robotics.messages.Vector3f\"\xb3\x04\n\x15LaikagoHighLevelState\x12-\n\ttimestamp\x18\x01 \x01(\x0b\x32\x1a.google.protobuf.Timestamp\x12\x15\n\rcontrol_level\x18\x02 \x01(\r\x12\x14\n\x0c\x63ontrol_mode\x18\x03 \x01(\r\x12,\n\x03imu\x18\x04 \x01(\x0b\x32\x1f.minitaur_fluxworks.control.Imu\x12/\n\nwalk_speed\x18\x05 \x01(\x0b\x32\x1b.robotics.messages.Vector3f\x12\x13\n\x0b\x62ody_height\x18\x08 \x01(\x02\x12\x15\n\rup_down_speed\x18\t \x01(\x02\x12\x31\n\x0c\x63om_position\x18\n \x01(\x0b\x32\x1b.robotics.messages.Vector3f\x12\x39\n\x14\x66oot_position_to_com\x18\x0b \x03(\x0b\x32\x1b.robotics.messages.Vector3f\x12\x39\n\x14\x66oot_velocity_to_com\x18\x0c \x03(\x0b\x32\x1b.robotics.messages.Vector3f\x12?\n\rcontact_state\x18\r \x03(\x0b\x32(.minitaur_fluxworks.control.ContactState\x12#\n\x1bmicrocontroller_time_millis\x18\x0e \x01(\r\x12\x17\n\x0fwireless_remote\x18\x0f \x01(\x0c\x12\x0b\n\x03\x63rc\x18\x10 \x01(\r\"\x1e\n\x1cLaikagoHighLevelStateRequest\"\xa6\x01\n\x1cLaikagoHighLevelCommandState\x12\x44\n\x07\x63ommand\x18\x01 \x01(\x0b\x32\x33.minitaur_fluxworks.control.LaikagoHighLevelCommand\x12@\n\x05state\x18\x02 \x01(\x0b\x32\x31.minitaur_fluxworks.control.LaikagoHighLevelState2\xed\x01\n\x1bLaikagoControlGrpcInterface\x12\x65\n\x0bSendCommand\x12*.minitaur_fluxworks.control.LaikagoCommand\x1a(.minitaur_fluxworks.control.LaikagoState\"\x00\x12g\n\x08GetState\x12/.minitaur_fluxworks.control.LaikagoStateRequest\x1a(.minitaur_fluxworks.control.LaikagoState\"\x00\x32\x9a\x02\n$LaikagoHighLevelControlGrpcInterface\x12w\n\x0bSendCommand\x12\x33.minitaur_fluxworks.control.LaikagoHighLevelCommand\x1a\x31.minitaur_fluxworks.control.LaikagoHighLevelState\"\x00\x12y\n\x08GetState\x12\x38.minitaur_fluxworks.control.LaikagoHighLevelStateRequest\x1a\x31.minitaur_fluxworks.control.LaikagoHighLevelState\"\x00\x62\x06proto3'
+ ,
+ dependencies=[timestamp__pb2.DESCRIPTOR,vector__pb2.DESCRIPTOR,])
+
+
+
+_LAIKAGOCOMMAND_CONTROLMODE = _descriptor.EnumDescriptor(
+ name='ControlMode',
+ full_name='minitaur_fluxworks.control.LaikagoCommand.ControlMode',
+ filename=None,
+ file=DESCRIPTOR,
+ create_key=_descriptor._internal_create_key,
+ values=[
+ _descriptor.EnumValueDescriptor(
+ name='CONTROL_MODE_UNSPECIFIED', index=0, number=0,
+ serialized_options=None,
+ type=None,
+ create_key=_descriptor._internal_create_key),
+ _descriptor.EnumValueDescriptor(
+ name='CONTROL_MODE_POSITION', index=1, number=1,
+ serialized_options=None,
+ type=None,
+ create_key=_descriptor._internal_create_key),
+ _descriptor.EnumValueDescriptor(
+ name='CONTROL_MODE_TORQUE', index=2, number=2,
+ serialized_options=None,
+ type=None,
+ create_key=_descriptor._internal_create_key),
+ _descriptor.EnumValueDescriptor(
+ name='CONTROL_MODE_HYBRID', index=3, number=3,
+ serialized_options=None,
+ type=None,
+ create_key=_descriptor._internal_create_key),
+ ],
+ containing_type=None,
+ serialized_options=None,
+ serialized_start=530,
+ serialized_end=650,
+)
+_sym_db.RegisterEnumDescriptor(_LAIKAGOCOMMAND_CONTROLMODE)
+
+
+_MOTORCOMMAND = _descriptor.Descriptor(
+ name='MotorCommand',
+ full_name='minitaur_fluxworks.control.MotorCommand',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='motor_id', full_name='minitaur_fluxworks.control.MotorCommand.motor_id', index=0,
+ number=1, type=13, cpp_type=3, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='position', full_name='minitaur_fluxworks.control.MotorCommand.position', index=1,
+ number=2, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='position_gain', full_name='minitaur_fluxworks.control.MotorCommand.position_gain', index=2,
+ number=3, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='velocity', full_name='minitaur_fluxworks.control.MotorCommand.velocity', index=3,
+ number=4, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='velocity_gain', full_name='minitaur_fluxworks.control.MotorCommand.velocity_gain', index=4,
+ number=5, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='torque', full_name='minitaur_fluxworks.control.MotorCommand.torque', index=5,
+ number=6, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=87,
+ serialized_end=217,
+)
+
+
+_LED = _descriptor.Descriptor(
+ name='Led',
+ full_name='minitaur_fluxworks.control.Led',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='leg_id', full_name='minitaur_fluxworks.control.Led.leg_id', index=0,
+ number=1, type=13, cpp_type=3, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='r', full_name='minitaur_fluxworks.control.Led.r', index=1,
+ number=2, type=13, cpp_type=3, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='g', full_name='minitaur_fluxworks.control.Led.g', index=2,
+ number=3, type=13, cpp_type=3, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='b', full_name='minitaur_fluxworks.control.Led.b', index=3,
+ number=4, type=13, cpp_type=3, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=219,
+ serialized_end=273,
+)
+
+
+_LAIKAGOCOMMAND = _descriptor.Descriptor(
+ name='LaikagoCommand',
+ full_name='minitaur_fluxworks.control.LaikagoCommand',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='timestamp', full_name='minitaur_fluxworks.control.LaikagoCommand.timestamp', index=0,
+ number=1, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='control_mode', full_name='minitaur_fluxworks.control.LaikagoCommand.control_mode', index=1,
+ number=2, type=14, cpp_type=8, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='motor_command', full_name='minitaur_fluxworks.control.LaikagoCommand.motor_command', index=2,
+ number=3, type=11, cpp_type=10, label=3,
+ has_default_value=False, default_value=[],
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='led', full_name='minitaur_fluxworks.control.LaikagoCommand.led', index=3,
+ number=4, type=11, cpp_type=10, label=3,
+ has_default_value=False, default_value=[],
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ _LAIKAGOCOMMAND_CONTROLMODE,
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=276,
+ serialized_end=650,
+)
+
+
+_LAIKAGOSTATEREQUEST = _descriptor.Descriptor(
+ name='LaikagoStateRequest',
+ full_name='minitaur_fluxworks.control.LaikagoStateRequest',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=652,
+ serialized_end=673,
+)
+
+
+_IMU = _descriptor.Descriptor(
+ name='Imu',
+ full_name='minitaur_fluxworks.control.Imu',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='quaternion', full_name='minitaur_fluxworks.control.Imu.quaternion', index=0,
+ number=1, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='gyroscope', full_name='minitaur_fluxworks.control.Imu.gyroscope', index=1,
+ number=2, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='acceleration', full_name='minitaur_fluxworks.control.Imu.acceleration', index=2,
+ number=3, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='rpy', full_name='minitaur_fluxworks.control.Imu.rpy', index=3,
+ number=4, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='temperature', full_name='minitaur_fluxworks.control.Imu.temperature', index=4,
+ number=5, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=676,
+ serialized_end=892,
+)
+
+
+_MOTORSTATE = _descriptor.Descriptor(
+ name='MotorState',
+ full_name='minitaur_fluxworks.control.MotorState',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='motor_id', full_name='minitaur_fluxworks.control.MotorState.motor_id', index=0,
+ number=1, type=13, cpp_type=3, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='mode', full_name='minitaur_fluxworks.control.MotorState.mode', index=1,
+ number=2, type=13, cpp_type=3, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='position', full_name='minitaur_fluxworks.control.MotorState.position', index=2,
+ number=3, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='position_gain', full_name='minitaur_fluxworks.control.MotorState.position_gain', index=3,
+ number=4, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='velocity', full_name='minitaur_fluxworks.control.MotorState.velocity', index=4,
+ number=5, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='velocity_gain', full_name='minitaur_fluxworks.control.MotorState.velocity_gain', index=5,
+ number=6, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='torque', full_name='minitaur_fluxworks.control.MotorState.torque', index=6,
+ number=7, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='temperature', full_name='minitaur_fluxworks.control.MotorState.temperature', index=7,
+ number=8, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=895,
+ serialized_end=1058,
+)
+
+
+_CONTACTSTATE = _descriptor.Descriptor(
+ name='ContactState',
+ full_name='minitaur_fluxworks.control.ContactState',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='leg_id', full_name='minitaur_fluxworks.control.ContactState.leg_id', index=0,
+ number=1, type=13, cpp_type=3, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='force', full_name='minitaur_fluxworks.control.ContactState.force', index=1,
+ number=2, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='axis', full_name='minitaur_fluxworks.control.ContactState.axis', index=2,
+ number=3, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=1060,
+ serialized_end=1148,
+)
+
+
+_LAIKAGOSTATE = _descriptor.Descriptor(
+ name='LaikagoState',
+ full_name='minitaur_fluxworks.control.LaikagoState',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='timestamp', full_name='minitaur_fluxworks.control.LaikagoState.timestamp', index=0,
+ number=1, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='control_level', full_name='minitaur_fluxworks.control.LaikagoState.control_level', index=1,
+ number=2, type=13, cpp_type=3, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='imu', full_name='minitaur_fluxworks.control.LaikagoState.imu', index=2,
+ number=3, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='motor_state', full_name='minitaur_fluxworks.control.LaikagoState.motor_state', index=3,
+ number=4, type=11, cpp_type=10, label=3,
+ has_default_value=False, default_value=[],
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='contact_state', full_name='minitaur_fluxworks.control.LaikagoState.contact_state', index=4,
+ number=5, type=11, cpp_type=10, label=3,
+ has_default_value=False, default_value=[],
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='microcontroller_time_millis', full_name='minitaur_fluxworks.control.LaikagoState.microcontroller_time_millis', index=5,
+ number=6, type=13, cpp_type=3, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='wireless_remote', full_name='minitaur_fluxworks.control.LaikagoState.wireless_remote', index=6,
+ number=7, type=12, cpp_type=9, label=1,
+ has_default_value=False, default_value=b"",
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='crc', full_name='minitaur_fluxworks.control.LaikagoState.crc', index=7,
+ number=8, type=13, cpp_type=3, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=1151,
+ serialized_end=1482,
+)
+
+
+_LAIKAGOCOMMANDSTATE = _descriptor.Descriptor(
+ name='LaikagoCommandState',
+ full_name='minitaur_fluxworks.control.LaikagoCommandState',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='command', full_name='minitaur_fluxworks.control.LaikagoCommandState.command', index=0,
+ number=1, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='state', full_name='minitaur_fluxworks.control.LaikagoCommandState.state', index=1,
+ number=2, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=1485,
+ serialized_end=1624,
+)
+
+
+_LAIKAGOHIGHLEVELCOMMAND = _descriptor.Descriptor(
+ name='LaikagoHighLevelCommand',
+ full_name='minitaur_fluxworks.control.LaikagoHighLevelCommand',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='timestamp', full_name='minitaur_fluxworks.control.LaikagoHighLevelCommand.timestamp', index=0,
+ number=1, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='control_level', full_name='minitaur_fluxworks.control.LaikagoHighLevelCommand.control_level', index=1,
+ number=2, type=13, cpp_type=3, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='control_mode', full_name='minitaur_fluxworks.control.LaikagoHighLevelCommand.control_mode', index=2,
+ number=3, type=13, cpp_type=3, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='walk_speed', full_name='minitaur_fluxworks.control.LaikagoHighLevelCommand.walk_speed', index=3,
+ number=4, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='body_height', full_name='minitaur_fluxworks.control.LaikagoHighLevelCommand.body_height', index=4,
+ number=5, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='foot_clearance_height', full_name='minitaur_fluxworks.control.LaikagoHighLevelCommand.foot_clearance_height', index=5,
+ number=6, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='rpy', full_name='minitaur_fluxworks.control.LaikagoHighLevelCommand.rpy', index=6,
+ number=7, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=1627,
+ serialized_end=1887,
+)
+
+
+_LAIKAGOHIGHLEVELSTATE = _descriptor.Descriptor(
+ name='LaikagoHighLevelState',
+ full_name='minitaur_fluxworks.control.LaikagoHighLevelState',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='timestamp', full_name='minitaur_fluxworks.control.LaikagoHighLevelState.timestamp', index=0,
+ number=1, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='control_level', full_name='minitaur_fluxworks.control.LaikagoHighLevelState.control_level', index=1,
+ number=2, type=13, cpp_type=3, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='control_mode', full_name='minitaur_fluxworks.control.LaikagoHighLevelState.control_mode', index=2,
+ number=3, type=13, cpp_type=3, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='imu', full_name='minitaur_fluxworks.control.LaikagoHighLevelState.imu', index=3,
+ number=4, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='walk_speed', full_name='minitaur_fluxworks.control.LaikagoHighLevelState.walk_speed', index=4,
+ number=5, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='body_height', full_name='minitaur_fluxworks.control.LaikagoHighLevelState.body_height', index=5,
+ number=8, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='up_down_speed', full_name='minitaur_fluxworks.control.LaikagoHighLevelState.up_down_speed', index=6,
+ number=9, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='com_position', full_name='minitaur_fluxworks.control.LaikagoHighLevelState.com_position', index=7,
+ number=10, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='foot_position_to_com', full_name='minitaur_fluxworks.control.LaikagoHighLevelState.foot_position_to_com', index=8,
+ number=11, type=11, cpp_type=10, label=3,
+ has_default_value=False, default_value=[],
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='foot_velocity_to_com', full_name='minitaur_fluxworks.control.LaikagoHighLevelState.foot_velocity_to_com', index=9,
+ number=12, type=11, cpp_type=10, label=3,
+ has_default_value=False, default_value=[],
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='contact_state', full_name='minitaur_fluxworks.control.LaikagoHighLevelState.contact_state', index=10,
+ number=13, type=11, cpp_type=10, label=3,
+ has_default_value=False, default_value=[],
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='microcontroller_time_millis', full_name='minitaur_fluxworks.control.LaikagoHighLevelState.microcontroller_time_millis', index=11,
+ number=14, type=13, cpp_type=3, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='wireless_remote', full_name='minitaur_fluxworks.control.LaikagoHighLevelState.wireless_remote', index=12,
+ number=15, type=12, cpp_type=9, label=1,
+ has_default_value=False, default_value=b"",
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='crc', full_name='minitaur_fluxworks.control.LaikagoHighLevelState.crc', index=13,
+ number=16, type=13, cpp_type=3, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=1890,
+ serialized_end=2453,
+)
+
+
+_LAIKAGOHIGHLEVELSTATEREQUEST = _descriptor.Descriptor(
+ name='LaikagoHighLevelStateRequest',
+ full_name='minitaur_fluxworks.control.LaikagoHighLevelStateRequest',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=2455,
+ serialized_end=2485,
+)
+
+
+_LAIKAGOHIGHLEVELCOMMANDSTATE = _descriptor.Descriptor(
+ name='LaikagoHighLevelCommandState',
+ full_name='minitaur_fluxworks.control.LaikagoHighLevelCommandState',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='command', full_name='minitaur_fluxworks.control.LaikagoHighLevelCommandState.command', index=0,
+ number=1, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='state', full_name='minitaur_fluxworks.control.LaikagoHighLevelCommandState.state', index=1,
+ number=2, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=2488,
+ serialized_end=2654,
+)
+
+_LAIKAGOCOMMAND.fields_by_name['timestamp'].message_type = timestamp__pb2._TIMESTAMP
+_LAIKAGOCOMMAND.fields_by_name['control_mode'].enum_type = _LAIKAGOCOMMAND_CONTROLMODE
+_LAIKAGOCOMMAND.fields_by_name['motor_command'].message_type = _MOTORCOMMAND
+_LAIKAGOCOMMAND.fields_by_name['led'].message_type = _LED
+_LAIKAGOCOMMAND_CONTROLMODE.containing_type = _LAIKAGOCOMMAND
+_IMU.fields_by_name['quaternion'].message_type = vector__pb2._VECTOR4F
+_IMU.fields_by_name['gyroscope'].message_type = vector__pb2._VECTOR3F
+_IMU.fields_by_name['acceleration'].message_type = vector__pb2._VECTOR3F
+_IMU.fields_by_name['rpy'].message_type = vector__pb2._VECTOR3F
+_CONTACTSTATE.fields_by_name['axis'].message_type = vector__pb2._VECTOR3F
+_LAIKAGOSTATE.fields_by_name['timestamp'].message_type = timestamp__pb2._TIMESTAMP
+_LAIKAGOSTATE.fields_by_name['imu'].message_type = _IMU
+_LAIKAGOSTATE.fields_by_name['motor_state'].message_type = _MOTORSTATE
+_LAIKAGOSTATE.fields_by_name['contact_state'].message_type = _CONTACTSTATE
+_LAIKAGOCOMMANDSTATE.fields_by_name['command'].message_type = _LAIKAGOCOMMAND
+_LAIKAGOCOMMANDSTATE.fields_by_name['state'].message_type = _LAIKAGOSTATE
+_LAIKAGOHIGHLEVELCOMMAND.fields_by_name['timestamp'].message_type = timestamp__pb2._TIMESTAMP
+_LAIKAGOHIGHLEVELCOMMAND.fields_by_name['walk_speed'].message_type = vector__pb2._VECTOR3F
+_LAIKAGOHIGHLEVELCOMMAND.fields_by_name['rpy'].message_type = vector__pb2._VECTOR3F
+_LAIKAGOHIGHLEVELSTATE.fields_by_name['timestamp'].message_type = timestamp__pb2._TIMESTAMP
+_LAIKAGOHIGHLEVELSTATE.fields_by_name['imu'].message_type = _IMU
+_LAIKAGOHIGHLEVELSTATE.fields_by_name['walk_speed'].message_type = vector__pb2._VECTOR3F
+_LAIKAGOHIGHLEVELSTATE.fields_by_name['com_position'].message_type = vector__pb2._VECTOR3F
+_LAIKAGOHIGHLEVELSTATE.fields_by_name['foot_position_to_com'].message_type = vector__pb2._VECTOR3F
+_LAIKAGOHIGHLEVELSTATE.fields_by_name['foot_velocity_to_com'].message_type = vector__pb2._VECTOR3F
+_LAIKAGOHIGHLEVELSTATE.fields_by_name['contact_state'].message_type = _CONTACTSTATE
+_LAIKAGOHIGHLEVELCOMMANDSTATE.fields_by_name['command'].message_type = _LAIKAGOHIGHLEVELCOMMAND
+_LAIKAGOHIGHLEVELCOMMANDSTATE.fields_by_name['state'].message_type = _LAIKAGOHIGHLEVELSTATE
+DESCRIPTOR.message_types_by_name['MotorCommand'] = _MOTORCOMMAND
+DESCRIPTOR.message_types_by_name['Led'] = _LED
+DESCRIPTOR.message_types_by_name['LaikagoCommand'] = _LAIKAGOCOMMAND
+DESCRIPTOR.message_types_by_name['LaikagoStateRequest'] = _LAIKAGOSTATEREQUEST
+DESCRIPTOR.message_types_by_name['Imu'] = _IMU
+DESCRIPTOR.message_types_by_name['MotorState'] = _MOTORSTATE
+DESCRIPTOR.message_types_by_name['ContactState'] = _CONTACTSTATE
+DESCRIPTOR.message_types_by_name['LaikagoState'] = _LAIKAGOSTATE
+DESCRIPTOR.message_types_by_name['LaikagoCommandState'] = _LAIKAGOCOMMANDSTATE
+DESCRIPTOR.message_types_by_name['LaikagoHighLevelCommand'] = _LAIKAGOHIGHLEVELCOMMAND
+DESCRIPTOR.message_types_by_name['LaikagoHighLevelState'] = _LAIKAGOHIGHLEVELSTATE
+DESCRIPTOR.message_types_by_name['LaikagoHighLevelStateRequest'] = _LAIKAGOHIGHLEVELSTATEREQUEST
+DESCRIPTOR.message_types_by_name['LaikagoHighLevelCommandState'] = _LAIKAGOHIGHLEVELCOMMANDSTATE
+_sym_db.RegisterFileDescriptor(DESCRIPTOR)
+
+MotorCommand = _reflection.GeneratedProtocolMessageType('MotorCommand', (_message.Message,), {
+ 'DESCRIPTOR' : _MOTORCOMMAND,
+ '__module__' : 'laikago_interface_pb2'
+ # @@protoc_insertion_point(class_scope:minitaur_fluxworks.control.MotorCommand)
+ })
+_sym_db.RegisterMessage(MotorCommand)
+
+Led = _reflection.GeneratedProtocolMessageType('Led', (_message.Message,), {
+ 'DESCRIPTOR' : _LED,
+ '__module__' : 'laikago_interface_pb2'
+ # @@protoc_insertion_point(class_scope:minitaur_fluxworks.control.Led)
+ })
+_sym_db.RegisterMessage(Led)
+
+LaikagoCommand = _reflection.GeneratedProtocolMessageType('LaikagoCommand', (_message.Message,), {
+ 'DESCRIPTOR' : _LAIKAGOCOMMAND,
+ '__module__' : 'laikago_interface_pb2'
+ # @@protoc_insertion_point(class_scope:minitaur_fluxworks.control.LaikagoCommand)
+ })
+_sym_db.RegisterMessage(LaikagoCommand)
+
+LaikagoStateRequest = _reflection.GeneratedProtocolMessageType('LaikagoStateRequest', (_message.Message,), {
+ 'DESCRIPTOR' : _LAIKAGOSTATEREQUEST,
+ '__module__' : 'laikago_interface_pb2'
+ # @@protoc_insertion_point(class_scope:minitaur_fluxworks.control.LaikagoStateRequest)
+ })
+_sym_db.RegisterMessage(LaikagoStateRequest)
+
+Imu = _reflection.GeneratedProtocolMessageType('Imu', (_message.Message,), {
+ 'DESCRIPTOR' : _IMU,
+ '__module__' : 'laikago_interface_pb2'
+ # @@protoc_insertion_point(class_scope:minitaur_fluxworks.control.Imu)
+ })
+_sym_db.RegisterMessage(Imu)
+
+MotorState = _reflection.GeneratedProtocolMessageType('MotorState', (_message.Message,), {
+ 'DESCRIPTOR' : _MOTORSTATE,
+ '__module__' : 'laikago_interface_pb2'
+ # @@protoc_insertion_point(class_scope:minitaur_fluxworks.control.MotorState)
+ })
+_sym_db.RegisterMessage(MotorState)
+
+ContactState = _reflection.GeneratedProtocolMessageType('ContactState', (_message.Message,), {
+ 'DESCRIPTOR' : _CONTACTSTATE,
+ '__module__' : 'laikago_interface_pb2'
+ # @@protoc_insertion_point(class_scope:minitaur_fluxworks.control.ContactState)
+ })
+_sym_db.RegisterMessage(ContactState)
+
+LaikagoState = _reflection.GeneratedProtocolMessageType('LaikagoState', (_message.Message,), {
+ 'DESCRIPTOR' : _LAIKAGOSTATE,
+ '__module__' : 'laikago_interface_pb2'
+ # @@protoc_insertion_point(class_scope:minitaur_fluxworks.control.LaikagoState)
+ })
+_sym_db.RegisterMessage(LaikagoState)
+
+LaikagoCommandState = _reflection.GeneratedProtocolMessageType('LaikagoCommandState', (_message.Message,), {
+ 'DESCRIPTOR' : _LAIKAGOCOMMANDSTATE,
+ '__module__' : 'laikago_interface_pb2'
+ # @@protoc_insertion_point(class_scope:minitaur_fluxworks.control.LaikagoCommandState)
+ })
+_sym_db.RegisterMessage(LaikagoCommandState)
+
+LaikagoHighLevelCommand = _reflection.GeneratedProtocolMessageType('LaikagoHighLevelCommand', (_message.Message,), {
+ 'DESCRIPTOR' : _LAIKAGOHIGHLEVELCOMMAND,
+ '__module__' : 'laikago_interface_pb2'
+ # @@protoc_insertion_point(class_scope:minitaur_fluxworks.control.LaikagoHighLevelCommand)
+ })
+_sym_db.RegisterMessage(LaikagoHighLevelCommand)
+
+LaikagoHighLevelState = _reflection.GeneratedProtocolMessageType('LaikagoHighLevelState', (_message.Message,), {
+ 'DESCRIPTOR' : _LAIKAGOHIGHLEVELSTATE,
+ '__module__' : 'laikago_interface_pb2'
+ # @@protoc_insertion_point(class_scope:minitaur_fluxworks.control.LaikagoHighLevelState)
+ })
+_sym_db.RegisterMessage(LaikagoHighLevelState)
+
+LaikagoHighLevelStateRequest = _reflection.GeneratedProtocolMessageType('LaikagoHighLevelStateRequest', (_message.Message,), {
+ 'DESCRIPTOR' : _LAIKAGOHIGHLEVELSTATEREQUEST,
+ '__module__' : 'laikago_interface_pb2'
+ # @@protoc_insertion_point(class_scope:minitaur_fluxworks.control.LaikagoHighLevelStateRequest)
+ })
+_sym_db.RegisterMessage(LaikagoHighLevelStateRequest)
+
+LaikagoHighLevelCommandState = _reflection.GeneratedProtocolMessageType('LaikagoHighLevelCommandState', (_message.Message,), {
+ 'DESCRIPTOR' : _LAIKAGOHIGHLEVELCOMMANDSTATE,
+ '__module__' : 'laikago_interface_pb2'
+ # @@protoc_insertion_point(class_scope:minitaur_fluxworks.control.LaikagoHighLevelCommandState)
+ })
+_sym_db.RegisterMessage(LaikagoHighLevelCommandState)
+
+
+
+_LAIKAGOCONTROLGRPCINTERFACE = _descriptor.ServiceDescriptor(
+ name='LaikagoControlGrpcInterface',
+ full_name='minitaur_fluxworks.control.LaikagoControlGrpcInterface',
+ file=DESCRIPTOR,
+ index=0,
+ serialized_options=None,
+ create_key=_descriptor._internal_create_key,
+ serialized_start=2657,
+ serialized_end=2894,
+ methods=[
+ _descriptor.MethodDescriptor(
+ name='SendCommand',
+ full_name='minitaur_fluxworks.control.LaikagoControlGrpcInterface.SendCommand',
+ index=0,
+ containing_service=None,
+ input_type=_LAIKAGOCOMMAND,
+ output_type=_LAIKAGOSTATE,
+ serialized_options=None,
+ create_key=_descriptor._internal_create_key,
+ ),
+ _descriptor.MethodDescriptor(
+ name='GetState',
+ full_name='minitaur_fluxworks.control.LaikagoControlGrpcInterface.GetState',
+ index=1,
+ containing_service=None,
+ input_type=_LAIKAGOSTATEREQUEST,
+ output_type=_LAIKAGOSTATE,
+ serialized_options=None,
+ create_key=_descriptor._internal_create_key,
+ ),
+])
+_sym_db.RegisterServiceDescriptor(_LAIKAGOCONTROLGRPCINTERFACE)
+
+DESCRIPTOR.services_by_name['LaikagoControlGrpcInterface'] = _LAIKAGOCONTROLGRPCINTERFACE
+
+
+_LAIKAGOHIGHLEVELCONTROLGRPCINTERFACE = _descriptor.ServiceDescriptor(
+ name='LaikagoHighLevelControlGrpcInterface',
+ full_name='minitaur_fluxworks.control.LaikagoHighLevelControlGrpcInterface',
+ file=DESCRIPTOR,
+ index=1,
+ serialized_options=None,
+ create_key=_descriptor._internal_create_key,
+ serialized_start=2897,
+ serialized_end=3179,
+ methods=[
+ _descriptor.MethodDescriptor(
+ name='SendCommand',
+ full_name='minitaur_fluxworks.control.LaikagoHighLevelControlGrpcInterface.SendCommand',
+ index=0,
+ containing_service=None,
+ input_type=_LAIKAGOHIGHLEVELCOMMAND,
+ output_type=_LAIKAGOHIGHLEVELSTATE,
+ serialized_options=None,
+ create_key=_descriptor._internal_create_key,
+ ),
+ _descriptor.MethodDescriptor(
+ name='GetState',
+ full_name='minitaur_fluxworks.control.LaikagoHighLevelControlGrpcInterface.GetState',
+ index=1,
+ containing_service=None,
+ input_type=_LAIKAGOHIGHLEVELSTATEREQUEST,
+ output_type=_LAIKAGOHIGHLEVELSTATE,
+ serialized_options=None,
+ create_key=_descriptor._internal_create_key,
+ ),
+])
+_sym_db.RegisterServiceDescriptor(_LAIKAGOHIGHLEVELCONTROLGRPCINTERFACE)
+
+DESCRIPTOR.services_by_name['LaikagoHighLevelControlGrpcInterface'] = _LAIKAGOHIGHLEVELCONTROLGRPCINTERFACE
+
+# @@protoc_insertion_point(module_scope)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago_kinematic_constants.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago_kinematic_constants.py
new file mode 100644
index 000000000..eae1662da
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago_kinematic_constants.py
@@ -0,0 +1,106 @@
+# Lint as: python3
+"""Defines the LaikagoKinematic robot related constants and URDF specs."""
+
+import collections
+import gin
+
+LAIKAGO_KINEMATIC_URDF_PATH = "robotics/reinforcement_learning/minitaur/robots/data/urdf/laikago/laikago_toes_zup_kinematic.urdf"
+INIT_POSITION = (0, 0, 0.5)
+INIT_ORIENTATION_QUAT = (0, 0, 0, 1)
+INIT_ORIENTATION_RPY = (0, 0, 0)
+
+NUM_LEGS = 4
+# TODO(b/153405332): Use link name instead of joint name to identify the
+# base link.
+BASE_NAMES = ("kinematic_drive_joint_th",)
+
+JOINT_NAMES = (
+ "kinematic_drive_joint_x",
+ "kinematic_drive_joint_y",
+ "kinematic_drive_joint_th",
+ # front right leg
+ "FR_hip_motor_2_chassis_joint",
+ "FR_upper_leg_2_hip_motor_joint",
+ "FR_lower_leg_2_upper_leg_joint",
+ # front left leg
+ "FL_hip_motor_2_chassis_joint",
+ "FL_upper_leg_2_hip_motor_joint",
+ "FL_lower_leg_2_upper_leg_joint",
+ # rear right leg
+ "RR_hip_motor_2_chassis_joint",
+ "RR_upper_leg_2_hip_motor_joint",
+ "RR_lower_leg_2_upper_leg_joint",
+ # rear left leg
+ "RL_hip_motor_2_chassis_joint",
+ "RL_upper_leg_2_hip_motor_joint",
+ "RL_lower_leg_2_upper_leg_joint",
+)
+
+# A default joint pose where the arm is tucked near the base, and head looking
+# forward.
+INIT_ABDUCTION_ANGLE = 0
+INIT_HIP_ANGLE = 0.67
+INIT_KNEE_ANGLE = -1.25
+
+# Note this matches the Laikago SDK/control convention, but is different from
+# URDF's internal joint angles which needs to be computed using the joint
+# offsets and directions. The conversion formula is (sdk_joint_angle + offset) *
+# joint direction.
+INIT_JOINT_ANGLES = collections.OrderedDict(
+ zip(JOINT_NAMES, (0, 0, 0) +
+ (INIT_ABDUCTION_ANGLE, INIT_HIP_ANGLE, INIT_KNEE_ANGLE) * NUM_LEGS))
+
+# Used to convert the robot SDK joint angles to URDF joint angles.
+JOINT_DIRECTIONS = collections.OrderedDict(
+ zip(JOINT_NAMES, (1, 1, 1, -1, 1, 1, 1, 1, 1, -1, 1, 1, 1, 1, 1)))
+
+HIP_JOINT_OFFSET = 0.0
+UPPER_LEG_JOINT_OFFSET = -0.6
+KNEE_JOINT_OFFSET = 0.66
+
+# Used to convert the robot SDK joint angles to URDF joint angles.
+JOINT_OFFSETS = collections.OrderedDict(
+ zip(JOINT_NAMES, [0, 0, 0] +
+ [HIP_JOINT_OFFSET, UPPER_LEG_JOINT_OFFSET, KNEE_JOINT_OFFSET] *
+ NUM_LEGS))
+
+LEG_NAMES = (
+ "front_right",
+ "front_left",
+ "rear_right",
+ "rear_left",
+)
+
+LEG_ORDER = (
+ "front_right",
+ "front_left",
+ "back_right",
+ "back_left",
+)
+
+END_EFFECTOR_NAMES = (
+ "jtoeFR",
+ "jtoeFL",
+ "jtoeRR",
+ "jtoeRL",
+)
+
+MOTOR_NAMES = JOINT_NAMES
+MOTOR_GROUP = collections.OrderedDict((("body_motors", JOINT_NAMES[3:]),))
+
+# Add the gin constants to be used for gin binding in config.
+gin.constant("laikago_kinematic_constants.LAIKAGO_KINEMATIC_URDF_PATH",
+ LAIKAGO_KINEMATIC_URDF_PATH)
+gin.constant("laikago_kinematic_constants.INIT_POSITION", INIT_POSITION)
+gin.constant("laikago_kinematic_constants.INIT_ORIENTATION_QUAT",
+ INIT_ORIENTATION_QUAT)
+gin.constant("laikago_kinematic_constants.INIT_ORIENTATION_RPY",
+ INIT_ORIENTATION_RPY)
+gin.constant("laikago_kinematic_constants.BASE_NAMES", BASE_NAMES)
+gin.constant("laikago_kinematic_constants.INIT_JOINT_ANGLES", INIT_JOINT_ANGLES)
+gin.constant("laikago_kinematic_constants.JOINT_DIRECTIONS", JOINT_DIRECTIONS)
+gin.constant("laikago_kinematic_constants.JOINT_OFFSETS", JOINT_OFFSETS)
+gin.constant("laikago_kinematic_constants.MOTOR_NAMES", MOTOR_NAMES)
+gin.constant("laikago_kinematic_constants.END_EFFECTOR_NAMES",
+ END_EFFECTOR_NAMES)
+gin.constant("laikago_kinematic_constants.MOTOR_GROUP", MOTOR_GROUP)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago_motor.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago_motor.py
new file mode 100644
index 000000000..a57c0a964
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago_motor.py
@@ -0,0 +1,149 @@
+"""Motor model for laikago."""
+
+import collections
+import numpy as np
+
+from pybullet_envs.minitaur.robots import robot_config
+
+NUM_MOTORS = 12
+
+MOTOR_COMMAND_DIMENSION = 5
+
+# These values represent the indices of each field in the motor command tuple
+POSITION_INDEX = 0
+POSITION_GAIN_INDEX = 1
+VELOCITY_INDEX = 2
+VELOCITY_GAIN_INDEX = 3
+TORQUE_INDEX = 4
+
+
+class LaikagoMotorModel(object):
+ """A simple motor model for Laikago.
+
+ When in POSITION mode, the torque is calculated according to the difference
+ between current and desired joint angle, as well as the joint velocity.
+ For more information about PD control, please refer to:
+ https://en.wikipedia.org/wiki/PID_controller.
+
+ The model supports a HYBRID mode in which each motor command can be a tuple
+ (desired_motor_angle, position_gain, desired_motor_velocity, velocity_gain,
+ torque).
+
+ """
+
+ def __init__(self,
+ kp=60,
+ kd=1,
+ torque_limits=None,
+ motor_control_mode=robot_config.MotorControlMode.POSITION):
+ self._kp = kp
+ self._kd = kd
+ self._torque_limits = torque_limits
+ if torque_limits is not None:
+ if isinstance(torque_limits, (collections.Sequence, np.ndarray)):
+ self._torque_limits = np.asarray(torque_limits)
+ else:
+ self._torque_limits = np.full(NUM_MOTORS, torque_limits)
+ self._motor_control_mode = motor_control_mode
+ self._strength_ratios = np.full(NUM_MOTORS, 1)
+
+ def set_strength_ratios(self, ratios):
+ """Set the strength of each motors relative to the default value.
+
+ Args:
+ ratios: The relative strength of motor output. A numpy array ranging from
+ 0.0 to 1.0.
+ """
+ self._strength_ratios = ratios
+
+ def set_motor_gains(self, kp, kd):
+ """Set the gains of all motors.
+
+ These gains are PD gains for motor positional control. kp is the
+ proportional gain and kd is the derivative gain.
+
+ Args:
+ kp: proportional gain of the motors.
+ kd: derivative gain of the motors.
+ """
+ self._kp = kp
+ self._kd = kd
+
+ def set_voltage(self, voltage):
+ pass
+
+ def get_voltage(self):
+ return 0.0
+
+ def set_viscous_damping(self, viscous_damping):
+ pass
+
+ def get_viscous_dampling(self):
+ return 0.0
+
+ def convert_to_torque(self,
+ motor_commands,
+ motor_angle,
+ motor_velocity,
+ true_motor_velocity,
+ motor_control_mode=None):
+ """Convert the commands (position control or torque control) to torque.
+
+ Args:
+ motor_commands: The desired motor angle if the motor is in position
+ control mode. The pwm signal if the motor is in torque control mode.
+ motor_angle: The motor angle observed at the current time step. It is
+ actually the true motor angle observed a few milliseconds ago (pd
+ latency).
+ motor_velocity: The motor velocity observed at the current time step, it
+ is actually the true motor velocity a few milliseconds ago (pd latency).
+ true_motor_velocity: The true motor velocity. The true velocity is used to
+ compute back EMF voltage and viscous damping.
+ motor_control_mode: A MotorControlMode enum.
+
+ Returns:
+ actual_torque: The torque that needs to be applied to the motor.
+ observed_torque: The torque observed by the sensor.
+ """
+ del true_motor_velocity
+ if not motor_control_mode:
+ motor_control_mode = self._motor_control_mode
+
+ # No processing for motor torques
+ if motor_control_mode is robot_config.MotorControlMode.TORQUE:
+ assert len(motor_commands) == NUM_MOTORS
+ motor_torques = self._strength_ratios * motor_commands
+ return motor_torques, motor_torques
+
+ desired_motor_angles = None
+ desired_motor_velocities = None
+ kp = None
+ kd = None
+ additional_torques = np.full(NUM_MOTORS, 0)
+ if motor_control_mode is robot_config.MotorControlMode.POSITION:
+ assert len(motor_commands) == NUM_MOTORS
+ kp = self._kp
+ kd = self._kd
+ desired_motor_angles = motor_commands
+ desired_motor_velocities = np.full(NUM_MOTORS, 0)
+ elif motor_control_mode is robot_config.MotorControlMode.HYBRID:
+ # The input should be a 60 dimension vector
+ assert len(motor_commands) == MOTOR_COMMAND_DIMENSION * NUM_MOTORS
+ kp = motor_commands[POSITION_GAIN_INDEX::MOTOR_COMMAND_DIMENSION]
+ kd = motor_commands[VELOCITY_GAIN_INDEX::MOTOR_COMMAND_DIMENSION]
+ desired_motor_angles = motor_commands[
+ POSITION_INDEX::MOTOR_COMMAND_DIMENSION]
+ desired_motor_velocities = motor_commands[
+ VELOCITY_INDEX::MOTOR_COMMAND_DIMENSION]
+ additional_torques = motor_commands[TORQUE_INDEX::MOTOR_COMMAND_DIMENSION]
+ motor_torques = -1 * (kp * (motor_angle - desired_motor_angles)) - kd * (
+ motor_velocity - desired_motor_velocities) + additional_torques
+ motor_torques = self._strength_ratios * motor_torques
+ if self._torque_limits is not None:
+ if len(self._torque_limits) != len(motor_torques):
+ raise ValueError(
+ "Torque limits dimension does not match the number of motors.")
+ motor_torques = np.clip(motor_torques, -1.0 * self._torque_limits,
+ self._torque_limits)
+
+ return motor_torques, motor_torques
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago_v2.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago_v2.py
new file mode 100644
index 000000000..dcabd05df
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago_v2.py
@@ -0,0 +1,34 @@
+# Lint as: python3
+"""Add the new laikago robot."""
+import gin
+
+from pybullet_envs.minitaur.robots import laikago_constants
+from pybullet_envs.minitaur.robots import quadruped_base
+from pybullet_envs.minitaur.robots import robot_urdf_loader
+
+
+@gin.configurable
+class Laikago(quadruped_base.QuadrupedBase):
+ """The Laikago class that simulates the quadruped from Unitree."""
+
+ def _pre_load(self):
+ """Import the Laikago specific constants.
+ """
+ self._urdf_loader = robot_urdf_loader.RobotUrdfLoader(
+ pybullet_client=self._pybullet_client,
+ urdf_path=laikago_constants.URDF_PATH,
+ enable_self_collision=True,
+ init_base_position=laikago_constants.INIT_POSITION,
+ init_base_orientation_quaternion=laikago_constants.INIT_ORIENTATION,
+ init_joint_angles=laikago_constants.INIT_JOINT_ANGLES,
+ joint_offsets=laikago_constants.JOINT_OFFSETS,
+ joint_directions=laikago_constants.JOINT_DIRECTIONS,
+ motor_names=laikago_constants.MOTOR_NAMES,
+ end_effector_names=laikago_constants.END_EFFECTOR_NAMES,
+ user_group=laikago_constants.MOTOR_GROUP,
+ )
+
+ @classmethod
+ def get_constants(cls):
+ del cls
+ return laikago_constants
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/mini_cheetah.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/mini_cheetah.py
new file mode 100644
index 000000000..bc8fb1089
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/mini_cheetah.py
@@ -0,0 +1,133 @@
+"""Pybullet simulation of a vision60 robot."""
+import math
+import os
+
+import gin
+import numpy as np
+
+from pybullet_envs.minitaur.robots import laikago_motor
+from pybullet_envs.minitaur.robots import minitaur
+from pybullet_envs.minitaur.robots import robot_config
+
+NUM_MOTORS = 12
+NUM_LEGS = 4
+MOTOR_NAMES = [
+ "torso_to_abduct_fl_j", # Left front abduction (hip0).
+ "abduct_fl_to_thigh_fl_j", # Left front hip (upper0).
+ "thigh_fl_to_knee_fl_j", # Left front knee (lower0).
+ "torso_to_abduct_hl_j", # Left rear abduction (hip1).
+ "abduct_hl_to_thigh_hl_j", # Left rear hip (upper1).
+ "thigh_hl_to_knee_hl_j", # Left rear knee (lower1).
+ "torso_to_abduct_fr_j", # Right front abduction (hip2).
+ "abduct_fr_to_thigh_fr_j", # Right front hip (upper2).
+ "thigh_fr_to_knee_fr_j", # Right front knee (lower2).
+ "torso_to_abduct_hr_j", # Right rear abduction (hip3).
+ "abduct_hr_to_thigh_hr_j", # Right rear hip (upper3).
+ "thigh_hr_to_knee_hr_j", # Right rear knee (lower3).
+]
+_DEFAULT_TORQUE_LIMITS = [12, 18, 12] * 4
+INIT_RACK_POSITION = [0, 0, 1.4]
+INIT_POSITION = [0, 0, 0.4]
+JOINT_DIRECTIONS = np.array([1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1])
+HIP_JOINT_OFFSET = 0.0
+UPPER_LEG_JOINT_OFFSET = 0.0
+KNEE_JOINT_OFFSET = 0.0
+DOFS_PER_LEG = 3
+JOINT_OFFSETS = np.array(
+ [HIP_JOINT_OFFSET, UPPER_LEG_JOINT_OFFSET, KNEE_JOINT_OFFSET] * 4)
+PI = math.pi
+DEFAULT_ABDUCTION_ANGLE = 0.0
+DEFAULT_HIP_ANGLE = -1.1
+DEFAULT_KNEE_ANGLE = 2.3
+# Bases on the readings from 's default pose.
+INIT_MOTOR_ANGLES = [
+ DEFAULT_ABDUCTION_ANGLE, DEFAULT_HIP_ANGLE, DEFAULT_KNEE_ANGLE
+] * NUM_LEGS
+DEFAULT_LOCAL_TOE_POSITIONS = [[0.17, -0.11, -0.16], [0.17, 0.11, -0.16],
+ [-0.20, -0.11, -0.16], [-0.20, 0.11, -0.16]]
+
+
+@gin.configurable
+class MiniCheetah(minitaur.Minitaur):
+ """A simulation for the mini cheetah robot."""
+
+ def __init__(self, **kwargs):
+ if "motor_kp" not in kwargs:
+ kwargs["motor_kp"] = 100.0
+ if "motor_kd" not in kwargs:
+ kwargs["motor_kd"] = 2.0
+ if "motor_torque_limits" not in kwargs:
+ kwargs["motor_torque_limits"] = _DEFAULT_TORQUE_LIMITS
+
+ # The follwing parameters are fixed for the vision60 robot.
+ kwargs["num_motors"] = NUM_MOTORS
+ kwargs["dofs_per_leg"] = DOFS_PER_LEG
+ kwargs["motor_direction"] = JOINT_DIRECTIONS
+ kwargs["motor_offset"] = JOINT_OFFSETS
+ kwargs["motor_overheat_protection"] = False
+ kwargs["motor_model_class"] = laikago_motor.LaikagoMotorModel
+ super(MiniCheetah, self).__init__(**kwargs)
+
+ def _LoadRobotURDF(self):
+ mini_cheetah_urdf_path = "mini_cheetah/mini_cheetah.urdf"
+ if self._self_collision_enabled:
+ self.quadruped = self._pybullet_client.loadURDF(
+ mini_cheetah_urdf_path,
+ self._GetDefaultInitPosition(),
+ self._GetDefaultInitOrientation(),
+ flags=self._pybullet_client.URDF_USE_SELF_COLLISION)
+ else:
+ self.quadruped = self._pybullet_client.loadURDF(
+ mini_cheetah_urdf_path, self._GetDefaultInitPosition(),
+ self._GetDefaultInitOrientation())
+
+ def _SettleDownForReset(self, default_motor_angles, reset_time):
+ self.ReceiveObservation()
+ for _ in range(500):
+ self.ApplyAction(
+ INIT_MOTOR_ANGLES,
+ motor_control_mode=robot_config.MotorControlMode.POSITION)
+ self._pybullet_client.stepSimulation()
+ self.ReceiveObservation()
+ if default_motor_angles is not None:
+ num_steps_to_reset = int(reset_time / self.time_step)
+ for _ in range(num_steps_to_reset):
+ self.ApplyAction(
+ default_motor_angles,
+ motor_control_mode=robot_config.MotorControlMode.POSITION)
+ self._pybullet_client.stepSimulation()
+ self.ReceiveObservation()
+
+ def GetURDFFile(self):
+ return os.path.join(self._urdf_root, "mini_cheetah/mini_cheetah.urdf")
+
+ def ResetPose(self, add_constraint):
+ del add_constraint
+ for name in self._joint_name_to_id:
+ joint_id = self._joint_name_to_id[name]
+ self._pybullet_client.setJointMotorControl2(
+ bodyIndex=self.quadruped,
+ jointIndex=(joint_id),
+ controlMode=self._pybullet_client.VELOCITY_CONTROL,
+ targetVelocity=0,
+ force=0)
+ for name, i in zip(MOTOR_NAMES, range(len(MOTOR_NAMES))):
+ angle = INIT_MOTOR_ANGLES[i]
+ self._pybullet_client.resetJointState(
+ self.quadruped, self._joint_name_to_id[name], angle, targetVelocity=0)
+
+ def _BuildUrdfIds(self):
+ pass
+
+ def _GetMotorNames(self):
+ return MOTOR_NAMES
+
+ def _GetDefaultInitPosition(self):
+ if self._on_rack:
+ return INIT_RACK_POSITION
+ else:
+ return INIT_POSITION
+
+ def _GetDefaultInitOrientation(self):
+ init_orientation = [0, 0, 0, 1.0]
+ return init_orientation
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/mini_cheetah_test.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/mini_cheetah_test.py
new file mode 100644
index 000000000..94acf54c0
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/mini_cheetah_test.py
@@ -0,0 +1,58 @@
+"""Tests for pybullet_envs.minitaur.robots.mini_cheetah.
+
+blaze test -c opt
+//robotics/reinforcement_learning/minitaur/robots:mini_cheetah_test
+"""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import math
+import numpy as np
+from pybullet_envs.minitaur.envs import bullet_client
+from pybullet_envs.minitaur.robots import mini_cheetah
+from google3.testing.pybase import googletest
+
+PI = math.pi
+NUM_STEPS = 500
+TIME_STEP = 0.002
+INIT_MOTOR_ANGLES = [0, -0.6, 1.4] * 4
+
+
+class MiniCheetahTest(googletest.TestCase):
+
+ def test_init(self):
+ pybullet_client = bullet_client.BulletClient()
+ pybullet_client.enable_cns()
+ robot = mini_cheetah.MiniCheetah(
+ pybullet_client=pybullet_client, time_step=TIME_STEP, on_rack=True)
+ self.assertIsNotNone(robot)
+
+ def test_static_pose_on_rack(self):
+ pybullet_client = bullet_client.BulletClient()
+ pybullet_client.enable_cns()
+ pybullet_client.resetSimulation()
+ pybullet_client.setPhysicsEngineParameter(numSolverIterations=60)
+ pybullet_client.setTimeStep(TIME_STEP)
+ pybullet_client.setGravity(0, 0, -10)
+
+ robot = (
+ mini_cheetah.MiniCheetah(
+ pybullet_client=pybullet_client,
+ action_repeat=5,
+ time_step=0.002,
+ on_rack=True))
+ robot.Reset(
+ reload_urdf=False,
+ default_motor_angles=INIT_MOTOR_ANGLES,
+ reset_time=0.5)
+ for _ in range(NUM_STEPS):
+ robot.Step(INIT_MOTOR_ANGLES)
+ motor_angles = robot.GetMotorAngles()
+ np.testing.assert_array_almost_equal(
+ motor_angles, INIT_MOTOR_ANGLES, decimal=2)
+
+
+if __name__ == '__main__':
+ googletest.main()
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/minitaur.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/minitaur.py
new file mode 100644
index 000000000..9494c9ef0
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/minitaur.py
@@ -0,0 +1,1479 @@
+"""This file implements the functionalities of a minitaur using pybullet."""
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import collections
+import copy
+import logging
+import math
+import re
+import numpy as np
+import gin
+from pybullet_envs.minitaur.robots import minitaur_constants
+from pybullet_envs.minitaur.robots import minitaur_motor
+from pybullet_envs.minitaur.robots import robot_config
+from pybullet_envs.minitaur.robots.safety import safety_checker
+from pybullet_envs.minitaur.robots.safety import safety_error
+from pybullet_envs.minitaur.robots.utilities import action_filter
+from pybullet_envs.minitaur.robots.utilities import kinematics
+
+INIT_POSITION = [0, 0, .2]
+INIT_RACK_POSITION = [0, 0, 1]
+INIT_ORIENTATION = [0, 0, 0, 1]
+KNEE_CONSTRAINT_POINT_RIGHT = [0, 0.005, 0.2]
+KNEE_CONSTRAINT_POINT_LEFT = [0, 0.01, 0.2]
+OVERHEAT_SHUTDOWN_TORQUE = 2.45
+OVERHEAT_SHUTDOWN_TIME = 1.0
+LEG_POSITION = ["front_left", "back_left", "front_right", "back_right"]
+MOTOR_NAMES = [
+ "motor_front_leftL_joint", "motor_front_leftR_joint",
+ "motor_back_leftL_joint", "motor_back_leftR_joint",
+ "motor_front_rightL_joint", "motor_front_rightR_joint",
+ "motor_back_rightL_joint", "motor_back_rightR_joint"
+]
+_CHASSIS_NAME_PATTERN = re.compile(r"chassis\D*center")
+_MOTOR_NAME_PATTERN = re.compile(r"motor\D*joint")
+_KNEE_NAME_PATTERN = re.compile(r"knee\D*")
+_BRACKET_NAME_PATTERN = re.compile(r"motor\D*_bracket_joint")
+_LEG_NAME_PATTERN1 = re.compile(r"hip\D*joint")
+_LEG_NAME_PATTERN2 = re.compile(r"hip\D*link")
+_LEG_NAME_PATTERN3 = re.compile(r"motor\D*link")
+SENSOR_NOISE_STDDEV = (0.0, 0.0, 0.0, 0.0, 0.0)
+MINITAUR_DEFAULT_MOTOR_DIRECTIONS = (-1, -1, -1, -1, 1, 1, 1, 1)
+MINITAUR_DEFAULT_MOTOR_OFFSETS = (0, 0, 0, 0, 0, 0, 0, 0)
+MINITAUR_NUM_MOTORS = 8
+TWO_PI = 2 * math.pi
+MINITAUR_DOFS_PER_LEG = 2
+
+URDF_ROOT = "robotics/reinforcement_learning/minitaur/robots/data/urdf/"
+
+
+def MapToMinusPiToPi(angles):
+ """Maps a list of angles to [-pi, pi].
+
+ Args:
+ angles: A list of angles in rad.
+
+ Returns:
+ A list of angle mapped to [-pi, pi].
+ """
+ mapped_angles = copy.deepcopy(angles)
+ for i in range(len(angles)):
+ mapped_angles[i] = math.fmod(angles[i], TWO_PI)
+ if mapped_angles[i] >= math.pi:
+ mapped_angles[i] -= TWO_PI
+ elif mapped_angles[i] < -math.pi:
+ mapped_angles[i] += TWO_PI
+ return mapped_angles
+
+
+@gin.configurable
+class Minitaur(object):
+ """The minitaur class that simulates a quadruped robot from Ghost Robotics."""
+
+ def __init__(self,
+ pybullet_client,
+ num_motors=MINITAUR_NUM_MOTORS,
+ dofs_per_leg=MINITAUR_DOFS_PER_LEG,
+ urdf_root=URDF_ROOT,
+ time_step=0.01,
+ action_repeat=1,
+ self_collision_enabled=False,
+ motor_control_mode=robot_config.MotorControlMode.POSITION,
+ motor_model_class=minitaur_motor.MotorModel,
+ motor_kp=1.0,
+ motor_kd=0.02,
+ motor_torque_limits=None,
+ pd_latency=0.0,
+ control_latency=0.0,
+ observation_noise_stdev=SENSOR_NOISE_STDDEV,
+ motor_overheat_protection=False,
+ motor_direction=MINITAUR_DEFAULT_MOTOR_DIRECTIONS,
+ motor_offset=MINITAUR_DEFAULT_MOTOR_OFFSETS,
+ on_rack=False,
+ reset_at_current_position=False,
+ sensors=None,
+ safety_config=None,
+ enable_action_interpolation=False,
+ enable_action_filter=False):
+ """Constructs a minitaur and reset it to the initial states.
+
+ Args:
+ pybullet_client: The instance of BulletClient to manage different
+ simulations.
+ num_motors: The number of the motors on the robot.
+ dofs_per_leg: The number of degrees of freedom for each leg.
+ urdf_root: The path to the urdf folder.
+ time_step: The time step of the simulation.
+ action_repeat: The number of ApplyAction() for each control step.
+ self_collision_enabled: Whether to enable self collision.
+ motor_control_mode: Enum. Can either be POSITION, TORQUE, or HYBRID.
+ motor_model_class: We can choose from simple pd model to more accureate DC
+ motor models.
+ motor_kp: proportional gain for the motors.
+ motor_kd: derivative gain for the motors.
+ motor_torque_limits: Torque limits for the motors. Can be a single float
+ or a list of floats specifying different limits for different robots. If
+ not provided, the default limit of the robot is used.
+ pd_latency: The latency of the observations (in seconds) used to calculate
+ PD control. On the real hardware, it is the latency between the
+ microcontroller and the motor controller.
+ control_latency: The latency of the observations (in second) used to
+ calculate action. On the real hardware, it is the latency from the motor
+ controller, the microcontroller to the host (Nvidia TX2).
+ observation_noise_stdev: The standard deviation of a Gaussian noise model
+ for the sensor. It should be an array for separate sensors in the
+ following order [motor_angle, motor_velocity, motor_torque,
+ base_roll_pitch_yaw, base_angular_velocity]
+ motor_overheat_protection: Whether to shutdown the motor that has exerted
+ large torque (OVERHEAT_SHUTDOWN_TORQUE) for an extended amount of time
+ (OVERHEAT_SHUTDOWN_TIME). See ApplyAction() in minitaur.py for more
+ details.
+ motor_direction: A list of direction values, either 1 or -1, to compensate
+ the axis difference of motors between the simulation and the real robot.
+ motor_offset: A list of offset value for the motor angles. This is used to
+ compensate the angle difference between the simulation and the real
+ robot.
+ on_rack: Whether to place the minitaur on rack. This is only used to debug
+ the walking gait. In this mode, the minitaur's base is hanged midair so
+ that its walking gait is clearer to visualize.
+ reset_at_current_position: Whether to reset the minitaur at the current
+ position and orientation. This is for simulating the reset behavior in
+ the real world.
+ sensors: a list of sensors that are attached to the robot.
+ safety_config: A SafetyConfig class to configure the safety layer. If
+ None, the safety layer will be disabled.
+ enable_action_interpolation: Whether to interpolate the current action
+ with the previous action in order to produce smoother motions
+ enable_action_filter: Boolean specifying if a lowpass filter should be
+ used to smooth actions.
+ """
+ self.num_motors = num_motors
+ self.num_legs = self.num_motors // dofs_per_leg
+ self._pybullet_client = pybullet_client
+ self._action_repeat = action_repeat
+ self._urdf_root = urdf_root
+ self._self_collision_enabled = self_collision_enabled
+ self._motor_direction = motor_direction
+ self._motor_offset = motor_offset
+ self._observed_motor_torques = np.zeros(self.num_motors)
+ self._applied_motor_torques = np.zeros(self.num_motors)
+ self._max_force = 3.5
+ self._pd_latency = pd_latency
+ self._control_latency = control_latency
+ self._observation_noise_stdev = observation_noise_stdev
+ self._observation_history = collections.deque(maxlen=100)
+ self._control_observation = []
+ self._chassis_link_ids = [-1]
+ self._leg_link_ids = []
+ self._motor_link_ids = []
+ self._foot_link_ids = []
+ self._motor_overheat_protection = motor_overheat_protection
+ self._on_rack = on_rack
+ self._reset_at_current_position = reset_at_current_position
+ self.SetAllSensors(sensors if sensors is not None else list())
+ self.safety_config = safety_config
+ self._is_safe = True
+ self._safety_checker = None
+
+ self._enable_action_interpolation = enable_action_interpolation
+ self._enable_action_filter = enable_action_filter
+ self._last_action = None
+
+ if not motor_model_class:
+ raise ValueError("Must provide a motor model class!")
+
+ if self._on_rack and self._reset_at_current_position:
+ raise ValueError("on_rack and reset_at_current_position "
+ "cannot be enabled together")
+
+ if isinstance(motor_kp, (collections.Sequence, np.ndarray)):
+ self._motor_kps = np.asarray(motor_kp)
+ else:
+ self._motor_kps = np.full(num_motors, motor_kp)
+
+ if isinstance(motor_kd, (collections.Sequence, np.ndarray)):
+ self._motor_kds = np.asarray(motor_kd)
+ else:
+ self._motor_kds = np.full(num_motors, motor_kd)
+
+ if isinstance(motor_torque_limits, (collections.Sequence, np.ndarray)):
+ self._motor_torque_limits = np.asarray(motor_torque_limits)
+ elif motor_torque_limits is None:
+ self._motor_torque_limits = None
+ else:
+ self._motor_torque_limits = motor_torque_limits
+
+ self._motor_control_mode = motor_control_mode
+ self._motor_model = motor_model_class(
+ kp=motor_kp,
+ kd=motor_kd,
+ torque_limits=self._motor_torque_limits,
+ motor_control_mode=motor_control_mode)
+
+ self.time_step = time_step
+ self._step_counter = 0
+
+ # This also includes the time spent during the Reset motion.
+ self._state_action_counter = 0
+ _, self._init_orientation_inv = self._pybullet_client.invertTransform(
+ position=[0, 0, 0], orientation=self._GetDefaultInitOrientation())
+
+ if self._enable_action_filter:
+ self._action_filter = self._BuildActionFilter()
+ # reset_time=-1.0 means skipping the reset motion.
+ # See Reset for more details.
+ self.Reset(reset_time=-1.0)
+ self.ReceiveObservation()
+
+ return
+
+ def GetTimeSinceReset(self):
+ return self._step_counter * self.time_step
+
+ def _StepInternal(self, action, motor_control_mode=None):
+ self.ApplyAction(action, motor_control_mode)
+ self._pybullet_client.stepSimulation()
+ self.ReceiveObservation()
+ self._state_action_counter += 1
+
+ return
+
+ def Step(self, action):
+ """Steps simulation."""
+ if self._enable_action_filter:
+ action = self._FilterAction(action)
+
+ for i in range(self._action_repeat):
+ proc_action = self.ProcessAction(action, i)
+ self._StepInternal(proc_action)
+ self._step_counter += 1
+
+ self._last_action = action
+ return
+
+ def Terminate(self):
+ pass
+
+ def GetKneeLinkIDs(self):
+ """Get list of IDs for all knee links."""
+ return self._knee_link_ids
+
+ def GetFootLinkIDs(self):
+ """Get list of IDs for all foot links."""
+ return self._foot_link_ids
+
+ def _RecordMassInfoFromURDF(self):
+ """Records the mass information from the URDF file."""
+ self._base_mass_urdf = []
+ for chassis_id in self._chassis_link_ids:
+ self._base_mass_urdf.append(
+ self._pybullet_client.getDynamicsInfo(self.quadruped, chassis_id)[0])
+ self._leg_masses_urdf = []
+ for leg_id in self._leg_link_ids:
+ self._leg_masses_urdf.append(
+ self._pybullet_client.getDynamicsInfo(self.quadruped, leg_id)[0])
+ for motor_id in self._motor_link_ids:
+ self._leg_masses_urdf.append(
+ self._pybullet_client.getDynamicsInfo(self.quadruped, motor_id)[0])
+
+ def _RecordInertiaInfoFromURDF(self):
+ """Record the inertia of each body from URDF file."""
+ self._link_urdf = []
+ num_bodies = self._pybullet_client.getNumJoints(self.quadruped)
+ for body_id in range(-1, num_bodies): # -1 is for the base link.
+ inertia = self._pybullet_client.getDynamicsInfo(self.quadruped,
+ body_id)[2]
+ self._link_urdf.append(inertia)
+ # We need to use id+1 to index self._link_urdf because it has the base
+ # (index = -1) at the first element.
+ self._base_inertia_urdf = [
+ self._link_urdf[chassis_id + 1] for chassis_id in self._chassis_link_ids
+ ]
+ self._leg_inertia_urdf = [
+ self._link_urdf[leg_id + 1] for leg_id in self._leg_link_ids
+ ]
+ self._leg_inertia_urdf.extend(
+ [self._link_urdf[motor_id + 1] for motor_id in self._motor_link_ids])
+
+ def _BuildJointNameToIdDict(self):
+ num_joints = self._pybullet_client.getNumJoints(self.quadruped)
+ self._joint_name_to_id = {}
+ for i in range(num_joints):
+ joint_info = self._pybullet_client.getJointInfo(self.quadruped, i)
+ self._joint_name_to_id[joint_info[1].decode("UTF-8")] = joint_info[0]
+
+ def _BuildUrdfIds(self):
+ """Build the link Ids from its name in the URDF file.
+
+ Raises:
+ ValueError: Unknown category of the joint name.
+ """
+ num_joints = self._pybullet_client.getNumJoints(self.quadruped)
+ self._chassis_link_ids = [-1]
+ # The self._leg_link_ids include both the upper and lower links of the leg.
+ self._leg_link_ids = []
+ self._motor_link_ids = []
+ self._foot_link_ids = []
+ self._bracket_link_ids = []
+ for i in range(num_joints):
+ joint_info = self._pybullet_client.getJointInfo(self.quadruped, i)
+ joint_name = joint_info[1].decode("UTF-8")
+ joint_id = self._joint_name_to_id[joint_name]
+ if _CHASSIS_NAME_PATTERN.match(joint_name):
+ self._chassis_link_ids.append(joint_id)
+ elif _BRACKET_NAME_PATTERN.match(joint_name):
+ self._bracket_link_ids.append(joint_id)
+ elif _MOTOR_NAME_PATTERN.match(joint_name):
+ self._motor_link_ids.append(joint_id)
+ elif _KNEE_NAME_PATTERN.match(joint_name):
+ self._foot_link_ids.append(joint_id)
+ elif (_LEG_NAME_PATTERN1.match(joint_name) or
+ _LEG_NAME_PATTERN2.match(joint_name) or
+ _LEG_NAME_PATTERN3.match(joint_name)):
+ self._leg_link_ids.append(joint_id)
+ else:
+ raise ValueError("Unknown category of joint %s" % joint_name)
+
+ self._leg_link_ids.extend(self._foot_link_ids)
+ self._chassis_link_ids.sort()
+ self._motor_link_ids.sort()
+ self._foot_link_ids.sort()
+ self._leg_link_ids.sort()
+ self._bracket_link_ids.sort()
+
+ def _RemoveDefaultJointDamping(self):
+ num_joints = self._pybullet_client.getNumJoints(self.quadruped)
+ for i in range(num_joints):
+ joint_info = self._pybullet_client.getJointInfo(self.quadruped, i)
+ self._pybullet_client.changeDynamics(
+ joint_info[0], -1, linearDamping=0, angularDamping=0)
+
+ def _BuildMotorIdList(self):
+ self._motor_id_list = [
+ self._joint_name_to_id[motor_name]
+ for motor_name in self._GetMotorNames()
+ ]
+
+ def _CreateRackConstraint(self, init_position, init_orientation):
+ """Create a constraint that keeps the chassis at a fixed frame.
+
+ This frame is defined by init_position and init_orientation.
+
+ Args:
+ init_position: initial position of the fixed frame.
+ init_orientation: initial orientation of the fixed frame in quaternion
+ format [x,y,z,w].
+
+ Returns:
+ Return the constraint id.
+ """
+ fixed_constraint = self._pybullet_client.createConstraint(
+ parentBodyUniqueId=self.quadruped,
+ parentLinkIndex=-1,
+ childBodyUniqueId=-1,
+ childLinkIndex=-1,
+ jointType=self._pybullet_client.JOINT_FIXED,
+ jointAxis=[0, 0, 0],
+ parentFramePosition=[0, 0, 0],
+ childFramePosition=init_position,
+ childFrameOrientation=init_orientation)
+ return fixed_constraint
+
+ def IsObservationValid(self):
+ """Whether the observation is valid for the current time step.
+
+ In simulation, observations are always valid. In real hardware, it may not
+ be valid from time to time when communication error happens between the
+ Nvidia TX2 and the microcontroller.
+
+ Returns:
+ Whether the observation is valid for the current time step.
+ """
+ return True
+
+ def Reset(self, reload_urdf=True, default_motor_angles=None, reset_time=3.0):
+ """Reset the minitaur to its initial states.
+
+ Args:
+ reload_urdf: Whether to reload the urdf file. If not, Reset() just place
+ the minitaur back to its starting position.
+ default_motor_angles: The default motor angles. If it is None, minitaur
+ will hold a default pose (motor angle math.pi / 2) for 100 steps. In
+ torque control mode, the phase of holding the default pose is skipped.
+ reset_time: The duration (in seconds) to hold the default motor angles. If
+ reset_time <= 0 or in torque control mode, the phase of holding the
+ default pose is skipped.
+ """
+ if reload_urdf:
+ self._LoadRobotURDF()
+ if self._on_rack:
+ self.rack_constraint = (
+ self._CreateRackConstraint(self._GetDefaultInitPosition(),
+ self._GetDefaultInitOrientation()))
+ self._BuildJointNameToIdDict()
+ self._BuildUrdfIds()
+ self._RemoveDefaultJointDamping()
+ self._BuildMotorIdList()
+ self._RecordMassInfoFromURDF()
+ self._RecordInertiaInfoFromURDF()
+ self.ResetPose(add_constraint=True)
+ else:
+ self._pybullet_client.resetBasePositionAndOrientation(
+ self.quadruped, self._GetDefaultInitPosition(),
+ self._GetDefaultInitOrientation())
+ self._pybullet_client.resetBaseVelocity(self.quadruped, [0, 0, 0],
+ [0, 0, 0])
+ self.ResetPose(add_constraint=False)
+
+ self._overheat_counter = np.zeros(self.num_motors)
+ self._motor_enabled_list = [True] * self.num_motors
+ self._observation_history.clear()
+ self._step_counter = 0
+ self._state_action_counter = 0
+ self._is_safe = True
+ self._last_action = None
+
+ # Enable the safety layer before we perform any reset motions.
+ if self.safety_config:
+ self._safety_checker = safety_checker.SafetyChecker(self)
+ self._SettleDownForReset(default_motor_angles, reset_time)
+
+ if self._enable_action_filter:
+ self._ResetActionFilter()
+
+ return
+
+ def _LoadRobotURDF(self):
+ """Loads the URDF file for the robot."""
+ urdf_file = self.GetURDFFile()
+ if self._self_collision_enabled:
+ self.quadruped = self._pybullet_client.loadURDF(
+ urdf_file,
+ self._GetDefaultInitPosition(),
+ self._GetDefaultInitOrientation(),
+ flags=self._pybullet_client.URDF_USE_SELF_COLLISION)
+ else:
+ self.quadruped = self._pybullet_client.loadURDF(
+ urdf_file, self._GetDefaultInitPosition(),
+ self._GetDefaultInitOrientation())
+
+ def _SettleDownForReset(self, default_motor_angles, reset_time):
+ """Sets the default motor angles and waits for the robot to settle down.
+
+ The reset is skipped is reset_time is less than zereo.
+
+ Args:
+ default_motor_angles: A list of motor angles that the robot will achieve
+ at the end of the reset phase.
+ reset_time: The time duration for the reset phase.
+ """
+ if reset_time <= 0:
+ return
+
+ # Important to fill the observation buffer.
+ self.ReceiveObservation()
+ for _ in range(100):
+ self._StepInternal(
+ [math.pi / 2] * self.num_motors,
+ motor_control_mode=robot_config.MotorControlMode.POSITION)
+ # Don't continue to reset if a safety error has occurred.
+ if not self._is_safe:
+ return
+
+ if default_motor_angles is None:
+ return
+
+ num_steps_to_reset = int(reset_time / self.time_step)
+ for _ in range(num_steps_to_reset):
+ self._StepInternal(
+ default_motor_angles,
+ motor_control_mode=robot_config.MotorControlMode.POSITION)
+ # Don't continue to reset if a safety error has occurred.
+ if not self._is_safe:
+ return
+
+ def _SetMotorTorqueById(self, motor_id, torque):
+ self._pybullet_client.setJointMotorControl2(
+ bodyIndex=self.quadruped,
+ jointIndex=motor_id,
+ controlMode=self._pybullet_client.TORQUE_CONTROL,
+ force=torque)
+
+ def _SetMotorTorqueByIds(self, motor_ids, torques):
+ self._pybullet_client.setJointMotorControlArray(
+ bodyIndex=self.quadruped,
+ jointIndices=motor_ids,
+ controlMode=self._pybullet_client.TORQUE_CONTROL,
+ forces=torques)
+
+ def _SetDesiredMotorAngleByName(self, motor_name, desired_angle):
+ self._SetDesiredMotorAngleById(self._joint_name_to_id[motor_name],
+ desired_angle)
+
+ def GetURDFFile(self):
+ return "%s/quadruped/minitaur.urdf" % self._urdf_root
+
+ def ResetPose(self, add_constraint):
+ """Reset the pose of the minitaur.
+
+ Args:
+ add_constraint: Whether to add a constraint at the joints of two feet.
+ """
+ for i in range(self.num_legs):
+ self._ResetPoseForLeg(i, add_constraint)
+
+ def _ResetPoseForLeg(self, leg_id, add_constraint):
+ """Reset the initial pose for the leg.
+
+ Args:
+ leg_id: It should be 0, 1, 2, or 3, which represents the leg at
+ front_left, back_left, front_right and back_right.
+ add_constraint: Whether to add a constraint at the joints of two feet.
+ """
+ knee_friction_force = 0
+ half_pi = math.pi / 2.0
+ knee_angle = -2.1834
+
+ leg_position = LEG_POSITION[leg_id]
+ self._pybullet_client.resetJointState(
+ self.quadruped,
+ self._joint_name_to_id["motor_" + leg_position + "L_joint"],
+ self._motor_direction[2 * leg_id] * half_pi,
+ targetVelocity=0)
+ self._pybullet_client.resetJointState(
+ self.quadruped,
+ self._joint_name_to_id["knee_" + leg_position + "L_link"],
+ self._motor_direction[2 * leg_id] * knee_angle,
+ targetVelocity=0)
+ self._pybullet_client.resetJointState(
+ self.quadruped,
+ self._joint_name_to_id["motor_" + leg_position + "R_joint"],
+ self._motor_direction[2 * leg_id + 1] * half_pi,
+ targetVelocity=0)
+ self._pybullet_client.resetJointState(
+ self.quadruped,
+ self._joint_name_to_id["knee_" + leg_position + "R_link"],
+ self._motor_direction[2 * leg_id + 1] * knee_angle,
+ targetVelocity=0)
+ if add_constraint:
+ self._pybullet_client.createConstraint(
+ self.quadruped,
+ self._joint_name_to_id["knee_" + leg_position + "R_link"],
+ self.quadruped,
+ self._joint_name_to_id["knee_" + leg_position + "L_link"],
+ self._pybullet_client.JOINT_POINT2POINT, [0, 0, 0],
+ KNEE_CONSTRAINT_POINT_RIGHT, KNEE_CONSTRAINT_POINT_LEFT)
+
+ # Disable the default motor in pybullet.
+ self._pybullet_client.setJointMotorControl2(
+ bodyIndex=self.quadruped,
+ jointIndex=(self._joint_name_to_id["motor_" + leg_position +
+ "L_joint"]),
+ controlMode=self._pybullet_client.VELOCITY_CONTROL,
+ targetVelocity=0,
+ force=knee_friction_force)
+ self._pybullet_client.setJointMotorControl2(
+ bodyIndex=self.quadruped,
+ jointIndex=(self._joint_name_to_id["motor_" + leg_position +
+ "R_joint"]),
+ controlMode=self._pybullet_client.VELOCITY_CONTROL,
+ targetVelocity=0,
+ force=knee_friction_force)
+
+ self._pybullet_client.setJointMotorControl2(
+ bodyIndex=self.quadruped,
+ jointIndex=(self._joint_name_to_id["knee_" + leg_position + "L_link"]),
+ controlMode=self._pybullet_client.VELOCITY_CONTROL,
+ targetVelocity=0,
+ force=knee_friction_force)
+ self._pybullet_client.setJointMotorControl2(
+ bodyIndex=self.quadruped,
+ jointIndex=(self._joint_name_to_id["knee_" + leg_position + "R_link"]),
+ controlMode=self._pybullet_client.VELOCITY_CONTROL,
+ targetVelocity=0,
+ force=knee_friction_force)
+
+ def GetBasePosition(self):
+ """Get the position of minitaur's base.
+
+ Returns:
+ The position of minitaur's base.
+ """
+ return self._base_position
+
+ def GetBaseVelocity(self):
+ """Get the linear velocity of minitaur's base.
+
+ Returns:
+ The velocity of minitaur's base.
+ """
+ velocity, _ = self._pybullet_client.getBaseVelocity(self.quadruped)
+ return velocity
+
+ def GetTrueBaseRollPitchYaw(self):
+ """Get minitaur's base orientation in euler angle in the world frame.
+
+ Returns:
+ A tuple (roll, pitch, yaw) of the base in world frame.
+ """
+ orientation = self.GetTrueBaseOrientation()
+ roll_pitch_yaw = self._pybullet_client.getEulerFromQuaternion(orientation)
+ return np.asarray(roll_pitch_yaw)
+
+ def GetBaseRollPitchYaw(self):
+ """Get minitaur's base orientation in euler angle in the world frame.
+
+ This function mimicks the noisy sensor reading and adds latency.
+ Returns:
+ A tuple (roll, pitch, yaw) of the base in world frame polluted by noise
+ and latency.
+ """
+ delayed_orientation = np.array(
+ self._control_observation[3 * self.num_motors:3 * self.num_motors + 4])
+ delayed_roll_pitch_yaw = self._pybullet_client.getEulerFromQuaternion(
+ delayed_orientation)
+ roll_pitch_yaw = self._AddSensorNoise(
+ np.array(delayed_roll_pitch_yaw), self._observation_noise_stdev[3])
+ return roll_pitch_yaw
+
+ def GetHipPositionsInBaseFrame(self):
+ """Get the hip joint positions of the robot within its base frame."""
+ raise NotImplementedError("Not implemented for Minitaur.")
+
+ def _EndEffectorIK(self, leg_id, position, position_in_world_frame):
+ """Calculate the joint positions from the end effector position."""
+ assert len(self._foot_link_ids) == self.num_legs
+ toe_id = self._foot_link_ids[leg_id]
+ motors_per_leg = self.num_motors // self.num_legs
+ joint_position_idxs = [
+ i for i in range(leg_id * motors_per_leg, leg_id * motors_per_leg +
+ motors_per_leg)
+ ]
+ joint_angles = kinematics.joint_angles_from_link_position(
+ robot=self,
+ link_position=position,
+ link_id=toe_id,
+ joint_ids=joint_position_idxs,
+ position_in_world_frame=position_in_world_frame)
+ # Joint offset is necessary for Laikago.
+ joint_angles = np.multiply(
+ np.asarray(joint_angles) -
+ np.asarray(self._motor_offset)[joint_position_idxs],
+ self._motor_direction[joint_position_idxs])
+ # Return the joing index (the same as when calling GetMotorAngles) as well
+ # as the angles.
+ return joint_position_idxs, joint_angles.tolist()
+
+ # TODO(b/154361633): Implements an array version of this following function.
+ def ComputeMotorAnglesFromFootWorldPosition(self, leg_id,
+ foot_world_position):
+ """Use IK to compute the motor angles, given the foot link's position.
+
+ Args:
+ leg_id: The leg index.
+ foot_world_position: The foot link's position in the world frame.
+
+ Returns:
+ A tuple. The position indices and the angles for all joints along the
+ leg. The position indices is consistent with the joint orders as returned
+ by GetMotorAngles API.
+ """
+ return self._EndEffectorIK(
+ leg_id, foot_world_position, position_in_world_frame=True)
+
+ def ComputeMotorAnglesFromFootLocalPosition(self, leg_id,
+ foot_local_position):
+ """Use IK to compute the motor angles, given the foot link's local position.
+
+ Args:
+ leg_id: The leg index.
+ foot_local_position: The foot link's position in the base frame.
+
+ Returns:
+ A tuple. The position indices and the angles for all joints along the
+ leg. The position indices is consistent with the joint orders as returned
+ by GetMotorAngles API.
+ """
+ return self._EndEffectorIK(
+ leg_id, foot_local_position, position_in_world_frame=False)
+
+ def ComputeJacobian(self, leg_id):
+ """Compute the Jacobian for a given leg."""
+ # Does not work for Minitaur which has the four bar mechanism for now.
+ assert len(self._foot_link_ids) == self.num_legs
+ return kinematics.compute_jacobian(
+ robot=self,
+ link_id=self._foot_link_ids[leg_id],
+ )
+
+ def MapContactForceToJointTorques(self, leg_id, contact_force):
+ """Maps the foot contact force to the leg joint torques."""
+ jv = self.ComputeJacobian(leg_id)
+ all_motor_torques = np.matmul(contact_force, jv)
+ motor_torques = {}
+ motors_per_leg = self.num_motors // self.num_legs
+ com_dof = 6
+ for joint_id in range(leg_id * motors_per_leg,
+ (leg_id + 1) * motors_per_leg):
+ motor_torques[joint_id] = all_motor_torques[
+ com_dof + joint_id] * self._motor_direction[joint_id]
+
+ return motor_torques
+
+ def GetFootContacts(self):
+ """Get minitaur's foot contact situation with the ground.
+
+ Returns:
+ A list of 4 booleans. The ith boolean is True if leg i is in contact with
+ ground.
+ """
+ contacts = []
+ for leg_idx in range(MINITAUR_NUM_MOTORS // 2):
+ link_id_1 = self._foot_link_ids[leg_idx * 2]
+ link_id_2 = self._foot_link_ids[leg_idx * 2 + 1]
+ contact_1 = bool(
+ self._pybullet_client.getContactPoints(
+ bodyA=0,
+ bodyB=self.quadruped,
+ linkIndexA=-1,
+ linkIndexB=link_id_1))
+ contact_2 = bool(
+ self._pybullet_client.getContactPoints(
+ bodyA=0,
+ bodyB=self.quadruped,
+ linkIndexA=-1,
+ linkIndexB=link_id_2))
+ contacts.append(contact_1 or contact_2)
+ return contacts
+
+ def GetFootPositionsInWorldFrame(self):
+ """Get the robot's foot position in the base frame."""
+ assert len(self._foot_link_ids) == self.num_legs
+ foot_positions = []
+ for foot_id in self.GetFootLinkIDs():
+ foot_positions.append(
+ kinematics.link_position_in_world_frame(
+ robot=self,
+ link_id=foot_id,
+ ))
+ return np.array(foot_positions)
+
+ def GetFootPositionsInBaseFrame(self):
+ """Get the robot's foot position in the base frame."""
+ assert len(self._foot_link_ids) == self.num_legs
+ foot_positions = []
+ for foot_id in self.GetFootLinkIDs():
+ foot_positions.append(
+ kinematics.link_position_in_base_frame(
+ robot=self,
+ link_id=foot_id,
+ ))
+ return np.array(foot_positions)
+
+ def GetTrueMotorAngles(self):
+ """Gets the eight motor angles at the current moment, mapped to [-pi, pi].
+
+ Returns:
+ Motor angles, mapped to [-pi, pi].
+ """
+ motor_angles = [state[0] for state in self._joint_states]
+ motor_angles = np.multiply(
+ np.asarray(motor_angles) - np.asarray(self._motor_offset),
+ self._motor_direction)
+ return motor_angles
+
+ def GetMotorAngles(self):
+ """Gets the eight motor angles.
+
+ This function mimicks the noisy sensor reading and adds latency. The motor
+ angles that are delayed, noise polluted, and mapped to [-pi, pi].
+
+ Returns:
+ Motor angles polluted by noise and latency, mapped to [-pi, pi].
+ """
+ motor_angles = self._AddSensorNoise(
+ np.array(self._control_observation[0:self.num_motors]),
+ self._observation_noise_stdev[0])
+ return MapToMinusPiToPi(motor_angles)
+
+ def GetTrueMotorVelocities(self):
+ """Get the velocity of all eight motors.
+
+ Returns:
+ Velocities of all eight motors.
+ """
+ motor_velocities = [state[1] for state in self._joint_states]
+
+ motor_velocities = np.multiply(motor_velocities, self._motor_direction)
+ return motor_velocities
+
+ def GetMotorVelocities(self):
+ """Get the velocity of all eight motors.
+
+ This function mimicks the noisy sensor reading and adds latency.
+ Returns:
+ Velocities of all eight motors polluted by noise and latency.
+ """
+ return self._AddSensorNoise(
+ np.array(self._control_observation[self.num_motors:2 *
+ self.num_motors]),
+ self._observation_noise_stdev[1])
+
+ def GetTrueMotorTorques(self):
+ """Get the amount of torque the motors are exerting.
+
+ Returns:
+ Motor torques of all eight motors.
+ """
+ return self._observed_motor_torques
+
+ def GetMotorTorques(self):
+ """Get the amount of torque the motors are exerting.
+
+ This function mimicks the noisy sensor reading and adds latency.
+ Returns:
+ Motor torques of all eight motors polluted by noise and latency.
+ """
+ return self._AddSensorNoise(
+ np.array(self._control_observation[2 * self.num_motors:3 *
+ self.num_motors]),
+ self._observation_noise_stdev[2])
+
+ def GetEnergyConsumptionPerControlStep(self):
+ """Get the amount of energy used in last one time step.
+
+ Returns:
+ Energy Consumption based on motor velocities and torques (Nm^2/s).
+ """
+ return np.abs(np.dot(
+ self.GetMotorTorques(),
+ self.GetMotorVelocities())) * self.time_step * self._action_repeat
+
+ def GetTrueBaseOrientation(self):
+ """Get the orientation of minitaur's base, represented as quaternion.
+
+ Returns:
+ The orientation of minitaur's base.
+ """
+ return self._base_orientation
+
+ def GetBaseOrientation(self):
+ """Get the orientation of minitaur's base, represented as quaternion.
+
+ This function mimicks the noisy sensor reading and adds latency.
+ Returns:
+ The orientation of minitaur's base polluted by noise and latency.
+ """
+ return self._pybullet_client.getQuaternionFromEuler(
+ self.GetBaseRollPitchYaw())
+
+ def GetTrueBaseRollPitchYawRate(self):
+ """Get the rate of orientation change of the minitaur's base in euler angle.
+
+ Returns:
+ rate of (roll, pitch, yaw) change of the minitaur's base.
+ """
+ angular_velocity = self._pybullet_client.getBaseVelocity(self.quadruped)[1]
+ orientation = self.GetTrueBaseOrientation()
+ return self.TransformAngularVelocityToLocalFrame(angular_velocity,
+ orientation)
+
+ def TransformAngularVelocityToLocalFrame(self, angular_velocity, orientation):
+ """Transform the angular velocity from world frame to robot's frame.
+
+ Args:
+ angular_velocity: Angular velocity of the robot in world frame.
+ orientation: Orientation of the robot represented as a quaternion.
+
+ Returns:
+ angular velocity of based on the given orientation.
+ """
+ # Treat angular velocity as a position vector, then transform based on the
+ # orientation given by dividing (or multiplying with inverse).
+ # Get inverse quaternion assuming the vector is at 0,0,0 origin.
+ _, orientation_inversed = self._pybullet_client.invertTransform([0, 0, 0],
+ orientation)
+ # Transform the angular_velocity at neutral orientation using a neutral
+ # translation and reverse of the given orientation.
+ relative_velocity, _ = self._pybullet_client.multiplyTransforms(
+ [0, 0, 0], orientation_inversed, angular_velocity,
+ self._pybullet_client.getQuaternionFromEuler([0, 0, 0]))
+ return np.asarray(relative_velocity)
+
+ def GetBaseRollPitchYawRate(self):
+ """Get the rate of orientation change of the minitaur's base in euler angle.
+
+ This function mimicks the noisy sensor reading and adds latency.
+ Returns:
+ rate of (roll, pitch, yaw) change of the minitaur's base polluted by noise
+ and latency.
+ """
+ return self._AddSensorNoise(
+ np.array(self._control_observation[3 * self.num_motors +
+ 4:3 * self.num_motors + 7]),
+ self._observation_noise_stdev[4])
+
+ def GetActionDimension(self):
+ """Get the length of the action list.
+
+ Returns:
+ The length of the action list.
+ """
+ return self.num_motors
+
+ def _ApplyOverheatProtection(self, actual_torque):
+ if self._motor_overheat_protection:
+ for i in range(self.num_motors):
+ if abs(actual_torque[i]) > OVERHEAT_SHUTDOWN_TORQUE:
+ self._overheat_counter[i] += 1
+ else:
+ self._overheat_counter[i] = 0
+ if (self._overheat_counter[i] >
+ OVERHEAT_SHUTDOWN_TIME / self.time_step):
+ self._motor_enabled_list[i] = False
+
+ def ApplyAction(self, motor_commands, motor_control_mode=None):
+ """Apply the motor commands using the motor model.
+
+ Args:
+ motor_commands: np.array. Can be motor angles, torques, hybrid commands,
+ or motor pwms (for Minitaur only).
+ motor_control_mode: A MotorControlMode enum.
+ """
+ self.last_action_time = self._state_action_counter * self.time_step
+ control_mode = motor_control_mode
+ if control_mode is None:
+ control_mode = self._motor_control_mode
+ if self._safety_checker:
+ try:
+ self._safety_checker.check_motor_action(motor_commands, control_mode)
+ except safety_error.SafetyError as e:
+ logging.info("A safety error occurred: %s", e)
+ self._is_safe = False
+ return
+ motor_commands = np.asarray(motor_commands)
+
+ q, qdot = self._GetPDObservation()
+ qdot_true = self.GetTrueMotorVelocities()
+ actual_torque, observed_torque = self._motor_model.convert_to_torque(
+ motor_commands, q, qdot, qdot_true, control_mode)
+
+ # May turn off the motor
+ self._ApplyOverheatProtection(actual_torque)
+
+ # The torque is already in the observation space because we use
+ # GetMotorAngles and GetMotorVelocities.
+ self._observed_motor_torques = observed_torque
+
+ # Transform into the motor space when applying the torque.
+ self._applied_motor_torque = np.multiply(actual_torque,
+ self._motor_direction)
+ motor_ids = []
+ motor_torques = []
+
+ for motor_id, motor_torque, motor_enabled in zip(self._motor_id_list,
+ self._applied_motor_torque,
+ self._motor_enabled_list):
+ if motor_enabled:
+ motor_ids.append(motor_id)
+ motor_torques.append(motor_torque)
+ else:
+ motor_ids.append(motor_id)
+ motor_torques.append(0)
+ self._SetMotorTorqueByIds(motor_ids, motor_torques)
+
+ def ConvertFromLegModel(self, actions):
+ """Convert the actions that use leg model to the real motor actions.
+
+ Args:
+ actions: The theta, phi of the leg model.
+
+ Returns:
+ The eight desired motor angles that can be used in ApplyActions().
+ """
+ motor_angle = copy.deepcopy(actions)
+ scale_for_singularity = 1
+ offset_for_singularity = 1.5
+ half_num_motors = self.num_motors // 2
+ quater_pi = math.pi / 4
+ for i in range(self.num_motors):
+ action_idx = i // 2
+ forward_backward_component = (
+ -scale_for_singularity * quater_pi *
+ (actions[action_idx + half_num_motors] + offset_for_singularity))
+ extension_component = (-1)**i * quater_pi * actions[action_idx]
+ if i >= half_num_motors:
+ extension_component = -extension_component
+ motor_angle[i] = (
+ math.pi + forward_backward_component + extension_component)
+ return motor_angle
+
+ def GetBaseMassesFromURDF(self):
+ """Get the mass of the base from the URDF file."""
+ return self._base_mass_urdf
+
+ def GetBaseInertiasFromURDF(self):
+ """Get the inertia of the base from the URDF file."""
+ return self._base_inertia_urdf
+
+ def GetLegMassesFromURDF(self):
+ """Get the mass of the legs from the URDF file."""
+ return self._leg_masses_urdf
+
+ def GetLegInertiasFromURDF(self):
+ """Get the inertia of the legs from the URDF file."""
+ return self._leg_inertia_urdf
+
+ def SetBaseMasses(self, base_mass):
+ """Set the mass of minitaur's base.
+
+ Args:
+ base_mass: A list of masses of each body link in CHASIS_LINK_IDS. The
+ length of this list should be the same as the length of CHASIS_LINK_IDS.
+
+ Raises:
+ ValueError: It is raised when the length of base_mass is not the same as
+ the length of self._chassis_link_ids.
+ """
+ if len(base_mass) != len(self._chassis_link_ids):
+ raise ValueError(
+ "The length of base_mass {} and self._chassis_link_ids {} are not "
+ "the same.".format(len(base_mass), len(self._chassis_link_ids)))
+ for chassis_id, chassis_mass in zip(self._chassis_link_ids, base_mass):
+ self._pybullet_client.changeDynamics(
+ self.quadruped, chassis_id, mass=chassis_mass)
+
+ def SetLegMasses(self, leg_masses):
+ """Set the mass of the legs.
+
+ A leg includes leg_link and motor. 4 legs contain 16 links (4 links each)
+ and 8 motors. First 16 numbers correspond to link masses, last 8 correspond
+ to motor masses (24 total).
+
+ Args:
+ leg_masses: The leg and motor masses for all the leg links and motors.
+
+ Raises:
+ ValueError: It is raised when the length of masses is not equal to number
+ of links + motors.
+ """
+ if len(leg_masses) != len(self._leg_link_ids) + len(self._motor_link_ids):
+ raise ValueError("The number of values passed to SetLegMasses are "
+ "different than number of leg links and motors.")
+ for leg_id, leg_mass in zip(self._leg_link_ids, leg_masses):
+ self._pybullet_client.changeDynamics(
+ self.quadruped, leg_id, mass=leg_mass)
+ motor_masses = leg_masses[len(self._leg_link_ids):]
+ for link_id, motor_mass in zip(self._motor_link_ids, motor_masses):
+ self._pybullet_client.changeDynamics(
+ self.quadruped, link_id, mass=motor_mass)
+
+ def SetBaseInertias(self, base_inertias):
+ """Set the inertias of minitaur's base.
+
+ Args:
+ base_inertias: A list of inertias of each body link in CHASIS_LINK_IDS.
+ The length of this list should be the same as the length of
+ CHASIS_LINK_IDS.
+
+ Raises:
+ ValueError: It is raised when the length of base_inertias is not the same
+ as the length of self._chassis_link_ids and base_inertias contains
+ negative values.
+ """
+ if len(base_inertias) != len(self._chassis_link_ids):
+ raise ValueError(
+ "The length of base_inertias {} and self._chassis_link_ids {} are "
+ "not the same.".format(
+ len(base_inertias), len(self._chassis_link_ids)))
+ for chassis_id, chassis_inertia in zip(self._chassis_link_ids,
+ base_inertias):
+ for inertia_value in chassis_inertia:
+ if (np.asarray(inertia_value) < 0).any():
+ raise ValueError("Values in inertia matrix should be non-negative.")
+ self._pybullet_client.changeDynamics(
+ self.quadruped, chassis_id, localInertiaDiagonal=chassis_inertia)
+
+ def SetLegInertias(self, leg_inertias):
+ """Set the inertias of the legs.
+
+ A leg includes leg_link and motor. 4 legs contain 16 links (4 links each)
+ and 8 motors. First 16 numbers correspond to link inertia, last 8 correspond
+ to motor inertia (24 total).
+
+ Args:
+ leg_inertias: The leg and motor inertias for all the leg links and motors.
+
+ Raises:
+ ValueError: It is raised when the length of inertias is not equal to
+ the number of links + motors or leg_inertias contains negative values.
+ """
+
+ if len(leg_inertias) != len(self._leg_link_ids) + len(self._motor_link_ids):
+ raise ValueError("The number of values passed to SetLegMasses are "
+ "different than number of leg links and motors.")
+ for leg_id, leg_inertia in zip(self._leg_link_ids, leg_inertias):
+ for inertia_value in leg_inertias:
+ if (np.asarray(inertia_value) < 0).any():
+ raise ValueError("Values in inertia matrix should be non-negative.")
+ self._pybullet_client.changeDynamics(
+ self.quadruped, leg_id, localInertiaDiagonal=leg_inertia)
+
+ motor_inertias = leg_inertias[len(self._leg_link_ids):]
+ for link_id, motor_inertia in zip(self._motor_link_ids, motor_inertias):
+ for inertia_value in motor_inertias:
+ if (np.asarray(inertia_value) < 0).any():
+ raise ValueError("Values in inertia matrix should be non-negative.")
+ self._pybullet_client.changeDynamics(
+ self.quadruped, link_id, localInertiaDiagonal=motor_inertia)
+
+ def SetFootFriction(self, foot_friction):
+ """Set the lateral friction of the feet.
+
+ Args:
+ foot_friction: The lateral friction coefficient of the foot. This value is
+ shared by all four feet.
+ """
+ for link_id in self._foot_link_ids:
+ self._pybullet_client.changeDynamics(
+ self.quadruped, link_id, lateralFriction=foot_friction)
+
+ # TODO(b/73748980): Add more API's to set other contact parameters.
+ def SetFootRestitution(self, foot_restitution):
+ """Set the coefficient of restitution at the feet.
+
+ Args:
+ foot_restitution: The coefficient of restitution (bounciness) of the feet.
+ This value is shared by all four feet.
+ """
+ for link_id in self._foot_link_ids:
+ self._pybullet_client.changeDynamics(
+ self.quadruped, link_id, restitution=foot_restitution)
+
+ def SetJointFriction(self, joint_frictions):
+ for knee_joint_id, friction in zip(self._foot_link_ids, joint_frictions):
+ self._pybullet_client.setJointMotorControl2(
+ bodyIndex=self.quadruped,
+ jointIndex=knee_joint_id,
+ controlMode=self._pybullet_client.VELOCITY_CONTROL,
+ targetVelocity=0,
+ force=friction)
+
+ def GetNumKneeJoints(self):
+ return len(self._foot_link_ids)
+
+ def SetBatteryVoltage(self, voltage):
+ self._motor_model.set_voltage(voltage)
+
+ def SetMotorViscousDamping(self, viscous_damping):
+ self._motor_model.set_viscous_damping(viscous_damping)
+
+ def GetTrueObservation(self):
+ observation = []
+ observation.extend(self.GetTrueMotorAngles())
+ observation.extend(self.GetTrueMotorVelocities())
+ observation.extend(self.GetTrueMotorTorques())
+ observation.extend(self.GetTrueBaseOrientation())
+ observation.extend(self.GetTrueBaseRollPitchYawRate())
+ return observation
+
+ def ReceiveObservation(self):
+ """Receive the observation from sensors.
+
+ This function is called once per step. The observations are only updated
+ when this function is called.
+ """
+ self._joint_states = self._pybullet_client.getJointStates(
+ self.quadruped, self._motor_id_list)
+ self._base_position, orientation = (
+ self._pybullet_client.getBasePositionAndOrientation(self.quadruped))
+ # Computes the relative orientation relative to the robot's
+ # initial_orientation.
+ _, self._base_orientation = self._pybullet_client.multiplyTransforms(
+ positionA=[0, 0, 0],
+ orientationA=orientation,
+ positionB=[0, 0, 0],
+ orientationB=self._init_orientation_inv)
+ self._observation_history.appendleft(self.GetTrueObservation())
+ self._control_observation = self._GetControlObservation()
+ self.last_state_time = self._state_action_counter * self.time_step
+ if self._safety_checker:
+ try:
+ self._safety_checker.check_state()
+ except safety_error.SafetyError as e:
+ logging.info("A safety error occurred: %s", e)
+ self._is_safe = False
+
+ def _GetDelayedObservation(self, latency):
+ """Get observation that is delayed by the amount specified in latency.
+
+ Args:
+ latency: The latency (in seconds) of the delayed observation.
+
+ Returns:
+ observation: The observation which was actually latency seconds ago.
+ """
+ if latency <= 0 or len(self._observation_history) == 1:
+ observation = self._observation_history[0]
+ else:
+ n_steps_ago = int(latency / self.time_step)
+ if n_steps_ago + 1 >= len(self._observation_history):
+ return self._observation_history[-1]
+ remaining_latency = latency - n_steps_ago * self.time_step
+ blend_alpha = remaining_latency / self.time_step
+ observation = (
+ (1.0 - blend_alpha) * np.array(self._observation_history[n_steps_ago])
+ + blend_alpha * np.array(self._observation_history[n_steps_ago + 1]))
+ return observation
+
+ def _GetPDObservation(self):
+ pd_delayed_observation = self._GetDelayedObservation(self._pd_latency)
+ q = pd_delayed_observation[0:self.num_motors]
+ qdot = pd_delayed_observation[self.num_motors:2 * self.num_motors]
+ return (np.array(q), np.array(qdot))
+
+ def _GetControlObservation(self):
+ control_delayed_observation = self._GetDelayedObservation(
+ self._control_latency)
+ return control_delayed_observation
+
+ def _AddSensorNoise(self, sensor_values, noise_stdev):
+ if noise_stdev <= 0:
+ return sensor_values
+ observation = sensor_values + np.random.normal(
+ scale=noise_stdev, size=sensor_values.shape)
+ return observation
+
+ def SetControlLatency(self, latency):
+ """Set the latency of the control loop.
+
+ It measures the duration between sending an action from Nvidia TX2 and
+ receiving the observation from microcontroller.
+
+ Args:
+ latency: The latency (in seconds) of the control loop.
+ """
+ self._control_latency = latency
+
+ def GetControlLatency(self):
+ """Get the control latency.
+
+ Returns:
+ The latency (in seconds) between when the motor command is sent and when
+ the sensor measurements are reported back to the controller.
+ """
+ return self._control_latency
+
+ # TODO(b/73666007): Change the API to SetMotorPGains and SetMotorDGains.
+ def SetMotorGains(self, kp, kd):
+ """Set the gains of all motors.
+
+ These gains are PD gains for motor positional control. kp is the
+ proportional gain and kd is the derivative gain.
+
+ Args:
+ kp: proportional gain(s) of the motors.
+ kd: derivative gain(s) of the motors.
+ """
+ if isinstance(kp, (collections.Sequence, np.ndarray)):
+ self._motor_kps = np.asarray(kp)
+ else:
+ self._motor_kps = np.full(self.num_motors, kp)
+
+ if isinstance(kd, (collections.Sequence, np.ndarray)):
+ self._motor_kds = np.asarray(kd)
+ else:
+ self._motor_kds = np.full(self.num_motors, kd)
+
+ self._motor_model.set_motor_gains(kp, kd)
+
+ # TODO(b/73666007): Change the API to GetMotorPGains and GetMotorDGains.
+ def GetMotorGains(self):
+ """Get the gains of the motor.
+
+ Returns:
+ The proportional gain.
+ The derivative gain.
+ """
+ return self._motor_kps, self._motor_kds
+
+ def GetMotorPositionGains(self):
+ """Get the position gains of the motor.
+
+ Returns:
+ The proportional gain.
+ """
+ return self._motor_kps
+
+ def GetMotorVelocityGains(self):
+ """Get the velocity gains of the motor.
+
+ Returns:
+ The derivative gain.
+ """
+ return self._motor_kds
+
+ def SetMotorStrengthRatio(self, ratio):
+ """Set the strength of all motors relative to the default value.
+
+ Args:
+ ratio: The relative strength. A scalar range from 0.0 to 1.0.
+ """
+ self._motor_model.set_strength_ratios([ratio] * self.num_motors)
+
+ def SetMotorStrengthRatios(self, ratios):
+ """Set the strength of each motor relative to the default value.
+
+ Args:
+ ratios: The relative strength. A numpy array ranging from 0.0 to 1.0.
+ """
+ self._motor_model.set_strength_ratios(ratios)
+
+ def SetTimeSteps(self, action_repeat, simulation_step):
+ """Set the time steps of the control and simulation.
+
+ Args:
+ action_repeat: The number of simulation steps that the same action is
+ repeated.
+ simulation_step: The simulation time step.
+ """
+ self.time_step = simulation_step
+ self._action_repeat = action_repeat
+
+ def _GetMotorNames(self):
+ return MOTOR_NAMES
+
+ def _GetDefaultInitPosition(self):
+ """Returns the init position of the robot.
+
+ It can be either 1) origin (INIT_POSITION), 2) origin with a rack
+ (INIT_RACK_POSITION), or 3) the previous position.
+ """
+ # If we want continuous resetting and is not the first episode.
+ if self._reset_at_current_position and self._observation_history:
+ x, y, _ = self.GetBasePosition()
+ _, _, z = INIT_POSITION
+ return [x, y, z]
+
+ if self._on_rack:
+ return INIT_RACK_POSITION
+ else:
+ return INIT_POSITION
+
+ def _GetDefaultInitOrientation(self):
+ """Returns the init position of the robot.
+
+ It can be either 1) INIT_ORIENTATION or 2) the previous rotation in yaw.
+ """
+ # If we want continuous resetting and is not the first episode.
+ if self._reset_at_current_position and self._observation_history:
+ _, _, yaw = self.GetBaseRollPitchYaw()
+ return self._pybullet_client.getQuaternionFromEuler([0.0, 0.0, yaw])
+ return INIT_ORIENTATION
+
+ @property
+ def chassis_link_ids(self):
+ return self._chassis_link_ids
+
+ def SetAllSensors(self, sensors):
+ """set all sensors to this robot and move the ownership to this robot.
+
+ Args:
+ sensors: a list of sensors to this robot.
+ """
+ for s in sensors:
+ s.set_robot(self)
+ self._sensors = sensors
+
+ def GetAllSensors(self):
+ """get all sensors associated with this robot.
+
+ Returns:
+ sensors: a list of all sensors.
+ """
+ return self._sensors
+
+ def GetSensor(self, name):
+ """get the first sensor with the given name.
+
+ This function return None if a sensor with the given name does not exist.
+
+ Args:
+ name: the name of the sensor we are looking
+
+ Returns:
+ sensor: a sensor with the given name. None if not exists.
+ """
+ for s in self._sensors:
+ if s.get_name() == name:
+ return s
+ return None
+
+ @property
+ def is_safe(self):
+ return self._is_safe
+
+ @property
+ def last_action(self):
+ return self._last_action
+
+ def ProcessAction(self, action, substep_count):
+ """If enabled, interpolates between the current and previous actions.
+
+ Args:
+ action: current action.
+ substep_count: the step count should be between [0, self.__action_repeat).
+
+ Returns:
+ If interpolation is enabled, returns interpolated action depending on
+ the current action repeat substep.
+ """
+ if self._enable_action_interpolation:
+ if self._last_action is not None:
+ prev_action = self._last_action
+ else:
+ prev_action = self.GetMotorAngles()
+
+ lerp = float(substep_count + 1) / self._action_repeat
+ proc_action = prev_action + lerp * (action - prev_action)
+ else:
+ proc_action = action
+
+ return proc_action
+
+ def _BuildActionFilter(self):
+ sampling_rate = 1 / (self.time_step * self._action_repeat)
+ num_joints = self.GetActionDimension()
+ a_filter = action_filter.ActionFilterButter(
+ sampling_rate=sampling_rate, num_joints=num_joints)
+ return a_filter
+
+ def _ResetActionFilter(self):
+ self._action_filter.reset()
+ return
+
+ def _FilterAction(self, action):
+ # initialize the filter history, since resetting the filter will fill
+ # the history with zeros and this can cause sudden movements at the start
+ # of each episode
+ if self._step_counter == 0:
+ default_action = self.GetMotorAngles()
+ self._action_filter.init_history(default_action)
+
+ filtered_action = self._action_filter.filter(action)
+ return filtered_action
+
+ @property
+ def pybullet_client(self):
+ return self._pybullet_client
+
+ @property
+ def joint_states(self):
+ return self._joint_states
+
+ @classmethod
+ def GetConstants(cls):
+ del cls
+ return minitaur_constants
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/minitaur_constants.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/minitaur_constants.py
new file mode 100644
index 000000000..9a9ed2e69
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/minitaur_constants.py
@@ -0,0 +1,62 @@
+# Lint as: python3
+"""Defines the minitaur robot related constants and URDF specs."""
+
+import collections
+import math
+
+import gin
+
+MINITAUR_URDF_PATH = "quadruped/minitaur_rainbow_dash.urdf"
+
+INIT_POSITION = (0, 0, 0.2)
+INIT_RACK_POSITION = (0, 0, 1)
+INIT_ORIENTATION_QUAT = (0, 0, 0, 1)
+INIT_ORIENTATION_RPY = (0, 0, 0)
+
+NUM_LEGS = 4
+
+JOINT_NAMES = ("motor_front_leftL_joint", "motor_front_leftR_joint",
+ "motor_back_leftL_joint", "motor_back_leftR_joint",
+ "motor_front_rightL_joint", "motor_front_rightR_joint",
+ "motor_back_rightL_joint", "motor_back_rightR_joint")
+
+INIT_JOINT_ANGLES = collections.OrderedDict(
+ zip(JOINT_NAMES, [math.pi / 2, math.pi / 2] * NUM_LEGS))
+
+# Used to convert the robot SDK joint angles to URDF joint angles.
+JOINT_DIRECTIONS = collections.OrderedDict(
+ zip(JOINT_NAMES, (-1, -1, -1, -1, 1, 1, 1, 1)))
+
+# Used to convert the robot SDK joint angles to URDF joint angles.
+JOINT_OFFSETS = collections.OrderedDict(
+ zip(JOINT_NAMES, (0, 0, 0, 0, 0, 0, 0, 0)))
+
+LEG_ORDER = ["front_left", "back_left", "front_right", "back_right"]
+
+END_EFFECTOR_NAMES = (
+ "knee_front_rightR_joint",
+ "knee_front_leftL_joint",
+ "knee_back_rightR_joint",
+ "knee_back_leftL_joint",
+)
+
+MOTOR_NAMES = JOINT_NAMES
+MOTOR_GROUP = collections.OrderedDict((("body_motors", JOINT_NAMES),))
+
+KNEE_CONSTRAINT_POINT_LONG = [0, 0.0045, 0.088]
+KNEE_CONSTRAINT_POINT_SHORT = [0, 0.0045, 0.100]
+
+# Add the gin constants to be used for gin binding in config.
+gin.constant("minitaur_constants.MINITAUR_URDF_PATH", MINITAUR_URDF_PATH)
+gin.constant("minitaur_constants.MINITAUR_INIT_POSITION", INIT_POSITION)
+gin.constant("minitaur_constants.MINITAUR_INIT_ORIENTATION_QUAT",
+ INIT_ORIENTATION_QUAT)
+gin.constant("minitaur_constants.MINITAUR_INIT_ORIENTATION_RPY",
+ INIT_ORIENTATION_RPY)
+gin.constant("minitaur_constants.MINITAUR_INIT_JOINT_ANGLES", INIT_JOINT_ANGLES)
+gin.constant("minitaur_constants.MINITAUR_JOINT_DIRECTIONS", JOINT_DIRECTIONS)
+gin.constant("minitaur_constants.MINITAUR_JOINT_OFFSETS", JOINT_OFFSETS)
+gin.constant("minitaur_constants.MINITAUR_MOTOR_NAMES", MOTOR_NAMES)
+gin.constant("minitaur_constants.MINITAUR_END_EFFECTOR_NAMES",
+ END_EFFECTOR_NAMES)
+gin.constant("minitaur_constants.MINITAUR_MOTOR_GROUP", MOTOR_GROUP)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/minitaur_motor.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/minitaur_motor.py
new file mode 100644
index 000000000..2ec0c927e
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/minitaur_motor.py
@@ -0,0 +1,171 @@
+"""This file implements an accurate motor model."""
+
+import numpy as np
+
+from pybullet_envs.minitaur.robots import robot_config
+
+VOLTAGE_CLIPPING = 50
+# TODO(b/73728631): Clamp the pwm signal instead of the OBSERVED_TORQUE_LIMIT.
+OBSERVED_TORQUE_LIMIT = 5.7
+MOTOR_VOLTAGE = 16.0
+MOTOR_RESISTANCE = 0.186
+MOTOR_TORQUE_CONSTANT = 0.0954
+MOTOR_VISCOUS_DAMPING = 0
+MOTOR_SPEED_LIMIT = MOTOR_VOLTAGE / (
+ MOTOR_VISCOUS_DAMPING + MOTOR_TORQUE_CONSTANT)
+NUM_MOTORS = 8
+MOTOR_POS_LB = 0.5
+MOTOR_POS_UB = 2.5
+
+
+class MotorModel(object):
+ """The accurate motor model, which is based on the physics of DC motors.
+
+ The motor model support two types of control: position control and torque
+ control. In position control mode, a desired motor angle is specified, and a
+ torque is computed based on the internal motor model. When the torque control
+ is specified, a pwm signal in the range of [-1.0, 1.0] is converted to the
+ torque.
+
+ The internal motor model takes the following factors into consideration:
+ pd gains, viscous friction, back-EMF voltage and current-torque profile.
+ """
+
+ def __init__(self,
+ kp=1.2,
+ kd=0,
+ torque_limits=None,
+ motor_control_mode=robot_config.MotorControlMode.POSITION):
+ self._kp = kp
+ self._kd = kd
+ self._torque_limits = torque_limits
+ self._motor_control_mode = motor_control_mode
+ self._resistance = MOTOR_RESISTANCE
+ self._voltage = MOTOR_VOLTAGE
+ self._torque_constant = MOTOR_TORQUE_CONSTANT
+ self._viscous_damping = MOTOR_VISCOUS_DAMPING
+ self._current_table = [0, 10, 20, 30, 40, 50, 60]
+ self._torque_table = [0, 1, 1.9, 2.45, 3.0, 3.25, 3.5]
+ self._strength_ratios = [1.0] * NUM_MOTORS
+
+ def set_strength_ratios(self, ratios):
+ """Set the strength of each motors relative to the default value.
+
+ Args:
+ ratios: The relative strength of motor output. A numpy array ranging from
+ 0.0 to 1.0.
+ """
+ self._strength_ratios = np.array(ratios)
+
+ def set_motor_gains(self, kp, kd):
+ """Set the gains of all motors.
+
+ These gains are PD gains for motor positional control. kp is the
+ proportional gain and kd is the derivative gain.
+
+ Args:
+ kp: proportional gain of the motors.
+ kd: derivative gain of the motors.
+ """
+ self._kp = kp
+ self._kd = kd
+
+ def set_voltage(self, voltage):
+ self._voltage = voltage
+
+ def get_voltage(self):
+ return self._voltage
+
+ def set_viscous_damping(self, viscous_damping):
+ self._viscous_damping = viscous_damping
+
+ def get_viscous_dampling(self):
+ return self._viscous_damping
+
+ def convert_to_torque(self,
+ motor_commands,
+ motor_angle,
+ motor_velocity,
+ true_motor_velocity,
+ motor_control_mode=None):
+ """Convert the commands (position control or pwm control) to torque.
+
+ Args:
+ motor_commands: The desired motor angle if the motor is in position
+ control mode. The pwm signal if the motor is in torque control mode.
+ motor_angle: The motor angle observed at the current time step. It is
+ actually the true motor angle observed a few milliseconds ago (pd
+ latency).
+ motor_velocity: The motor velocity observed at the current time step, it
+ is actually the true motor velocity a few milliseconds ago (pd latency).
+ true_motor_velocity: The true motor velocity. The true velocity is used to
+ compute back EMF voltage and viscous damping.
+ motor_control_mode: A MotorControlMode enum.
+
+ Returns:
+ actual_torque: The torque that needs to be applied to the motor.
+ observed_torque: The torque observed by the sensor.
+ """
+ if not motor_control_mode:
+ motor_control_mode = self._motor_control_mode
+
+ if (motor_control_mode is robot_config.MotorControlMode.TORQUE) or (
+ motor_control_mode is robot_config.MotorControlMode.HYBRID):
+ raise ValueError(
+ "{} is not a supported motor control mode".format(motor_control_mode))
+
+ kp = self._kp
+ kd = self._kd
+
+ if motor_control_mode is robot_config.MotorControlMode.PWM:
+ # The following implements a safety controller that softly enforces the
+ # joint angles to remain within safe region: If PD controller targeting
+ # the positive (negative) joint limit outputs a negative (positive)
+ # signal, the corresponding joint violates the joint constraint, so
+ # we should add the PD output to motor_command to bring it back to the
+ # safe region.
+ pd_max = -1 * kp * (motor_angle - MOTOR_POS_UB) - kd / 2. * motor_velocity
+ pd_min = -1 * kp * (motor_angle - MOTOR_POS_LB) - kd / 2. * motor_velocity
+ pwm = motor_commands + np.minimum(pd_max, 0) + np.maximum(pd_min, 0)
+ else:
+ pwm = -1 * kp * (motor_angle - motor_commands) - kd * motor_velocity
+ pwm = np.clip(pwm, -1.0, 1.0)
+ return self._convert_to_torque_from_pwm(pwm, true_motor_velocity)
+
+ def _convert_to_torque_from_pwm(self, pwm, true_motor_velocity):
+ """Convert the pwm signal to torque.
+
+ Args:
+ pwm: The pulse width modulation.
+ true_motor_velocity: The true motor velocity at the current moment. It is
+ used to compute the back EMF voltage and the viscous damping.
+
+ Returns:
+ actual_torque: The torque that needs to be applied to the motor.
+ observed_torque: The torque observed by the sensor.
+ """
+ observed_torque = np.clip(
+ self._torque_constant *
+ (np.asarray(pwm) * self._voltage / self._resistance),
+ -OBSERVED_TORQUE_LIMIT, OBSERVED_TORQUE_LIMIT)
+ if self._torque_limits is not None:
+ observed_torque = np.clip(observed_torque, -1.0 * self._torque_limits,
+ self._torque_limits)
+
+ # Net voltage is clipped at 50V by diodes on the motor controller.
+ voltage_net = np.clip(
+ np.asarray(pwm) * self._voltage -
+ (self._torque_constant + self._viscous_damping) *
+ np.asarray(true_motor_velocity), -VOLTAGE_CLIPPING, VOLTAGE_CLIPPING)
+ current = voltage_net / self._resistance
+ current_sign = np.sign(current)
+ current_magnitude = np.absolute(current)
+ # Saturate torque based on empirical current relation.
+ actual_torque = np.interp(current_magnitude, self._current_table,
+ self._torque_table)
+ actual_torque = np.multiply(current_sign, actual_torque)
+ actual_torque = np.multiply(self._strength_ratios, actual_torque)
+ if self._torque_limits is not None:
+ actual_torque = np.clip(actual_torque, -1.0 * self._torque_limits,
+ self._torque_limits)
+ return actual_torque, observed_torque
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/minitaur_motor_model_v2.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/minitaur_motor_model_v2.py
new file mode 100644
index 000000000..40927cc7c
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/minitaur_motor_model_v2.py
@@ -0,0 +1,145 @@
+# Lint as: python3
+"""This file implements an accurate motor model."""
+
+from typing import Tuple
+
+import gin
+import numpy as np
+
+from pybullet_envs.minitaur.robots import hybrid_motor_model
+from pybullet_envs.minitaur.robots import robot_config
+
+VOLTAGE_CLIPPING = 50
+# TODO(b/73728631): Clamp the pwm signal instead of the OBSERVED_TORQUE_LIMIT.
+OBSERVED_TORQUE_LIMIT = 5.7
+MOTOR_VOLTAGE = 16.0
+MOTOR_RESISTANCE = 0.186
+MOTOR_TORQUE_CONSTANT = 0.0954
+MOTOR_VISCOUS_DAMPING = 0
+MOTOR_POS_LB = 0.5
+MOTOR_POS_UB = 2.5
+
+
+@gin.configurable
+class MinitaurMotorModel(hybrid_motor_model.HybridMotorModel):
+ """The accurate motor model, which is based on the physics of DC motors.
+
+ The motor model support two types of control: position control and torque
+ control. In position control mode, a desired motor angle is specified, and a
+ torque is computed based on the internal motor model. When the torque control
+ is specified, a pwm signal in the range of [-1.0, 1.0] is converted to the
+ torque.
+
+ The internal motor model takes the following factors into consideration:
+ pd gains, viscous friction, back-EMF voltage and current-torque profile.
+ """
+
+ def __init__(self,
+ num_motors: int,
+ voltage_clipping: float = VOLTAGE_CLIPPING,
+ observed_torque_limit: float = OBSERVED_TORQUE_LIMIT,
+ motor_voltage: float = MOTOR_VOLTAGE,
+ motor_resistance: float = MOTOR_RESISTANCE,
+ motor_torque_constant: float = MOTOR_TORQUE_CONSTANT,
+ motor_viscous_damping: float = MOTOR_VISCOUS_DAMPING,
+ motor_pos_lb: float = MOTOR_POS_LB,
+ motor_pos_ub: float = MOTOR_POS_UB,
+ **kwargs):
+ super(MinitaurMotorModel, self).__init__(num_motors, **kwargs)
+ self._voltage_clipping = voltage_clipping
+ self._observed_torque_limit = observed_torque_limit
+ self._voltage = motor_voltage
+ self._resistance = motor_resistance
+ self._torque_constant = motor_torque_constant
+ self._viscous_damping = motor_viscous_damping
+ self._motor_pos_lb = motor_pos_lb
+ self._motor_pos_ub = motor_pos_ub
+ self._current_table = [0, 10, 20, 30, 40, 50, 60]
+ self._torque_table = [0, 1, 1.9, 2.45, 3.0, 3.25, 3.5]
+
+ def set_voltage(self, voltage):
+ self._voltage = voltage
+
+ def get_voltage(self):
+ return self._voltage
+
+ def set_viscous_damping(self, viscous_damping):
+ self._viscous_damping = viscous_damping
+
+ def get_viscous_dampling(self):
+ return self._viscous_damping
+
+ def get_motor_torques(
+ self,
+ motor_commands: np.ndarray,
+ motor_control_mode=None) -> Tuple[np.ndarray, np.ndarray]:
+ """Convert the commands (position control or pwm control) to torque.
+
+ Args:
+ motor_commands: The desired motor angle if the motor is in position
+ control mode. The pwm signal if the motor is in torque control mode.
+ motor_control_mode: A MotorControlMode enum.
+
+ Returns:
+ actual_torque: The torque that needs to be applied to the motor.
+ observed_torque: The torque observed by the sensor.
+ """
+ if not motor_control_mode:
+ motor_control_mode = self._motor_control_mode
+
+ if (motor_control_mode is robot_config.MotorControlMode.TORQUE) or (
+ motor_control_mode is robot_config.MotorControlMode.HYBRID):
+ raise ValueError(
+ "{} is not a supported motor control mode".format(motor_control_mode))
+
+ motor_angle, motor_velocity = self.get_motor_states()
+ _, true_motor_velocity = self.get_motor_states(latency=0)
+
+ kp = self._kp
+ kd = self._kd
+
+ pwm = -1 * kp * (motor_angle - motor_commands) - kd * motor_velocity
+ pwm = np.clip(pwm, -1.0, 1.0)
+ return self._convert_to_torque_from_pwm(pwm, true_motor_velocity)
+
+ def _convert_to_torque_from_pwm(self, pwm: np.ndarray,
+ true_motor_velocity: np.ndarray):
+ """Convert the pwm signal to torque.
+
+ Args:
+ pwm: The pulse width modulation.
+ true_motor_velocity: The true motor velocity at the current moment. It is
+ used to compute the back EMF voltage and the viscous damping.
+
+ Returns:
+ actual_torque: The torque that needs to be applied to the motor.
+ observed_torque: The torque observed by the sensor.
+ """
+ observed_torque = np.clip(
+ self._torque_constant *
+ (np.asarray(pwm) * self._voltage / self._resistance),
+ -self._observed_torque_limit, self._observed_torque_limit)
+ if (self._torque_lower_limits is not None or
+ self._torque_upper_limits is not None):
+ observed_torque = np.clip(observed_torque, self._torque_lower_limits,
+ self._torque_upper_limits)
+
+ # Net voltage is clipped at 50V by diodes on the motor controller.
+ voltage_net = np.clip(
+ np.asarray(pwm) * self._voltage -
+ (self._torque_constant + self._viscous_damping) *
+ np.asarray(true_motor_velocity), -self._voltage_clipping,
+ self._voltage_clipping)
+ current = voltage_net / self._resistance
+ current_sign = np.sign(current)
+ current_magnitude = np.absolute(current)
+ # Saturate torque based on empirical current relation.
+ actual_torque = np.interp(current_magnitude, self._current_table,
+ self._torque_table)
+ actual_torque = np.multiply(current_sign, actual_torque)
+ actual_torque = np.multiply(self._strength_ratios, actual_torque)
+ if (self._torque_lower_limits is not None or
+ self._torque_upper_limits is not None):
+ actual_torque = np.clip(actual_torque, self._torque_lower_limits,
+ self._torque_upper_limits)
+ return observed_torque, actual_torque
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/minitaur_v2.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/minitaur_v2.py
new file mode 100644
index 000000000..34d97a50d
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/minitaur_v2.py
@@ -0,0 +1,122 @@
+# Lint as: python3
+"""Pybullet simulation of Minitaur robot."""
+import math
+from typing import Dict, Tuple, Union, Text
+
+from absl import logging
+import gin
+
+from pybullet_envs.minitaur.robots import minitaur_constants
+from pybullet_envs.minitaur.robots import quadruped_base
+from pybullet_envs.minitaur.robots import robot_config
+from pybullet_envs.minitaur.robots import robot_urdf_loader
+
+
+@gin.configurable
+class Minitaur(quadruped_base.QuadrupedBase):
+ """Minitaur simulation model in pyBullet."""
+
+ def _pre_load(self):
+ try:
+ use_constrained_base = gin.query_parameter(
+ "robot_urdf_loader.RobotUrdfLoader.constrained_base")
+ except ValueError:
+ use_constrained_base = False
+ if use_constrained_base:
+ logging.warn(
+ "use_constrained_base is currently not compatible with Minitaur's "
+ "leg constraints."
+ )
+
+ self._urdf_loader = robot_urdf_loader.RobotUrdfLoader(
+ pybullet_client=self._pybullet_client,
+ enable_self_collision=True,
+ urdf_path=minitaur_constants.MINITAUR_URDF_PATH,
+ init_base_position=minitaur_constants.INIT_POSITION,
+ init_base_orientation_quaternion=minitaur_constants
+ .INIT_ORIENTATION_QUAT,
+ init_base_orientation_rpy=minitaur_constants.INIT_ORIENTATION_RPY,
+ init_joint_angles=minitaur_constants.INIT_JOINT_ANGLES,
+ joint_offsets=minitaur_constants.JOINT_OFFSETS,
+ joint_directions=minitaur_constants.JOINT_DIRECTIONS,
+ motor_names=minitaur_constants.MOTOR_NAMES,
+ end_effector_names=minitaur_constants.END_EFFECTOR_NAMES,
+ user_group=minitaur_constants.MOTOR_GROUP,
+ )
+
+ def _on_load(self):
+ """Add hinge constraint for Minitaur's diamond shaped leg after loading."""
+ half_pi = math.pi / 2.0
+ knee_angle = -2.1834
+ for (leg_id, leg_position) in enumerate(minitaur_constants.LEG_ORDER):
+ self._pybullet_client.resetJointState(
+ self._urdf_loader.robot_id,
+ self._joint_id_dict["motor_" + leg_position + "L_joint"],
+ self._motor_directions[2 * leg_id] * half_pi,
+ targetVelocity=0)
+ self._pybullet_client.resetJointState(
+ self._urdf_loader.robot_id,
+ self._joint_id_dict["knee_" + leg_position + "L_joint"],
+ self._motor_directions[2 * leg_id] * knee_angle,
+ targetVelocity=0)
+ self._pybullet_client.resetJointState(
+ self._urdf_loader.robot_id,
+ self._joint_id_dict["motor_" + leg_position + "R_joint"],
+ self._motor_directions[2 * leg_id + 1] * half_pi,
+ targetVelocity=0)
+ self._pybullet_client.resetJointState(
+ self._urdf_loader.robot_id,
+ self._joint_id_dict["knee_" + leg_position + "R_joint"],
+ self._motor_directions[2 * leg_id + 1] * knee_angle,
+ targetVelocity=0)
+
+ if leg_id < 2:
+ self._pybullet_client.createConstraint(
+ self._urdf_loader.robot_id,
+ self._joint_id_dict["knee_" + leg_position + "R_joint"],
+ self._urdf_loader.robot_id,
+ self._joint_id_dict["knee_" + leg_position + "L_joint"],
+ self._pybullet_client.JOINT_POINT2POINT, [0, 0, 0],
+ minitaur_constants.KNEE_CONSTRAINT_POINT_SHORT,
+ minitaur_constants.KNEE_CONSTRAINT_POINT_LONG)
+ else:
+ self._pybullet_client.createConstraint(
+ self._urdf_loader.robot_id,
+ self._joint_id_dict["knee_" + leg_position + "R_joint"],
+ self._urdf_loader.robot_id,
+ self._joint_id_dict["knee_" + leg_position + "L_joint"],
+ self._pybullet_client.JOINT_POINT2POINT, [0, 0, 0],
+ minitaur_constants.KNEE_CONSTRAINT_POINT_LONG,
+ minitaur_constants.KNEE_CONSTRAINT_POINT_SHORT)
+ self.receive_observation()
+
+ def _reset_joint_angles(self,
+ joint_angles: Union[Tuple[float], Dict[Text,
+ float]] = None,
+ num_reset_steps: int = 100):
+ """Resets joint angles of the robot.
+
+ Note that since Minitaur has additional leg constraints on the end
+ effectors, directly setting joint angles will lead to constraint violation.
+ Instead, we apply motor commands to move the motors to the desired position.
+
+ Args:
+ joint_angles: the desired joint angles to reset to.
+ num_reset_steps: number of reset steps.
+ """
+ if joint_angles is None:
+ joint_angles = minitaur_constants.INIT_JOINT_ANGLES
+ actions = joint_angles
+ if isinstance(joint_angles, dict):
+ actions = [
+ joint_angles[joint_name]
+ for joint_name in minitaur_constants.JOINT_NAMES
+ ]
+
+ # TODO(b/157786642): since the simulation clock is not stepped here, this
+ # reset behaves slightly different compared to the old robot class.
+ for _ in range(num_reset_steps):
+ self.apply_action(
+ actions, motor_control_mode=robot_config.MotorControlMode.POSITION)
+ self._pybullet_client.stepSimulation()
+ self.receive_observation()
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/object_controller.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/object_controller.py
new file mode 100644
index 000000000..2e7dbae81
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/object_controller.py
@@ -0,0 +1,810 @@
+# Lint as: python3
+"""Module for controllers of autonomous objects."""
+
+import abc
+import bisect
+import enum
+from typing import Any, Dict, Optional, Sequence, Text, Tuple, Union
+
+from absl import logging
+import dataclasses
+import gin
+import numpy as np
+
+
+# A constant to be passed into act as parameter t for initial value.
+INIT_TIME = -1.0
+
+# Distance that is deemed close enough in ChaseController.
+_EPS_DISTANCE = 1e-4
+
+ControllerOutput = Tuple[np.ndarray, np.ndarray, Dict[Text, Any]]
+
+ANIMATION_FRAME_NUMBER_KEY = "animation_frame_number"
+
+
+class ControllerBase(metaclass=abc.ABCMeta):
+ """Base class of object controllers.
+
+ Controller is similar to a policy in that its output controls autonomous
+ object just as policy output controls agent. To reflect this similarity,
+ get_action(), the function that "commands" to autonomous object, is named
+ similar to the counterpart in policy.
+ """
+
+ @abc.abstractmethod
+ def get_action(self,
+ time_sec: float,
+ observations: Dict[Text, Any]) -> ControllerOutput:
+ """Returns position, orientation and pose based on time and observations.
+
+ Args:
+ time_sec: Time since simulation reset in seconds. If time < 0, returns
+ initial values and ignores observations.
+ observations: A dict of all observations.
+
+ Returns:
+ Position, orientation and an extra info dict for robot joints, human
+ skeletal pose, etc.
+ """
+
+
+@gin.configurable
+class StationaryController(ControllerBase):
+ """Controller that keeps constant position and orientation."""
+
+ def __init__(self,
+ position: Sequence[float] = None,
+ orientation: Sequence[float] = None):
+ self._position = np.array(position if position is not None else (0, 0, 0))
+ self._orientation = np.array(
+ orientation if orientation is not None else (0, 0, 0, 1))
+
+ def get_action(self,
+ t: float,
+ observations: Dict[Text, Any]) -> ControllerOutput:
+ """Returns constant position orientation."""
+ del t, observations
+ return self._position, self._orientation, {}
+
+
+@gin.configurable
+class CircularMotionController(ControllerBase):
+ """Controller for circular motion.
+
+ The motion trajectory goes around a center in a circle in xy-plane.
+ """
+
+ def __init__(self,
+ center: Sequence[float],
+ radius: float,
+ angular_velocity: float = np.pi,
+ face_travel_direction: bool = False):
+ """Constructor.
+
+ Args:
+ center: Center of circular motion, [x, y, z] in meters.
+ radius: Radius of the circle in meters.
+ angular_velocity: Angular velocity of motion, unit rad/s, e.g. pi means
+ completing a circle in 2 sec.
+ face_travel_direction: If True, object will face direction of motion.
+ """
+
+ self._center = np.array(center)
+ self._radius = radius
+ self._angular_velocity = angular_velocity
+ self._face_travel_direction = face_travel_direction
+
+ def get_action(self,
+ t: float,
+ observations: Dict[Text, Any]) -> ControllerOutput:
+ """Returns position on the circle based on time and constant orientation."""
+ del observations
+
+ t = max(0.0, t)
+ position = np.array(
+ [np.cos(self._angular_velocity * t),
+ np.sin(self._angular_velocity * t),
+ 0]) * self._radius + self._center
+ if self._face_travel_direction:
+ yaw = self._angular_velocity * t + (
+ np.sign(self._angular_velocity) * np.pi / 2)
+ orientation = np.array((0, 0, np.sin(yaw / 2), np.cos(yaw / 2)))
+ else:
+ orientation = np.array((0, 0, 0, 1))
+ return position, orientation, {}
+
+
+class PatrolRepeatMode(enum.Enum):
+ """Enums that defines trajectory repeat mode for patrol type controller."""
+
+ # Trajectory does not repeat. For 3 points a, b, c, the trajectory moves
+ # along a -> b -> c and then stops at c forever.
+ NO_REPEAT = 0
+
+ # Trajectory repeats as a loop. For 3 points a, b, c, the trajectory moves
+ # along a -> b -> c -> a -> b ...
+ LOOP = 1
+
+ # Trajectory repeats back tracking previous point first. For 3 points a, b, c,
+ # the trajectory moves along a -> b -> c -> b -> a -> b -> c ...
+ BACK_TRACK = 2
+
+ # Trajectory repeats by resetting to the initial position after reaching end.
+ # For 3 points a, b, c, the trajectory moves a -> b -> c then immediately
+ # jumps back to a before continue moving along a -> b -> c again.
+ RESET = 3
+
+
+@dataclasses.dataclass
+class PatrolSegmentData:
+ """A data class that describes a patrol segment."""
+
+ # Time in a single cycle to start this segment, range [0, cycle_time).
+ start_time: float
+
+ # Segment start position.
+ start_position: np.ndarray
+
+ # Segment velocity vector.
+ velocity: np.ndarray
+
+ # Orientation quaternion of this segment.
+ orientation: np.ndarray
+
+
+@gin.configurable
+class WayPointPatrolController(ControllerBase):
+ """Controller for patrolling along define waypoints."""
+
+ def __init__(self,
+ points: Sequence[Sequence[float]],
+ yaw_angle: float = 0,
+ face_travel_direction: bool = True,
+ repeat_mode: Union[
+ PatrolRepeatMode, Text] = PatrolRepeatMode.NO_REPEAT,
+ speed_mps: Optional[float] = 1.0,
+ time_points: Optional[Sequence[float]] = None):
+ """Constructor.
+
+ Args:
+ points: List of waypoints, shape Nx3 or Nx2, N is number of points.
+ yaw_angle: Yaw angle of the object in radians.
+ face_travel_direction: If True, yaw angle 'zero' will be redefined to be
+ object's travel direction. Setting yaw_angle to zero with
+ face_travel_direction == True will results in object always facing its
+ travel direction. Non-zero yaw_angle will cause additional yaw offsets.
+ repeat_mode: Behavior of object after reaching the last way point in list.
+ If the value is Text, it is converted to PatrolRepeatMode.
+ speed_mps: Speed in meters per second.
+ time_points: List of times associated with points. These times
+ represent when the object should arrive at the associated waypoint.
+ Optional, but if provided it must have the same length as 'points'.
+ If 'speed_mps' is None, then 'time_points' will be used as-is and there
+ is no maximum segment speed. If 'speed_mps' is also defined, then it
+ serves as a maximum speed value and time points which would result in a
+ segment speed above this value will be altered such that the maximum
+ segment speed is 'speed_mps'.
+ """
+ self._repeat = (repeat_mode if isinstance(repeat_mode, PatrolRepeatMode)
+ else PatrolRepeatMode[repeat_mode])
+ self._yaw_angle = yaw_angle
+ self._face_travel_direction = face_travel_direction
+ self._speed_mps = speed_mps
+
+ if len(points) < 2:
+ raise ValueError(
+ f"Need at least two points in 'points', got {len(points)}")
+
+ if time_points is not None and self._repeat is PatrolRepeatMode.LOOP:
+ raise ValueError("Time points are not compatible with LOOP mode.")
+
+ if (self._repeat is PatrolRepeatMode.NO_REPEAT or
+ self._repeat is PatrolRepeatMode.RESET):
+ augmented_points = points
+
+ if time_points is not None:
+ augmented_time_points = time_points
+ elif self._repeat is PatrolRepeatMode.LOOP:
+ augmented_points = list(points) + [points[0]]
+ elif self._repeat is PatrolRepeatMode.BACK_TRACK:
+ augmented_points = list(points) + list(reversed(points[:-1]))
+
+ if time_points is not None:
+ # Time strictly increases, so add the timepoints again on top
+ # of the last entry. We add the difference between the last time
+ # and the previous elements (in reverse order), on top of the last
+ # element where we left off.
+ augmented_time_points = list(time_points) + \
+ list(time_points[-1] + (time_points[-1] - np.array(time_points[1::-1])))
+ else:
+ raise NotImplementedError(
+ f"Repeat mode {self._repeat} is not supported yet.")
+
+ augmented_points = np.array(augmented_points)
+ # For Nx2 inputs, pad it to Nx3 with default z value of 0.
+ if augmented_points.shape[1] == 2:
+ augmented_points = np.hstack(
+ augmented_points, np.zeros(augmented_points.shape[0], 1))
+ elif augmented_points.shape[1] != 3:
+ raise ValueError("Expect 'points' to be Nx2 or Nx3.")
+
+ t = 0
+ self._segments = []
+
+ if time_points is None:
+ for from_point, to_point in zip(
+ augmented_points[:-1], augmented_points[1:]):
+ segment, t = self._get_patrol_segment_by_speed(from_point, to_point, t)
+ self._segments.append(segment)
+ else:
+ for from_point, to_point, from_time, to_time in zip(
+ augmented_points[:-1], augmented_points[1:],
+ augmented_time_points[:-1], augmented_time_points[1:]):
+ segment, t = self._get_patrol_segment_by_time(from_point, to_point, t,
+ t + (to_time - from_time))
+ self._segments.append(segment)
+
+ self._segment_times = [l.start_time for l in self._segments]
+
+ self._cycle_time = t
+
+ def _get_patrol_segment_by_time(self,
+ from_point,
+ to_point,
+ from_time,
+ to_time):
+ """Returns a PatrolSegmentData for the given points and times."""
+ unit_vector, length = self._get_vector(from_point, to_point)
+ orientation = self._get_orientation(unit_vector)
+
+ if np.isclose(to_time, from_time):
+ speed_mps = 0
+ else:
+ speed_mps = length / (to_time - from_time)
+
+ if self._speed_mps is not None:
+ speed_mps = np.min([self._speed_mps, speed_mps])
+
+ if np.isclose(0, speed_mps):
+ new_to_time = to_time
+ else:
+ new_to_time = np.max([to_time, from_time + (length / speed_mps)])
+
+ segment = PatrolSegmentData(
+ start_time=from_time,
+ start_position=np.array(from_point),
+ velocity=unit_vector * speed_mps,
+ orientation=orientation)
+
+ return segment, new_to_time
+
+ def _get_patrol_segment_by_speed(self, from_point, to_point, current_time):
+ """Returns a PatrolSegmentData for the given points and a constant speed."""
+ unit_vector, length = self._get_vector(from_point, to_point)
+ orientation = self._get_orientation(unit_vector)
+
+ segment = PatrolSegmentData(
+ start_time=current_time,
+ start_position=np.array(from_point),
+ velocity=unit_vector * self._speed_mps,
+ orientation=orientation)
+ time = current_time + length / self._speed_mps
+
+ return segment, time
+
+ def _get_vector(self, from_point, to_point):
+ """Gets the unit vector and length of a from/to point pair."""
+ vector = np.array(to_point) - np.array(from_point)
+ length = np.linalg.norm(vector)
+
+ if length == 0:
+ raise ValueError(f"Length of patrol segment equal to 0, "
+ f"from {from_point} to {to_point}.")
+ unit_vector = vector / length
+
+ return unit_vector, length
+
+ def _get_orientation(self, unit_vector):
+ """Gets the orientation quaternion given a unit vector."""
+ yaw = (np.arctan2(unit_vector[1], unit_vector[0])
+ if self._face_travel_direction else 0) + self._yaw_angle
+
+ orientation = np.array((0, 0, np.sin(yaw / 2), np.cos(yaw / 2)))
+
+ return orientation
+
+ def get_action(self,
+ t: float,
+ observations: Dict[Text, Any]) -> ControllerOutput:
+ """Returns position on, and orientation along the patrol segment."""
+ del observations
+
+ # t < 0 means initial condition, which is the same as the value at t = 0.
+ t = max(0, t)
+
+ if t > self._cycle_time:
+ t = (self._cycle_time if self._repeat is PatrolRepeatMode.NO_REPEAT
+ else np.fmod(t, self._cycle_time))
+
+ segment = self._segments[bisect.bisect_right(self._segment_times, t) - 1]
+ position = (
+ segment.start_position + segment.velocity * (t - segment.start_time))
+
+ return position, segment.orientation.copy(), {}
+
+
+@gin.configurable
+class LinearPatrolController(WayPointPatrolController):
+ """Controller for patrolling along a line segment (back and forth)."""
+
+ def __init__(self,
+ from_point: Sequence[float],
+ to_point: Sequence[float],
+ **kwargs):
+ """Constructor.
+
+ Args:
+ from_point: Starting point of motion, [x, y, z] in meters.
+ to_point: Returning point of motion, [x, y, z] in meters.
+ **kwargs: Keyword arguments to pass onto base class.
+ """
+ super().__init__([from_point, to_point],
+ repeat_mode=PatrolRepeatMode.LOOP,
+ **kwargs)
+
+
+# TODO(b/156126975): migrates this to use difference equation controller.
+@gin.configurable
+class ChaseController(ControllerBase):
+ """Controller for an object to chase another object at certain speed."""
+
+ def __init__(self,
+ self_key: Text,
+ target_key: Text,
+ initial_position: Sequence[float] = (0, 0, 0),
+ initial_orientation: Sequence[float] = (0, 0, 0, 1),
+ speed_mps: float = 1.0,
+ verbose: bool = False):
+ """Constructor.
+
+ Args:
+ self_key: Observation dict key of position of object being controlled.
+ target_key: Observation dict key of position of target object.
+ initial_position: Initial position of the object.
+ initial_orientation: Initial orientation of the object in xyzw quaternion.
+ speed_mps: Speed in meters per second, always positive.
+ verbose: If True, log details of get_action() calculation for debugging.
+ """
+ self._init_position = np.array(initial_position)
+ self._init_orientation = np.array(initial_orientation)
+ self._previous_orientation = self._init_orientation
+ if speed_mps <= 0:
+ raise ValueError(
+ f"'speed_mps' should be a positive value, got {speed_mps}.")
+ self._speed_mps = speed_mps
+
+ self._self_key = self_key
+ self._target_key = target_key
+ self._verbose = verbose
+
+ self._time_sec = 0
+
+ def get_action(self,
+ time_sec: float,
+ observations: Dict[Text, Any]) -> ControllerOutput:
+ """Returns position and orientation of the object being controlled.
+
+ Args:
+ time_sec: Time since simulation reset in seconds. If time < 0, returns
+ initial values and ignores observations.
+ observations: A dict of all observations.
+ """
+ if time_sec < 0:
+ # Initializes internal time.
+ self._time_sec = 0
+ return self._init_position.copy(), self._init_orientation.copy(), {}
+
+ self_position = observations[self._self_key]
+ target_position = observations[self._target_key]
+
+ # Calculates delta vector and projects it to xy-plane.
+ delta_vector = (target_position - self_position) * (1, 1, 0)
+ delta_t = time_sec - self._time_sec
+
+ # Advances internal time.
+ self._time_sec = time_sec
+
+ if self._verbose:
+ with np.printoptions(precision=3, suppress=True):
+ logging.info("t = %.1f, self %s: %s, target %s: %s, v: %s, dt %.1f.",
+ self._t,
+ self._self_key, observations[self._self_key],
+ self._target_key, observations[self._target_key],
+ delta_vector, delta_t)
+
+ # Avoids sigularity when it is close enough. Keeps previous orientation.
+ distance = np.linalg.norm(delta_vector)
+ if distance < _EPS_DISTANCE:
+ return target_position.copy(), self._previous_orientation.copy(), {}
+
+ unit_delta_vector = delta_vector / distance
+ new_position = (unit_delta_vector * min(self._speed_mps * delta_t, distance)
+ + self_position)
+
+ new_yaw = np.arctan2(unit_delta_vector[1], unit_delta_vector[0])
+ new_orientation = np.array((0, 0, np.sin(new_yaw / 2), np.cos(new_yaw / 2)))
+ self._previous_orientation = new_orientation
+
+ return new_position, new_orientation.copy(), {}
+
+
+@gin.configurable
+class AnimationFrameController(ControllerBase):
+ """An extra action controller to control playback of animation sequence."""
+
+ def __init__(self, fps: float = 10.0,
+ pause_between_repeat_sec: float = 0.0,
+ start_time_sec: float = 0.0):
+ """Constructor.
+
+ Args:
+ fps: Frame per second of animation.
+ pause_between_repeat_sec: Pause between repeat in second.
+ start_time_sec: The time when animation starts to play.
+ """
+ self._fps = fps
+ self._total_length = None
+ self._pause_between_repeat_sec = pause_between_repeat_sec
+ self._start_time_sec = start_time_sec
+
+ def set_total_length(self, total_length: int):
+ """Sets total animation frame length."""
+ if total_length <= 0:
+ raise ValueError(
+ f"Total number of frame must be >= 0, got {total_length}.")
+ self._total_length = total_length
+
+ def get_action(self,
+ time_sec: float,
+ observations: Dict[Text, Any]) -> ControllerOutput:
+ """Returns animation frame number with default position and orientation.
+
+ Args:
+ time_sec: Time since simulation reset in seconds. If time < 0, returns
+ initial values and ignores observations.
+ observations: A dict of all observations.
+ """
+ time_sec = max(0, time_sec - self._start_time_sec)
+ frame = int(time_sec * self._fps)
+ if self._total_length:
+ frame = frame % (
+ self._total_length + int(self._pause_between_repeat_sec * self._fps))
+ frame = min(frame, self._total_length - 1)
+ return np.ndarray((0, 0, 0)), np.ndarray((0, 0, 0, 1)), {
+ ANIMATION_FRAME_NUMBER_KEY: frame}
+
+
+@gin.configurable
+class ConversationController(ControllerBase):
+ """Controller for an object that mimics conversational behavior.
+
+ A controlled object is arrayed in a conversation about a center point.
+ When a target object reaches a thresholded distance away from the center,
+ the controlled object will face the target object and move away from
+ the target's intended path along an orthogonal direction vector until it
+ passes.
+ """
+
+ def __init__(self,
+ self_key: Text,
+ target_key: Text,
+ position: Sequence[float] = None,
+ orientation: Sequence[float] = None,
+ conversation_center: Sequence[float] = None,
+ proximity_threshold: float = 1.0,
+ speed_mps: float = 0.1):
+ """Constructor.
+
+ Args:
+ self_key: Observation dict key of position of object being controlled.
+ target_key: Observation dict key of position of target object.
+ position: Initial position of the object.
+ orientation: Initial orientation of the object in xyzw quaternion.
+ conversation_center: Position of the center of the conversation group.
+ proximity_threshold: The distance from the conversation center that the
+ target must reach in order to prompt a response from the controlled
+ object.
+ speed_mps: Speed in meters per second, always positive.
+ """
+ self._self_key = self_key
+ self._target_key = target_key
+ self._position = np.array(position or (0, 0, 0))
+ self._orientation = np.array(orientation or (0, 0, 0, 1))
+ self._conversation_center = np.array(conversation_center or (0, 0, 0))
+ self._proximity_threshold = proximity_threshold
+ self._speed_mps = speed_mps
+
+ self._prev_target_position = None
+ self._wait_position = None
+
+ def _get_wait_position(self, self_position, target_position):
+ """Gets the waiting position for the controlled object.
+
+ This returns the position that the controlled object should move towards
+ to create physical space such that the target object may pass through
+ the conversation space.
+
+ Args:
+ self_position: The current position of the controlled object.
+ target_position: The current position of the target object.
+
+ Returns:
+ An xyz position representing the waiting position that the controlled
+ object should move towards to create space.
+ """
+ # Find an orthogonal projection to the target's path
+ target_path = np.array(target_position - self._prev_target_position)
+ self_path = np.array(self_position - self._prev_target_position)
+
+ unit_target_path = target_path / np.linalg.norm(target_path)
+
+ projected_point = np.dot(self_path, unit_target_path) * unit_target_path
+ projected_point += self._prev_target_position
+
+ # Get the position that lies along the orthogonal projection vector
+ # but in the opposite direction from the target's path and exactly
+ # proximity_threshold distance away.
+ return self._get_position(
+ projected_point,
+ self_position,
+ self._proximity_threshold,
+ self._proximity_threshold)
+
+ def _get_orientation(self, source_position, target_position):
+ """Gets orientation required to face target_position from source_position.
+
+ Args:
+ source_position: The source position where an object would be located.
+ target_position: The target position that an object should face.
+
+ Returns:
+ A xyzw quaternion indicating the orientation.
+ """
+ if np.allclose(source_position, target_position):
+ return self._orientation
+
+ delta_vector = (target_position - source_position) * (1, 1, 0)
+
+ new_yaw = np.arctan2(delta_vector[1], delta_vector[0])
+ new_orientation = np.array(
+ (0, 0, np.sin(new_yaw / 2), np.cos(new_yaw / 2)))
+
+ return new_orientation
+
+ def _get_position(self,
+ source_position,
+ target_position,
+ min_delta,
+ max_delta):
+ """Gets the next position along the vector from source to target.
+
+ This returns the position that should be moved to next which lies along
+ the direction vector from source -> target with a minimum length of
+ min_delta and a maximum distance of max_delta.
+
+ Args:
+ source_position: The current position of the controlled object.
+ target_position: The target position to move to.
+ min_delta: The minimum amount of distance to move.
+ max_delta: The maximum amount of distance to move.
+
+ Returns:
+ An xyz position representing the next position to move to.
+ """
+ delta_vector = (target_position - source_position) * (1, 1, 0)
+ distance = np.linalg.norm(delta_vector)
+
+ # If the distance to the target is greater than the maximum step delta,
+ # then normalize the vector and set it to the max step delta.
+ if distance > max_delta:
+ delta_vector = (delta_vector / distance) * max_delta
+ # If the distance is less than the minimum step delta, then normalize
+ # the vector and set it to the min step delta.
+ elif distance < min_delta:
+ delta_vector = (delta_vector / distance) * min_delta
+
+ new_position = (delta_vector + source_position)
+
+ return new_position
+
+ def _get_target_distance_to_center(self, target_position):
+ """Gets the distance from the target to the conversation center point.
+
+ Args:
+ target_position: The target position.
+
+ Returns:
+ The scalar distance from the target position to the conversation center.
+ """
+ # Calculates delta vector and projects it to xy-plane.
+ delta_vector = (target_position - self._conversation_center) * (1, 1, 0)
+
+ # Compute the length of the delta vector.
+ return np.linalg.norm(delta_vector)
+
+ def get_action(self,
+ t: float,
+ observations: Dict[Text, Any]) -> ControllerOutput:
+ """Gets the position and orientation of the controlled object.
+
+ Args:
+ t: The current time step.
+ observations: Dict containing sensor observations for current time step.
+
+ Returns:
+ The new position and orientation for the controlled object.
+ """
+
+ position = self._position
+ orientation = self._orientation
+
+ # Observations are only available for positive time steps.
+ if t >= 0:
+ self_position = observations[self._self_key]
+ target_position = observations[self._target_key]
+
+ target_distance = self._get_target_distance_to_center(target_position)
+
+ # Check if the target is within the threshold distance of the
+ # conversation center.
+ if(target_distance < self._proximity_threshold and
+ self._prev_target_position is not None):
+
+ # If it is, get the position that the controlled object should move to
+ # in order to create space and get the resulting action/orientation.
+ wait_position = self._get_wait_position(self_position, target_position)
+
+ orientation = self._get_orientation(
+ self_position,
+ target_position)
+
+ position = self._get_position(
+ self_position,
+ wait_position,
+ 0.0,
+ self._speed_mps)
+ else:
+ # Otherwise, get the position/orientation required to move back to
+ # the original position and face the conversation center.
+ orientation = self._get_orientation(
+ self_position,
+ self._conversation_center)
+
+ position = self._get_position(
+ self_position,
+ self._position,
+ 0.0,
+ self._speed_mps)
+
+ self._prev_target_position = target_position
+
+ # Only return a new position along the x/y axis, z should be unaffected.
+ position = np.array([position[0], position[1], self._position[2]])
+ return position, orientation, {}
+
+
+@gin.configurable
+class PauseIfCloseByWrapper(ControllerBase):
+ """A controller wrapper that pauses controller if object is close to others.
+
+ This wrapper works best if the underlying controller is time based. It is
+ intended to be a simple way to stop agent when blocked and is not for
+ reliable collision avoidance.
+ """
+
+ _DEFAULT_PAUSE_DISTANCE_M = 1.0
+
+ def __init__(
+ self,
+ wrapped_controller: ControllerBase,
+ self_pos_key: Text,
+ others_pos_keys: Sequence[Text],
+ pause_distance: Union[float, Sequence[float]] = _DEFAULT_PAUSE_DISTANCE_M,
+ self_yaw_key: Optional[Text] = None,
+ active_front_sector: Optional[float] = None):
+ """Constructor.
+
+ Args:
+ wrapped_controller: The controller being wrapped.
+ self_pos_key: Observation key of self position.
+ others_pos_keys: Observation keys of others' positions.
+ pause_distance: The distance limit before the controller pauses in meters.
+ Can be a float value which applies to all objects specified in
+ others_pos_keys or a Sequence of float values with the same length
+ as others_pos_keys denoting the pause distance for each individual
+ object in the same order in others_pos_keys. Default pause distance is
+ one meter.
+ self_yaw_key: Observation key of self yaw. Required if active_front_sector
+ is specified.
+ active_front_sector: If specified, it defines pie-shaped active region in
+ front of controlled object. The pie-shaped area is symmetric about the
+ forward direction of controlled object with it angle defined by this
+ arg in radians, Only when other objects shows up in this region and
+ pause distance requirement is met, pause is actived.
+ """
+
+ self._controller = wrapped_controller
+ self._pause_start_t = -1
+ self._shift_t = 0
+ self._last_action = None
+ self._self_pos_key = self_pos_key
+ self._others_pos_keys = list(others_pos_keys) # Make a copy.
+
+ if isinstance(pause_distance, float):
+ pause_distance = [pause_distance] * len(others_pos_keys)
+
+ if len(pause_distance) != len(others_pos_keys):
+ raise ValueError(
+ "pause_distance and others_pos_keys must have the same length.")
+ self._pause_distance = list(pause_distance) # Make a copy.
+
+ if active_front_sector is not None and self_yaw_key is None:
+ raise ValueError(
+ "self_yaw_key must be specified if active_front_sector is specified.")
+
+ self._self_yaw_key = self_yaw_key
+ self._active_front_sector = active_front_sector
+
+ def get_action(self,
+ t: float,
+ observations: Dict[Text, Any]) -> ControllerOutput:
+
+ """Gets the position and orientation of the controlled object.
+
+ Args:
+ t: The current time step.
+ observations: Dict containing sensor observations for current time step.
+
+ Returns:
+ The new position and orientation for the controlled object.
+ """
+ if t < 0:
+ self._pause_start_t = -1
+ self._shift_t = 0
+ self._last_action = self._controller.get_action(
+ t, observations)
+ return self._last_action
+
+ if self._should_pause(observations):
+ # Only record the start time of pause.
+ if self._pause_start_t < 0:
+ self._pause_start_t = t
+ return self._last_action
+ else:
+ if self._pause_start_t >= 0:
+ self._shift_t += t - self._pause_start_t
+ self._pause_start_t = -1
+
+ self._last_action = self._controller.get_action(
+ t - self._shift_t, observations)
+ return self._last_action
+
+ def _should_pause(self, observations) -> bool:
+ """Determines whether the controller should pause."""
+ self_position_2d = observations[self._self_pos_key][:2]
+ for pos_key, pause_distance in zip(
+ self._others_pos_keys, self._pause_distance):
+ position_2d = observations[pos_key][:2]
+ vector_2d = position_2d - self_position_2d
+ distance = np.linalg.norm(vector_2d)
+
+ if self._active_front_sector is None:
+ return distance <= pause_distance
+ else:
+ yaw = observations[self._self_yaw_key][0]
+ dot = np.dot(vector_2d / distance, np.array([np.cos(yaw), np.sin(yaw)]))
+ return (distance <= pause_distance and
+ np.arccos(dot) < self._active_front_sector / 2)
+
+ return False
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/quadruped_base.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/quadruped_base.py
new file mode 100644
index 000000000..960c278ed
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/quadruped_base.py
@@ -0,0 +1,724 @@
+# Lint as: python3
+"""The base class for all quadrupeds."""
+from typing import Any, Callable, Dict, Sequence, Tuple, Text, Union
+import gin
+import gym
+import numpy as np
+
+from pybullet_utils import bullet_client
+from pybullet_envs.minitaur.envs_v2.sensors import sensor as sensor_lib
+from pybullet_envs.minitaur.robots import hybrid_motor_model
+from pybullet_envs.minitaur.robots import robot_base
+from pybullet_envs.minitaur.robots import robot_config
+from pybullet_envs.minitaur.robots import robot_urdf_loader
+from pybullet_envs.minitaur.robots.safety import data_types as safety_data_types
+from pybullet_envs.minitaur.robots.utilities import kinematics_utils
+
+_UNIT_QUATERNION = (0, 0, 0, 1)
+_GRAVITY_ACCELERATION_OFFSET = (0, 0, 10)
+
+
+@gin.configurable
+class QuadrupedBase(robot_base.RobotBase):
+ """The basic quadruped class for both sim and real robots."""
+
+ def __init__(
+ self,
+ pybullet_client: bullet_client.BulletClient,
+ clock: Callable[..., float],
+ motor_control_mode: robot_config.MotorControlMode,
+ motor_limits: robot_config.MotorLimits,
+ motor_model_class: Any = hybrid_motor_model.HybridMotorModel,
+ action_filter: Any = None,
+ sensors: Sequence[sensor_lib.Sensor] = (),
+ safety_config: safety_data_types.SafetyConfig = None,
+ **kwargs,
+ ):
+ """Initializes the class.
+
+ Args:
+ pybullet_client: The PyBullet client.
+ clock: The sim or real clock. The clock function is typically provided by
+ the gym environment.
+ motor_control_mode: Specifies in which mode the motor operates.
+ motor_limits: The motor limits of the robot. Used by the motor_model_class
+ and action space building.
+ motor_model_class: The motor model to use. Not needed for real robots.
+ action_filter: The filter to smooth and/or regulate the actions.
+ sensors: All sensors mounted on the robot.
+ safety_config: The safety setting for the robot.
+ **kwargs: Additional args.
+ """
+
+ self._pybullet_client = pybullet_client
+ self._clock = clock
+ self._motor_control_mode = motor_control_mode
+ self._motor_model_class = motor_model_class
+ self._motor_limits = motor_limits
+ self._action_space = None
+ self._action_names = None
+ self._action_filter = action_filter
+ self._sensors = sensors
+ self._safety_config = safety_config
+ self._urdf_loader = None
+ self._last_base_velocity = np.zeros(3)
+ self._last_observation_time = self._clock()
+ self._last_base_acceleration_world = np.zeros(3)
+ self._last_base_acceleration_accelerometer = np.zeros(3)
+
+ self.load()
+
+ def load(
+ self,
+ base_position: Tuple[float] = None,
+ base_orientation_quaternion: Tuple[float] = None,
+ joint_angles: Union[Dict[Text, float], Tuple[float]] = None,
+ ):
+ """Loads the URDF with the configured pose.
+
+ Args:
+ base_position: The base position after URDF loading. Will use the
+ configured pose in gin if None.
+ base_orientation_quaternion: The base orientation after URDF loading. Will
+ use the configured values in gin if not specified.
+ joint_angles: The desired joint angles after loading. Will use the
+ configured values if None.
+ """
+ # A robot specific pre loading routing.
+ self._pre_load()
+
+ if not self._urdf_loader:
+ self._urdf_loader = robot_urdf_loader.RobotUrdfLoader(
+ pybullet_client=self._pybullet_client)
+
+ # Record the urdf pose at loading, which will be used as the rotation
+ # reference for base rotation computation.
+ self._init_urdf_position, self._init_orientation_quat = (
+ self._pybullet_client.getBasePositionAndOrientation(
+ self._urdf_loader.robot_id))
+ unused_position, self._init_orientation_inv_quat = (
+ self._pybullet_client.invertTransform(
+ position=(0, 0, 0), orientation=self._init_orientation_quat))
+
+ # Joint ids may be different from the motor ids.
+ self._joint_id_dict = self._urdf_loader.get_joint_id_dict()
+ for joint_id in self._joint_id_dict.values():
+ # Disables the default motors in PyBullet.
+ self._pybullet_client.setJointMotorControl2(
+ bodyIndex=self._urdf_loader.robot_id,
+ jointIndex=joint_id,
+ controlMode=self._pybullet_client.VELOCITY_CONTROL,
+ targetVelocity=0,
+ force=0)
+ # Removes the default joint damping in PyBullet.
+ self._pybullet_client.changeDynamics(
+ self._urdf_loader.robot_id,
+ joint_id,
+ linearDamping=0,
+ angularDamping=0)
+
+ # We expect that this is non-empty for all quadrupedes, and should be an
+ # OrderedDict.
+ self._motor_id_dict = self._urdf_loader.get_motor_id_dict()
+ if not self._motor_id_dict:
+ raise ValueError("Motor id dict cannot be empty for quadrupeds.")
+ self._motor_ids = self._motor_id_dict.values()
+ self._num_motors = len(self._motor_id_dict)
+
+ self._build_action_space()
+
+ # Not needed for real robots.
+ if self._motor_model_class:
+ # TODO(b/151664871): Also supports position/velocity limits in the motor
+ # model.
+ self._motor_model = self._motor_model_class(
+ num_motors=self._num_motors,
+ motor_control_mode=self._motor_control_mode,
+ torque_lower_limits=self._motor_limits.torque_lower_limits,
+ torque_upper_limits=self._motor_limits.torque_upper_limits,
+ )
+
+ # Caches the variable for faster computation during stepping.
+ self._motor_direction_dict = self._urdf_loader.get_joint_direction_dict(
+ self._motor_id_dict.keys())
+ self._motor_directions = np.array(list(self._motor_direction_dict.values()))
+
+ self._motor_offset_dict = self._urdf_loader.get_joint_offset_dict(
+ self._motor_id_dict.keys())
+ self._motor_offsets = np.array(list(self._motor_offset_dict.values()))
+
+ # A robot specific routine post loading.
+ self._on_load()
+
+ # Robot sensors may use information from the class. So we initialize them
+ # after the loading is done.
+ for sensor in self._sensors:
+ sensor.set_robot(self)
+
+ def _build_action_space(self):
+ """Builds the action space of the robot using the motor limits."""
+ if self._motor_control_mode == robot_config.MotorControlMode.POSITION:
+ self._action_space = gym.spaces.Box(
+ low=self._motor_limits.angle_lower_limits,
+ high=self._motor_limits.angle_upper_limits,
+ shape=(self._num_motors,),
+ dtype=np.float32) # TODO(b/159160184) Make dtype configurable.
+ self._action_names = tuple(
+ "POSITION_{}".format(motor) for motor in self._motor_id_dict.keys())
+ elif self._motor_control_mode == robot_config.MotorControlMode.TORQUE:
+ self._action_space = gym.spaces.Box(
+ low=self._motor_limits.torque_lower_limits,
+ high=self._motor_limits.torque_upper_limits,
+ shape=(self._num_motors,),
+ dtype=np.float32)
+ self._action_names = tuple(
+ "TORQUE_{}".format(motor) for motor in self._motor_id_dict.keys())
+ elif self._motor_control_mode == robot_config.MotorControlMode.HYBRID:
+ hybrid_action_limits_low = [
+ self._motor_limits.angle_lower_limits, # q
+ # q_dot
+ self._motor_limits.velocity_lower_limits,
+ 0, # kp
+ 0, # kd
+ self._motor_limits.torque_lower_limits
+ ] # tau
+ hybrid_action_limits_high = [
+ self._motor_limits.angle_upper_limits,
+ self._motor_limits.velocity_upper_limits, np.inf, np.inf,
+ self._motor_limits.torque_upper_limits
+ ]
+ space_low = np.full(
+ (self._num_motors, robot_config.HYBRID_ACTION_DIMENSION),
+ hybrid_action_limits_low).ravel()
+ space_high = np.full(
+ (self._num_motors, robot_config.HYBRID_ACTION_DIMENSION),
+ hybrid_action_limits_high).ravel()
+ self._action_space = gym.spaces.Box(
+ low=space_low, high=space_high, dtype=np.float32)
+ self._action_names = tuple(
+ "HYBRID_{}".format(motor) for motor in self._motor_id_dict.keys())
+ else:
+ raise NotImplementedError("Not yet implemented!")
+
+ def _pre_load(self):
+ """Robot specific pre load routine.
+
+ For example, this allows configuration of the URDF loader.
+ """
+ pass
+
+ def _on_load(self):
+ """Robot specific post load routine.
+
+ For example, we need to add add additional hinge constraints to the leg
+ components of Minitaur after loading.
+
+ """
+ pass
+
+ @gin.configurable
+ def reset(
+ self,
+ base_position: Tuple[float] = None,
+ base_orientation_quaternion: Tuple[float] = None,
+ joint_angles: Union[Dict[Text, float], Tuple[float]] = None,
+ save_base_pose: bool = False,
+ **kwargs,
+ ):
+ """Resets the robot base and joint pose without reloading the URDF.
+
+ Base pose resetting only works for simulated robots or visualization of real
+ robots. This routine also updates the initial observation dict.
+
+ Args:
+ base_position: The desired base position. Will use the configured pose in
+ gin if None. Does not affect the position of the real robots in general.
+ base_orientation_quaternion: The base orientation after resetting. Will
+ use the configured values in gin if not specified.
+ joint_angles: The desired joint angles after resetting. Will use the
+ configured values if None.
+ save_base_pose: Save the base position and orientation as the default pose
+ after resetting.
+ **kwargs: Other args for backward compatibility. TODO(b/151975607): Remove
+ after migration.
+ """
+ # Reset the robot's motor model.
+ self._motor_model.reset()
+
+ # Reset the quantities for computing base acceleration.
+ self._last_base_velocity = np.zeros(3)
+ self._last_observation_time = self._clock()
+ self._last_base_acceleration_world = np.zeros(3)
+ self._last_base_acceleration_accelerometer = np.zeros(3)
+
+ # Solves chicken and egg problem. We need to run a control step to obtain
+ # the first motor torques.
+ self._motor_torques = np.zeros(self._num_motors)
+
+ # Receives a set of observation from the robot in case the reset function
+ # needs to use them.
+ self.receive_observation()
+
+ self._reset_base_pose(base_position, base_orientation_quaternion)
+ self._reset_joint_angles(joint_angles)
+
+ if save_base_pose:
+ # Records the base pose at resetting again, in case Reset is called with a
+ # different base orientation. This base pose will be used as zero
+ # rotation reference for base rotation computation.
+ self._init_urdf_position, self._init_orientation_quat = (
+ self._pybullet_client.getBasePositionAndOrientation(
+ self._urdf_loader.robot_id))
+ unused_position, self._init_orientation_inv_quat = (
+ self._pybullet_client.invertTransform(
+ position=(0, 0, 0), orientation=self._init_orientation_quat))
+
+ # Updates the observation at the end of resetting.
+ self.receive_observation()
+ self._time_at_reset = self._clock()
+
+ def GetTimeSinceReset(self):
+ return self._clock() - self._time_at_reset
+
+ def _reset_base_pose(self, position=None, orientation_quat=None):
+ """Resets the pose of the robot's base.
+
+ Base pose resetting only works for simulated robots or visualization of real
+ robots.
+
+ Args:
+ position: The desired base position. Will use the configured pose in gin
+ if None.
+ orientation_quat: The desired base rotation. Will use the configured
+ default pose in None.
+ """
+ self._urdf_loader.reset_base_pose(position, orientation_quat)
+
+ def _reset_joint_angles(self,
+ joint_angles: Union[Tuple[float],
+ Dict[Text, float]] = None):
+ """Resets the joint pose.
+
+ Real robots need to specify their routine to send joint angles. Simulated
+ Minitaur robots also needs to use dynamics to drive the motor joints, due to
+ the additional hinge joints not present in the URDF.
+
+ Args:
+ joint_angles: The joint pose if provided. Will use the robot default pose
+ from configuration.
+ """
+ # TODO(b/148897311): Supports tuple as the input.
+ self._urdf_loader.reset_joint_angles(joint_angles)
+
+ def terminate(self):
+ """The safe exit routine for the robot.
+
+ Only implemented for real robots.
+
+ """
+ pass
+
+ def step(self, action: Any, num_sub_steps: int = 1):
+ """Steps the simulation.
+
+ This is maintained for backward compatibility with the old robot class.
+
+ Args:
+ action: The control command to be executed by the robot.
+ num_sub_steps: Each action can be applied (possibly with interpolation)
+ multiple timesteps, to simulate the elapsed time between two consecutive
+ commands on real robots.
+ """
+ action = self.pre_control_step(action)
+
+ for _ in range(num_sub_steps):
+ # TODO(b/149252003): Add sub sampling.
+ self.apply_action(action)
+ # Timestep is pre-determined at simulation setup.
+ self._pybullet_client.stepSimulation()
+ self.receive_observation()
+
+ self.post_control_step()
+
+ def pre_control_step(self, action: Any, control_timestep: float = None):
+ """Processes the action and updates per control step quantities.
+
+ Args:
+ action: The input control command.
+ control_timestep: The control time step in the environment.
+ TODO(b/153835005), we can remove this once we pass env to the robot.
+
+ Returns:
+ The filtered action.
+ """
+ if self._action_filter:
+ # We assume the filter will create a set of interpolated results.
+ action = self._action_filter.filter(action)
+ return action
+
+ def apply_action(self, motor_commands, motor_control_mode=None):
+
+ # TODO(b/148897311): Supports dict in the future.
+ motor_commands = np.asarray(motor_commands)
+
+ # We always use torque based control at the lowest level for quadrupeds.
+ unused_observed_torques, actual_torques = (
+ self._motor_model.get_motor_torques(motor_commands, motor_control_mode))
+ self._motor_torques = actual_torques
+
+ # Converts the motor torques to URDF joint space, which may have different
+ # directions.
+ applied_motor_torques = np.multiply(actual_torques, self._motor_directions)
+
+ self._pybullet_client.setJointMotorControlArray(
+ bodyIndex=self._urdf_loader.robot_id,
+ jointIndices=self._motor_ids,
+ controlMode=self._pybullet_client.TORQUE_CONTROL,
+ forces=applied_motor_torques)
+
+ def _get_base_roll_pitch_yaw_rate(self):
+ _, angular_velocity = self._pybullet_client.getBaseVelocity(
+ self._urdf_loader.robot_id)
+ return kinematics_utils.rotate_to_base_frame(
+ self._pybullet_client, self.urdf_loader.robot_id, angular_velocity,
+ self._init_orientation_inv_quat)
+
+ def _get_base_velocity(self):
+ base_velocity, _ = self._pybullet_client.getBaseVelocity(
+ self._urdf_loader.robot_id)
+ return base_velocity
+
+ def _update_base_acceleration(self):
+ """Update the base acceleration using finite difference."""
+ if self._last_observation_time < self.timestamp:
+ self._last_base_acceleration_world = (
+ np.array(self._base_velocity) - self._last_base_velocity) / (
+ self.timestamp - self._last_observation_time)
+ _, inv_base_orientation = self.pybullet_client.invertTransform(
+ np.zeros(3), np.array(self.base_orientation_quaternion))
+
+ # An offset is added to the acceleration measured in the world frame
+ # because the accelerometer reading is in the frame of free-falling robot.
+ base_acceleration_accelerometer = self.pybullet_client.multiplyTransforms(
+ np.zeros(3), inv_base_orientation,
+ self._last_base_acceleration_world + _GRAVITY_ACCELERATION_OFFSET,
+ _UNIT_QUATERNION)[0]
+ self._last_base_acceleration_accelerometer = np.array(
+ base_acceleration_accelerometer)
+
+ def receive_observation(self):
+ """Receives the observations for all sensors."""
+ # Update the intrinsic values including the joint angles, joint
+ # velocities, and imu readings.
+ self._base_position, base_orientation_quat = (
+ self._pybullet_client.getBasePositionAndOrientation(
+ self._urdf_loader.robot_id))
+ _, self._base_orientation_quat = self._pybullet_client.multiplyTransforms(
+ positionA=(0, 0, 0),
+ orientationA=self._init_orientation_inv_quat,
+ positionB=(0, 0, 0),
+ orientationB=base_orientation_quat)
+ self._base_velocity = self._get_base_velocity()
+ self._base_roll_pitch_yaw = self._pybullet_client.getEulerFromQuaternion(
+ self._base_orientation_quat)
+
+ self._base_roll_pitch_yaw_rate = self._get_base_roll_pitch_yaw_rate()
+
+ self._joint_states = self._pybullet_client.getJointStates(
+ self._urdf_loader.robot_id, self._motor_ids)
+ self._motor_angles = np.array(
+ [joint_state[0] for joint_state in self._joint_states])
+ self._motor_angles = (self._motor_angles -
+ self._motor_offsets) * self._motor_directions
+
+ self._motor_velocities = np.array(
+ [joint_state[1] for joint_state in self._joint_states])
+ self._motor_velocities = self._motor_velocities * self._motor_directions
+
+ # We use motor models to track the delayed motor positions and velocities
+ # buffer.
+ if self._motor_model:
+ self._motor_model.update(self._clock(), self._motor_angles,
+ self._motor_velocities)
+
+ self._update_base_acceleration()
+ # Update the latest base velocity and timestamp at the end of the API.
+ self._last_base_velocity = np.array(self._base_velocity)
+ self._last_observation_time = self.timestamp
+
+ def post_control_step(self):
+ """Called at the end of a control step outside the action repeat loop."""
+ pass
+
+ # TODO(tingnan): Change from "foot_positions" to "feet_positions".
+ def motor_angles_from_foot_positions(self,
+ foot_positions,
+ position_in_world_frame=False):
+ """Use IK to compute the motor angles, given the feet links' positions.
+
+ Args:
+ foot_positions: The foot links' positions in frame specified by the next
+ parameter. The input is a numpy array of size (4, 3).
+ position_in_world_frame: Whether the foot_positions are specified in the
+ world frame.
+
+ Returns:
+ A tuple. The position indices and the angles for all joints along the
+ leg. The position indices is consistent with the joint orders as returned
+ by GetMotorAngles API.
+ """
+ joint_position_idxs = np.arange(self.num_motors)
+ foot_link_ids = tuple(self._urdf_loader.get_end_effector_id_dict().values())
+ joint_angles = kinematics_utils.joint_angles_from_link_positions(
+ pybullet_client=self.pybullet_client,
+ urdf_id=self.robot_id,
+ link_positions=foot_positions,
+ link_ids=foot_link_ids,
+ joint_dof_ids=joint_position_idxs,
+ positions_are_in_world_frame=position_in_world_frame)
+ joint_angles = np.multiply(
+ np.asarray(joint_angles) - np.asarray(self._motor_offsets),
+ self._motor_directions)
+ return joint_position_idxs, joint_angles
+
+ # TODO(tingnan): Change from "foot_positions" to "feet_positions".
+ def foot_positions(self, position_in_world_frame=False):
+ """Returns the robot's foot positions in the base/world frame."""
+ foot_positions = []
+ foot_link_ids = tuple(self._urdf_loader.get_end_effector_id_dict().values())
+ for foot_id in foot_link_ids:
+ if not position_in_world_frame:
+ foot_positions.append(
+ kinematics_utils.link_position_in_base_frame(
+ pybullet_client=self.pybullet_client,
+ urdf_id=self.robot_id,
+ link_id=foot_id,
+ ))
+ else:
+ foot_positions.append(
+ kinematics_utils.link_position_in_world_frame(
+ pybullet_client=self.pybullet_client,
+ urdf_id=self.robot_id,
+ link_id=foot_id,
+ ))
+ return np.array(foot_positions)
+
+ def feet_contact_forces(self) -> Sequence[np.ndarray]:
+ """Gets the contact forces on all feet.
+
+ Reals robot may use a robot specific implementation. For example, the
+ Laikago will measure each contact force in the corresponding foot's local
+ frame, and this force will not be the total contact force due to the sensor
+ limitation.
+
+ For simulated robots, we wll always report the force in the base frame.
+
+ Returns:
+ A list of foot contact forces.
+ """
+ foot_link_ids = tuple(self._urdf_loader.get_end_effector_id_dict().values())
+ contact_forces = [np.zeros(3) for _ in range(len(foot_link_ids))]
+
+ all_contacts = self._pybullet_client.getContactPoints(
+ bodyA=self._urdf_loader.robot_id)
+
+ for contact in all_contacts:
+ (unused_flag, body_a_id, body_b_id, link_a_id, unused_link_b_id,
+ unused_pos_on_a, unused_pos_on_b, contact_normal_b_to_a, unused_distance,
+ normal_force, friction_1, friction_direction_1, friction_2,
+ friction_direction_2) = contact
+
+ # Ignore self contacts
+ if body_b_id == body_a_id:
+ continue
+
+ if link_a_id in foot_link_ids:
+ normal_force = np.array(contact_normal_b_to_a) * normal_force
+ friction_force = np.array(friction_direction_1) * friction_1 + np.array(
+ friction_direction_2) * friction_2
+ force = normal_force + friction_force
+ local_force = kinematics_utils.rotate_to_base_frame(
+ self._pybullet_client, self.urdf_loader.robot_id, force,
+ self._init_orientation_inv_quat)
+ local_force_norm = np.linalg.norm(local_force)
+ toe_link_order = foot_link_ids.index(link_a_id)
+ if local_force_norm > 0:
+ contact_forces[toe_link_order] += local_force
+ else:
+ continue
+ return contact_forces
+
+ def compute_jacobian_for_one_leg(self, leg_id: int) -> np.ndarray:
+ """Compute the Jacobian for a given leg.
+
+ Args:
+ leg_id: Index of the leg for which the jacobian is computed.
+
+ Returns:
+ The 3 x N transposed Jacobian matrix. where N is the total DoFs of the
+ robot. For a quadruped, the first 6 columns of the matrix corresponds to
+ the CoM translation and rotation. The columns corresponds to a leg can be
+ extracted with indices [6 + leg_id * 3: 6 + leg_id * 3 + 3]. Note that
+ the jacobian is calculated for motors, which takes motor directions into
+ consideration.
+ """
+ com_dof = self._urdf_loader.com_dof
+ foot_link_ids = tuple(self._urdf_loader.get_end_effector_id_dict().values())
+ return kinematics_utils.compute_jacobian(
+ pybullet_client=self.pybullet_client,
+ urdf_id=self.robot_id,
+ link_id=foot_link_ids[leg_id],
+ all_joint_positions=[
+ state[0] for state in self._joint_states
+ ]) * np.concatenate([np.ones(com_dof), self._motor_directions])
+
+ def map_contact_force_to_joint_torques(
+ self, leg_id: int, contact_force: np.ndarray) -> Dict[int, float]:
+ """Maps the foot contact force to the leg joint torques.
+
+ Args:
+ leg_id: Index of the leg for which the jacobian is computed.
+ contact_force: Desired contact force experted by the leg.
+
+ Returns:
+ A dict containing the torques for each motor on the leg.
+ """
+ foot_link_ids = tuple(self._urdf_loader.get_end_effector_id_dict().values())
+ jv = self.compute_jacobian_for_one_leg(leg_id)
+ all_motor_torques = np.matmul(contact_force, jv)
+ motor_torques = {}
+ motors_per_leg = self.num_motors // len(foot_link_ids)
+ com_dof = self._urdf_loader.com_dof
+ for joint_id in range(leg_id * motors_per_leg,
+ (leg_id + 1) * motors_per_leg):
+ motor_torques[joint_id] = all_motor_torques[com_dof + joint_id]
+
+ return motor_torques
+
+ @classmethod
+ def get_constants(cls):
+ raise NotImplementedError("Not yet implemented!")
+
+ @property
+ def timestamp(self):
+ return self._clock()
+
+ @property
+ def action_space(self):
+ return self._action_space
+
+ @property
+ def action_names(self):
+ return self._action_names
+
+ @property
+ def base_orientation_quaternion(self):
+ """Gets the base orientation as a quaternion.
+
+ The base orientation is always relative to the init_orientation, which
+ can be updated by Reset function. This is necessary as many URDF can have an
+ internal frame that is not z-up, so if we don't provide an init_orientation
+ (through Reset), the loaded robot can have its belly facing the horizontal
+ direction.
+
+ Returns:
+ The base orientation in quaternion.
+ """
+ return self._base_orientation_quat
+
+ @property
+ def base_orientation_quaternion_default_frame(self):
+ """Gets the base orientation in the robot's default frame.
+
+ This is the base orientation in whatever frame the robot specifies. For
+ simulated robot this is the URDF's internal frame. For real robot this can
+ be based on the rpy reading determined by the IMU.
+
+ Returns:
+ The base orientation in quaternion in a robot default frame.
+ """
+ _, base_orientation_quat = (
+ self._pybullet_client.getBasePositionAndOrientation(
+ self._urdf_loader.robot_id))
+ return base_orientation_quat
+
+ @property
+ def sensors(self):
+ return self._sensors
+
+ @property
+ def base_roll_pitch_yaw(self):
+ return self._base_roll_pitch_yaw
+
+ @property
+ def base_roll_pitch_yaw_rate(self):
+ return self._base_roll_pitch_yaw_rate
+
+ @property
+ def base_position(self):
+ return self._base_position
+
+ @property
+ def base_velocity(self):
+ return self._base_velocity
+
+ @property
+ def is_safe(self):
+ return True
+
+ @property
+ def num_motors(self):
+ return self._num_motors
+
+ @property
+ def motor_model(self):
+ return self._motor_model
+
+ @property
+ def motor_limits(self) -> robot_config.MotorLimits:
+ return self._motor_limits
+
+ @property
+ def motor_angles(self):
+ return self._motor_angles
+
+ @property
+ def motor_velocities(self):
+ return self._motor_velocities
+
+ @property
+ def motor_torques(self):
+ return self._motor_torques
+
+ @property
+ def pybullet_client(self):
+ return self._pybullet_client
+
+ @property
+ def urdf_loader(self):
+ return self._urdf_loader
+
+ @property
+ def robot_id(self):
+ return self._urdf_loader.robot_id
+
+ @property
+ def initital_orientation_inverse_quaternion(self):
+ return self._init_orientation_inv_quat
+
+ @property
+ def base_acceleration_accelerometer(self):
+ """Get the base acceleration measured by an accelerometer.
+
+ The acceleration is measured in the local frame of a free-falling robot,
+ which is consistent with the robot's IMU measurements. Here the
+ gravitational acceleration is first added to the acceleration in the world
+ frame, which is then converted to the local frame of the robot.
+
+ """
+ return np.array(self._last_base_acceleration_accelerometer)
+
+ @property
+ def base_acceleration(self):
+ """Get the base acceleration in the world frame."""
+ return np.array(self._last_base_acceleration_world)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/robot_base.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/robot_base.py
new file mode 100644
index 000000000..75dc127cb
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/robot_base.py
@@ -0,0 +1,133 @@
+# Lint as: python3
+"""The abstract robot class."""
+
+import abc
+from typing import Optional, Sequence
+
+# Action names for robots operating kinematically.
+LINEAR_VELOCITY = "linear_velocity"
+ANGULAR_VELOCITY = "angular_velocity"
+
+
+class RobotBase(metaclass=abc.ABCMeta):
+ """The base class for all robots used in the mobility team."""
+
+ @abc.abstractmethod
+ def reset(
+ self,
+ base_position: Optional[Sequence[float]] = None,
+ base_orientation_quaternion: Optional[Sequence[float]] = None) -> None:
+ """Resets the states (e.g. pose and sensor readings) of the robot.
+
+ This is called at the start of each episode by the environment.
+
+ Args:
+ base_position: Robot base position after reset. If None, robot stay where
+ it was after reset. For robot that does not support reset with position
+ change, a ValueError should be raised.
+ base_orientation_quaternion: Robot base orientation after reset. If None,
+ robot stays in pre-reset orientation. For robot that does not support
+ reset with orientation change, a ValueError should be raised.
+ """
+ pass
+
+ @abc.abstractmethod
+ def terminate(self):
+ """Shuts down the robot."""
+ pass
+
+ @abc.abstractmethod
+ def pre_control_step(self, action):
+ """Processes the input action before the action repeat loop.
+
+ We assume that an action sent to the real robot is sticky, i.e. it will be
+ executed until a new action is received after some time. To simulate this,
+ we introduced the action_repeat parameter, to reflect how many time steps it
+ takes for the policy to generate a new action. That is, for each control
+ step, the simulation contains an inner loop:
+
+ robot.pre_control_step(action) # Smooth or interpolate the action
+ for i in range(action_repeat):
+ robot.apply_action(action)
+ bullet.stepSimulation(time_step) # Step the sim for one time step
+ robot.receive_observation() # Update the sensor observations
+ robot.post_control_step() # Update some internal variables.
+
+ Args:
+ action: Data type depends on the robot. Can be desired motor
+ position/torques for legged robots, or desired velocity/angular velocity
+ for wheeled robots.
+ """
+ pass
+
+ @abc.abstractmethod
+ def apply_action(self, action):
+ """Applies the action to the robot."""
+ pass
+
+ @abc.abstractmethod
+ def receive_observation(self):
+ """Updates the robot sensor readings."""
+ pass
+
+ @abc.abstractmethod
+ def post_control_step(self):
+ """Updates some internal variables such as step counters."""
+ pass
+
+ @property
+ def action_space(self):
+ """The action spec of the robot."""
+ raise NotImplementedError("action_space is not implemented")
+
+ @property
+ @abc.abstractmethod
+ def action_names(self):
+ """Name of each action in the action_space.
+
+ This is a structure of strings with the same shape as the action space,
+ where each string describes the corresponding element of the action space
+ (for example, a kinematic robot might return ("linear_velocity",
+ "angular_velocity")). Used for logging in the safety layer.
+ """
+
+ @property
+ def sensors(self):
+ """Returns the sensors on this robot.
+
+ Sensors are the main interface between the robot class and the gym
+ environment. Sensors can return what the robot can measure (e.g.
+ joint angles, IMU readings), and can represent more general quantities, i.e.
+ the last action taken, that can be part of the observation space.
+ Sensor classes are used by the robot class to the specify its observation
+ space.
+
+ """
+ raise NotImplementedError("sensors property not implemented")
+
+ @property
+ def base_orientation_quaternion(self):
+ """Returns the base pose as a quaternion in format (x, y, z, w).
+
+ These properties differ from the sensor interfaces, as they represent
+ the built-in measurable quantities. We assume most robots have an IMU at
+ its base to measure the base pose. Actually, some sensor classes like the
+ base pose sensor and joint angle sensor will call these built-in methods. In
+ general, how these quantities can be extracted depends on the specific real
+ robots.
+
+ """
+ raise NotImplementedError("base_orientation_quaternion is not implemented")
+
+ @property
+ def base_roll_pitch_yaw(self):
+ """Returns the base roll, pitch, and yaw angles."""
+ raise NotImplementedError("base_roll_pitch_yaw is not implemented")
+
+ @property
+ def base_roll_pitch_yaw_rate(self):
+ raise NotImplementedError("base_roll_pitch_yaw_rate is not implemented")
+
+ @property
+ def base_position(self):
+ raise NotImplementedError("base_position is not implemented")
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/robot_config.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/robot_config.py
new file mode 100644
index 000000000..255e686d2
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/robot_config.py
@@ -0,0 +1,105 @@
+# Lint as: python3
+"""The configuration parameters for our robots."""
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import enum
+from typing import Sequence, Union
+import dataclasses
+import gin
+import numpy as np
+
+
+@gin.constants_from_enum
+class MotorControlMode(enum.Enum):
+ """The supported motor control modes."""
+ POSITION = 1,
+
+ # Apply motor torques directly.
+ TORQUE = 2,
+
+ # Apply a tuple (q, qdot, kp, kd, tau) for each motor. Here q, qdot are motor
+ # position and velocities. kp and kd are PD gains. tau is the additional
+ # motor torque. This is the most flexible control mode.
+ HYBRID = 3,
+
+ # PWM mode is only availalbe for Minitaur
+ PWM = 4
+
+
+# TODO(b/127675924): Group other parameters in the named attrib class.
+
+# Each hybrid action is a tuple (position, position_gain, velocity,
+# velocity_gain, torque)
+HYBRID_ACTION_DIMENSION = 5
+
+
+class HybridActionIndex(enum.Enum):
+ # The index of each component within the hybrid action tuple.
+ POSITION = 0
+ POSITION_GAIN = 1
+ VELOCITY = 2
+ VELOCITY_GAIN = 3
+ TORQUE = 4
+
+
+@gin.configurable
+class MotorLimits(object):
+ """The data class for motor limits."""
+
+ def __init__(
+ self,
+ angle_lower_limits: Union[float, Sequence[float]] = float('-inf'),
+ angle_upper_limits: Union[float, Sequence[float]] = float('inf'),
+ velocity_lower_limits: Union[float, Sequence[float]] = float('-inf'),
+ velocity_upper_limits: Union[float, Sequence[float]] = float('inf'),
+ torque_lower_limits: Union[float, Sequence[float]] = float('-inf'),
+ torque_upper_limits: Union[float, Sequence[float]] = float('inf'),
+ ):
+ """Initializes the class."""
+ self.angle_lower_limits = angle_lower_limits
+ self.angle_upper_limits = angle_upper_limits
+ self.velocity_lower_limits = velocity_lower_limits
+ self.velocity_upper_limits = velocity_upper_limits
+ self.torque_lower_limits = torque_lower_limits
+ self.torque_upper_limits = torque_upper_limits
+
+
+@gin.constants_from_enum
+class WheeledRobotControlMode(enum.Enum):
+ """The control mode for wheeled robots."""
+ # Controls the base of the robot (i.e. in kinematic mode.) or the base wheels
+ # using motor commands.
+ BASE = 1
+ # Controls arm only
+ ARM = 2
+ # Controls both base and arm
+ BASE_AND_ARM = 3
+ # Controls both base and head
+ BASE_AND_HEAD = 4
+ # Controls the non-wheel motors. This include arms and heads.
+ BODY = 5
+ # Controls all degrees of freedom, i.e. the base and arm/head simultaneously.
+ ALL = 6
+ # High-level navigation target.
+ NAVIGATION_TARGET = 7
+ # Individually addressable actions for body joints, with nested dict actions.
+ ADDRESSABLE = 8
+
+
+@dataclasses.dataclass
+class TwistActionLimits:
+ """The data class for twist action limits.
+
+ Common abbreviations used in variable names suffix:
+ mps = Meters per Second
+ rps = Radians per Second
+ """
+ max_linear_mps: float
+ min_linear_mps: float
+ max_angular_rps: float
+ min_angular_rps: float
+
+
+
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/robot_urdf_loader.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/robot_urdf_loader.py
new file mode 100644
index 000000000..2d4bf710a
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/robot_urdf_loader.py
@@ -0,0 +1,371 @@
+# Lint as: python3
+"""Helper class to load and manage the robot URDF file in simulation."""
+
+import collections
+from typing import Dict, Text, Tuple
+
+import gin
+import numpy as np
+
+from pybullet_utils import bullet_client
+
+# Base link does not have a parent joint. So we just use the string "robot_base"
+# for reference. The corresponding link/joint id is always -1 in pybullet.
+ROBOT_BASE = "robot_base"
+
+
+def _sub_dict(joint_name_to_id: Dict[Text, int],
+ joint_names: Tuple[Text]) -> Dict[Text, int]:
+ sub_dict = collections.OrderedDict()
+ if joint_names is None:
+ return sub_dict
+ for name in joint_names:
+ sub_dict[name] = joint_name_to_id[name]
+ return sub_dict
+
+
+def convert_to_urdf_joint_angles(
+ robot_space_joint_angles: np.ndarray,
+ joint_offsets: np.ndarray,
+ joint_directions: np.ndarray,
+):
+ return robot_space_joint_angles * joint_directions + joint_offsets
+
+
+def convert_to_robot_joint_angles(
+ urdf_space_joint_angles: np.ndarray,
+ joint_offsets: np.ndarray,
+ joint_directions: np.ndarray,
+):
+ return (urdf_space_joint_angles - joint_offsets) * joint_directions
+
+
+@gin.configurable
+class RobotUrdfLoader(object):
+ """A abstract class to manage the robot urdf in sim."""
+
+ def __init__(
+ self,
+ pybullet_client: bullet_client.BulletClient,
+ urdf_path: Text,
+ constrained_base: bool = False,
+ enable_self_collision: bool = True,
+ init_base_position: Tuple[float] = (0, 0, 0),
+ init_base_orientation_quaternion: Tuple[float] = None,
+ init_base_orientation_rpy: Tuple[float] = None,
+ base_names: Tuple[Text] = None,
+ init_joint_angles: Dict[Text, float] = None,
+ joint_offsets: Dict[Text, float] = None,
+ joint_directions: Dict[Text, int] = None,
+ motor_names: Tuple[Text] = None,
+ end_effector_names: Tuple[Text] = None,
+ user_group: Dict[Text, Tuple[Text]] = None,
+ ):
+ """Initialize the class.
+
+ Args:
+ pybullet_client: A pybullet client.
+ urdf_path: The path to the URDF to load.
+ constrained_base: Whether to create a FIXED constraint to the base of the
+ URDF. Needs to be True for kinematic robots. This allows us to "hang"
+ the simulated robot in air, and the hanging point can follow arbitrary
+ provided paths.
+ enable_self_collision: Determines if the robot can collide with itself.
+ init_base_position: The base x, y, z after loading.
+ init_base_orientation_quaternion: The base rotation after loading.
+ init_base_orientation_rpy: The base rotation after loading, presented in
+ roll, pitch, yaw.
+ base_names: The base joint names. Used to find additional links that
+ belong to the base. Optional because the base might only contain a
+ single mesh/block, which always has the id of "-1".
+ init_joint_angles: Maps joint name to the desired joint pose after loading
+ URDF. If not provided, will use the URDF default. This can be a subset
+ of all joints in the URDF. This should be in the robot joint convention,
+ which can be different from the URDF convention.
+ joint_offsets: The "zero" position of joint angles in the URDF space.
+ joint_directions: To convert between robot sdk/control and urdf joint
+ convention.
+ motor_names: The motor joint names in the URDF. Typically a subset of all
+ movable joints/DoFs.
+ end_effector_names: A subset of joints specifying the end-effector
+ joint(s). For example for legged robots the end effectors are the toe
+ joints (if provided). For arms this group includes left and right
+ grippers.
+ user_group: User defined joint groups. For example for quadrupeds, we may
+ want to organize all joints according to which leg they belong to.
+ """
+ self._pybullet_client = pybullet_client
+ self._urdf_path = urdf_path
+ self._init_base_position = init_base_position
+ if init_base_orientation_quaternion is not None:
+ self._init_base_orientation_quaternion = init_base_orientation_quaternion
+ else:
+ if init_base_orientation_rpy is None:
+ raise ValueError("Either init_base_orientation_quaterion "
+ "or init_base_orientation_rpy is required")
+ self._init_base_orientation_quaternion = (
+ self._pybullet_client.getQuaternionFromEuler(
+ init_base_orientation_rpy))
+ self._constrained_base = constrained_base
+ self._enable_self_collision = enable_self_collision
+ self._base_names = base_names
+ self._init_joint_angles = init_joint_angles
+ self._joint_offsets = joint_offsets
+ self._joint_directions = joint_directions
+ self._motor_names = motor_names
+ self._end_effector_names = end_effector_names
+ self._user_group = user_group
+ self.load(
+ enable_self_collision=enable_self_collision,
+ init_base_position=init_base_position,
+ init_base_orientation_quaternion=init_base_orientation_quaternion,
+ init_joint_angles=init_joint_angles)
+
+ def get_base_id_dict(self, name: Tuple[Text] = None) -> Dict[Text, int]:
+ if name is None:
+ return self._base_dict
+ return _sub_dict(self._base_dict, name)
+
+ def get_joint_id_dict(self, name: Tuple[Text] = None) -> Dict[Text, int]:
+ if name is None:
+ return self._joint_name_to_id
+ return _sub_dict(self._joint_name_to_id, name)
+
+ def get_link_id_dict(self, name: Tuple[Text] = None) -> Dict[Text, int]:
+ if name is None:
+ return self._link_name_to_id
+ return _sub_dict(self._link_name_to_id, name)
+
+ def get_motor_id_dict(self, name: Tuple[Text] = None) -> Dict[Text, int]:
+ if name is None:
+ return self._motor_dict
+ return _sub_dict(self._motor_dict, name)
+
+ def get_joint_direction_dict(self,
+ name: Tuple[Text] = None) -> Dict[Text, int]:
+ if name is None:
+ return self._joint_directions
+ return _sub_dict(self._joint_directions, name)
+
+ def get_joint_offset_dict(self,
+ name: Tuple[Text] = None) -> Dict[Text, float]:
+ if name is None:
+ return self._joint_offsets
+ return _sub_dict(self._joint_offsets, name)
+
+ def get_end_effector_id_dict(self,
+ name: Tuple[Text] = None) -> Dict[Text, int]:
+ if name is None:
+ return self._end_effector_dict
+ return _sub_dict(self._end_effector_dict, name)
+
+ @property
+ def robot_id(self):
+ """Returns the unique object instance id of this loaded URDF in pybullet.
+
+ Note this is different from all other get_{}_id APIs, which returns the
+ joint/link id within this robot instance.
+
+ Returns:
+ The object id as returned by loadURDF.
+ """
+ return self._robot_id
+
+ @property
+ def all_joint_info(self):
+ return self._all_joint_info
+
+ @property
+ def user_dict(self):
+ return self._user_dict
+
+ @property
+ def motor_names(self):
+ return self._motor_names
+
+ @property
+ def constrained_base(self):
+ return self._constrained_base
+
+ def _build_base_dict(self):
+ """Builds the base joints dictionary.
+
+ In pybullet, a link's id within the robot always equal to its parent joint
+ id. So this base joint dict functionaly is equivalent to the base link dict.
+ The dictionary may only contain {ROBOT_BASE: -1} if self._base_names is
+ empty.
+
+ Returns:
+ The base link (joint) ordered dictionary.
+ """
+ base_dict = collections.OrderedDict()
+ if self._base_names is None:
+ base_dict[ROBOT_BASE] = -1
+ else:
+ base_dict.update(_sub_dict(self._joint_name_to_id, self._base_names))
+ return base_dict
+
+ def _build_user_dict(self):
+ """Builds a dictionary using user defined joint groups."""
+ user_dict = collections.OrderedDict()
+ if self._user_group is None:
+ return user_dict
+ for group_name, group_joint_names in self._user_group.items():
+ user_dict[group_name] = collections.OrderedDict()
+ user_dict[group_name].update(
+ _sub_dict(self._joint_name_to_id, group_joint_names))
+ return user_dict
+
+ def _build_all_joint_dict(self):
+ """Extracts all joints from the URDF.
+
+ Finds all joints (fixed or movable) in the URDF and extracts the info. This
+ includes actuated joints (i.e. motors), and non-actuated joints, e.g. the
+ passive joints in Minitaur's four bar mechanism, and fixed joints connecting
+ the toe and the lower legs, etc.
+
+ Returns:
+ number of joints, all joint information as returned by pybullet, and the
+ joint_name_to_id dictionary.
+
+ """
+ num_joints = self._pybullet_client.getNumJoints(self._robot_id)
+ all_joint_info = [
+ self._pybullet_client.getJointInfo(self._robot_id, i)
+ for i in range(num_joints)
+ ]
+
+ # Remove the default joint dampings to increase sim fidelity.
+ for joint_info in all_joint_info:
+ joint_id = joint_info[0]
+ self._pybullet_client.changeDynamics(
+ joint_id, -1, linearDamping=0, angularDamping=0)
+
+ joint_name_to_id = collections.OrderedDict()
+ link_name_to_id = collections.OrderedDict([(ROBOT_BASE, -1)])
+ for joint_info in all_joint_info:
+ joint_name = joint_info[1].decode("UTF-8")
+ joint_id = joint_info[0]
+ joint_name_to_id[joint_name] = joint_id
+ # Index 12 is the name of the joint's child link, and in PyBullet a child
+ # link id is always equal to its parent joint id.
+ link_name_to_id[joint_info[12].decode("UTF-8")] = joint_id
+
+ return num_joints, all_joint_info, joint_name_to_id, link_name_to_id
+
+ def load(
+ self,
+ enable_self_collision: bool = None,
+ init_base_position: Tuple[float] = None,
+ init_base_orientation_quaternion: Tuple[float] = None,
+ init_joint_angles: Dict[Text, float] = None,
+ ):
+ """Reloads the URDF and rebuilds the dictionaries."""
+ if enable_self_collision is None:
+ enable_self_collision = self._enable_self_collision
+ if init_base_position is None:
+ init_base_position = self._init_base_position
+ if init_base_orientation_quaternion is None:
+ init_base_orientation_quaternion = self._init_base_orientation_quaternion
+
+ self._robot_id = self._load_urdf(enable_self_collision, init_base_position,
+ init_base_orientation_quaternion)
+
+ (self._num_joints, self._all_joint_info, self._joint_name_to_id,
+ self._link_name_to_id) = self._build_all_joint_dict()
+ self._base_dict = self._build_base_dict()
+ self._motor_dict = _sub_dict(self._joint_name_to_id, self._motor_names)
+ self._end_effector_dict = _sub_dict(self._joint_name_to_id,
+ self._end_effector_names)
+ self._user_dict = self._build_user_dict()
+
+ self.reset_base_pose(init_base_position, init_base_orientation_quaternion)
+ self.reset_joint_angles(init_joint_angles)
+
+ def _load_urdf(self, enable_self_collision: bool,
+ init_base_position: Tuple[float],
+ init_base_orientation_quaternion: Tuple[float]) -> int:
+ """Loads the URDF and returns the pybullet id."""
+ try:
+ if enable_self_collision:
+ return self._pybullet_client.loadURDF(
+ self._urdf_path,
+ init_base_position,
+ init_base_orientation_quaternion,
+ useFixedBase=self._constrained_base,
+ flags=self._pybullet_client.URDF_USE_SELF_COLLISION)
+ else:
+ return self._pybullet_client.loadURDF(
+ self._urdf_path,
+ init_base_position,
+ init_base_orientation_quaternion,
+ useFixedBase=self._constrained_base,
+ )
+ except:
+ print("!!!!!!!!!!!!!!!!")
+ print("Error: cannot find file:")
+ print(self._urdf_path)
+ import sys
+ sys.exit(0)
+
+ def reset_joint_angles(self, joint_angles: Dict[Text, float] = None):
+ """Resets the joint angles.
+
+ Resets the joint poses. This is instanteneously and will ignore the physics
+ (e.g. collisions, inertias, etc). Should only be used during the episode
+ reset time. Does not work for real robots (other than changing the
+ visualization). This API has no effect if both the input joint_angles and
+ the self._init_joint_angles are None.
+
+ Args:
+ joint_angles: The joint angles in the robot's joint space.
+
+ Raises:
+ AttributeError if the joint directions and joint offsets are not provided
+ during init.
+ """
+ if self._init_joint_angles is None:
+ return
+
+ if joint_angles is None:
+ joint_angles = self._init_joint_angles
+
+ if self._joint_directions is None or self._joint_offsets is None:
+ raise AttributeError(
+ "joint directions and joint offsets not provided in __init__")
+
+ for joint_name, angle in joint_angles.items():
+ urdf_joint_angle = angle * self._joint_directions[
+ joint_name] + self._joint_offsets[joint_name]
+ self._pybullet_client.resetJointState(
+ self._robot_id,
+ self._joint_name_to_id[joint_name],
+ urdf_joint_angle,
+ targetVelocity=0)
+
+ def reset_base_pose(
+ self,
+ base_position: Tuple[float] = None,
+ base_orientation_quaternion: Tuple[float] = None,
+ ):
+ """Resets the base position and orientation.
+
+ Instanteneously re-position the base pose of the robot. Does not work for
+ real robots except for the visualization.
+
+ Args:
+ base_position: Base x, y, z position.
+ base_orientation_quaternion: Base rotation.
+ """
+ if base_position is None:
+ base_position = self._init_base_position
+ if base_orientation_quaternion is None:
+ base_orientation_quaternion = self._init_base_orientation_quaternion
+ self._pybullet_client.resetBasePositionAndOrientation(
+ self._robot_id, base_position, base_orientation_quaternion)
+ self._pybullet_client.resetBaseVelocity(self._robot_id, (0, 0, 0),
+ (0, 0, 0))
+
+ @property
+ def com_dof(self):
+ return 0 if self._constrained_base else 6
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/__init__.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/data_types.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/data_types.py
new file mode 100644
index 000000000..853bb567b
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/data_types.py
@@ -0,0 +1,67 @@
+"""Definitions of safety layer data types."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import attr
+
+
+@attr.s
+class Bound(object):
+ """Struct for inclusive lower and upper bounds."""
+ lower = attr.ib(type=float, default=0)
+ upper = attr.ib(type=float, default=0)
+
+ @upper.validator # pytype: disable=attribute-error
+ def _upper_greator_equal_to_lower(self, attribute, value):
+ del attribute
+ assert value >= self.lower, (
+ "upper bound {} is less than lower bound {}".format(value, self.lower))
+
+
+@attr.s
+class SafetyConfig(object):
+ """Struct to configure the safety module."""
+ motor_position_bound = attr.ib(type=list)
+ motor_position_gain_bound = attr.ib(type=list)
+ motor_velocity_bound = attr.ib(type=list)
+ motor_velocity_gain_bound = attr.ib(type=list)
+ motor_torque_bound = attr.ib(type=list)
+ timestamp_delta_bound = attr.ib(type=Bound)
+ motor_average_abs_velocity_bound = attr.ib(type=list)
+ motor_average_abs_power_bound = attr.ib(type=list)
+ state_action_timestamp_delta_bound = attr.ib(type=float)
+ motor_delta_position_bound = attr.ib(type=list)
+ motor_average_abs_delta_position_bound = attr.ib(type=list)
+
+
+@attr.s
+class MotorState(object):
+ """A generic type for motor state.
+
+ Motor states are what we can potentially read from the motor encoder or
+ firmware APIs.
+
+ """
+ timestamp = attr.ib(type=float, default=None)
+ position = attr.ib(type=float, default=None)
+ position_gain = attr.ib(type=float, default=None)
+ velocity = attr.ib(type=float, default=None)
+ velocity_gain = attr.ib(type=float, default=None)
+ torque = attr.ib(type=float, default=None)
+
+
+@attr.s
+class MotorAction(object):
+ """A generic type for motor action.
+
+ Motor actions are the potential command structure we can send to the motor.
+ While similar to MotorState, they are logically very different entities.
+ """
+ timestamp = attr.ib(type=float, default=None)
+ position = attr.ib(type=float, default=None)
+ position_gain = attr.ib(type=float, default=None)
+ velocity = attr.ib(type=float, default=None)
+ velocity_gain = attr.ib(type=float, default=None)
+ torque = attr.ib(type=float, default=None)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/motor_action_validator.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/motor_action_validator.py
new file mode 100644
index 000000000..567add414
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/motor_action_validator.py
@@ -0,0 +1,128 @@
+"""Validates the motor commands."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import typing
+from pybullet_envs.minitaur.robots import robot_config
+from pybullet_envs.minitaur.robots.safety import data_types
+from pybullet_envs.minitaur.robots.safety import utilities
+from pybullet_envs.minitaur.robots.safety.python import moving_window_filter
+
+_DEQUE_SIZE = 200
+
+
+class MotorActionValidator(object):
+ """A safety guard to check motor actions.
+
+ Monitors the commands sent to the motor and detect unsafe behaviors.
+ """
+
+ def __init__(
+ self,
+ motor_id: typing.Any,
+ position_bound: data_types.Bound,
+ position_gain_bound: data_types.Bound,
+ velocity_bound: data_types.Bound,
+ velocity_gain_bound: data_types.Bound,
+ torque_bound: data_types.Bound,
+ timestamp_delta_bound: data_types.Bound,
+ delta_position_bound: data_types.Bound,
+ average_abs_delta_position_bound: data_types.Bound,
+ state_buffer_size: int = _DEQUE_SIZE,
+ ):
+ """Initializes the class.
+
+ Args:
+ motor_id: Unique ID for the motor.
+ position_bound: The lower/upper bound of the motor angle.
+ position_gain_bound: The lower/upper bound of the motor position gain for
+ PD control.
+ velocity_bound: The lower/upper bound of the motor speed.
+ velocity_gain_bound: The lower/upper bound of the motor velocity gain for
+ PD control.
+ torque_bound: The lower/upper bound of the measured motor torque.
+ timestamp_delta_bound: The range of timestamp difference between two
+ consecutively received motor states.
+ delta_position_bound: The bound between the current motor position and the
+ command position, in position control mode.
+ average_abs_delta_position_bound: The bound for average motor position and
+ command poisition difference.
+ state_buffer_size: The buffer size used to calculate the average.
+ """
+ assert state_buffer_size > 1
+ self._last_motor_state = None
+
+ self._motor_id = motor_id
+ self._position_bound = position_bound
+ self._position_gain_bound = position_gain_bound
+ self._velocity_bound = velocity_bound
+ self._velocity_gain_bound = velocity_gain_bound
+ self._torque_bound = torque_bound
+ self._timestamp_delta_bound = timestamp_delta_bound
+ self._delta_position_bound = delta_position_bound
+ self._average_abs_delta_position_bound = average_abs_delta_position_bound
+ self._abs_delta_position_filter = moving_window_filter.MovingWindowFilter(
+ state_buffer_size)
+
+ def on_state(self, new_state: data_types.MotorState):
+ """Updates the last motor state.
+
+ Args:
+ new_state: The latest motor state.
+ """
+ self._last_motor_state = new_state
+
+ def on_action(self, new_action: data_types.MotorAction,
+ control_mode: robot_config.MotorControlMode):
+ """Adds a new motor action and validates it.
+
+ Args:
+ new_action: A new action that will be send to the motor.
+ control_mode: The motor control mode.
+
+ Raises:
+ safety_error.OutOfBoundError: When any of the motor action fields or
+ state-action difference is out of bound.
+ """
+
+ # We first validate the new state.
+
+ motor_str = "motor {} ".format(self._motor_id)
+ if (control_mode == robot_config.MotorControlMode.POSITION or
+ control_mode == robot_config.MotorControlMode.HYBRID):
+ utilities.assert_in_bound(motor_str + "position", new_action.position,
+ self._position_bound)
+ utilities.assert_in_bound(motor_str + "velocity", new_action.velocity,
+ self._velocity_bound)
+ utilities.assert_in_bound(motor_str + "position gain",
+ new_action.position_gain,
+ self._position_gain_bound)
+ utilities.assert_in_bound(motor_str + "velocity gain",
+ new_action.velocity_gain,
+ self._velocity_gain_bound)
+
+ utilities.assert_in_bound(motor_str + "torque", new_action.torque,
+ self._torque_bound)
+
+ if self._last_motor_state is None:
+ return
+
+ delta_time = new_action.timestamp - self._last_motor_state.timestamp
+ utilities.assert_in_bound(motor_str + "state-action timestamp difference",
+ delta_time, self._timestamp_delta_bound)
+
+ # To detect the bang-bang type controller behavior.
+ if (control_mode == robot_config.MotorControlMode.POSITION or
+ control_mode == robot_config.MotorControlMode.HYBRID):
+ delta_position = new_action.position - self._last_motor_state.position
+ utilities.assert_in_bound(motor_str + "state-action position difference",
+ delta_position, self._delta_position_bound)
+
+ average_abs_delta_position = (
+ self._abs_delta_position_filter.CalculateAverage(
+ abs(delta_position)))
+ utilities.assert_in_bound(
+ motor_str + "average state-action position difference",
+ average_abs_delta_position, self._average_abs_delta_position_bound)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/motor_state_validator.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/motor_state_validator.py
new file mode 100644
index 000000000..3ccf34c02
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/motor_state_validator.py
@@ -0,0 +1,137 @@
+"""Software safety layer for robot control.
+
+Validates the motor states received from the motor encoder.
+
+"""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import typing
+
+from pybullet_envs.minitaur.robots.safety import data_types
+from pybullet_envs.minitaur.robots.safety import utilities
+from pybullet_envs.minitaur.robots.safety.python import moving_window_filter
+
+# The default internal buffer size for the MotorStateValidator.
+_DEQUE_SIZE = 200
+
+
+class MotorStateValidator(object):
+ """A safety guard to check motor states.
+
+ Monitors the status of the motor and detects anomalies in the
+ readings. For example, the class will throw safety errors if the motor
+ velocity is too large. Currently we support checking of motor angle, velocity,
+ gain, torque, as well as the timestamp interval.
+
+ Attributes:
+ last_state: The last received motor state.
+ """
+
+ def __init__(
+ self,
+ motor_id: typing.Any,
+ position_bound: data_types.Bound,
+ position_gain_bound: data_types.Bound,
+ velocity_bound: data_types.Bound,
+ velocity_gain_bound: data_types.Bound,
+ torque_bound: data_types.Bound,
+ timestamp_delta_bound: data_types.Bound,
+ average_abs_velocity_bound: data_types.Bound,
+ average_abs_power_bound: data_types.Bound,
+ state_buffer_size: int = _DEQUE_SIZE,
+ ):
+ """Initializes the class.
+
+ Args:
+ motor_id: Unique ID for the motor.
+ position_bound: The lower/upper bound of the motor angle.
+ position_gain_bound: The lower/upper bound of the motor position gain for
+ PD control.
+ velocity_bound: The lower/upper bound of the motor speed.
+ velocity_gain_bound: The lower/upper bound of the motor velocity gain for
+ PD control.
+ torque_bound: The lower/upper bound of the measured motor torque.
+ timestamp_delta_bound: The range of timestamp difference between two
+ consecutively received motor states.
+ average_abs_velocity_bound: The average absolute velocity limit.
+ average_abs_power_bound: The average absolute mechanical power limit.
+ state_buffer_size: The buffer size used to calculate the average.
+ """
+ assert state_buffer_size > 1
+ self.last_state = None
+
+ self._motor_id = motor_id
+ self._position_bound = position_bound
+ self._position_gain_bound = position_gain_bound
+ self._velocity_bound = velocity_bound
+ self._velocity_gain_bound = velocity_gain_bound
+ self._torque_bound = torque_bound
+ self._timestamp_delta_bound = timestamp_delta_bound
+ self._average_abs_velocity_bound = average_abs_velocity_bound
+ self._average_abs_power_bound = average_abs_power_bound
+
+ # For velocity/power, we use a filter to compute their averages
+ # over a small period. This is to avoid the noisy readings giving false
+ # positive.
+ self._abs_velocity_filter = moving_window_filter.MovingWindowFilter(
+ state_buffer_size)
+ self._abs_power_filter = moving_window_filter.MovingWindowFilter(
+ state_buffer_size)
+
+ def on_state(self, new_state: data_types.MotorState):
+ """Adds a new motor state and validates it.
+
+ Will validate both the instantenous state as well as statitical
+ averages.
+
+ Args:
+ new_state: A new state from the motor encoder.
+
+ Raises:
+ safety_error.OutOfBoundError: When any of the motor readings (e.g.
+ position, torque) is out of bound.
+ """
+
+ # We first validate the new state.
+
+ motor_str = "motor {} ".format(self._motor_id)
+ utilities.assert_in_bound(motor_str + "position", new_state.position,
+ self._position_bound)
+ utilities.assert_in_bound(motor_str + "velocity", new_state.velocity,
+ self._velocity_bound)
+ utilities.assert_in_bound(motor_str + "position gain",
+ new_state.position_gain,
+ self._position_gain_bound)
+ utilities.assert_in_bound(motor_str + "velocity gain",
+ new_state.velocity_gain,
+ self._velocity_gain_bound)
+ utilities.assert_in_bound(motor_str + "torque", new_state.torque,
+ self._torque_bound)
+
+ if not self.last_state:
+ self.last_state = new_state
+ return
+
+ last_state = self.last_state
+
+ # Check if the time interval between two received states are large.
+
+ delta_time = new_state.timestamp - last_state.timestamp
+ utilities.assert_in_bound(motor_str + "timestamp", delta_time,
+ self._timestamp_delta_bound)
+
+ average_abs_velocity = self._abs_velocity_filter.CalculateAverage(
+ abs(new_state.velocity))
+ utilities.assert_in_bound(motor_str + "average velocity",
+ average_abs_velocity,
+ self._average_abs_velocity_bound)
+
+ average_abs_power = self._abs_power_filter.CalculateAverage(
+ abs(new_state.velocity * new_state.torque))
+ utilities.assert_in_bound(motor_str + "average power", average_abs_power,
+ self._average_abs_power_bound)
+
+ self.last_state = new_state
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/python/__init__.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/python/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/python/moving_window_filter.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/python/moving_window_filter.py
new file mode 100644
index 000000000..4249677c5
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/python/moving_window_filter.py
@@ -0,0 +1,68 @@
+"""Moving window filter to smooth out sensor readings."""
+
+import collections
+
+class MovingWindowFilter(object):
+ """A stable O(1) moving filter for incoming data streams.
+
+ We implement the Neumaier's algorithm to calculate the moving window average,
+ which is numerically stable.
+
+ """
+
+ def __init__(self, window_size: int):
+ """Initializes the class.
+
+ Args:
+ window_size: The moving window size.
+ """
+ assert window_size > 0
+ self._window_size = window_size
+ self._value_deque = collections.deque(maxlen=window_size)
+ # The moving window sum.
+ self._sum = 0
+ # The correction term to compensate numerical precision loss during
+ # calculation.
+ self._correction = 0
+
+ def _neumaier_sum(self, value: float):
+ """Update the moving window sum using Neumaier's algorithm.
+
+ For more details please refer to:
+ https://en.wikipedia.org/wiki/Kahan_summation_algorithm#Further_enhancements
+
+ Args:
+ value: The new value to be added to the window.
+ """
+
+ new_sum = self._sum + value
+ if abs(self._sum) >= abs(value):
+ # If self._sum is bigger, low-order digits of value are lost.
+ self._correction += (self._sum - new_sum) + value
+ else:
+ # low-order digits of sum are lost
+ self._correction += (value - new_sum) + self._sum
+
+ self._sum = new_sum
+
+ def calculate_average(self, new_value: float) -> float:
+ """Computes the moving window average in O(1) time.
+
+ Args:
+ new_value: The new value to enter the moving window.
+
+ Returns:
+ The average of the values in the window.
+
+ """
+ deque_len = len(self._value_deque)
+ if deque_len < self._value_deque.maxlen:
+ pass
+ else:
+ # The left most value to be subtracted from the moving sum.
+ self._neumaier_sum(-self._value_deque[0])
+
+ self._neumaier_sum(new_value)
+ self._value_deque.append(new_value)
+
+ return (self._sum + self._correction) / self._window_size
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/safety_checker.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/safety_checker.py
new file mode 100644
index 000000000..765028c0e
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/safety_checker.py
@@ -0,0 +1,119 @@
+"""The generic safety checking interface.
+
+Defines the generic safety checker class that can detect bad motor states, imu
+states, self-collisions, unsafe motor commands, unusual temperature reading,
+etc. Safety criterions are provided by the robot class.
+"""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import typing
+from pybullet_envs.minitaur.robots import robot_config
+from pybullet_envs.minitaur.robots.safety import data_types
+from pybullet_envs.minitaur.robots.safety import motor_action_validator
+from pybullet_envs.minitaur.robots.safety import motor_state_validator
+from pybullet_envs.minitaur.robots.safety import utilities
+
+_MOTOR_STATE_BUFFER_SIZE = 500
+
+
+class SafetyChecker(object):
+ """The generic safety checking interface."""
+
+ def __init__(
+ self,
+ robot: typing.Any,
+ ):
+ """Initilaizes the class.
+
+ TODO(b/131377892): Implement other state checkings including the
+ IMU/temperature/contact force if enabled.
+
+ Args:
+ robot: A robot instance such like Minitaur/Laikago/Vision60.
+ """
+ self._robot = robot
+
+ self._motor_state_validators = []
+ self._motor_action_validators = []
+ for i in range(robot.num_motors):
+ self._motor_state_validators.append(
+ motor_state_validator.MotorStateValidator(
+ motor_id=i,
+ position_bound=robot.safety_config.motor_position_bound[i],
+ position_gain_bound=robot.safety_config
+ .motor_position_gain_bound[i],
+ velocity_bound=robot.safety_config.motor_velocity_bound[i],
+ velocity_gain_bound=robot.safety_config
+ .motor_velocity_gain_bound[i],
+ torque_bound=robot.safety_config.motor_torque_bound[i],
+ timestamp_delta_bound=robot.safety_config.timestamp_delta_bound,
+ average_abs_velocity_bound=robot.safety_config
+ .motor_average_abs_velocity_bound[i],
+ average_abs_power_bound=robot.safety_config
+ .motor_average_abs_power_bound[i],
+ state_buffer_size=_MOTOR_STATE_BUFFER_SIZE,
+ ))
+ self._motor_action_validators.append(
+ motor_action_validator.MotorActionValidator(
+ motor_id=i,
+ position_bound=robot.safety_config.motor_position_bound[i],
+ position_gain_bound=robot.safety_config
+ .motor_position_gain_bound[i],
+ velocity_bound=robot.safety_config.motor_velocity_bound[i],
+ velocity_gain_bound=robot.safety_config
+ .motor_velocity_gain_bound[i],
+ torque_bound=robot.safety_config.motor_torque_bound[i],
+ timestamp_delta_bound=robot.safety_config
+ .state_action_timestamp_delta_bound,
+ delta_position_bound=robot.safety_config
+ .motor_delta_position_bound[i],
+ average_abs_delta_position_bound=robot.safety_config
+ .motor_average_abs_delta_position_bound[i],
+ state_buffer_size=_MOTOR_STATE_BUFFER_SIZE,
+ ))
+
+ def check_state(self) -> None:
+ """Validates the state of the robot.
+
+ TODO(b/131377892): Implement other state checkings including the
+ IMU/temperature/contact force if enabled.
+
+ Raises:
+ A safety exception if any state checking (motor/imu/etc) fails.
+ """
+
+ for motor_id, state_validator, action_validator in zip(
+ range(self._robot.num_motors), self._motor_state_validators,
+ self._motor_action_validators):
+ motor_state = data_types.MotorState(
+ timestamp=self._robot.last_state_time,
+ position=self._robot.GetMotorAngles()[motor_id],
+ velocity=self._robot.GetMotorVelocities()[motor_id],
+ position_gain=self._robot.GetMotorPositionGains()[motor_id],
+ velocity_gain=self._robot.GetMotorVelocityGains()[motor_id],
+ torque=self._robot.GetMotorTorques()[motor_id],
+ )
+ state_validator.on_state(motor_state)
+ action_validator.on_state(motor_state)
+
+ def check_motor_action(
+ self,
+ action: typing.Sequence[float],
+ control_mode: robot_config.MotorControlMode,
+ ) -> None:
+ """Validate the action w.r.t to the motor states.
+
+ Args:
+ action: The motor commands sent to the robot.
+ control_mode: The motor control mode.
+
+ Raises:
+ A safety exception if action checking fails.
+ """
+ motor_action_list = utilities.convert_to_motor_action(
+ self._robot, action, control_mode)
+ for motor_id, validator in enumerate(self._motor_action_validators):
+ validator.on_action(motor_action_list[motor_id], control_mode)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/safety_error.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/safety_error.py
new file mode 100644
index 000000000..5ff078a6f
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/safety_error.py
@@ -0,0 +1,11 @@
+"""Exception types for safety related error."""
+
+
+class SafetyError(Exception):
+ """The base safety exception."""
+ pass
+
+
+class OutOfBoundError(SafetyError):
+ """Rasied when values like motor position or velocity is out of bound."""
+ pass
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/utilities.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/utilities.py
new file mode 100644
index 000000000..9703b6bf0
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/utilities.py
@@ -0,0 +1,163 @@
+"""Utilities for safety layers."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import collections
+import typing
+from pybullet_envs.minitaur.robots import robot_config
+from pybullet_envs.minitaur.robots.safety import data_types
+from pybullet_envs.minitaur.robots.safety import safety_error
+
+
+def assert_in_bound(name: typing.Text, value: float, bound: data_types.Bound):
+ """Check if the given value is within the provided bound.
+
+ Args:
+ name: The name of the value.
+ value: Number to be checked.
+ bound: Contains the lower and upper bounds. The bound is inclusive.
+
+ Raises:
+ safety_error.OutofBoundError: when the value is outside the bound.
+ """
+ if bound.lower <= value <= bound.upper:
+ return
+ else:
+ raise safety_error.OutOfBoundError("{} is out of bound {} for {}".format(
+ value, bound, name))
+
+
+def convert_to_motor_action(
+ robot: typing.Any,
+ action: typing.Sequence[float],
+ control_mode: robot_config.MotorControlMode,
+):
+ """Converts the input action to generic MotorAction classes.
+
+ Args:
+ robot: An robot instance.
+ action: The motor commands sent to the robot.
+ control_mode: The motor control mode.
+
+ Returns:
+ The list of converted MotorAction instances.
+ """
+ motor_action_list = []
+ if control_mode == robot_config.MotorControlMode.POSITION:
+ for motor_id, position in enumerate(action):
+ motor_action_list.append(
+ data_types.MotorAction(
+ timestamp=robot.last_action_time,
+ position=position,
+ position_gain=robot.GetMotorPositionGains()[motor_id],
+ velocity=0,
+ velocity_gain=robot.GetMotorVelocityGains()[motor_id],
+ torque=0))
+
+ if (control_mode == robot_config.MotorControlMode.TORQUE or
+ control_mode == robot_config.MotorControlMode.PWM):
+ for motor_id, torque in enumerate(action):
+ motor_action_list.append(
+ data_types.MotorAction(
+ timestamp=robot.last_action_time,
+ position=0,
+ position_gain=0,
+ velocity=0,
+ velocity_gain=0,
+ torque=torque))
+
+ if control_mode == robot_config.MotorControlMode.HYBRID:
+ for motor_id in range(robot.num_motors):
+ position_index = (
+ motor_id * robot_config.HYBRID_ACTION_DIMENSION +
+ robot_config.HybridActionIndex.POSITION.value)
+ position_gain_index = (
+ motor_id * robot_config.HYBRID_ACTION_DIMENSION +
+ robot_config.HybridActionIndex.POSITION_GAIN.value)
+ velocity_index = (
+ motor_id * robot_config.HYBRID_ACTION_DIMENSION +
+ robot_config.HybridActionIndex.VELOCITY.value)
+ velocity_gain_index = (
+ motor_id * robot_config.HYBRID_ACTION_DIMENSION +
+ robot_config.HybridActionIndex.VELOCITY_GAIN.value)
+ torque_index = (
+ motor_id * robot_config.HYBRID_ACTION_DIMENSION +
+ robot_config.HybridActionIndex.TORQUE.value)
+ motor_action_list.append(
+ data_types.MotorAction(
+ timestamp=robot.last_action_time,
+ position=action[position_index],
+ position_gain=action[position_gain_index],
+ velocity=action[velocity_index],
+ velocity_gain=action[velocity_gain_index],
+ torque=action[torque_index]))
+
+ return motor_action_list
+
+
+class MovingWindowFilter(object):
+ """A stable O(1) moving filter for incoming data streams.
+
+ We implement the Neumaier's algorithm to calculate the moving window average,
+ which is numerically stable.
+
+ """
+
+ def __init__(self, window_size: int):
+ """Initializes the class.
+
+ Args:
+ window_size: The moving window size.
+ """
+ assert window_size > 0
+ self._window_size = window_size
+ self._value_deque = collections.deque(maxlen=window_size)
+ # The moving window sum.
+ self._sum = 0
+ # The correction term to compensate numerical precision loss during
+ # calculation.
+ self._correction = 0
+
+ def _neumaier_sum(self, value: float):
+ """Update the moving window sum using Neumaier's algorithm.
+
+ For more details please refer to:
+ https://en.wikipedia.org/wiki/Kahan_summation_algorithm#Further_enhancements
+
+ Args:
+ value: The new value to be added to the window.
+ """
+
+ new_sum = self._sum + value
+ if abs(self._sum) >= abs(value):
+ # If self._sum is bigger, low-order digits of value are lost.
+ self._correction += (self._sum - new_sum) + value
+ else:
+ # low-order digits of sum are lost
+ self._correction += (value - new_sum) + self._sum
+
+ self._sum = new_sum
+
+ def calculate_average(self, new_value: float) -> float:
+ """Computes the moving window average in O(1) time.
+
+ Args:
+ new_value: The new value to enter the moving window.
+
+ Returns:
+ The average of the values in the window.
+
+ """
+ deque_len = len(self._value_deque)
+ if deque_len < self._value_deque.maxlen:
+ pass
+ else:
+ # The left most value to be subtracted from the moving sum.
+ self._neumaier_sum(-self._value_deque[0])
+
+ self._neumaier_sum(new_value)
+ self._value_deque.append(new_value)
+
+ return (self._sum + self._correction) / self._window_size
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/time_ordered_buffer.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/time_ordered_buffer.py
new file mode 100644
index 000000000..52cf6b86d
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/time_ordered_buffer.py
@@ -0,0 +1,263 @@
+# Lint as: python3
+"""The common class to manage the a stream of past data."""
+
+import collections
+from typing import Any, List, Sequence, Union
+import dataclasses
+import gin
+import numpy as np
+
+
+@dataclasses.dataclass
+class BufferTuple:
+ value_0: Any
+ value_1: Any
+ coeff: float
+
+
+FloatOrArray = Union[float, Sequence[float]]
+BufferTupleOrArray = Union[BufferTuple, Sequence[BufferTuple]]
+TIME_IDX = 0
+VALUE_IDX = 1
+
+
+@gin.configurable
+class TimeOrderedBuffer(object):
+ """A buffer to hold and extract history data."""
+
+ def __init__(self,
+ max_buffer_timespan: float,
+ error_on_timestamp_reversal: bool = True,
+ error_on_duplicate_timestamp: bool = True,
+ replace_value_on_duplicate_timestamp: bool = False,
+ ):
+ """Initializes the class.
+
+ Args:
+ max_buffer_timespan: Maximum amount of buffer by time to keep.
+ error_on_timestamp_reversal: Whether to throw error if inverted timestamps
+ are found.
+ error_on_duplicate_timestamp: Whether to throw error if the incoming
+ data has the same timestamp as the latest timestamp in the buffer.
+ replace_value_on_duplicate_timestamp: Whether to keep the new value when
+ a duplicate timestamp has occurred. This only applies if we are not
+ throwing an error on duplicate timestamps.
+ """
+ if max_buffer_timespan < 0:
+ raise ValueError(
+ "Invalid max_buffer_timespan: {}".format(max_buffer_timespan))
+ self._max_buffer_timespan = max_buffer_timespan
+ self._error_on_timestamp_reversal = error_on_timestamp_reversal
+ self._error_on_duplicate_timestamp = error_on_duplicate_timestamp
+ self._replace_value_on_duplicate_timestamp = (
+ replace_value_on_duplicate_timestamp)
+ # TODO(tsangwei): Look for a ring buffer implementation.
+ self._buffer = collections.deque()
+
+ def reset(self):
+ self._buffer.clear()
+
+ def _compute_coeff(self,
+ newer_time: float,
+ older_time: float,
+ target_time: float,
+ ) -> float:
+ """Compute the coefficient value between the two timestamps.
+
+ Args:
+ newer_time: The newer timestamp.
+ older_time: The older timestamp.
+ target_time: Target timestamp that is between the newer and older values.
+
+ Returns:
+ The coefficient as a float.
+ """
+ coeff = 0.0
+ # Prevents divide by 0 error.
+ if newer_time != older_time:
+ assert older_time <= target_time <= newer_time
+ coeff = (newer_time - target_time) / (newer_time - older_time)
+ return coeff
+
+ def _pack_data(self,
+ obs_newer: Any,
+ obs_older: Any,
+ target_time: float,
+ ) -> BufferTuple:
+ """Packs up buffer data as BufferTuple dataclass.
+
+ Args:
+ obs_newer: Timestamp and value of newer observation.
+ obs_older: Timestamp and value of older observation.
+ target_time: Target timestamp of the observation we are looking for.
+
+ Returns:
+ BufferTuple dataclass.
+ """
+ coeff = self._compute_coeff(newer_time=obs_newer[TIME_IDX],
+ older_time=obs_older[TIME_IDX],
+ target_time=target_time)
+ return BufferTuple(value_0=obs_newer[VALUE_IDX],
+ value_1=obs_older[VALUE_IDX],
+ coeff=coeff)
+
+ def _find_values_at(self, timestamp_targets: Sequence[float]) -> List[Any]:
+ """Get the lower/upper bound values for given target timestamp.
+
+ Args:
+ timestamp_targets: Actual timestamp value to match against.
+
+ Returns:
+ List of BufferTuple dataclass.
+ """
+ results = [None] * len(timestamp_targets)
+ oldest_obs = self._buffer[0]
+ latest_obs = self._buffer[-1]
+
+ search_start_idx = None
+ search_end_idx = None
+
+ # Check to make sure we do not try to search for values outside of the
+ # current buffer.
+ for i in range(len(timestamp_targets)):
+ # Oldest observation have the smallest timestamp.
+ if timestamp_targets[i] <= oldest_obs[TIME_IDX]:
+ results[i] = self._pack_data(obs_newer=oldest_obs,
+ obs_older=oldest_obs,
+ target_time=timestamp_targets[i])
+ elif timestamp_targets[i] >= latest_obs[TIME_IDX]:
+ results[i] = self._pack_data(obs_newer=latest_obs,
+ obs_older=latest_obs,
+ target_time=timestamp_targets[i])
+ else:
+ if search_end_idx is None:
+ search_end_idx = i
+ search_start_idx = i
+
+ if search_end_idx is not None:
+ results = self._walkthrough_buffer(timestamp_targets=timestamp_targets,
+ search_start_idx=search_start_idx,
+ search_end_idx=search_end_idx,
+ results=results)
+
+ return results
+
+ def _walkthrough_buffer(self,
+ timestamp_targets: List[float],
+ search_start_idx: int,
+ search_end_idx: int,
+ results: List[BufferTuple],
+ ) -> List[BufferTuple]:
+ """Actual method to walk through the buffer looking for requested values.
+
+ Args:
+ timestamp_targets: List of timestamps to search for in buffer.
+ search_start_idx: Index number for timestamp_targets to start searching
+ from.
+ search_end_idx: Index number for timestamp_targets to stop searching at.
+ results: List of BufferTuple values that covers out of bound results.
+
+ Returns:
+ List of BufferTuple values.
+ """
+ value_older = None
+ target_idx = search_start_idx
+ target_timestamp = timestamp_targets[target_idx]
+ value_older = self._buffer[0]
+
+ # Searching from oldest timestamp to latest timestamp.
+ for value_newer in self._buffer:
+ # Catch edge case scenario where multiple timestamp targets are between
+ # the same two buffer timestamps. (b/157104935)
+ while value_newer[TIME_IDX] >= target_timestamp:
+ # Catch special edge case scenario if using older_obs_blender method.
+ obs_older = value_newer if (
+ value_newer[TIME_IDX] == target_timestamp) else value_older
+ results[target_idx] = self._pack_data(obs_newer=value_newer,
+ obs_older=obs_older,
+ target_time=target_timestamp)
+ if target_idx - 1 >= search_end_idx:
+ target_idx -= 1
+ target_timestamp = timestamp_targets[target_idx]
+ else:
+ return results
+ value_older = value_newer
+
+ return results
+
+ def add(self, timestamp: float, value: Any):
+ """Inserts timestamp and value into buffer.
+
+ Args:
+ timestamp: Timestamp of the data value.
+ value: Data value to be saved into the buffer.
+ """
+ if self._buffer:
+ last_timestamp = self._buffer[-1][TIME_IDX]
+ if last_timestamp == timestamp:
+ if (not np.array_equal(self._buffer[-1][VALUE_IDX], value) and
+ self._error_on_duplicate_timestamp):
+ raise ValueError("Duplicate timestamp detected: {}".format(timestamp))
+ else:
+ # Duplicate message detected.
+ if self._replace_value_on_duplicate_timestamp:
+ self._buffer[-1] = (timestamp, value)
+ return
+ if last_timestamp > timestamp and self._error_on_timestamp_reversal:
+ raise ValueError(
+ "Time reversal detected: new timestamp is {} vs last timestamp {}"
+ .format(timestamp, last_timestamp))
+ # Dropping old buffer data that exceed buffer timespan limit and making
+ # sure the buffer does not go empty.
+ while (len(self._buffer) > 1 and self._max_buffer_timespan <
+ (timestamp - self._buffer[1][TIME_IDX])):
+ self._buffer.popleft()
+ self._buffer.append((timestamp, value))
+
+ def get_delayed_value(self, latency: FloatOrArray) -> BufferTupleOrArray:
+ """Retrieves value in the history buffer according to latency.
+
+ Finds the closest pair of values that are some time (i.e. latency) ago from
+ the most recent timestamp. Suppose the history buffer looks like this:
+
+ [(0, val_x),...,(0.6, val_k-1), (0.7, val_k), (0.8, val_k+1),...,(2, val_0)]
+
+ And the latency is '1.33', then this API will locate the values with
+ timestamps close to 2 - 1.33 = 0.67. So in this case, it will return the
+ pair (0.7, val_k) and (0.6, val_k+1), as well as a blending coefficient
+ which is calculated by (0.7 - 0.67) / (0.7 - 0.6) = 0.3. This blending coeff
+ can be used to linearly interpolate the returned values, i.e. val_1 * (1 -
+ coeff) + val_2 * coeff, if the multiply operator is defined.
+
+ Args:
+ latency: The time interval(s) to look backwards in the history buffer from
+ the most recent timestamp.
+
+ Returns:
+ An array of BufferTuple dataclass.
+
+ Raises:
+ ValueError: if the latency is negative.
+ BufferError: if the buffer is empty.
+ """
+ buffer_len = len(self._buffer)
+ if buffer_len == 0:
+ raise BufferError("The buffer is empty. Have you called 'add'?")
+
+ single_latency = isinstance(latency, (int, float))
+
+ if single_latency:
+ if latency < 0:
+ raise ValueError("Latency cannot be negative.")
+ else:
+ if any(value < 0 for value in latency):
+ raise ValueError("Latency list contains negative values.")
+ if latency != sorted(latency):
+ raise ValueError("Invalid unsorted latency list.")
+
+ target_list = [latency] if single_latency else latency
+ current_time = self._buffer[-1][TIME_IDX]
+ target_list = [current_time - offset for offset in target_list]
+
+ result = self._find_values_at(target_list)
+ return result[0] if single_latency else result
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/timestamp.proto b/examples/pybullet/gym/pybullet_envs/minitaur/robots/timestamp.proto
new file mode 100644
index 000000000..9c4c75cc3
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/timestamp.proto
@@ -0,0 +1,147 @@
+// Protocol Buffers - Google's data interchange format
+// Copyright 2008 Google Inc. All rights reserved.
+// https://developers.google.com/protocol-buffers/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Google Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+syntax = "proto3";
+
+package google.protobuf;
+
+option csharp_namespace = "Google.Protobuf.WellKnownTypes";
+option cc_enable_arenas = true;
+option go_package = "google.golang.org/protobuf/types/known/timestamppb";
+option java_package = "com.google.protobuf";
+option java_outer_classname = "TimestampProto";
+option java_multiple_files = true;
+option objc_class_prefix = "GPB";
+
+// A Timestamp represents a point in time independent of any time zone or local
+// calendar, encoded as a count of seconds and fractions of seconds at
+// nanosecond resolution. The count is relative to an epoch at UTC midnight on
+// January 1, 1970, in the proleptic Gregorian calendar which extends the
+// Gregorian calendar backwards to year one.
+//
+// All minutes are 60 seconds long. Leap seconds are "smeared" so that no leap
+// second table is needed for interpretation, using a [24-hour linear
+// smear](https://developers.google.com/time/smear).
+//
+// The range is from 0001-01-01T00:00:00Z to 9999-12-31T23:59:59.999999999Z. By
+// restricting to that range, we ensure that we can convert to and from [RFC
+// 3339](https://www.ietf.org/rfc/rfc3339.txt) date strings.
+//
+// # Examples
+//
+// Example 1: Compute Timestamp from POSIX `time()`.
+//
+// Timestamp timestamp;
+// timestamp.set_seconds(time(NULL));
+// timestamp.set_nanos(0);
+//
+// Example 2: Compute Timestamp from POSIX `gettimeofday()`.
+//
+// struct timeval tv;
+// gettimeofday(&tv, NULL);
+//
+// Timestamp timestamp;
+// timestamp.set_seconds(tv.tv_sec);
+// timestamp.set_nanos(tv.tv_usec * 1000);
+//
+// Example 3: Compute Timestamp from Win32 `GetSystemTimeAsFileTime()`.
+//
+// FILETIME ft;
+// GetSystemTimeAsFileTime(&ft);
+// UINT64 ticks = (((UINT64)ft.dwHighDateTime) << 32) | ft.dwLowDateTime;
+//
+// // A Windows tick is 100 nanoseconds. Windows epoch 1601-01-01T00:00:00Z
+// // is 11644473600 seconds before Unix epoch 1970-01-01T00:00:00Z.
+// Timestamp timestamp;
+// timestamp.set_seconds((INT64) ((ticks / 10000000) - 11644473600LL));
+// timestamp.set_nanos((INT32) ((ticks % 10000000) * 100));
+//
+// Example 4: Compute Timestamp from Java `System.currentTimeMillis()`.
+//
+// long millis = System.currentTimeMillis();
+//
+// Timestamp timestamp = Timestamp.newBuilder().setSeconds(millis / 1000)
+// .setNanos((int) ((millis % 1000) * 1000000)).build();
+//
+//
+// Example 5: Compute Timestamp from Java `Instant.now()`.
+//
+// Instant now = Instant.now();
+//
+// Timestamp timestamp =
+// Timestamp.newBuilder().setSeconds(now.getEpochSecond())
+// .setNanos(now.getNano()).build();
+//
+//
+// Example 6: Compute Timestamp from current time in Python.
+//
+// timestamp = Timestamp()
+// timestamp.GetCurrentTime()
+//
+// # JSON Mapping
+//
+// In JSON format, the Timestamp type is encoded as a string in the
+// [RFC 3339](https://www.ietf.org/rfc/rfc3339.txt) format. That is, the
+// format is "{year}-{month}-{day}T{hour}:{min}:{sec}[.{frac_sec}]Z"
+// where {year} is always expressed using four digits while {month}, {day},
+// {hour}, {min}, and {sec} are zero-padded to two digits each. The fractional
+// seconds, which can go up to 9 digits (i.e. up to 1 nanosecond resolution),
+// are optional. The "Z" suffix indicates the timezone ("UTC"); the timezone
+// is required. A proto3 JSON serializer should always use UTC (as indicated by
+// "Z") when printing the Timestamp type and a proto3 JSON parser should be
+// able to accept both UTC and other timezones (as indicated by an offset).
+//
+// For example, "2017-01-15T01:30:15.01Z" encodes 15.01 seconds past
+// 01:30 UTC on January 15, 2017.
+//
+// In JavaScript, one can convert a Date object to this format using the
+// standard
+// [toISOString()](https://developer.mozilla.org/en-US/docs/Web/JavaScript/Reference/Global_Objects/Date/toISOString)
+// method. In Python, a standard `datetime.datetime` object can be converted
+// to this format using
+// [`strftime`](https://docs.python.org/2/library/time.html#time.strftime) with
+// the time format spec '%Y-%m-%dT%H:%M:%S.%fZ'. Likewise, in Java, one can use
+// the Joda Time's [`ISODateTimeFormat.dateTime()`](
+// http://www.joda.org/joda-time/apidocs/org/joda/time/format/ISODateTimeFormat.html#dateTime%2D%2D
+// ) to obtain a formatter capable of generating timestamps in this format.
+//
+//
+message Timestamp {
+ // Represents seconds of UTC time since Unix epoch
+ // 1970-01-01T00:00:00Z. Must be from 0001-01-01T00:00:00Z to
+ // 9999-12-31T23:59:59Z inclusive.
+ int64 seconds = 1;
+
+ // Non-negative fractions of a second at nanosecond resolution. Negative
+ // second values with fractions must still have non-negative nanos values
+ // that count forward in time. Must be from 0 to 999,999,999
+ // inclusive.
+ int32 nanos = 2;
+}
\ No newline at end of file
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/timestamp_pb2.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/timestamp_pb2.py
new file mode 100644
index 000000000..b14188991
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/timestamp_pb2.py
@@ -0,0 +1,78 @@
+# -*- coding: utf-8 -*-
+# Generated by the protocol buffer compiler. DO NOT EDIT!
+# source: timestamp.proto
+"""Generated protocol buffer code."""
+from google.protobuf import descriptor as _descriptor
+from google.protobuf import message as _message
+from google.protobuf import reflection as _reflection
+from google.protobuf import symbol_database as _symbol_database
+# @@protoc_insertion_point(imports)
+
+_sym_db = _symbol_database.Default()
+
+
+
+
+DESCRIPTOR = _descriptor.FileDescriptor(
+ name='timestamp.proto',
+ package='google.protobuf',
+ syntax='proto3',
+ serialized_options=b'\n\023com.google.protobufB\016TimestampProtoP\001Z2google.golang.org/protobuf/types/known/timestamppb\370\001\001\242\002\003GPB\252\002\036Google.Protobuf.WellKnownTypes',
+ create_key=_descriptor._internal_create_key,
+ serialized_pb=b'\n\x0ftimestamp.proto\x12\x0fgoogle.protobuf\"+\n\tTimestamp\x12\x0f\n\x07seconds\x18\x01 \x01(\x03\x12\r\n\x05nanos\x18\x02 \x01(\x05\x42\x85\x01\n\x13\x63om.google.protobufB\x0eTimestampProtoP\x01Z2google.golang.org/protobuf/types/known/timestamppb\xf8\x01\x01\xa2\x02\x03GPB\xaa\x02\x1eGoogle.Protobuf.WellKnownTypesb\x06proto3'
+)
+
+
+
+
+_TIMESTAMP = _descriptor.Descriptor(
+ name='Timestamp',
+ full_name='google.protobuf.Timestamp',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='seconds', full_name='google.protobuf.Timestamp.seconds', index=0,
+ number=1, type=3, cpp_type=2, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='nanos', full_name='google.protobuf.Timestamp.nanos', index=1,
+ number=2, type=5, cpp_type=1, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=36,
+ serialized_end=79,
+)
+
+DESCRIPTOR.message_types_by_name['Timestamp'] = _TIMESTAMP
+_sym_db.RegisterFileDescriptor(DESCRIPTOR)
+
+Timestamp = _reflection.GeneratedProtocolMessageType('Timestamp', (_message.Message,), {
+ 'DESCRIPTOR' : _TIMESTAMP,
+ '__module__' : 'timestamp_pb2'
+ # @@protoc_insertion_point(class_scope:google.protobuf.Timestamp)
+ })
+_sym_db.RegisterMessage(Timestamp)
+
+
+DESCRIPTOR._options = None
+# @@protoc_insertion_point(module_scope)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/utilities/__init__.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/utilities/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/utilities/action_filter.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/utilities/action_filter.py
new file mode 100644
index 000000000..6fb100cb3
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/utilities/action_filter.py
@@ -0,0 +1,239 @@
+"""Two types of filters which can be applied to policy output sequences.
+
+1. Simple exponential filter
+2. Butterworth filter - lowpass or bandpass
+
+The implementation of the butterworth filter follows scipy's lfilter
+https://github.com/scipy/scipy/blob/v1.2.1/scipy/signal/signaltools.py
+
+We re-implement the logic in order to explicitly manage the y states
+
+The filter implements::
+ a[0]*y[n] = b[0]*x[n] + b[1]*x[n-1] + ... + b[M]*x[n-M]
+ - a[1]*y[n-1] - ... - a[N]*y[n-N]
+
+We assume M == N.
+"""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import collections
+from absl import logging
+import gin
+import numpy as np
+from scipy.signal import butter
+
+ACTION_FILTER_ORDER = 2
+ACTION_FILTER_LOW_CUT = 0.0
+ACTION_FILTER_HIGH_CUT = 4.0
+
+
+@gin.configurable
+class ActionFilter(object):
+ """Implements a generic lowpass or bandpass action filter."""
+
+ def __init__(self, a, b, order, num_joints, ftype='lowpass'):
+ """Initializes filter.
+
+ Either one per joint or same for all joints.
+
+ Args:
+ a: filter output history coefficients
+ b: filter input coefficients
+ order: filter order
+ num_joints: robot DOF
+ ftype: filter type. 'lowpass' or 'bandpass'
+ """
+ self.num_joints = num_joints
+ if isinstance(a, list):
+ self.a = a
+ self.b = b
+ else:
+ self.a = [a]
+ self.b = [b]
+
+ # Either a set of parameters per joint must be specified as a list
+ # Or one filter is applied to every joint
+ if not ((len(self.a) == len(self.b) == num_joints) or (
+ len(self.a) == len(self.b) == 1)):
+ raise ValueError('Incorrect number of filter values specified')
+
+ # Normalize by a[0]
+ for i in range(len(self.a)):
+ self.b[i] /= self.a[i][0]
+ self.a[i] /= self.a[i][0]
+
+ # Convert single filter to same format as filter per joint
+ if len(self.a) == 1:
+ self.a *= num_joints
+ self.b *= num_joints
+ self.a = np.stack(self.a)
+ self.b = np.stack(self.b)
+
+ if ftype == 'bandpass':
+ assert len(self.b[0]) == len(self.a[0]) == 2 * order + 1
+ self.hist_len = 2 * order
+ elif ftype == 'lowpass':
+ assert len(self.b[0]) == len(self.a[0]) == order + 1
+ self.hist_len = order
+ else:
+ raise ValueError('%s filter type not supported' % (ftype))
+
+ logging.info('Filter shapes: a: %s, b: %s', self.a.shape, self.b.shape)
+ logging.info('Filter type:%s', ftype)
+
+ self.yhist = collections.deque(maxlen=self.hist_len)
+ self.xhist = collections.deque(maxlen=self.hist_len)
+ self.reset()
+
+ def reset(self):
+ """Resets the history buffers to 0."""
+ self.yhist.clear()
+ self.xhist.clear()
+ for _ in range(self.hist_len):
+ self.yhist.appendleft(np.zeros((self.num_joints, 1)))
+ self.xhist.appendleft(np.zeros((self.num_joints, 1)))
+
+ def filter(self, x):
+ """Returns filtered x."""
+ xs = np.concatenate(list(self.xhist), axis=-1)
+ ys = np.concatenate(list(self.yhist), axis=-1)
+ y = np.multiply(x, self.b[:, 0]) + np.sum(
+ np.multiply(xs, self.b[:, 1:]), axis=-1) - np.sum(
+ np.multiply(ys, self.a[:, 1:]), axis=-1)
+ self.xhist.appendleft(x.reshape((self.num_joints, 1)).copy())
+ self.yhist.appendleft(y.reshape((self.num_joints, 1)).copy())
+ return y
+
+ def init_history(self, x):
+ x = np.expand_dims(x, axis=-1)
+ for i in range(self.hist_len):
+ self.xhist[i] = x
+ self.yhist[i] = x
+ return
+
+
+@gin.configurable
+class ActionFilterButter(ActionFilter):
+ """Butterworth filter."""
+
+ def __init__(self,
+ lowcut=None,
+ highcut=None,
+ sampling_rate=None,
+ order=ACTION_FILTER_ORDER,
+ num_joints=None):
+ """Initializes a butterworth filter.
+
+ Either one per joint or same for all joints.
+
+ Args:
+ lowcut: list of strings defining the low cutoff frequencies.
+ The list must contain either 1 element (same filter for all joints)
+ or num_joints elements
+ 0 for lowpass, > 0 for bandpass. Either all values must be 0
+ or all > 0
+ highcut: list of strings defining the high cutoff frequencies.
+ The list must contain either 1 element (same filter for all joints)
+ or num_joints elements
+ All must be > 0
+ sampling_rate: frequency of samples in Hz
+ order: filter order
+ num_joints: robot DOF
+ """
+ self.lowcut = ([float(x) for x in lowcut]
+ if lowcut is not None else [ACTION_FILTER_LOW_CUT])
+ self.highcut = ([float(x) for x in highcut]
+ if highcut is not None else [ACTION_FILTER_HIGH_CUT])
+ if len(self.lowcut) != len(self.highcut):
+ raise ValueError('Number of lowcut and highcut filter values should '
+ 'be the same')
+
+ if sampling_rate is None:
+ raise ValueError('sampling_rate should be provided.')
+
+ if num_joints is None:
+ raise ValueError('num_joints should be provided.')
+
+ if np.any(self.lowcut):
+ if not np.all(self.lowcut):
+ raise ValueError('All the filters must be of the same type: '
+ 'lowpass or bandpass')
+ self.ftype = 'bandpass'
+ else:
+ self.ftype = 'lowpass'
+
+ a_coeffs = []
+ b_coeffs = []
+ for i, (l, h) in enumerate(zip(self.lowcut, self.highcut)):
+ if h <= 0.0:
+ raise ValueError('Highcut must be > 0')
+
+ b, a = self.butter_filter(l, h, sampling_rate, order)
+ logging.info(
+ 'Butterworth filter: joint: %d, lowcut: %f, highcut: %f, '
+ 'sampling rate: %d, order: %d, num joints: %d', i, l, h,
+ sampling_rate, order, num_joints)
+ b_coeffs.append(b)
+ a_coeffs.append(a)
+
+ super(ActionFilterButter, self).__init__(
+ a_coeffs, b_coeffs, order, num_joints, self.ftype)
+
+ def butter_filter(self, lowcut, highcut, fs, order=5):
+ """Returns the coefficients of a butterworth filter.
+
+ If lowcut = 0, the function returns the coefficients of a low pass filter.
+ Otherwise, the coefficients of a band pass filter are returned.
+ Highcut should be > 0
+
+ Args:
+ lowcut: low cutoff frequency
+ highcut: high cutoff frequency
+ fs: sampling rate
+ order: filter order
+ Return:
+ b, a: parameters of a butterworth filter
+ """
+ nyq = 0.5 * fs
+ low = lowcut / nyq
+ high = highcut / nyq
+ if low:
+ b, a = butter(order, [low, high], btype='band')
+ else:
+ b, a = butter(order, [high], btype='low')
+ return b, a
+
+
+class ActionFilterExp(ActionFilter):
+ """Filter by way of simple exponential smoothing.
+
+ y = alpha * x + (1 - alpha) * previous_y
+ """
+
+ def __init__(self, alpha, num_joints):
+ """Initialize the filter.
+
+ Args:
+ alpha: list of strings defining the alphas.
+ The list must contain either 1 element (same filter for all joints)
+ or num_joints elements
+ 0 < alpha <= 1
+ num_joints: robot DOF
+ """
+ self.alphas = [float(x) for x in alpha]
+ logging.info('Exponential filter: alpha: %d', self.alphas)
+
+ a_coeffs = []
+ b_coeffs = []
+ for a in self.alphas:
+ a_coeffs.append(np.asarray([1., a - 1.]))
+ b_coeffs.append(np.asarray([a, 0]))
+
+ order = 1
+ self.ftype = 'lowpass'
+
+ super(ActionFilterExp, self).__init__(
+ a_coeffs, b_coeffs, order, num_joints, self.ftype)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/utilities/kinematics.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/utilities/kinematics.py
new file mode 100644
index 000000000..be5824400
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/utilities/kinematics.py
@@ -0,0 +1,127 @@
+"""The inverse kinematic utilities."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import numpy as np
+import typing
+
+_IDENTITY_ORIENTATION = (0, 0, 0, 1)
+
+
+def joint_angles_from_link_position(
+ robot: typing.Any,
+ link_position: typing.Sequence[float],
+ link_id: int,
+ joint_ids: typing.Sequence[int],
+ position_in_world_frame=False,
+ base_translation: typing.Sequence[float] = (0, 0, 0),
+ base_rotation: typing.Sequence[float] = (0, 0, 0, 1)):
+ """Uses Inverse Kinematics to calculate joint angles.
+
+ Args:
+ robot: A robot instance.
+ link_position: The (x, y, z) of the link in the body or the world frame,
+ depending on whether the argument position_in_world_frame is true.
+ link_id: The link id as returned from loadURDF.
+ joint_ids: The positional index of the joints. This can be different from
+ the joint unique ids.
+ position_in_world_frame: Whether the input link_position is specified
+ in the world frame or the robot's base frame.
+ base_translation: Additional base translation.
+ base_rotation: Additional base rotation.
+
+ Returns:
+ A list of joint angles.
+ """
+ if not position_in_world_frame:
+ # Projects to local frame.
+ base_position, base_orientation = robot.GetBasePosition(
+ ), robot.GetBaseOrientation()
+ base_position, base_orientation = robot.pybullet_client.multiplyTransforms(
+ base_position, base_orientation, base_translation, base_rotation)
+
+ # Projects to world space.
+ world_link_pos, _ = robot.pybullet_client.multiplyTransforms(
+ base_position, base_orientation, link_position, _IDENTITY_ORIENTATION)
+ else:
+ world_link_pos = link_position
+
+ ik_solver = 0
+ all_joint_angles = robot.pybullet_client.calculateInverseKinematics(
+ robot.quadruped, link_id, world_link_pos, solver=ik_solver)
+
+ # Extract the relevant joint angles.
+ joint_angles = [all_joint_angles[i] for i in joint_ids]
+ return joint_angles
+
+
+def link_position_in_world_frame(
+ robot: typing.Any,
+ link_id: int,
+):
+ """Computes the link's position in the world frame.
+
+ Args:
+ robot: A robot instance.
+ link_id: The link id to calculate its position.
+
+ Returns:
+ The position of the link in the world frame.
+ """
+ return np.array(
+ robot.pybullet_client.getLinkState(robot.quadruped, link_id)[0])
+
+
+def link_position_in_base_frame(
+ robot: typing.Any,
+ link_id: int,
+):
+ """Computes the link's local position in the robot frame.
+
+ Args:
+ robot: A robot instance.
+ link_id: The link to calculate its relative position.
+
+ Returns:
+ The relative position of the link.
+ """
+ base_position, base_orientation = robot.GetBasePosition(
+ ), robot.GetBaseOrientation()
+ inverse_translation, inverse_rotation = robot.pybullet_client.invertTransform(
+ base_position, base_orientation)
+
+ link_state = robot.pybullet_client.getLinkState(robot.quadruped, link_id)
+ link_position = link_state[0]
+ link_local_position, _ = robot.pybullet_client.multiplyTransforms(
+ inverse_translation, inverse_rotation, link_position, (0, 0, 0, 1))
+
+ return np.array(link_local_position)
+
+
+def compute_jacobian(
+ robot: typing.Any,
+ link_id: int,
+):
+ """Computes the Jacobian matrix for the given link.
+
+ Args:
+ robot: A robot instance.
+ link_id: The link id as returned from loadURDF.
+
+ Returns:
+ The 3 x N transposed Jacobian matrix. where N is the total DoFs of the
+ robot. For a quadruped, the first 6 columns of the matrix corresponds to
+ the CoM translation and rotation. The columns corresponds to a leg can be
+ extracted with indices [6 + leg_id * 3: 6 + leg_id * 3 + 3].
+ """
+
+ all_joint_angles = [state[0] for state in robot.joint_states]
+ zero_vec = [0] * len(all_joint_angles)
+ jv, _ = robot.pybullet_client.calculateJacobian(robot.quadruped, link_id,
+ (0, 0, 0), all_joint_angles,
+ zero_vec, zero_vec)
+ jacobian = np.array(jv)
+ assert jacobian.shape[0] == 3
+ return jacobian
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/utilities/kinematics_utils.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/utilities/kinematics_utils.py
new file mode 100644
index 000000000..088f2bcb9
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/utilities/kinematics_utils.py
@@ -0,0 +1,208 @@
+# Lint as: python3
+"""The inverse kinematic utilities."""
+from typing import Optional, Sequence
+
+import numpy as np
+
+from pybullet_utils import bullet_client
+
+_IDENTITY_ROTATION_QUAT = (0, 0, 0, 1)
+_IK_SOLVER_TYPE = 0
+_LINK_POS_INDEX = 0
+
+
+def joint_angles_from_link_positions(
+ pybullet_client: bullet_client.BulletClient,
+ urdf_id: int,
+ link_ids: Sequence[int],
+ link_positions: Sequence[Sequence[float]],
+ positions_are_in_world_frame: bool = False,
+ joint_dof_ids: Optional[Sequence[int]] = None,
+) -> np.ndarray:
+ """Uses Inverse Kinematics to calculate joint angles.
+
+ Args:
+ pybullet_client: The bullet client.
+ urdf_id: The unique id returned after loading URDF.
+ link_ids: The link ids to compute the IK.
+ link_positions: The (x, y, z) of the links in the body or the world frame,
+ depending on whether the argument link_position_in_world_frame is true.
+ positions_are_in_world_frame: Whether the input link positions are specified
+ in the world frame or the robot's base frame.
+ joint_dof_ids: The degrees of freedom index of the joints we want to extract
+ the angles. This can be different from the joint unique ids. For example,
+ a fixed joint will increase the joint unique id but will not increase the
+ number of degree of freedoms. The joint dof id can be extracted in
+ PyBullet by getJointInfo, which corresponds to the "qIndex" in the
+ returned values. If not specified, will return all movable joint angles.
+
+ Returns:
+ A list of joint angles.
+ """
+ if positions_are_in_world_frame:
+ world_link_positions = link_positions
+ else:
+ # The PyBullet inverse Kinematics Calculation depends on the current URDF
+ # position/orientation, and we cannot pass them to the API. So we have to
+ # always query the current base position/orientation to compute world frame
+ # link positions.
+ urdf_base_position, urdf_base_orientation = (
+ pybullet_client.getBasePositionAndOrientation(urdf_id))
+ world_link_positions = []
+ for link_position in link_positions:
+ world_link_position, _ = pybullet_client.multiplyTransforms(
+ urdf_base_position, urdf_base_orientation, link_position,
+ _IDENTITY_ROTATION_QUAT)
+ world_link_positions.append(world_link_position)
+
+ # Notice that the API expects the link positions in the world frame.
+ all_joint_angles = pybullet_client.calculateInverseKinematics2(
+ urdf_id, link_ids, world_link_positions, solver=_IK_SOLVER_TYPE)
+
+ # Extract the relevant joint angles.
+ if joint_dof_ids is None:
+ return np.array(all_joint_angles)
+
+ return np.array([all_joint_angles[i] for i in joint_dof_ids])
+
+
+def link_position_in_world_frame(
+ pybullet_client: bullet_client.BulletClient,
+ urdf_id: int,
+ link_id: int,
+):
+ """Computes the link's position in the world frame.
+
+ Args:
+ pybullet_client: The bullet client.
+ urdf_id: The unique id returned after loading URDF.
+ link_id: The link id to calculate its position.
+
+ Returns:
+ The position of the link in the world frame.
+ """
+ return np.array(pybullet_client.getLinkState(urdf_id, link_id)[0])
+
+
+def link_position_in_base_frame(
+ pybullet_client: bullet_client.BulletClient,
+ urdf_id: int,
+ link_id: int,
+ base_link_id: Optional[int] = None,
+):
+ """Computes the link's local position in the robot frame.
+
+ Args:
+ pybullet_client: The bullet client.
+ urdf_id: The unique id returned after loading URDF.
+ link_id: The link to calculate its relative position.
+ base_link_id: The link id of the base. For the kinematics robot, such as
+ wheeled_robot_base, three additional joints are added to connect the world
+ and the base. The base_link_id is no longer -1, and need to be passed in.
+
+
+ Returns:
+ The relative position of the link.
+ """
+ if base_link_id is None:
+ base_position, base_orientation = (
+ pybullet_client.getBasePositionAndOrientation(urdf_id))
+ else:
+ base_link_state = pybullet_client.getLinkState(urdf_id, base_link_id)
+ base_position, base_orientation = base_link_state[0], base_link_state[1]
+
+ inverse_translation, inverse_rotation = pybullet_client.invertTransform(
+ base_position, base_orientation)
+
+ link_state = pybullet_client.getLinkState(urdf_id, link_id)
+ link_position = link_state[0]
+ link_local_position, _ = pybullet_client.multiplyTransforms(
+ inverse_translation, inverse_rotation, link_position, (0, 0, 0, 1))
+
+ return np.array(link_local_position)
+
+
+def compute_jacobian(
+ pybullet_client: bullet_client.BulletClient,
+ urdf_id: int,
+ link_id: int,
+ all_joint_positions: Sequence[float],
+ additional_translation: Optional[Sequence[float]] = (0, 0, 0),
+) -> np.ndarray:
+ """Computes the Jacobian matrix for the given point on a link.
+
+ CAVEAT: If during URDF loading process additional rotations are provided, the
+ computed Jacobian are also transformed.
+
+ Args:
+ pybullet_client: The bullet client.
+ urdf_id: The unique id returned after loading URDF.
+ link_id: The link id as returned from loadURDF.
+ all_joint_positions: all the joint positions of the robot. This should
+ include the dummy/kinematic drive joints for the wheeled robot.
+ additional_translation: The additional translation of the point in the link
+ CoM frame.
+
+ Returns:
+ The 3 x N transposed Jacobian matrix. where N is the total DoFs of the
+ robot. For a quadruped, the first 6 columns of the matrix corresponds to
+ the CoM translation and rotation. The columns corresponds to a leg can be
+ extracted with indices [6 + leg_id * 3: 6 + leg_id * 3 + 3].
+ """
+
+ zero_vec = [0] * len(all_joint_positions)
+ jv, _ = pybullet_client.calculateJacobian(
+ urdf_id,
+ link_id,
+ additional_translation,
+ all_joint_positions,
+ objVelocities=zero_vec,
+ objAccelerations=zero_vec)
+ jacobian = np.array(jv)
+ assert jacobian.shape[0] == 3
+ return jacobian
+
+
+def rotate_to_base_frame(
+ pybullet_client: bullet_client.BulletClient,
+ urdf_id: int,
+ vector: Sequence[float],
+ init_orientation_inv_quat: Optional[Sequence[float]] = (0, 0, 0, 1)
+) -> np.ndarray:
+ """Rotates the input vector to the base coordinate systems.
+
+ Note: This is different from world frame to base frame transformation, as we
+ do not apply any translation here.
+
+ Args:
+ pybullet_client: The bullet client.
+ urdf_id: The unique id returned after loading URDF.
+ vector: Input vector in the world frame.
+ init_orientation_inv_quat:
+
+ Returns:
+ A rotated vector in the base frame.
+ """
+ _, base_orientation_quat = (
+ pybullet_client.getBasePositionAndOrientation(urdf_id))
+ _, base_orientation_quat_from_init = pybullet_client.multiplyTransforms(
+ positionA=(0, 0, 0),
+ orientationA=init_orientation_inv_quat,
+ positionB=(0, 0, 0),
+ orientationB=base_orientation_quat)
+ _, inverse_base_orientation = pybullet_client.invertTransform(
+ [0, 0, 0], base_orientation_quat_from_init)
+
+ # PyBullet transforms requires simple list/tuple or it may crash.
+ if isinstance(vector, np.ndarray):
+ vector_list = vector.tolist()
+ else:
+ vector_list = vector
+
+ local_vector, _ = pybullet_client.multiplyTransforms(
+ positionA=(0, 0, 0),
+ orientationA=inverse_base_orientation,
+ positionB=vector_list,
+ orientationB=(0, 0, 0, 1),
+ )
+ return np.array(local_vector)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/utilities/urdf_utils.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/utilities/urdf_utils.py
new file mode 100644
index 000000000..f45d4ff86
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/utilities/urdf_utils.py
@@ -0,0 +1,59 @@
+# Lint as: python3
+"""Utilities for robot URDF models."""
+
+from typing import Text
+
+# In the return from pybullet.getJointInfo, the name of the link whose parent is
+# that joint.
+LINK_NAME_INDEX = 12
+# Indication that link_name_to_id should return -1. This is a constant because
+# different URDFs use different names for their base links.
+BASE_LINK = ""
+
+
+def link_name_to_id(link_name: Text, robot_id: int, pybullet_client) -> int:
+ """Returns the pybullet integer link id corresponding to link_name.
+
+ Args:
+ link_name: The name of the link from the URDF. If this is BASE_LINK, returns
+ -1, the link id of the base according to pybullet convention.
+ robot_id: Integer id of the robot to which the link belongs, as returned by
+ pybullet.loadURDF().
+ pybullet_client: Client in which the robot is loaded.
+
+ Returns:
+ Integer id of the link.
+
+ Raises:
+ ValueError if the link_name is not found in the robot.
+ """
+ if link_name == BASE_LINK:
+ return -1
+ link_name_list = []
+ for i in range(pybullet_client.getNumJoints(robot_id)):
+ joint_info = pybullet_client.getJointInfo(robot_id, i)
+ link_name_i = joint_info[LINK_NAME_INDEX].decode("UTF-8")
+ if link_name_i == link_name:
+ return i
+ link_name_list.append(link_name_i)
+ raise ValueError("Link name '{}' not found in URDF. Options are: {}".format(
+ link_name, link_name_list))
+
+
+def set_collision_filter_group_mask(urdf_id: int, group: int, mask: int,
+ pybullet_client):
+ """Sets the collision filter group and mask to the robot.
+
+ TODO(tingnan): Check if this has side effects with self collision flags
+ when loading URDF.
+
+ Args:
+ urdf_id: The URDF id as returned by the loadURDF.
+ group: The collision group the robot is in. By default, all dynamics objects
+ in PyBullet use collision group 1.
+ mask: The collision bit mask to use. See go/pybullet for details.
+ pybullet_client: The bullet client to use.
+ """
+ # We includes "-1" for the base link of the URDF.
+ for link_id in range(-1, pybullet_client.getNumJoints(urdf_id)):
+ pybullet_client.setCollisionFilterGroupMask(urdf_id, link_id, group, mask)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/vector.proto b/examples/pybullet/gym/pybullet_envs/minitaur/robots/vector.proto
new file mode 100644
index 000000000..92c48f859
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/vector.proto
@@ -0,0 +1,84 @@
+syntax = "proto3";
+
+package robotics.messages;
+
+option cc_enable_arenas = true;
+
+// A four-dimensional double precision vector.
+message Vector4d {
+ double x = 1;
+ double y = 2;
+ double z = 3;
+ double w = 4;
+}
+
+// A four-dimensional single precision vector.
+message Vector4f {
+ float x = 1;
+ float y = 2;
+ float z = 3;
+ float w = 4;
+}
+
+// A four-dimensional integer vector.
+message Vector4i {
+ int64 x = 1;
+ int64 y = 2;
+ int64 z = 3;
+ int64 w = 4;
+}
+
+// A three-dimensional double precision vector.
+message Vector3d {
+ double x = 1;
+ double y = 2;
+ double z = 3;
+}
+
+// A three-dimensional single precision vector.
+message Vector3f {
+ float x = 1;
+ float y = 2;
+ float z = 3;
+}
+
+// A three-dimensional integer vector.
+message Vector3i {
+ int64 x = 1;
+ int64 y = 2;
+ int64 z = 3;
+}
+
+// A two-dimensional double precision vector.
+message Vector2d {
+ double x = 1;
+ double y = 2;
+}
+
+// A two-dimensional single precision vector.
+message Vector2f {
+ float x = 1;
+ float y = 2;
+}
+
+// A two-dimensional integer vector.
+message Vector2i {
+ int64 x = 1;
+ int64 y = 2;
+}
+
+// Double precision vector of arbitrary size.
+message Vectord {
+ repeated double data = 1 [packed = true];
+}
+
+// Single precision vector of arbitrary size.
+message Vectorf {
+ repeated float data = 1 [packed = true];
+}
+
+// Integer vector of arbitrary size.
+message Vectori {
+ repeated int64 data = 1 [packed = true];
+}
+
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/vector_pb2.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/vector_pb2.py
new file mode 100644
index 000000000..ccaad55b4
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/vector_pb2.py
@@ -0,0 +1,640 @@
+# -*- coding: utf-8 -*-
+# Generated by the protocol buffer compiler. DO NOT EDIT!
+# source: vector.proto
+"""Generated protocol buffer code."""
+from google.protobuf import descriptor as _descriptor
+from google.protobuf import message as _message
+from google.protobuf import reflection as _reflection
+from google.protobuf import symbol_database as _symbol_database
+# @@protoc_insertion_point(imports)
+
+_sym_db = _symbol_database.Default()
+
+
+
+
+DESCRIPTOR = _descriptor.FileDescriptor(
+ name='vector.proto',
+ package='robotics.messages',
+ syntax='proto3',
+ serialized_options=b'\370\001\001',
+ create_key=_descriptor._internal_create_key,
+ serialized_pb=b'\n\x0cvector.proto\x12\x11robotics.messages\"6\n\x08Vector4d\x12\t\n\x01x\x18\x01 \x01(\x01\x12\t\n\x01y\x18\x02 \x01(\x01\x12\t\n\x01z\x18\x03 \x01(\x01\x12\t\n\x01w\x18\x04 \x01(\x01\"6\n\x08Vector4f\x12\t\n\x01x\x18\x01 \x01(\x02\x12\t\n\x01y\x18\x02 \x01(\x02\x12\t\n\x01z\x18\x03 \x01(\x02\x12\t\n\x01w\x18\x04 \x01(\x02\"6\n\x08Vector4i\x12\t\n\x01x\x18\x01 \x01(\x03\x12\t\n\x01y\x18\x02 \x01(\x03\x12\t\n\x01z\x18\x03 \x01(\x03\x12\t\n\x01w\x18\x04 \x01(\x03\"+\n\x08Vector3d\x12\t\n\x01x\x18\x01 \x01(\x01\x12\t\n\x01y\x18\x02 \x01(\x01\x12\t\n\x01z\x18\x03 \x01(\x01\"+\n\x08Vector3f\x12\t\n\x01x\x18\x01 \x01(\x02\x12\t\n\x01y\x18\x02 \x01(\x02\x12\t\n\x01z\x18\x03 \x01(\x02\"+\n\x08Vector3i\x12\t\n\x01x\x18\x01 \x01(\x03\x12\t\n\x01y\x18\x02 \x01(\x03\x12\t\n\x01z\x18\x03 \x01(\x03\" \n\x08Vector2d\x12\t\n\x01x\x18\x01 \x01(\x01\x12\t\n\x01y\x18\x02 \x01(\x01\" \n\x08Vector2f\x12\t\n\x01x\x18\x01 \x01(\x02\x12\t\n\x01y\x18\x02 \x01(\x02\" \n\x08Vector2i\x12\t\n\x01x\x18\x01 \x01(\x03\x12\t\n\x01y\x18\x02 \x01(\x03\"\x1b\n\x07Vectord\x12\x10\n\x04\x64\x61ta\x18\x01 \x03(\x01\x42\x02\x10\x01\"\x1b\n\x07Vectorf\x12\x10\n\x04\x64\x61ta\x18\x01 \x03(\x02\x42\x02\x10\x01\"\x1b\n\x07Vectori\x12\x10\n\x04\x64\x61ta\x18\x01 \x03(\x03\x42\x02\x10\x01\x42\x03\xf8\x01\x01\x62\x06proto3'
+)
+
+
+
+
+_VECTOR4D = _descriptor.Descriptor(
+ name='Vector4d',
+ full_name='robotics.messages.Vector4d',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='x', full_name='robotics.messages.Vector4d.x', index=0,
+ number=1, type=1, cpp_type=5, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='y', full_name='robotics.messages.Vector4d.y', index=1,
+ number=2, type=1, cpp_type=5, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='z', full_name='robotics.messages.Vector4d.z', index=2,
+ number=3, type=1, cpp_type=5, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='w', full_name='robotics.messages.Vector4d.w', index=3,
+ number=4, type=1, cpp_type=5, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=35,
+ serialized_end=89,
+)
+
+
+_VECTOR4F = _descriptor.Descriptor(
+ name='Vector4f',
+ full_name='robotics.messages.Vector4f',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='x', full_name='robotics.messages.Vector4f.x', index=0,
+ number=1, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='y', full_name='robotics.messages.Vector4f.y', index=1,
+ number=2, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='z', full_name='robotics.messages.Vector4f.z', index=2,
+ number=3, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='w', full_name='robotics.messages.Vector4f.w', index=3,
+ number=4, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=91,
+ serialized_end=145,
+)
+
+
+_VECTOR4I = _descriptor.Descriptor(
+ name='Vector4i',
+ full_name='robotics.messages.Vector4i',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='x', full_name='robotics.messages.Vector4i.x', index=0,
+ number=1, type=3, cpp_type=2, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='y', full_name='robotics.messages.Vector4i.y', index=1,
+ number=2, type=3, cpp_type=2, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='z', full_name='robotics.messages.Vector4i.z', index=2,
+ number=3, type=3, cpp_type=2, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='w', full_name='robotics.messages.Vector4i.w', index=3,
+ number=4, type=3, cpp_type=2, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=147,
+ serialized_end=201,
+)
+
+
+_VECTOR3D = _descriptor.Descriptor(
+ name='Vector3d',
+ full_name='robotics.messages.Vector3d',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='x', full_name='robotics.messages.Vector3d.x', index=0,
+ number=1, type=1, cpp_type=5, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='y', full_name='robotics.messages.Vector3d.y', index=1,
+ number=2, type=1, cpp_type=5, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='z', full_name='robotics.messages.Vector3d.z', index=2,
+ number=3, type=1, cpp_type=5, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=203,
+ serialized_end=246,
+)
+
+
+_VECTOR3F = _descriptor.Descriptor(
+ name='Vector3f',
+ full_name='robotics.messages.Vector3f',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='x', full_name='robotics.messages.Vector3f.x', index=0,
+ number=1, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='y', full_name='robotics.messages.Vector3f.y', index=1,
+ number=2, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='z', full_name='robotics.messages.Vector3f.z', index=2,
+ number=3, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=248,
+ serialized_end=291,
+)
+
+
+_VECTOR3I = _descriptor.Descriptor(
+ name='Vector3i',
+ full_name='robotics.messages.Vector3i',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='x', full_name='robotics.messages.Vector3i.x', index=0,
+ number=1, type=3, cpp_type=2, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='y', full_name='robotics.messages.Vector3i.y', index=1,
+ number=2, type=3, cpp_type=2, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='z', full_name='robotics.messages.Vector3i.z', index=2,
+ number=3, type=3, cpp_type=2, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=293,
+ serialized_end=336,
+)
+
+
+_VECTOR2D = _descriptor.Descriptor(
+ name='Vector2d',
+ full_name='robotics.messages.Vector2d',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='x', full_name='robotics.messages.Vector2d.x', index=0,
+ number=1, type=1, cpp_type=5, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='y', full_name='robotics.messages.Vector2d.y', index=1,
+ number=2, type=1, cpp_type=5, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=338,
+ serialized_end=370,
+)
+
+
+_VECTOR2F = _descriptor.Descriptor(
+ name='Vector2f',
+ full_name='robotics.messages.Vector2f',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='x', full_name='robotics.messages.Vector2f.x', index=0,
+ number=1, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='y', full_name='robotics.messages.Vector2f.y', index=1,
+ number=2, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=372,
+ serialized_end=404,
+)
+
+
+_VECTOR2I = _descriptor.Descriptor(
+ name='Vector2i',
+ full_name='robotics.messages.Vector2i',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='x', full_name='robotics.messages.Vector2i.x', index=0,
+ number=1, type=3, cpp_type=2, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='y', full_name='robotics.messages.Vector2i.y', index=1,
+ number=2, type=3, cpp_type=2, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=406,
+ serialized_end=438,
+)
+
+
+_VECTORD = _descriptor.Descriptor(
+ name='Vectord',
+ full_name='robotics.messages.Vectord',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='data', full_name='robotics.messages.Vectord.data', index=0,
+ number=1, type=1, cpp_type=5, label=3,
+ has_default_value=False, default_value=[],
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=b'\020\001', file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=440,
+ serialized_end=467,
+)
+
+
+_VECTORF = _descriptor.Descriptor(
+ name='Vectorf',
+ full_name='robotics.messages.Vectorf',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='data', full_name='robotics.messages.Vectorf.data', index=0,
+ number=1, type=2, cpp_type=6, label=3,
+ has_default_value=False, default_value=[],
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=b'\020\001', file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=469,
+ serialized_end=496,
+)
+
+
+_VECTORI = _descriptor.Descriptor(
+ name='Vectori',
+ full_name='robotics.messages.Vectori',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='data', full_name='robotics.messages.Vectori.data', index=0,
+ number=1, type=3, cpp_type=2, label=3,
+ has_default_value=False, default_value=[],
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=b'\020\001', file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=498,
+ serialized_end=525,
+)
+
+DESCRIPTOR.message_types_by_name['Vector4d'] = _VECTOR4D
+DESCRIPTOR.message_types_by_name['Vector4f'] = _VECTOR4F
+DESCRIPTOR.message_types_by_name['Vector4i'] = _VECTOR4I
+DESCRIPTOR.message_types_by_name['Vector3d'] = _VECTOR3D
+DESCRIPTOR.message_types_by_name['Vector3f'] = _VECTOR3F
+DESCRIPTOR.message_types_by_name['Vector3i'] = _VECTOR3I
+DESCRIPTOR.message_types_by_name['Vector2d'] = _VECTOR2D
+DESCRIPTOR.message_types_by_name['Vector2f'] = _VECTOR2F
+DESCRIPTOR.message_types_by_name['Vector2i'] = _VECTOR2I
+DESCRIPTOR.message_types_by_name['Vectord'] = _VECTORD
+DESCRIPTOR.message_types_by_name['Vectorf'] = _VECTORF
+DESCRIPTOR.message_types_by_name['Vectori'] = _VECTORI
+_sym_db.RegisterFileDescriptor(DESCRIPTOR)
+
+Vector4d = _reflection.GeneratedProtocolMessageType('Vector4d', (_message.Message,), {
+ 'DESCRIPTOR' : _VECTOR4D,
+ '__module__' : 'vector_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.Vector4d)
+ })
+_sym_db.RegisterMessage(Vector4d)
+
+Vector4f = _reflection.GeneratedProtocolMessageType('Vector4f', (_message.Message,), {
+ 'DESCRIPTOR' : _VECTOR4F,
+ '__module__' : 'vector_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.Vector4f)
+ })
+_sym_db.RegisterMessage(Vector4f)
+
+Vector4i = _reflection.GeneratedProtocolMessageType('Vector4i', (_message.Message,), {
+ 'DESCRIPTOR' : _VECTOR4I,
+ '__module__' : 'vector_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.Vector4i)
+ })
+_sym_db.RegisterMessage(Vector4i)
+
+Vector3d = _reflection.GeneratedProtocolMessageType('Vector3d', (_message.Message,), {
+ 'DESCRIPTOR' : _VECTOR3D,
+ '__module__' : 'vector_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.Vector3d)
+ })
+_sym_db.RegisterMessage(Vector3d)
+
+Vector3f = _reflection.GeneratedProtocolMessageType('Vector3f', (_message.Message,), {
+ 'DESCRIPTOR' : _VECTOR3F,
+ '__module__' : 'vector_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.Vector3f)
+ })
+_sym_db.RegisterMessage(Vector3f)
+
+Vector3i = _reflection.GeneratedProtocolMessageType('Vector3i', (_message.Message,), {
+ 'DESCRIPTOR' : _VECTOR3I,
+ '__module__' : 'vector_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.Vector3i)
+ })
+_sym_db.RegisterMessage(Vector3i)
+
+Vector2d = _reflection.GeneratedProtocolMessageType('Vector2d', (_message.Message,), {
+ 'DESCRIPTOR' : _VECTOR2D,
+ '__module__' : 'vector_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.Vector2d)
+ })
+_sym_db.RegisterMessage(Vector2d)
+
+Vector2f = _reflection.GeneratedProtocolMessageType('Vector2f', (_message.Message,), {
+ 'DESCRIPTOR' : _VECTOR2F,
+ '__module__' : 'vector_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.Vector2f)
+ })
+_sym_db.RegisterMessage(Vector2f)
+
+Vector2i = _reflection.GeneratedProtocolMessageType('Vector2i', (_message.Message,), {
+ 'DESCRIPTOR' : _VECTOR2I,
+ '__module__' : 'vector_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.Vector2i)
+ })
+_sym_db.RegisterMessage(Vector2i)
+
+Vectord = _reflection.GeneratedProtocolMessageType('Vectord', (_message.Message,), {
+ 'DESCRIPTOR' : _VECTORD,
+ '__module__' : 'vector_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.Vectord)
+ })
+_sym_db.RegisterMessage(Vectord)
+
+Vectorf = _reflection.GeneratedProtocolMessageType('Vectorf', (_message.Message,), {
+ 'DESCRIPTOR' : _VECTORF,
+ '__module__' : 'vector_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.Vectorf)
+ })
+_sym_db.RegisterMessage(Vectorf)
+
+Vectori = _reflection.GeneratedProtocolMessageType('Vectori', (_message.Message,), {
+ 'DESCRIPTOR' : _VECTORI,
+ '__module__' : 'vector_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.Vectori)
+ })
+_sym_db.RegisterMessage(Vectori)
+
+
+DESCRIPTOR._options = None
+_VECTORD.fields_by_name['data']._options = None
+_VECTORF.fields_by_name['data']._options = None
+_VECTORI.fields_by_name['data']._options = None
+# @@protoc_insertion_point(module_scope)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/vision/__init__.py b/examples/pybullet/gym/pybullet_envs/minitaur/vision/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/vision/imagery.proto b/examples/pybullet/gym/pybullet_envs/minitaur/vision/imagery.proto
new file mode 100644
index 000000000..ef3a639d7
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/vision/imagery.proto
@@ -0,0 +1,72 @@
+syntax = "proto3";
+
+package robotics.reinforcement_learning.minitaur.vision;
+
+import "google/protobuf/timestamp.proto";
+
+// The basic image protobuf. Used for RPC/IPC transmission.
+message Image {
+ // The buffer that contains the actual image data.
+ bytes content = 1;
+
+ // The image storage format. Can be raw or compressed types.
+ enum ImageFormat {
+ IMAGE_FORMAT_UNSPECIFIED = 0;
+
+ // An 32-bit gray-scale image/matrix with row-major (height, width) memroy
+ // layout. Each pixel is a 32-bit floating-point number.
+ IMAGE_FORMAT_GRAY_HW_32F = 1;
+
+ // An 8-bit BGRA raw image format, with HWC memory layout (e.g. the image is
+ // stored as a row-major matrix (height, width) of pixels, with each pixel
+ // an uint8[num_color_channels] packed array. This is the same format as
+ // CV_8UC4.
+ IMAGE_FORMAT_BGRA_HWC_8U = 2;
+
+ // The 16-bit grayscale images with row-major memory layout. This is the
+ // default depth format for intel RealSense cameras. Each pixel is a 16 bit
+ // unsigned integer.
+ IMAGE_FORMAT_GRAY_HW_16U = 3;
+
+ // An 8-bit RGB raw image format, with HWC memory layout. This is the
+ // default color format for intel RealSense cameras.
+ IMAGE_FORMAT_RGB_HWC_8U = 4;
+
+ // TODO(tingnan): Add supports for different image formats like I420 or
+ // JPEG.
+ }
+
+ ImageFormat image_format = 2;
+
+ // The UTC time at which the image is taken.
+ google.protobuf.Timestamp timestamp = 3;
+
+ // Image width and height in pixels. Critical for decoding raw images and
+ // optional for compressed JPEG/PNGs which already embed these information.
+ int32 width_px = 4;
+ int32 height_px = 5;
+}
+
+// A captured frame from camera can combine multiple images from different
+// streams, IR, depth, VGA.
+message CameraFrame {
+ map images = 1;
+ string camera_id = 2;
+}
+
+// Get the latest raw image from camera.
+message GetFrameRequest {
+ // TODO: Also enable camera id in this proto.
+}
+
+// Stacked frames. The imagery service can decide how many frames to transmit
+// for each GetFrameRequest.
+message CameraFrameCollection {
+ repeated CameraFrame frames = 1;
+}
+
+// The capture session start/stop request.
+message CaptureRequest {
+ string run_id = 1;
+ string logging_path = 2;
+}
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/vision/imagery_client.py b/examples/pybullet/gym/pybullet_envs/minitaur/vision/imagery_client.py
new file mode 100644
index 000000000..ca2a2e689
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/vision/imagery_client.py
@@ -0,0 +1,137 @@
+"""The imagery client to connect to the camera job."""
+
+from typing import Any, Dict, Sequence, Text
+import gin
+
+from pybullet_envs.minitaur.fw_bridge import worker_builder
+from pybullet_envs.minitaur.vision import imagery_pb2
+from pybullet_envs.minitaur.vision import imagery_utils
+from google3.third_party.fluxworks.core.fluxworks.python.genericutil_py import fwassert
+from google3.third_party.fluxworks.core.fluxworks.python.genericutil_py import timeutil
+
+_RPC_TIMEOUT = 1 * timeutil.TimeUtil.SEC
+
+_URI_START_CAPTURE = "fwuri://VisionJob/StartCapture"
+_URI_STOP_CAPTURE = "fwuri://VisionJob/StopCapture"
+_URI_GET_FRAME = "fwuri://VisionJob/GetFrame"
+
+
+@gin.configurable
+class ImageryClient(object):
+ """Sends commands and receives states from cameras."""
+
+ def __init__(
+ self,
+ fw_worker=None,
+ rpc_timeout_sec=_RPC_TIMEOUT,
+ ip_address=None,
+ port=None,
+ async_mode=False,
+ start_capture_uri: Text = _URI_START_CAPTURE,
+ stop_capture_uri: Text = _URI_STOP_CAPTURE,
+ get_frame_uri: Text = _URI_GET_FRAME,
+ ):
+ """Initializes the client.
+
+ Args:
+ fw_worker: A FluxWorks worker instance.
+ rpc_timeout_sec: The timeout for any RPC calls from this client.
+ ip_address: The ip address of the camera/vision process. If vision job is
+ also instantiated in the same FluxWorks worker, both ip address and port
+ are not needed.
+ port: The port of the camera/vision process.
+ async_mode: Whether the RPC calls in this client are async or synchronous.
+ Aync mode is only required when you have multiple workers communicating
+ with each other in the same Python process. If worker A is calling
+ worker B's RPC, worker B's RPC is trying to get GIL from it's thread but
+ caller (worker A) already holds the GIL, and this will cause a dead lock
+ if worker A's calls are synchronous. If worker A is calling its own RPC,
+ the same GIL can be used so there is no dead lock, and there is no need
+ for async mode. Async mode will require context switching and thus is a
+ bit slower.
+ start_capture_uri: The FluxWorks URI to start camera capture.
+ stop_capture_uri: The FluxWorks URI to stop camera capture.
+ get_frame_uri: The FluxWorks URI to get camera frames.
+ """
+ self._rpc_timeout_sec = rpc_timeout_sec
+ if fw_worker is None:
+ fw_worker = worker_builder.GetDefaultWorker()
+ self._worker = fw_worker
+
+ # TODO(tingnan): Use a single address and split the string for FW.
+ if ip_address is not None:
+ self._worker.ConnectToWorker(ip_address, port)
+
+ self._async_mode = async_mode
+ self._start_capture_uri = start_capture_uri
+ self._stop_capture_uri = stop_capture_uri
+ self._get_frame_uri = get_frame_uri
+
+ def _convert_camera_frame_to_image_dict(
+ self, camera_frame: imagery_pb2.CameraFrame):
+ """Converts the camera frame to an image dictionary."""
+ # Each camera frame might contain multiple image channels, such as rgb and
+ # depth.
+ images = {}
+ for image_name, image_proto in camera_frame.images.items():
+ image_array = imagery_utils.convert_image_to_array(image_proto)
+ images[image_name] = image_array
+ return images
+
+ def start_capture(self, run_id: Text = "vision"):
+ """Starts the camera capture session.
+
+ Args:
+ run_id: The capture session id. This id will determine the name of the
+ image logs' sub-direcotry.
+ """
+ capture_request = imagery_pb2.CaptureRequest()
+ capture_request.run_id = run_id
+ fwassert.FwAssert.CheckErrorMessage(
+ self._worker.CallOnewayProtoRpc(
+ self._start_capture_uri,
+ capture_request,
+ async_mode=self._async_mode))
+
+ def stop_capture(self):
+ """Concludes the current capture session."""
+ capture_request = imagery_pb2.CaptureRequest()
+ fwassert.FwAssert.CheckErrorMessage(
+ self._worker.CallOnewayProtoRpc(
+ self._stop_capture_uri,
+ capture_request,
+ async_mode=self._async_mode))
+
+ def get_camera_images(self) -> Dict[Text, Sequence[Any]]:
+ """Gets the latest camera images.
+
+ Camera images can only be obtained after self.start_capture() is called.
+
+ Returns:
+ A dictionary of camera frames, with the camera id as the key. Each camera
+ frame may contain multiple streams. For example, on a realsense camera we
+ may have "rgb" and "depth" streams, depending on the configuration.
+ """
+ get_frame_request = imagery_pb2.GetFrameRequest()
+ frame_collection = imagery_pb2.CameraFrameCollection()
+ fwassert.FwAssert.CheckErrorMessage(
+ self._worker.CallRoundtripProtoRpc(
+ self._get_frame_uri,
+ get_frame_request,
+ frame_collection,
+ self._rpc_timeout_sec,
+ async_mode=self._async_mode))
+
+ images_by_camera = {}
+ for camera_frame in frame_collection.frames:
+ camera_id = camera_frame.camera_id
+ # In case we received multiple frames, we apppend them in the order
+ # received.
+ if camera_id in images_by_camera:
+ images_by_camera[camera_id].append(
+ self._convert_camera_frame_to_image_dict(camera_frame))
+ else:
+ images_by_camera[camera_id] = [
+ self._convert_camera_frame_to_image_dict(camera_frame)
+ ]
+ return images_by_camera
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/vision/imagery_utils.py b/examples/pybullet/gym/pybullet_envs/minitaur/vision/imagery_utils.py
new file mode 100644
index 000000000..ccf8da044
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/vision/imagery_utils.py
@@ -0,0 +1,43 @@
+"""Utilities to convert imagery protobufs to other formats."""
+
+import numpy as np
+
+from pybullet_envs.minitaur.vision import imagery_pb2
+
+
+# TODO(b/123306148): Support the conversion from image array to the proto.
+def convert_image_to_array(image):
+ """Converts an Image proto into a numpy array.
+
+ Args:
+ image: An instance of the imagery_pb2.Image proto.
+
+ Returns:
+ A numpy array. For color images (e.g. BGRA), the converted ND array
+ has the format [Height, Width, Channel]. For gray images (e.g. depth), the
+ converted ND array has the format [Height, Width].
+ """
+
+ if image.image_format == imagery_pb2.Image.IMAGE_FORMAT_BGRA_HWC_8U:
+ img_buffer = np.fromstring(image.content, dtype=np.uint8)
+ img = np.reshape(
+ img_buffer, [image.height_px, image.width_px, 4], order="C")
+ return img
+
+ if image.image_format == imagery_pb2.Image.IMAGE_FORMAT_RGB_HWC_8U:
+ img_buffer = np.fromstring(image.content, dtype=np.uint8)
+ img = np.reshape(
+ img_buffer, [image.height_px, image.width_px, 3], order="C")
+ return img
+
+ if image.image_format == imagery_pb2.Image.IMAGE_FORMAT_GRAY_HW_32F:
+ img_buffer = np.fromstring(image.content, dtype=np.float32)
+ img = np.reshape(img_buffer, [image.height_px, image.width_px], order="C")
+ return img
+
+ if image.image_format == imagery_pb2.Image.IMAGE_FORMAT_GRAY_HW_16U:
+ img_buffer = np.fromstring(image.content, dtype=np.uint16)
+ img = np.reshape(img_buffer, [image.height_px, image.width_px], order="C")
+ return img
+
+ raise ValueError("Unsupported image format {}".format(image.image_format))
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/vision/imagery_utils_test.py b/examples/pybullet/gym/pybullet_envs/minitaur/vision/imagery_utils_test.py
new file mode 100644
index 000000000..5b5493f64
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/vision/imagery_utils_test.py
@@ -0,0 +1,84 @@
+"""Tests for imagery_utils."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import struct
+import numpy as np
+
+from pybullet_envs.minitaur.vision import imagery_pb2
+from pybullet_envs.minitaur.vision import imagery_utils
+from google3.testing.pybase import googletest
+
+
+class ImageryUtilsTest(googletest.TestCase):
+
+ def test_convert_bgra_images(self):
+ image = imagery_pb2.Image(
+ height_px=2,
+ width_px=2,
+ image_format=imagery_pb2.Image.IMAGE_FORMAT_BGRA_HWC_8U,
+ content=b'ABCDABCDABCDABCD',
+ )
+
+ image_array = imagery_utils.convert_image_to_array(image)
+
+ self.assertEqual(image_array.dtype, np.uint8)
+ self.assertEqual(image_array.shape, (image.height_px, image.width_px, 4))
+ self.assertEqual(image_array[0, 0, 0], ord('A'))
+ self.assertEqual(image_array[1, 0, 3], ord('D'))
+
+ def test_convert_rgb_images(self):
+ image = imagery_pb2.Image(
+ height_px=2,
+ width_px=2,
+ image_format=imagery_pb2.Image.IMAGE_FORMAT_RGB_HWC_8U,
+ content=b'ABCABCABCABC',
+ )
+
+ image_array = imagery_utils.convert_image_to_array(image)
+
+ self.assertEqual(image_array.dtype, np.uint8)
+ self.assertEqual(image_array.shape, (image.height_px, image.width_px, 3))
+ self.assertEqual(image_array[0, 0, 0], ord('A'))
+ self.assertEqual(image_array[1, 1, 2], ord('C'))
+
+ def test_convert_gray_32bit_images(self):
+ image = imagery_pb2.Image(
+ height_px=2,
+ width_px=3,
+ image_format=imagery_pb2.Image.IMAGE_FORMAT_GRAY_HW_32F,
+ content=b'AAAABBBBCCCCAAAABBBBCCCC',
+ )
+
+ image_array = imagery_utils.convert_image_to_array(image)
+
+ self.assertEqual(image_array.dtype, np.float32)
+ self.assertEqual(image_array.shape, (image.height_px, image.width_px))
+ self.assertEqual(image_array[0, 2], struct.unpack(b' math.pi):
+ a = -math.pi
+ time.sleep(.01)
+ p.setGravity(0, 0, -10)
+ pivot = [a, 0, 1]
+ orn = p.getQuaternionFromEuler([a, 0, 0])
+ p.changeConstraint(cid, pivot, jointChildFrameOrientation=orn, maxForce=50)
+
+p.removeConstraint(cid)
diff --git a/examples/pybullet/gym/pybullet_examples/createMultiBodyBatch.py b/examples/pybullet/gym/pybullet_examples/createMultiBodyBatch.py
new file mode 100644
index 000000000..87fde9c84
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_examples/createMultiBodyBatch.py
@@ -0,0 +1,143 @@
+import pybullet as p
+import time
+import math
+import pybullet_data
+cid = p.connect(p.SHARED_MEMORY)
+if (cid < 0):
+ p.connect(p.GUI, options="--minGraphicsUpdateTimeMs=16000")
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+
+p.setPhysicsEngineParameter(numSolverIterations=4, minimumSolverIslandSize=1024)
+p.setTimeStep(1. / 120.)
+logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "createMultiBodyBatch.json")
+#useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone)
+p.loadURDF("plane100.urdf", useMaximalCoordinates=True)
+#disable rendering during creation.
+p.setPhysicsEngineParameter(contactBreakingThreshold=0.04)
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
+p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
+#disable tinyrenderer, software (CPU) renderer, we don't use it here
+p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)
+
+shift = [0, -0.02, 0]
+meshScale = [0.1, 0.1, 0.1]
+
+vertices = [[-1.000000, -1.000000, 1.000000], [1.000000, -1.000000, 1.000000],
+ [1.000000, 1.000000, 1.000000], [-1.000000, 1.000000, 1.000000],
+ [-1.000000, -1.000000, -1.000000], [1.000000, -1.000000, -1.000000],
+ [1.000000, 1.000000, -1.000000], [-1.000000, 1.000000, -1.000000],
+ [-1.000000, -1.000000, -1.000000], [-1.000000, 1.000000, -1.000000],
+ [-1.000000, 1.000000, 1.000000], [-1.000000, -1.000000, 1.000000],
+ [1.000000, -1.000000, -1.000000], [1.000000, 1.000000, -1.000000],
+ [1.000000, 1.000000, 1.000000], [1.000000, -1.000000, 1.000000],
+ [-1.000000, -1.000000, -1.000000], [-1.000000, -1.000000, 1.000000],
+ [1.000000, -1.000000, 1.000000], [1.000000, -1.000000, -1.000000],
+ [-1.000000, 1.000000, -1.000000], [-1.000000, 1.000000, 1.000000],
+ [1.000000, 1.000000, 1.000000], [1.000000, 1.000000, -1.000000]]
+
+normals = [[0.000000, 0.000000, 1.000000], [0.000000, 0.000000, 1.000000],
+ [0.000000, 0.000000, 1.000000], [0.000000, 0.000000, 1.000000],
+ [0.000000, 0.000000, -1.000000], [0.000000, 0.000000, -1.000000],
+ [0.000000, 0.000000, -1.000000], [0.000000, 0.000000, -1.000000],
+ [-1.000000, 0.000000, 0.000000], [-1.000000, 0.000000, 0.000000],
+ [-1.000000, 0.000000, 0.000000], [-1.000000, 0.000000, 0.000000],
+ [1.000000, 0.000000, 0.000000], [1.000000, 0.000000, 0.000000],
+ [1.000000, 0.000000, 0.000000], [1.000000, 0.000000, 0.000000],
+ [0.000000, -1.000000, 0.000000], [0.000000, -1.000000, 0.000000],
+ [0.000000, -1.000000, 0.000000], [0.000000, -1.000000, 0.000000],
+ [0.000000, 1.000000, 0.000000], [0.000000, 1.000000, 0.000000],
+ [0.000000, 1.000000, 0.000000], [0.000000, 1.000000, 0.000000]]
+
+uvs = [[0.750000, 0.250000], [1.000000, 0.250000], [1.000000, 0.000000], [0.750000, 0.000000],
+ [0.500000, 0.250000], [0.250000, 0.250000], [0.250000, 0.000000], [0.500000, 0.000000],
+ [0.500000, 0.000000], [0.750000, 0.000000], [0.750000, 0.250000], [0.500000, 0.250000],
+ [0.250000, 0.500000], [0.250000, 0.250000], [0.000000, 0.250000], [0.000000, 0.500000],
+ [0.250000, 0.500000], [0.250000, 0.250000], [0.500000, 0.250000], [0.500000, 0.500000],
+ [0.000000, 0.000000], [0.000000, 0.250000], [0.250000, 0.250000], [0.250000, 0.000000]]
+indices = [
+ 0,
+ 1,
+ 2,
+ 0,
+ 2,
+ 3, #//ground face
+ 6,
+ 5,
+ 4,
+ 7,
+ 6,
+ 4, #//top face
+ 10,
+ 9,
+ 8,
+ 11,
+ 10,
+ 8,
+ 12,
+ 13,
+ 14,
+ 12,
+ 14,
+ 15,
+ 18,
+ 17,
+ 16,
+ 19,
+ 18,
+ 16,
+ 20,
+ 21,
+ 22,
+ 20,
+ 22,
+ 23
+]
+
+#p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
+#the visual shape and collision shape can be re-used by all createMultiBody instances (instancing)
+visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,
+ rgbaColor=[1, 1, 1, 1],
+ specularColor=[0.4, .4, 0],
+ visualFramePosition=shift,
+ meshScale=meshScale,
+ vertices=vertices,
+ indices=indices,
+ uvs=uvs,
+ normals=normals)
+collisionShapeId = p.createCollisionShape(
+ shapeType=p.GEOM_BOX, halfExtents=meshScale
+) #MESH, vertices=vertices, collisionFramePosition=shift,meshScale=meshScale)
+
+texUid = p.loadTexture("tex256.png")
+
+batchPositions = []
+
+for x in range(32):
+ for y in range(32):
+ for z in range(10):
+ batchPositions.append(
+ [x * meshScale[0] * 5.5, y * meshScale[1] * 5.5, (0.5 + z) * meshScale[2] * 2.5])
+
+bodyUids = p.createMultiBody(baseMass=0,
+ baseInertialFramePosition=[0, 0, 0],
+ baseCollisionShapeIndex=collisionShapeId,
+ baseVisualShapeIndex=visualShapeId,
+ basePosition=[0, 0, 2],
+ batchPositions=batchPositions,
+ useMaximalCoordinates=True)
+p.changeVisualShape(bodyUids[0], -1, textureUniqueId=texUid)
+
+p.syncBodyInfo()
+print("numBodies=", p.getNumBodies())
+p.stopStateLogging(logId)
+p.setGravity(0, 0, -10)
+
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
+
+colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1]]
+currentColor = 0
+
+while (1):
+ p.stepSimulation()
+ #time.sleep(1./120.)
+ #p.getCameraImage(320,200)
diff --git a/examples/pybullet/gym/pybullet_examples/createObstacleCourse.py b/examples/pybullet/gym/pybullet_examples/createObstacleCourse.py
new file mode 100644
index 000000000..a9ab11908
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_examples/createObstacleCourse.py
@@ -0,0 +1,130 @@
+import pybullet as p
+import time
+import math
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+#don't create a ground plane, to allow for gaps etc
+p.resetSimulation()
+#p.createCollisionShape(p.GEOM_PLANE)
+#p.createMultiBody(0,0)
+#p.resetDebugVisualizerCamera(5,75,-26,[0,0,1]);
+p.resetDebugVisualizerCamera(15, -346, -16, [-15, 0, 1])
+
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
+
+sphereRadius = 0.05
+colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius)
+
+#a few different ways to create a mesh:
+
+#convex mesh from obj
+stoneId = p.createCollisionShape(p.GEOM_MESH, fileName="stone.obj")
+
+boxHalfLength = 0.5
+boxHalfWidth = 2.5
+boxHalfHeight = 0.1
+segmentLength = 5
+
+colBoxId = p.createCollisionShape(p.GEOM_BOX,
+ halfExtents=[boxHalfLength, boxHalfWidth, boxHalfHeight])
+
+mass = 1
+visualShapeId = -1
+
+segmentStart = 0
+
+for i in range(segmentLength):
+ p.createMultiBody(baseMass=0,
+ baseCollisionShapeIndex=colBoxId,
+ basePosition=[segmentStart, 0, -0.1])
+ segmentStart = segmentStart - 1
+
+for i in range(segmentLength):
+ height = 0
+ if (i % 2):
+ height = 1
+ p.createMultiBody(baseMass=0,
+ baseCollisionShapeIndex=colBoxId,
+ basePosition=[segmentStart, 0, -0.1 + height])
+ segmentStart = segmentStart - 1
+
+baseOrientation = p.getQuaternionFromEuler([math.pi / 2., 0, math.pi / 2.])
+
+for i in range(segmentLength):
+ p.createMultiBody(baseMass=0,
+ baseCollisionShapeIndex=colBoxId,
+ basePosition=[segmentStart, 0, -0.1])
+ segmentStart = segmentStart - 1
+ if (i % 2):
+ p.createMultiBody(baseMass=0,
+ baseCollisionShapeIndex=colBoxId,
+ basePosition=[segmentStart, i % 3, -0.1 + height + boxHalfWidth],
+ baseOrientation=baseOrientation)
+
+for i in range(segmentLength):
+ p.createMultiBody(baseMass=0,
+ baseCollisionShapeIndex=colBoxId,
+ basePosition=[segmentStart, 0, -0.1])
+ width = 4
+ for j in range(width):
+ p.createMultiBody(baseMass=0,
+ baseCollisionShapeIndex=stoneId,
+ basePosition=[segmentStart, 0.5 * (i % 2) + j - width / 2., 0])
+ segmentStart = segmentStart - 1
+
+link_Masses = [1]
+linkCollisionShapeIndices = [colBoxId]
+linkVisualShapeIndices = [-1]
+linkPositions = [[0, 0, 0]]
+linkOrientations = [[0, 0, 0, 1]]
+linkInertialFramePositions = [[0, 0, 0]]
+linkInertialFrameOrientations = [[0, 0, 0, 1]]
+indices = [0]
+jointTypes = [p.JOINT_REVOLUTE]
+axis = [[1, 0, 0]]
+
+baseOrientation = [0, 0, 0, 1]
+for i in range(segmentLength):
+ boxId = p.createMultiBody(0,
+ colSphereId,
+ -1, [segmentStart, 0, -0.1],
+ baseOrientation,
+ linkMasses=link_Masses,
+ linkCollisionShapeIndices=linkCollisionShapeIndices,
+ linkVisualShapeIndices=linkVisualShapeIndices,
+ linkPositions=linkPositions,
+ linkOrientations=linkOrientations,
+ linkInertialFramePositions=linkInertialFramePositions,
+ linkInertialFrameOrientations=linkInertialFrameOrientations,
+ linkParentIndices=indices,
+ linkJointTypes=jointTypes,
+ linkJointAxis=axis)
+ p.changeDynamics(boxId, -1, spinningFriction=0.001, rollingFriction=0.001, linearDamping=0.0)
+ print(p.getNumJoints(boxId))
+ for joint in range(p.getNumJoints(boxId)):
+ targetVelocity = 10
+ if (i % 2):
+ targetVelocity = -10
+ p.setJointMotorControl2(boxId,
+ joint,
+ p.VELOCITY_CONTROL,
+ targetVelocity=targetVelocity,
+ force=100)
+ segmentStart = segmentStart - 1.1
+
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
+while (1):
+ camData = p.getDebugVisualizerCamera()
+ viewMat = camData[2]
+ projMat = camData[3]
+ p.getCameraImage(256,
+ 256,
+ viewMatrix=viewMat,
+ projectionMatrix=projMat,
+ renderer=p.ER_BULLET_HARDWARE_OPENGL)
+ keys = p.getKeyboardEvents()
+ p.stepSimulation()
+ #print(keys)
+ time.sleep(0.01)
diff --git a/examples/pybullet/gym/pybullet_examples/createVisualShapeArray.py b/examples/pybullet/gym/pybullet_examples/createVisualShapeArray.py
new file mode 100644
index 000000000..6156d808c
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_examples/createVisualShapeArray.py
@@ -0,0 +1,115 @@
+import pybullet as p
+import time
+import math
+import pybullet_data
+
+
+def getRayFromTo(mouseX, mouseY):
+ width, height, viewMat, projMat, cameraUp, camForward, horizon, vertical, _, _, dist, camTarget = p.getDebugVisualizerCamera(
+ )
+ camPos = [
+ camTarget[0] - dist * camForward[0], camTarget[1] - dist * camForward[1],
+ camTarget[2] - dist * camForward[2]
+ ]
+ farPlane = 10000
+ rayForward = [(camTarget[0] - camPos[0]), (camTarget[1] - camPos[1]), (camTarget[2] - camPos[2])]
+ invLen = farPlane * 1. / (math.sqrt(rayForward[0] * rayForward[0] + rayForward[1] *
+ rayForward[1] + rayForward[2] * rayForward[2]))
+ rayForward = [invLen * rayForward[0], invLen * rayForward[1], invLen * rayForward[2]]
+ rayFrom = camPos
+ oneOverWidth = float(1) / float(width)
+ oneOverHeight = float(1) / float(height)
+ dHor = [horizon[0] * oneOverWidth, horizon[1] * oneOverWidth, horizon[2] * oneOverWidth]
+ dVer = [vertical[0] * oneOverHeight, vertical[1] * oneOverHeight, vertical[2] * oneOverHeight]
+ rayToCenter = [
+ rayFrom[0] + rayForward[0], rayFrom[1] + rayForward[1], rayFrom[2] + rayForward[2]
+ ]
+ rayTo = [
+ rayFrom[0] + rayForward[0] - 0.5 * horizon[0] + 0.5 * vertical[0] + float(mouseX) * dHor[0] -
+ float(mouseY) * dVer[0], rayFrom[1] + rayForward[1] - 0.5 * horizon[1] + 0.5 * vertical[1] +
+ float(mouseX) * dHor[1] - float(mouseY) * dVer[1], rayFrom[2] + rayForward[2] -
+ 0.5 * horizon[2] + 0.5 * vertical[2] + float(mouseX) * dHor[2] - float(mouseY) * dVer[2]
+ ]
+ return rayFrom, rayTo
+
+
+cid = p.connect(p.SHARED_MEMORY)
+if (cid < 0):
+ p.connect(p.GUI)
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+p.setPhysicsEngineParameter(numSolverIterations=10)
+p.setTimeStep(1. / 120.)
+logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "visualShapeBench.json")
+#useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone)
+p.loadURDF("plane100.urdf", useMaximalCoordinates=True)
+#disable rendering during creation.
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
+p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
+#disable tinyrenderer, software (CPU) renderer, we don't use it here
+p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)
+
+shift = [0, -0.02, 0]
+shift1 = [0, 0.1, 0]
+shift2 = [0, 0, 0]
+
+meshScale = [0.1, 0.1, 0.1]
+#the visual shape and collision shape can be re-used by all createMultiBody instances (instancing)
+visualShapeId = p.createVisualShapeArray(shapeTypes=[p.GEOM_MESH, p.GEOM_BOX],
+ halfExtents=[[0, 0, 0], [0.1, 0.1, 0.1]],
+ fileNames=["duck.obj", ""],
+ visualFramePositions=[
+ shift1,
+ shift2,
+ ],
+ meshScales=[meshScale, meshScale])
+collisionShapeId = p.createCollisionShapeArray(shapeTypes=[p.GEOM_MESH, p.GEOM_BOX],
+ halfExtents=[[0, 0, 0], [0.1, 0.1, 0.1]],
+ fileNames=["duck_vhacd.obj", ""],
+ collisionFramePositions=[
+ shift1,
+ shift2,
+ ],
+ meshScales=[meshScale, meshScale])
+
+rangex = 2
+rangey = 2
+for i in range(rangex):
+ for j in range(rangey):
+ mb = p.createMultiBody(baseMass=1,
+ baseInertialFramePosition=[0, 0, 0],
+ baseCollisionShapeIndex=collisionShapeId,
+ baseVisualShapeIndex=visualShapeId,
+ basePosition=[((-rangex / 2) + i * 2) * meshScale[0] * 2,
+ (-rangey / 2 + j) * meshScale[1] * 4, 1],
+ useMaximalCoordinates=False)
+ p.changeVisualShape(mb, -1, rgbaColor=[1, 1, 1, 1])
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
+p.stopStateLogging(logId)
+p.setGravity(0, 0, -10)
+p.setRealTimeSimulation(1)
+
+colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1]]
+currentColor = 0
+
+p.getCameraImage(64, 64, renderer=p.ER_BULLET_HARDWARE_OPENGL)
+
+while (1):
+
+ mouseEvents = p.getMouseEvents()
+ for e in mouseEvents:
+ if ((e[0] == 2) and (e[3] == 0) and (e[4] & p.KEY_WAS_TRIGGERED)):
+ mouseX = e[1]
+ mouseY = e[2]
+ rayFrom, rayTo = getRayFromTo(mouseX, mouseY)
+ rayInfo = p.rayTest(rayFrom, rayTo)
+ #p.addUserDebugLine(rayFrom,rayTo,[1,0,0],3)
+ for l in range(len(rayInfo)):
+ hit = rayInfo[l]
+ objectUid = hit[0]
+ if (objectUid >= 0):
+ #p.removeBody(objectUid)
+ p.changeVisualShape(objectUid, -1, rgbaColor=colors[currentColor])
+ currentColor += 1
+ if (currentColor >= len(colors)):
+ currentColor = 0
diff --git a/examples/pybullet/gym/pybullet_examples/deformable_anchor.py b/examples/pybullet/gym/pybullet_examples/deformable_anchor.py
new file mode 100644
index 000000000..bf37cdb61
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_examples/deformable_anchor.py
@@ -0,0 +1,55 @@
+import pybullet as p
+from time import sleep
+
+physicsClient = p.connect(p.GUI)
+import pybullet_data
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD)
+
+gravZ=-10
+p.setGravity(0, 0, gravZ)
+
+planeOrn = [0,0,0,1]#p.getQuaternionFromEuler([0.3,0,0])
+#planeId = p.loadURDF("plane.urdf", [0,0,-2],planeOrn)
+
+boxId = p.loadURDF("cube.urdf", [0,1,2],useMaximalCoordinates = True)
+
+clothId = p.loadSoftBody("cloth_z_up.obj", basePosition = [0,0,2], scale = 0.5, mass = 1., useNeoHookean = 0, useBendingSprings=1,useMassSpring=1, springElasticStiffness=40, springDampingStiffness=.1, springDampingAllDirections = 1, useSelfCollision = 0, frictionCoeff = .5, useFaceContact=1)
+
+
+p.changeVisualShape(clothId, -1, flags=p.VISUAL_SHAPE_DOUBLE_SIDED)
+
+p.createSoftBodyAnchor(clothId ,24,-1,-1)
+p.createSoftBodyAnchor(clothId ,20,-1,-1)
+p.createSoftBodyAnchor(clothId ,15,boxId,-1, [0.5,-0.5,0])
+p.createSoftBodyAnchor(clothId ,19,boxId,-1, [-0.5,-0.5,0])
+p.setPhysicsEngineParameter(sparseSdfVoxelSize=0.25)
+
+debug = True
+if debug:
+ data = p.getMeshData(clothId, -1, flags=p.MESH_DATA_SIMULATION_MESH)
+ print("--------------")
+ print("data=",data)
+ print(data[0])
+ print(data[1])
+ text_uid = []
+ for i in range(data[0]):
+ pos = data[1][i]
+ uid = p.addUserDebugText(str(i), pos, textColorRGB=[1,1,1])
+ text_uid.append(uid)
+
+while p.isConnected():
+ p.getCameraImage(320,200)
+
+ if debug:
+ data = p.getMeshData(clothId, -1, flags=p.MESH_DATA_SIMULATION_MESH)
+ for i in range(data[0]):
+ pos = data[1][i]
+ uid = p.addUserDebugText(str(i), pos, textColorRGB=[1,1,1], replaceItemUniqueId=text_uid[i])
+
+ p.setGravity(0,0,gravZ)
+ p.stepSimulation()
+ #p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING,1)
+ #sleep(1./240.)
+
diff --git a/examples/pybullet/gym/pybullet_examples/deformable_torus.py b/examples/pybullet/gym/pybullet_examples/deformable_torus.py
new file mode 100644
index 000000000..b8d7cb052
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_examples/deformable_torus.py
@@ -0,0 +1,31 @@
+import pybullet as p
+from time import sleep
+import pybullet_data
+
+physicsClient = p.connect(p.GUI)
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+
+p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD)
+p.resetDebugVisualizerCamera(3,-420,-30,[0.3,0.9,-2])
+p.setGravity(0, 0, -10)
+
+tex = p.loadTexture("uvmap.png")
+planeId = p.loadURDF("plane.urdf", [0,0,-2])
+
+boxId = p.loadURDF("cube.urdf", [0,3,2],useMaximalCoordinates = True)
+
+bunnyId = p.loadSoftBody("torus/torus_textured.obj", simFileName="torus.vtk", mass = 3, useNeoHookean = 1, NeoHookeanMu = 180, NeoHookeanLambda = 600, NeoHookeanDamping = 0.01, collisionMargin = 0.006, useSelfCollision = 1, frictionCoeff = 0.5, repulsionStiffness = 800)
+p.changeVisualShape(bunnyId, -1, rgbaColor=[1,1,1,1], textureUniqueId=tex, flags=0)
+
+
+bunny2 = p.loadURDF("torus_deform.urdf", [0,1,0.2], flags=p.URDF_USE_SELF_COLLISION)
+
+p.changeVisualShape(bunny2, -1, rgbaColor=[1,1,1,1], textureUniqueId=tex, flags=0)
+p.setPhysicsEngineParameter(sparseSdfVoxelSize=0.25)
+p.setRealTimeSimulation(0)
+
+while p.isConnected():
+ p.stepSimulation()
+ p.getCameraImage(320,200)
+ p.setGravity(0,0,-10)
diff --git a/examples/pybullet/gym/pybullet_examples/experimentalCcdSphereRadius.py b/examples/pybullet/gym/pybullet_examples/experimentalCcdSphereRadius.py
new file mode 100644
index 000000000..ca7dd664a
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_examples/experimentalCcdSphereRadius.py
@@ -0,0 +1,55 @@
+import pybullet as p
+import time
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+p.setPhysicsEngineParameter(allowedCcdPenetration=0.0)
+
+terrain_mass = 0
+terrain_visual_shape_id = -1
+terrain_position = [0, 0, 0]
+terrain_orientation = [0, 0, 0, 1]
+terrain_collision_shape_id = p.createCollisionShape(shapeType=p.GEOM_MESH,
+ fileName="terrain.obj",
+ flags=p.GEOM_FORCE_CONCAVE_TRIMESH |
+ p.GEOM_CONCAVE_INTERNAL_EDGE,
+ meshScale=[0.5, 0.5, 0.5])
+p.createMultiBody(terrain_mass, terrain_collision_shape_id, terrain_visual_shape_id,
+ terrain_position, terrain_orientation)
+
+useMaximalCoordinates = True
+sphereRadius = 0.005
+colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius)
+colBoxId = p.createCollisionShape(p.GEOM_BOX,
+ halfExtents=[sphereRadius, sphereRadius, sphereRadius])
+
+mass = 1
+visualShapeId = -1
+
+for i in range(5):
+ for j in range(5):
+ for k in range(5):
+ #if (k&2):
+ sphereUid = p.createMultiBody(
+ mass,
+ colSphereId,
+ visualShapeId, [-i * 5 * sphereRadius, j * 5 * sphereRadius, k * 2 * sphereRadius + 1],
+ useMaximalCoordinates=useMaximalCoordinates)
+ #else:
+ # sphereUid = p.createMultiBody(mass,colBoxId,visualShapeId,[-i*2*sphereRadius,j*2*sphereRadius,k*2*sphereRadius+1], useMaximalCoordinates=useMaximalCoordinates)
+ p.changeDynamics(sphereUid,
+ -1,
+ spinningFriction=0.001,
+ rollingFriction=0.001,
+ linearDamping=0.0)
+ p.changeDynamics(sphereUid, -1, ccdSweptSphereRadius=0.002)
+
+p.setGravity(0, 0, -10)
+
+pts = p.getContactPoints()
+print("num points=", len(pts))
+print(pts)
+while (p.isConnected()):
+ time.sleep(1. / 240.)
+ p.stepSimulation()
diff --git a/examples/pybullet/gym/pybullet_examples/fileIOPlugin.py b/examples/pybullet/gym/pybullet_examples/fileIOPlugin.py
new file mode 100644
index 000000000..07f906ec0
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_examples/fileIOPlugin.py
@@ -0,0 +1,23 @@
+import pybullet as p
+import time
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+fileIO = p.loadPlugin("fileIOPlugin")
+if (fileIO >= 0):
+ #we can have a zipfile (pickup.zip) inside a zipfile (pickup2.zip)
+ p.executePluginCommand(fileIO, pybullet_data.getDataPath()+"/pickup2.zip", [p.AddFileIOAction, p.ZipFileIO])
+ p.executePluginCommand(fileIO, "pickup.zip", [p.AddFileIOAction, p.ZipFileIO])
+ objs = p.loadSDF("pickup/model.sdf")
+ dobot = objs[0]
+ p.changeVisualShape(dobot, -1, rgbaColor=[1, 1, 1, 1])
+
+else:
+ print("fileIOPlugin is disabled.")
+
+p.setPhysicsEngineParameter(enableFileCaching=False)
+
+while (1):
+ p.stepSimulation()
+ time.sleep(1. / 240.)
diff --git a/examples/pybullet/gym/pybullet_examples/getClosestPoints.py b/examples/pybullet/gym/pybullet_examples/getClosestPoints.py
new file mode 100644
index 000000000..55311f58d
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_examples/getClosestPoints.py
@@ -0,0 +1,75 @@
+import pybullet as p
+import time
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+useCollisionShapeQuery = True
+p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
+geom = p.createCollisionShape(p.GEOM_SPHERE, radius=0.1)
+geomBox = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.2, 0.2, 0.2])
+baseOrientationB = p.getQuaternionFromEuler([0, 0.3, 0]) #[0,0.5,0.5,0]
+basePositionB = [1.5, 0, 1]
+obA = -1
+obB = -1
+
+obA = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=geom, basePosition=[0.5, 0, 1])
+obB = p.createMultiBody(baseMass=0,
+ baseCollisionShapeIndex=geomBox,
+ basePosition=basePositionB,
+ baseOrientation=baseOrientationB)
+
+lineWidth = 3
+colorRGB = [1, 0, 0]
+lineId = p.addUserDebugLine(lineFromXYZ=[0, 0, 0],
+ lineToXYZ=[0, 0, 0],
+ lineColorRGB=colorRGB,
+ lineWidth=lineWidth,
+ lifeTime=0)
+pitch = 0
+yaw = 0
+
+while (p.isConnected()):
+ pitch += 0.01
+ if (pitch >= 3.1415 * 2.):
+ pitch = 0
+ yaw += 0.01
+ if (yaw >= 3.1415 * 2.):
+ yaw = 0
+
+ baseOrientationB = p.getQuaternionFromEuler([yaw, pitch, 0])
+ if (obB >= 0):
+ p.resetBasePositionAndOrientation(obB, basePositionB, baseOrientationB)
+
+ if (useCollisionShapeQuery):
+ pts = p.getClosestPoints(bodyA=-1,
+ bodyB=-1,
+ distance=100,
+ collisionShapeA=geom,
+ collisionShapeB=geomBox,
+ collisionShapePositionA=[0.5, 0, 1],
+ collisionShapePositionB=basePositionB,
+ collisionShapeOrientationB=baseOrientationB)
+ #pts = p.getClosestPoints(bodyA=obA, bodyB=-1, distance=100, collisionShapeB=geomBox, collisionShapePositionB=basePositionB, collisionShapeOrientationB=baseOrientationB)
+ else:
+ pts = p.getClosestPoints(bodyA=obA, bodyB=obB, distance=100)
+
+ if len(pts) > 0:
+ #print(pts)
+ distance = pts[0][8]
+ #print("distance=",distance)
+ ptA = pts[0][5]
+ ptB = pts[0][6]
+ p.addUserDebugLine(lineFromXYZ=ptA,
+ lineToXYZ=ptB,
+ lineColorRGB=colorRGB,
+ lineWidth=lineWidth,
+ lifeTime=0,
+ replaceItemUniqueId=lineId)
+ #time.sleep(1./240.)
+
+#removeCollisionShape is optional:
+#only use removeCollisionShape if the collision shape is not used to create a body
+#and if you want to keep on creating new collision shapes for different queries (not recommended)
+p.removeCollisionShape(geom)
+p.removeCollisionShape(geomBox)
diff --git a/examples/pybullet/gym/pybullet_examples/getTextureUid.py b/examples/pybullet/gym/pybullet_examples/getTextureUid.py
new file mode 100644
index 000000000..f4a31b2f0
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_examples/getTextureUid.py
@@ -0,0 +1,21 @@
+import pybullet as p
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+plane = p.loadURDF("plane.urdf")
+visualData = p.getVisualShapeData(plane, p.VISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS)
+print(visualData)
+curTexUid = visualData[0][8]
+print(curTexUid)
+texUid = p.loadTexture("tex256.png")
+print("texUid=", texUid)
+
+p.changeVisualShape(plane, -1, textureUniqueId=texUid)
+
+for i in range(100):
+ p.getCameraImage(320, 200)
+p.changeVisualShape(plane, -1, textureUniqueId=curTexUid)
+
+for i in range(100):
+ p.getCameraImage(320, 200)
diff --git a/examples/pybullet/gym/pybullet_examples/heightfield.py b/examples/pybullet/gym/pybullet_examples/heightfield.py
new file mode 100644
index 000000000..b01a5e0bb
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_examples/heightfield.py
@@ -0,0 +1,138 @@
+import pybullet as p
+import pybullet_data as pd
+import math
+import time
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pd.getDataPath())
+
+textureId = -1
+
+useProgrammatic = 0
+useTerrainFromPNG = 1
+useDeepLocoCSV = 2
+updateHeightfield = False
+
+heightfieldSource = useProgrammatic
+import random
+random.seed(10)
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
+heightPerturbationRange = 0.05
+if heightfieldSource==useProgrammatic:
+ numHeightfieldRows = 256
+ numHeightfieldColumns = 256
+ heightfieldData = [0]*numHeightfieldRows*numHeightfieldColumns
+ for j in range (int(numHeightfieldColumns/2)):
+ for i in range (int(numHeightfieldRows/2) ):
+ height = random.uniform(0,heightPerturbationRange)
+ heightfieldData[2*i+2*j*numHeightfieldRows]=height
+ heightfieldData[2*i+1+2*j*numHeightfieldRows]=height
+ heightfieldData[2*i+(2*j+1)*numHeightfieldRows]=height
+ heightfieldData[2*i+1+(2*j+1)*numHeightfieldRows]=height
+
+
+ terrainShape = p.createCollisionShape(shapeType = p.GEOM_HEIGHTFIELD, meshScale=[.05,.05,1], heightfieldTextureScaling=(numHeightfieldRows-1)/2, heightfieldData=heightfieldData, numHeightfieldRows=numHeightfieldRows, numHeightfieldColumns=numHeightfieldColumns)
+ terrain = p.createMultiBody(0, terrainShape)
+ p.resetBasePositionAndOrientation(terrain,[0,0,0], [0,0,0,1])
+
+if heightfieldSource==useDeepLocoCSV:
+ terrainShape = p.createCollisionShape(shapeType = p.GEOM_HEIGHTFIELD, meshScale=[.5,.5,2.5],fileName = "heightmaps/ground0.txt", heightfieldTextureScaling=128)
+ terrain = p.createMultiBody(0, terrainShape)
+ p.resetBasePositionAndOrientation(terrain,[0,0,0], [0,0,0,1])
+
+if heightfieldSource==useTerrainFromPNG:
+ terrainShape = p.createCollisionShape(shapeType = p.GEOM_HEIGHTFIELD, meshScale=[.1,.1,24],fileName = "heightmaps/wm_height_out.png")
+ textureId = p.loadTexture("heightmaps/gimp_overlay_out.png")
+ terrain = p.createMultiBody(0, terrainShape)
+ p.changeVisualShape(terrain, -1, textureUniqueId = textureId)
+
+
+p.changeVisualShape(terrain, -1, rgbaColor=[1,1,1,1])
+
+
+sphereRadius = 0.05
+colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius)
+colBoxId = p.createCollisionShape(p.GEOM_BOX,
+ halfExtents=[sphereRadius, sphereRadius, sphereRadius])
+
+mass = 1
+visualShapeId = -1
+
+link_Masses = [1]
+linkCollisionShapeIndices = [colBoxId]
+linkVisualShapeIndices = [-1]
+linkPositions = [[0, 0, 0.11]]
+linkOrientations = [[0, 0, 0, 1]]
+linkInertialFramePositions = [[0, 0, 0]]
+linkInertialFrameOrientations = [[0, 0, 0, 1]]
+indices = [0]
+jointTypes = [p.JOINT_REVOLUTE]
+axis = [[0, 0, 1]]
+
+for i in range(3):
+ for j in range(3):
+ for k in range(3):
+ basePosition = [
+ i * 5 * sphereRadius, j * 5 * sphereRadius, 1 + k * 5 * sphereRadius + 1
+ ]
+ baseOrientation = [0, 0, 0, 1]
+ if (k & 2):
+ sphereUid = p.createMultiBody(mass, colSphereId, visualShapeId, basePosition,
+ baseOrientation)
+ else:
+ sphereUid = p.createMultiBody(mass,
+ colBoxId,
+ visualShapeId,
+ basePosition,
+ baseOrientation,
+ linkMasses=link_Masses,
+ linkCollisionShapeIndices=linkCollisionShapeIndices,
+ linkVisualShapeIndices=linkVisualShapeIndices,
+ linkPositions=linkPositions,
+ linkOrientations=linkOrientations,
+ linkInertialFramePositions=linkInertialFramePositions,
+ linkInertialFrameOrientations=linkInertialFrameOrientations,
+ linkParentIndices=indices,
+ linkJointTypes=jointTypes,
+ linkJointAxis=axis)
+
+
+ p.changeDynamics(sphereUid,
+ -1,
+ spinningFriction=0.001,
+ rollingFriction=0.001,
+ linearDamping=0.0)
+ for joint in range(p.getNumJoints(sphereUid)):
+ p.setJointMotorControl2(sphereUid, joint, p.VELOCITY_CONTROL, targetVelocity=1, force=10)
+
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
+p.setGravity(0, 0, -10)
+p.setRealTimeSimulation(1)
+
+p.getNumJoints(sphereUid)
+for i in range(p.getNumJoints(sphereUid)):
+ p.getJointInfo(sphereUid, i)
+
+
+while (p.isConnected()):
+ keys = p.getKeyboardEvents()
+
+ if updateHeightfield and heightfieldSource==useProgrammatic:
+ for j in range (int(numHeightfieldColumns/2)):
+ for i in range (int(numHeightfieldRows/2) ):
+ height = random.uniform(0,heightPerturbationRange)#+math.sin(time.time())
+ heightfieldData[2*i+2*j*numHeightfieldRows]=height
+ heightfieldData[2*i+1+2*j*numHeightfieldRows]=height
+ heightfieldData[2*i+(2*j+1)*numHeightfieldRows]=height
+ heightfieldData[2*i+1+(2*j+1)*numHeightfieldRows]=height
+ #GEOM_CONCAVE_INTERNAL_EDGE may help avoid getting stuck at an internal (shared) edge of the triangle/heightfield.
+ #GEOM_CONCAVE_INTERNAL_EDGE is a bit slower to build though.
+ #flags = p.GEOM_CONCAVE_INTERNAL_EDGE
+ flags = 0
+ terrainShape2 = p.createCollisionShape(shapeType = p.GEOM_HEIGHTFIELD, flags = flags, meshScale=[.05,.05,1], heightfieldTextureScaling=(numHeightfieldRows-1)/2, heightfieldData=heightfieldData, numHeightfieldRows=numHeightfieldRows, numHeightfieldColumns=numHeightfieldColumns, replaceHeightfieldIndex = terrainShape)
+
+
+ #print(keys)
+ #getCameraImage note: software/TinyRenderer doesn't render/support heightfields!
+ #p.getCameraImage(320,200, renderer=p.ER_BULLET_HARDWARE_OPENGL)
+ time.sleep(0.01)
diff --git a/examples/pybullet/gym/pybullet_examples/humanoidMotionCapture.py b/examples/pybullet/gym/pybullet_examples/humanoidMotionCapture.py
new file mode 100644
index 000000000..fdc1675e1
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_examples/humanoidMotionCapture.py
@@ -0,0 +1,614 @@
+import pybullet as p
+import json
+import time
+import pybullet_data
+
+
+useGUI = True
+if useGUI:
+ p.connect(p.GUI)
+else:
+ p.connect(p.DIRECT)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+
+useZUp = False
+useYUp = not useZUp
+showJointMotorTorques = False
+
+if useYUp:
+ p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP, 1)
+
+from pybullet_examples.pdControllerExplicit import PDControllerExplicitMultiDof
+from pybullet_examples.pdControllerStable import PDControllerStableMultiDof
+
+explicitPD = PDControllerExplicitMultiDof(p)
+stablePD = PDControllerStableMultiDof(p)
+
+p.resetDebugVisualizerCamera(cameraDistance=7.4,
+ cameraYaw=-94,
+ cameraPitch=-14,
+ cameraTargetPosition=[0.24, -0.02, -0.09])
+
+import pybullet_data
+p.setTimeOut(10000)
+useMotionCapture = False
+useMotionCaptureReset = False #not useMotionCapture
+useExplicitPD = True
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+p.setPhysicsEngineParameter(numSolverIterations=30)
+#p.setPhysicsEngineParameter(solverResidualThreshold=1e-30)
+
+#explicit PD control requires small timestep
+timeStep = 1. / 600.
+#timeStep = 1./240.
+
+p.setPhysicsEngineParameter(fixedTimeStep=timeStep)
+
+path = pybullet_data.getDataPath() + "/data/motions/humanoid3d_backflip.txt"
+#path = pybullet_data.getDataPath()+"/data/motions/humanoid3d_cartwheel.txt"
+#path = pybullet_data.getDataPath()+"/data/motions/humanoid3d_walk.txt"
+
+#p.loadURDF("plane.urdf",[0,0,-1.03])
+print("path = ", path)
+with open(path, 'r') as f:
+ motion_dict = json.load(f)
+#print("motion_dict = ", motion_dict)
+print("len motion=", len(motion_dict))
+print(motion_dict['Loop'])
+numFrames = len(motion_dict['Frames'])
+print("#frames = ", numFrames)
+
+frameId = p.addUserDebugParameter("frame", 0, numFrames - 1, 0)
+
+erpId = p.addUserDebugParameter("erp", 0, 1, 0.2)
+
+kpMotorId = p.addUserDebugParameter("kpMotor", 0, 1, .2)
+forceMotorId = p.addUserDebugParameter("forceMotor", 0, 2000, 1000)
+
+jointTypes = [
+ "JOINT_REVOLUTE", "JOINT_PRISMATIC", "JOINT_SPHERICAL", "JOINT_PLANAR", "JOINT_FIXED"
+]
+
+startLocations = [[0, 0, 2], [0, 0, 0], [0, 0, -2], [0, 0, -4], [0, 0, 4]]
+
+p.addUserDebugText("Stable PD",
+ [startLocations[0][0], startLocations[0][1] + 1, startLocations[0][2]],
+ [0, 0, 0])
+p.addUserDebugText("Spherical Drive",
+ [startLocations[1][0], startLocations[1][1] + 1, startLocations[1][2]],
+ [0, 0, 0])
+p.addUserDebugText("Explicit PD",
+ [startLocations[2][0], startLocations[2][1] + 1, startLocations[2][2]],
+ [0, 0, 0])
+p.addUserDebugText("Kinematic",
+ [startLocations[3][0], startLocations[3][1] + 1, startLocations[3][2]],
+ [0, 0, 0])
+p.addUserDebugText("Stable PD (Py)",
+ [startLocations[4][0], startLocations[4][1] + 1, startLocations[4][2]],
+ [0, 0, 0])
+flags=p.URDF_MAINTAIN_LINK_ORDER+p.URDF_USE_SELF_COLLISION
+humanoid = p.loadURDF("humanoid/humanoid.urdf",
+ startLocations[0],
+ globalScaling=0.25,
+ useFixedBase=False,
+ flags=flags)
+humanoid2 = p.loadURDF("humanoid/humanoid.urdf",
+ startLocations[1],
+ globalScaling=0.25,
+ useFixedBase=False,
+ flags=flags)
+humanoid3 = p.loadURDF("humanoid/humanoid.urdf",
+ startLocations[2],
+ globalScaling=0.25,
+ useFixedBase=False,
+ flags=flags)
+humanoid4 = p.loadURDF("humanoid/humanoid.urdf",
+ startLocations[3],
+ globalScaling=0.25,
+ useFixedBase=False,
+ flags=flags)
+humanoid5 = p.loadURDF("humanoid/humanoid.urdf",
+ startLocations[4],
+ globalScaling=0.25,
+ useFixedBase=False,
+ flags=flags)
+
+humanoid_fix = p.createConstraint(humanoid, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0],
+ startLocations[0], [0, 0, 0, 1])
+humanoid2_fix = p.createConstraint(humanoid2, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0],
+ startLocations[1], [0, 0, 0, 1])
+humanoid3_fix = p.createConstraint(humanoid3, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0],
+ startLocations[2], [0, 0, 0, 1])
+humanoid3_fix = p.createConstraint(humanoid4, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0],
+ startLocations[3], [0, 0, 0, 1])
+humanoid4_fix = p.createConstraint(humanoid5, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0],
+ startLocations[4], [0, 0, 0, 1])
+
+startPose = [
+ 2, 0.847532, 0, 0.9986781045, 0.01410400148, -0.0006980000731, -0.04942300517, 0.9988133229,
+ 0.009485003066, -0.04756001538, -0.004475001447, 1, 0, 0, 0, 0.9649395871, 0.02436898957,
+ -0.05755497537, 0.2549218909, -0.249116, 0.9993661511, 0.009952001505, 0.03265400494,
+ 0.01009800153, 0.9854981188, -0.06440700776, 0.09324301124, -0.1262970152, 0.170571,
+ 0.9927545808, -0.02090099117, 0.08882396249, -0.07817796699, -0.391532, 0.9828788495,
+ 0.1013909845, -0.05515999155, 0.143618978, 0.9659421276, 0.1884590249, -0.1422460188,
+ 0.105854014, 0.581348
+]
+
+startVel = [
+ 1.235314324, -0.008525509087, 0.1515293946, -1.161516553, 0.1866449799, -0.1050802848, 0,
+ 0.935706195, 0.08277326387, 0.3002461862, 0, 0, 0, 0, 0, 1.114409628, 0.3618553952,
+ -0.4505575061, 0, -1.725374735, -0.5052852598, -0.8555179722, -0.2221173515, 0, -0.1837617357,
+ 0.00171895706, 0.03912837591, 0, 0.147945294, 1.837653345, 0.1534535548, 1.491385941, 0,
+ -4.632454387, -0.9111172777, -1.300648184, -1.345694622, 0, -1.084238535, 0.1313680236,
+ -0.7236998534, 0, -0.5278312973
+]
+
+p.resetBasePositionAndOrientation(humanoid, startLocations[0], [0, 0, 0, 1])
+p.resetBasePositionAndOrientation(humanoid2, startLocations[1], [0, 0, 0, 1])
+p.resetBasePositionAndOrientation(humanoid3, startLocations[2], [0, 0, 0, 1])
+p.resetBasePositionAndOrientation(humanoid4, startLocations[3], [0, 0, 0, 1])
+p.resetBasePositionAndOrientation(humanoid5, startLocations[4], [0, 0, 0, 1])
+
+index0 = 7
+for j in range(p.getNumJoints(humanoid)):
+ ji = p.getJointInfo(humanoid, j)
+ targetPosition = [0]
+ jointType = ji[2]
+ if (jointType == p.JOINT_SPHERICAL):
+ targetPosition = [
+ startPose[index0 + 1], startPose[index0 + 2], startPose[index0 + 3], startPose[index0 + 0]
+ ]
+ targetVel = [startVel[index0 + 0], startVel[index0 + 1], startVel[index0 + 2]]
+ index0 += 4
+ print("spherical position: ", targetPosition)
+ print("spherical velocity: ", targetVel)
+ p.resetJointStateMultiDof(humanoid, j, targetValue=targetPosition, targetVelocity=targetVel)
+ p.resetJointStateMultiDof(humanoid5, j, targetValue=targetPosition, targetVelocity=targetVel)
+ p.resetJointStateMultiDof(humanoid2, j, targetValue=targetPosition, targetVelocity=targetVel)
+ if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
+ targetPosition = [startPose[index0]]
+ targetVel = [startVel[index0]]
+ index0 += 1
+ print("revolute:", targetPosition)
+ print("revolute velocity:", targetVel)
+ p.resetJointStateMultiDof(humanoid, j, targetValue=targetPosition, targetVelocity=targetVel)
+ p.resetJointStateMultiDof(humanoid5, j, targetValue=targetPosition, targetVelocity=targetVel)
+ p.resetJointStateMultiDof(humanoid2, j, targetValue=targetPosition, targetVelocity=targetVel)
+
+for j in range(p.getNumJoints(humanoid)):
+ ji = p.getJointInfo(humanoid, j)
+ targetPosition = [0]
+ jointType = ji[2]
+ if (jointType == p.JOINT_SPHERICAL):
+ targetPosition = [0, 0, 0, 1]
+ p.setJointMotorControlMultiDof(humanoid,
+ j,
+ p.POSITION_CONTROL,
+ targetPosition,
+ targetVelocity=[0, 0, 0],
+ positionGain=0,
+ velocityGain=1,
+ force=[0, 0, 0])
+ p.setJointMotorControlMultiDof(humanoid5,
+ j,
+ p.POSITION_CONTROL,
+ targetPosition,
+ targetVelocity=[0, 0, 0],
+ positionGain=0,
+ velocityGain=1,
+ force=[0, 0, 0])
+ p.setJointMotorControlMultiDof(humanoid3,
+ j,
+ p.POSITION_CONTROL,
+ targetPosition,
+ targetVelocity=[0, 0, 0],
+ positionGain=0,
+ velocityGain=1,
+ force=[31, 31, 31])
+ p.setJointMotorControlMultiDof(humanoid4,
+ j,
+ p.POSITION_CONTROL,
+ targetPosition,
+ targetVelocity=[0, 0, 0],
+ positionGain=0,
+ velocityGain=1,
+ force=[1, 1, 1])
+
+ if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
+ p.setJointMotorControl2(humanoid, j, p.VELOCITY_CONTROL, targetVelocity=0, force=0)
+ p.setJointMotorControl2(humanoid3, j, p.VELOCITY_CONTROL, targetVelocity=0, force=31)
+ p.setJointMotorControl2(humanoid4, j, p.VELOCITY_CONTROL, targetVelocity=0, force=10)
+ p.setJointMotorControl2(humanoid5, j, p.VELOCITY_CONTROL, targetVelocity=0, force=0)
+
+ #print(ji)
+ print("joint[", j, "].type=", jointTypes[ji[2]])
+ print("joint[", j, "].name=", ji[1])
+
+jointIds = []
+paramIds = []
+for j in range(p.getNumJoints(humanoid)):
+ #p.changeDynamics(humanoid,j,linearDamping=0, angularDamping=0)
+ p.changeVisualShape(humanoid, j, rgbaColor=[1, 1, 1, 1])
+ info = p.getJointInfo(humanoid, j)
+ #print(info)
+ if (not useMotionCapture):
+ jointName = info[1]
+ jointType = info[2]
+ if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
+ jointIds.append(j)
+ #paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"),-4,4,0))
+ #print("jointName=",jointName, "at ", j)
+
+p.changeVisualShape(humanoid, 2, rgbaColor=[1, 0, 0, 1])
+chest = 1
+neck = 2
+rightHip = 3
+rightKnee = 4
+rightAnkle = 5
+rightShoulder = 6
+rightElbow = 7
+leftHip = 9
+leftKnee = 10
+leftAnkle = 11
+leftShoulder = 12
+leftElbow = 13
+
+#rightShoulder=3
+#rightElbow=4
+#leftShoulder=6
+#leftElbow = 7
+#rightHip = 9
+#rightKnee=10
+#rightAnkle=11
+#leftHip = 12
+#leftKnee=13
+#leftAnkle=14
+
+import time
+
+kpOrg = [
+ 0, 0, 0, 0, 0, 0, 0, 1000, 1000, 1000, 1000, 100, 100, 100, 100, 500, 500, 500, 500, 500, 400,
+ 400, 400, 400, 400, 400, 400, 400, 300, 500, 500, 500, 500, 500, 400, 400, 400, 400, 400, 400,
+ 400, 400, 300
+]
+kdOrg = [
+ 0, 0, 0, 0, 0, 0, 0, 100, 100, 100, 100, 10, 10, 10, 10, 50, 50, 50, 50, 50, 40, 40, 40, 40,
+ 40, 40, 40, 40, 30, 50, 50, 50, 50, 50, 40, 40, 40, 40, 40, 40, 40, 40, 30
+]
+
+once = True
+p.getCameraImage(320, 200)
+
+while (p.isConnected()):
+
+ if useGUI:
+ erp = p.readUserDebugParameter(erpId)
+ kpMotor = p.readUserDebugParameter(kpMotorId)
+ maxForce = p.readUserDebugParameter(forceMotorId)
+ frameReal = p.readUserDebugParameter(frameId)
+ else:
+ erp = 0.2
+ kpMotor = 0.2
+ maxForce = 1000
+ frameReal = 0
+
+ kp = kpMotor
+
+ frame = int(frameReal)
+ frameNext = frame + 1
+ if (frameNext >= numFrames):
+ frameNext = frame
+
+ frameFraction = frameReal - frame
+ #print("frameFraction=",frameFraction)
+ #print("frame=",frame)
+ #print("frameNext=", frameNext)
+
+ #getQuaternionSlerp
+
+ frameData = motion_dict['Frames'][frame]
+ frameDataNext = motion_dict['Frames'][frameNext]
+
+ #print("duration=",frameData[0])
+ #print(pos=[frameData])
+
+ basePos1Start = [frameData[1], frameData[2], frameData[3]]
+ basePos1End = [frameDataNext[1], frameDataNext[2], frameDataNext[3]]
+ basePos1 = [
+ basePos1Start[0] + frameFraction * (basePos1End[0] - basePos1Start[0]),
+ basePos1Start[1] + frameFraction * (basePos1End[1] - basePos1Start[1]),
+ basePos1Start[2] + frameFraction * (basePos1End[2] - basePos1Start[2])
+ ]
+ baseOrn1Start = [frameData[5], frameData[6], frameData[7], frameData[4]]
+ baseOrn1Next = [frameDataNext[5], frameDataNext[6], frameDataNext[7], frameDataNext[4]]
+ baseOrn1 = p.getQuaternionSlerp(baseOrn1Start, baseOrn1Next, frameFraction)
+ #pre-rotate to make z-up
+ if (useZUp):
+ y2zPos = [0, 0, 0.0]
+ y2zOrn = p.getQuaternionFromEuler([1.57, 0, 0])
+ basePos, baseOrn = p.multiplyTransforms(y2zPos, y2zOrn, basePos1, baseOrn1)
+ p.resetBasePositionAndOrientation(humanoid, basePos, baseOrn)
+
+ y2zPos = [0, 2, 0.0]
+ y2zOrn = p.getQuaternionFromEuler([1.57, 0, 0])
+ basePos, baseOrn = p.multiplyTransforms(y2zPos, y2zOrn, basePos1, baseOrn1)
+ p.resetBasePositionAndOrientation(humanoid2, basePos, baseOrn)
+
+ chestRotStart = [frameData[9], frameData[10], frameData[11], frameData[8]]
+ chestRotEnd = [frameDataNext[9], frameDataNext[10], frameDataNext[11], frameDataNext[8]]
+ chestRot = p.getQuaternionSlerp(chestRotStart, chestRotEnd, frameFraction)
+
+ neckRotStart = [frameData[13], frameData[14], frameData[15], frameData[12]]
+ neckRotEnd = [frameDataNext[13], frameDataNext[14], frameDataNext[15], frameDataNext[12]]
+ neckRot = p.getQuaternionSlerp(neckRotStart, neckRotEnd, frameFraction)
+
+ rightHipRotStart = [frameData[17], frameData[18], frameData[19], frameData[16]]
+ rightHipRotEnd = [frameDataNext[17], frameDataNext[18], frameDataNext[19], frameDataNext[16]]
+ rightHipRot = p.getQuaternionSlerp(rightHipRotStart, rightHipRotEnd, frameFraction)
+
+ rightKneeRotStart = [frameData[20]]
+ rightKneeRotEnd = [frameDataNext[20]]
+ rightKneeRot = [
+ rightKneeRotStart[0] + frameFraction * (rightKneeRotEnd[0] - rightKneeRotStart[0])
+ ]
+
+ rightAnkleRotStart = [frameData[22], frameData[23], frameData[24], frameData[21]]
+ rightAnkleRotEnd = [frameDataNext[22], frameDataNext[23], frameDataNext[24], frameDataNext[21]]
+ rightAnkleRot = p.getQuaternionSlerp(rightAnkleRotStart, rightAnkleRotEnd, frameFraction)
+
+ rightShoulderRotStart = [frameData[26], frameData[27], frameData[28], frameData[25]]
+ rightShoulderRotEnd = [
+ frameDataNext[26], frameDataNext[27], frameDataNext[28], frameDataNext[25]
+ ]
+ rightShoulderRot = p.getQuaternionSlerp(rightShoulderRotStart, rightShoulderRotEnd,
+ frameFraction)
+
+ rightElbowRotStart = [frameData[29]]
+ rightElbowRotEnd = [frameDataNext[29]]
+ rightElbowRot = [
+ rightElbowRotStart[0] + frameFraction * (rightElbowRotEnd[0] - rightElbowRotStart[0])
+ ]
+
+ leftHipRotStart = [frameData[31], frameData[32], frameData[33], frameData[30]]
+ leftHipRotEnd = [frameDataNext[31], frameDataNext[32], frameDataNext[33], frameDataNext[30]]
+ leftHipRot = p.getQuaternionSlerp(leftHipRotStart, leftHipRotEnd, frameFraction)
+
+ leftKneeRotStart = [frameData[34]]
+ leftKneeRotEnd = [frameDataNext[34]]
+ leftKneeRot = [leftKneeRotStart[0] + frameFraction * (leftKneeRotEnd[0] - leftKneeRotStart[0])]
+
+ leftAnkleRotStart = [frameData[36], frameData[37], frameData[38], frameData[35]]
+ leftAnkleRotEnd = [frameDataNext[36], frameDataNext[37], frameDataNext[38], frameDataNext[35]]
+ leftAnkleRot = p.getQuaternionSlerp(leftAnkleRotStart, leftAnkleRotEnd, frameFraction)
+
+ leftShoulderRotStart = [frameData[40], frameData[41], frameData[42], frameData[39]]
+ leftShoulderRotEnd = [frameDataNext[40], frameDataNext[41], frameDataNext[42], frameDataNext[39]]
+ leftShoulderRot = p.getQuaternionSlerp(leftShoulderRotStart, leftShoulderRotEnd, frameFraction)
+ leftElbowRotStart = [frameData[43]]
+ leftElbowRotEnd = [frameDataNext[43]]
+ leftElbowRot = [
+ leftElbowRotStart[0] + frameFraction * (leftElbowRotEnd[0] - leftElbowRotStart[0])
+ ]
+
+ if (0): #if (once):
+ p.resetJointStateMultiDof(humanoid, chest, chestRot)
+ p.resetJointStateMultiDof(humanoid, neck, neckRot)
+ p.resetJointStateMultiDof(humanoid, rightHip, rightHipRot)
+ p.resetJointStateMultiDof(humanoid, rightKnee, rightKneeRot)
+ p.resetJointStateMultiDof(humanoid, rightAnkle, rightAnkleRot)
+ p.resetJointStateMultiDof(humanoid, rightShoulder, rightShoulderRot)
+ p.resetJointStateMultiDof(humanoid, rightElbow, rightElbowRot)
+ p.resetJointStateMultiDof(humanoid, leftHip, leftHipRot)
+ p.resetJointStateMultiDof(humanoid, leftKnee, leftKneeRot)
+ p.resetJointStateMultiDof(humanoid, leftAnkle, leftAnkleRot)
+ p.resetJointStateMultiDof(humanoid, leftShoulder, leftShoulderRot)
+ p.resetJointStateMultiDof(humanoid, leftElbow, leftElbowRot)
+ once = False
+ #print("chestRot=",chestRot)
+ p.setGravity(0, 0, -10)
+
+ kp = kpMotor
+ if (useExplicitPD):
+ jointDofCounts = [4, 4, 4, 1, 4, 4, 1, 4, 1, 4, 4, 1]
+ #[x,y,z] base position and [x,y,z,w] base orientation!
+ totalDofs = 7
+ for dof in jointDofCounts:
+ totalDofs += dof
+
+ jointIndicesAll = [
+ chest, neck, rightHip, rightKnee, rightAnkle, rightShoulder, rightElbow, leftHip, leftKnee,
+ leftAnkle, leftShoulder, leftElbow
+ ]
+ basePos, baseOrn = p.getBasePositionAndOrientation(humanoid)
+ pose = [
+ basePos[0], basePos[1], basePos[2], baseOrn[0], baseOrn[1], baseOrn[2], baseOrn[3],
+ chestRot[0], chestRot[1], chestRot[2], chestRot[3], neckRot[0], neckRot[1], neckRot[2],
+ neckRot[3], rightHipRot[0], rightHipRot[1], rightHipRot[2], rightHipRot[3],
+ rightKneeRot[0], rightAnkleRot[0], rightAnkleRot[1], rightAnkleRot[2], rightAnkleRot[3],
+ rightShoulderRot[0], rightShoulderRot[1], rightShoulderRot[2], rightShoulderRot[3],
+ rightElbowRot[0], leftHipRot[0], leftHipRot[1], leftHipRot[2], leftHipRot[3],
+ leftKneeRot[0], leftAnkleRot[0], leftAnkleRot[1], leftAnkleRot[2], leftAnkleRot[3],
+ leftShoulderRot[0], leftShoulderRot[1], leftShoulderRot[2], leftShoulderRot[3],
+ leftElbowRot[0]
+ ]
+
+ #print("pose=")
+ #for po in pose:
+ # print(po)
+
+
+ taus = stablePD.computePD(bodyUniqueId=humanoid5,
+ jointIndices=jointIndicesAll,
+ desiredPositions=pose,
+ desiredVelocities=[0] * totalDofs,
+ kps=kpOrg,
+ kds=kdOrg,
+ maxForces=[maxForce] * totalDofs,
+ timeStep=timeStep)
+
+ indices = [chest, neck, rightHip, rightKnee,
+ rightAnkle, rightShoulder, rightElbow,
+ leftHip, leftKnee, leftAnkle,
+ leftShoulder, leftElbow]
+ targetPositions = [chestRot,neckRot,rightHipRot, rightKneeRot,
+ rightAnkleRot, rightShoulderRot, rightElbowRot,
+ leftHipRot, leftKneeRot, leftAnkleRot,
+ leftShoulderRot, leftElbowRot]
+ maxForces = [ [maxForce,maxForce,maxForce], [maxForce,maxForce,maxForce],[maxForce,maxForce,maxForce],[maxForce],
+ [maxForce,maxForce,maxForce],[maxForce,maxForce,maxForce],[maxForce],
+ [maxForce,maxForce,maxForce], [maxForce], [maxForce,maxForce,maxForce],
+ [maxForce,maxForce,maxForce], [maxForce]]
+
+
+ kps = [1000]*12
+ kds = [100]*12
+
+
+ p.setJointMotorControlMultiDofArray(humanoid,
+ indices,
+ p.STABLE_PD_CONTROL,
+ targetPositions=targetPositions,
+ positionGains=kps,
+ velocityGains=kds,
+ forces=maxForces)
+
+ taus3 = explicitPD.computePD(bodyUniqueId=humanoid3,
+ jointIndices=jointIndicesAll,
+ desiredPositions=pose,
+ desiredVelocities=[0] * totalDofs,
+ kps=kpOrg,
+ kds=kdOrg,
+ maxForces=[maxForce * 0.05] * totalDofs,
+ timeStep=timeStep)
+
+ #taus=[0]*43
+ dofIndex = 7
+ for index in range(len(jointIndicesAll)):
+ jointIndex = jointIndicesAll[index]
+ if jointDofCounts[index] == 4:
+
+ p.setJointMotorControlMultiDof(
+ humanoid5,
+ jointIndex,
+ p.TORQUE_CONTROL,
+ force=[taus[dofIndex + 0], taus[dofIndex + 1], taus[dofIndex + 2]])
+ p.setJointMotorControlMultiDof(
+ humanoid3,
+ jointIndex,
+ p.TORQUE_CONTROL,
+ force=[taus3[dofIndex + 0], taus3[dofIndex + 1], taus3[dofIndex + 2]])
+
+ if jointDofCounts[index] == 1:
+
+
+ p.setJointMotorControlMultiDof(humanoid5,
+ jointIndex,
+ controlMode=p.TORQUE_CONTROL,
+ force=[taus[dofIndex]])
+ p.setJointMotorControlMultiDof(humanoid3,
+ jointIndex,
+ controlMode=p.TORQUE_CONTROL,
+ force=[taus3[dofIndex]])
+
+ dofIndex += jointDofCounts[index]
+
+ #print("len(taus)=",len(taus))
+ #print("taus=",taus)
+
+ p.setJointMotorControlMultiDof(humanoid2,
+ chest,
+ p.POSITION_CONTROL,
+ targetPosition=chestRot,
+ positionGain=kp,
+ force=[maxForce])
+ p.setJointMotorControlMultiDof(humanoid2,
+ neck,
+ p.POSITION_CONTROL,
+ targetPosition=neckRot,
+ positionGain=kp,
+ force=[maxForce])
+ p.setJointMotorControlMultiDof(humanoid2,
+ rightHip,
+ p.POSITION_CONTROL,
+ targetPosition=rightHipRot,
+ positionGain=kp,
+ force=[maxForce])
+ p.setJointMotorControlMultiDof(humanoid2,
+ rightKnee,
+ p.POSITION_CONTROL,
+ targetPosition=rightKneeRot,
+ positionGain=kp,
+ force=[maxForce])
+ p.setJointMotorControlMultiDof(humanoid2,
+ rightAnkle,
+ p.POSITION_CONTROL,
+ targetPosition=rightAnkleRot,
+ positionGain=kp,
+ force=[maxForce])
+ p.setJointMotorControlMultiDof(humanoid2,
+ rightShoulder,
+ p.POSITION_CONTROL,
+ targetPosition=rightShoulderRot,
+ positionGain=kp,
+ force=[maxForce])
+ p.setJointMotorControlMultiDof(humanoid2,
+ rightElbow,
+ p.POSITION_CONTROL,
+ targetPosition=rightElbowRot,
+ positionGain=kp,
+ force=[maxForce])
+ p.setJointMotorControlMultiDof(humanoid2,
+ leftHip,
+ p.POSITION_CONTROL,
+ targetPosition=leftHipRot,
+ positionGain=kp,
+ force=[maxForce])
+ p.setJointMotorControlMultiDof(humanoid2,
+ leftKnee,
+ p.POSITION_CONTROL,
+ targetPosition=leftKneeRot,
+ positionGain=kp,
+ force=[maxForce])
+ p.setJointMotorControlMultiDof(humanoid2,
+ leftAnkle,
+ p.POSITION_CONTROL,
+ targetPosition=leftAnkleRot,
+ positionGain=kp,
+ force=[maxForce])
+ p.setJointMotorControlMultiDof(humanoid2,
+ leftShoulder,
+ p.POSITION_CONTROL,
+ targetPosition=leftShoulderRot,
+ positionGain=kp,
+ force=[maxForce])
+ p.setJointMotorControlMultiDof(humanoid2,
+ leftElbow,
+ p.POSITION_CONTROL,
+ targetPosition=leftElbowRot,
+ positionGain=kp,
+ force=[maxForce])
+
+ kinematicHumanoid4 = True
+ if (kinematicHumanoid4):
+ p.resetJointStateMultiDof(humanoid4, chest, chestRot)
+ p.resetJointStateMultiDof(humanoid4, neck, neckRot)
+ p.resetJointStateMultiDof(humanoid4, rightHip, rightHipRot)
+ p.resetJointStateMultiDof(humanoid4, rightKnee, rightKneeRot)
+ p.resetJointStateMultiDof(humanoid4, rightAnkle, rightAnkleRot)
+ p.resetJointStateMultiDof(humanoid4, rightShoulder, rightShoulderRot)
+ p.resetJointStateMultiDof(humanoid4, rightElbow, rightElbowRot)
+ p.resetJointStateMultiDof(humanoid4, leftHip, leftHipRot)
+ p.resetJointStateMultiDof(humanoid4, leftKnee, leftKneeRot)
+ p.resetJointStateMultiDof(humanoid4, leftAnkle, leftAnkleRot)
+ p.resetJointStateMultiDof(humanoid4, leftShoulder, leftShoulderRot)
+ p.resetJointStateMultiDof(humanoid4, leftElbow, leftElbowRot)
+ p.stepSimulation()
+
+ if showJointMotorTorques:
+ for j in range(p.getNumJoints(humanoid2)):
+ jointState = p.getJointStateMultiDof(humanoid2, j)
+ print("jointStateMultiDof[", j, "].pos=", jointState[0])
+ print("jointStateMultiDof[", j, "].vel=", jointState[1])
+ print("jointStateMultiDof[", j, "].jointForces=", jointState[3])
+ time.sleep(timeStep)
diff --git a/examples/pybullet/gym/pybullet_examples/inverse_dynamics.py b/examples/pybullet/gym/pybullet_examples/inverse_dynamics.py
new file mode 100644
index 000000000..5ec99aef8
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_examples/inverse_dynamics.py
@@ -0,0 +1,167 @@
+import pybullet as bullet
+import pybullet_data as pd
+
+plot = True
+import time
+
+if (plot):
+ import matplotlib.pyplot as plt
+import math
+verbose = False
+
+# Parameters:
+robot_base = [0., 0., 0.]
+robot_orientation = [0., 0., 0., 1.]
+delta_t = 0.0001
+
+# Initialize Bullet Simulator
+id_simulator = bullet.connect(bullet.GUI) # or bullet.DIRECT for non-graphical version
+bullet.setTimeStep(delta_t)
+bullet.setAdditionalSearchPath(pd.getDataPath())
+
+# Switch between URDF with/without FIXED joints
+with_fixed_joints = True
+
+if with_fixed_joints:
+ id_revolute_joints = [0, 3]
+ id_robot = bullet.loadURDF("TwoJointRobot_w_fixedJoints.urdf",
+ robot_base,
+ robot_orientation,
+ useFixedBase=True)
+else:
+ id_revolute_joints = [0, 1]
+ id_robot = bullet.loadURDF("TwoJointRobot_wo_fixedJoints.urdf",
+ robot_base,
+ robot_orientation,
+ useFixedBase=True)
+
+bullet.changeDynamics(id_robot, -1, linearDamping=0, angularDamping=0)
+bullet.changeDynamics(id_robot, 0, linearDamping=0, angularDamping=0)
+bullet.changeDynamics(id_robot, 1, linearDamping=0, angularDamping=0)
+
+jointTypeNames = [
+ "JOINT_REVOLUTE", "JOINT_PRISMATIC", "JOINT_SPHERICAL", "JOINT_PLANAR", "JOINT_FIXED",
+ "JOINT_POINT2POINT", "JOINT_GEAR"
+]
+
+# Disable the motors for torque control:
+bullet.setJointMotorControlArray(id_robot,
+ id_revolute_joints,
+ bullet.VELOCITY_CONTROL,
+ forces=[0.0, 0.0])
+
+# Target Positions:
+start = 0.0
+end = 1.0
+
+steps = int((end - start) / delta_t)
+t = [0] * steps
+q_pos_desired = [[0.] * steps, [0.] * steps]
+q_vel_desired = [[0.] * steps, [0.] * steps]
+q_acc_desired = [[0.] * steps, [0.] * steps]
+
+for s in range(steps):
+ t[s] = start + s * delta_t
+ q_pos_desired[0][s] = 1. / (2. * math.pi) * math.sin(2. * math.pi * t[s]) - t[s]
+ q_pos_desired[1][s] = -1. / (2. * math.pi) * (math.cos(2. * math.pi * t[s]) - 1.0)
+
+ q_vel_desired[0][s] = math.cos(2. * math.pi * t[s]) - 1.
+ q_vel_desired[1][s] = math.sin(2. * math.pi * t[s])
+
+ q_acc_desired[0][s] = -2. * math.pi * math.sin(2. * math.pi * t[s])
+ q_acc_desired[1][s] = 2. * math.pi * math.cos(2. * math.pi * t[s])
+
+q_pos = [[0.] * steps, [0.] * steps]
+q_vel = [[0.] * steps, [0.] * steps]
+q_tor = [[0.] * steps, [0.] * steps]
+
+# Do Torque Control:
+for i in range(len(t)):
+
+ # Read Sensor States:
+ joint_states = bullet.getJointStates(id_robot, id_revolute_joints)
+
+ q_pos[0][i] = joint_states[0][0]
+ a = joint_states[1][0]
+ if (verbose):
+ print("joint_states[1][0]")
+ print(joint_states[1][0])
+ q_pos[1][i] = a
+
+ q_vel[0][i] = joint_states[0][1]
+ q_vel[1][i] = joint_states[1][1]
+
+ # Computing the torque from inverse dynamics:
+ obj_pos = [q_pos[0][i], q_pos[1][i]]
+ obj_vel = [q_vel[0][i], q_vel[1][i]]
+ obj_acc = [q_acc_desired[0][i], q_acc_desired[1][i]]
+
+ if (verbose):
+ print("calculateInverseDynamics")
+ print("id_robot")
+ print(id_robot)
+ print("obj_pos")
+ print(obj_pos)
+ print("obj_vel")
+ print(obj_vel)
+ print("obj_acc")
+ print(obj_acc)
+
+ torque = bullet.calculateInverseDynamics(id_robot, obj_pos, obj_vel, obj_acc)
+ q_tor[0][i] = torque[0]
+ q_tor[1][i] = torque[1]
+ if (verbose):
+ print("torque=")
+ print(torque)
+
+ # Set the Joint Torques:
+ bullet.setJointMotorControlArray(id_robot,
+ id_revolute_joints,
+ bullet.TORQUE_CONTROL,
+ forces=[torque[0], torque[1]])
+
+ # Step Simulation
+ bullet.stepSimulation()
+
+# Plot the Position, Velocity and Acceleration:
+if plot:
+ figure = plt.figure(figsize=[15, 4.5])
+ figure.subplots_adjust(left=0.05, bottom=0.11, right=0.97, top=0.9, wspace=0.4, hspace=0.55)
+
+ ax_pos = figure.add_subplot(141)
+ ax_pos.set_title("Joint Position")
+ ax_pos.plot(t, q_pos_desired[0], '--r', lw=4, label='Desired q0')
+ ax_pos.plot(t, q_pos_desired[1], '--b', lw=4, label='Desired q1')
+ ax_pos.plot(t, q_pos[0], '-r', lw=1, label='Measured q0')
+ ax_pos.plot(t, q_pos[1], '-b', lw=1, label='Measured q1')
+ ax_pos.set_ylim(-1., 1.)
+ ax_pos.legend()
+
+ ax_vel = figure.add_subplot(142)
+ ax_vel.set_title("Joint Velocity")
+ ax_vel.plot(t, q_vel_desired[0], '--r', lw=4, label='Desired q0')
+ ax_vel.plot(t, q_vel_desired[1], '--b', lw=4, label='Desired q1')
+ ax_vel.plot(t, q_vel[0], '-r', lw=1, label='Measured q0')
+ ax_vel.plot(t, q_vel[1], '-b', lw=1, label='Measured q1')
+ ax_vel.set_ylim(-2., 2.)
+ ax_vel.legend()
+
+ ax_acc = figure.add_subplot(143)
+ ax_acc.set_title("Joint Acceleration")
+ ax_acc.plot(t, q_acc_desired[0], '--r', lw=4, label='Desired q0')
+ ax_acc.plot(t, q_acc_desired[1], '--b', lw=4, label='Desired q1')
+ ax_acc.set_ylim(-10., 10.)
+ ax_acc.legend()
+
+ ax_tor = figure.add_subplot(144)
+ ax_tor.set_title("Executed Torque")
+ ax_tor.plot(t, q_tor[0], '-r', lw=2, label='Torque q0')
+ ax_tor.plot(t, q_tor[1], '-b', lw=2, label='Torque q1')
+ ax_tor.set_ylim(-20., 20.)
+ ax_tor.legend()
+
+ plt.pause(0.01)
+
+while (1):
+ bullet.stepSimulation()
+ time.sleep(0.01)
diff --git a/examples/pybullet/gym/pybullet_examples/inverse_kinematics.py b/examples/pybullet/gym/pybullet_examples/inverse_kinematics.py
new file mode 100644
index 000000000..bf198ff32
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_examples/inverse_kinematics.py
@@ -0,0 +1,125 @@
+import pybullet as p
+import time
+import math
+from datetime import datetime
+import pybullet_data
+
+clid = p.connect(p.SHARED_MEMORY)
+if (clid < 0):
+ p.connect(p.GUI)
+ #p.connect(p.SHARED_MEMORY_GUI)
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+
+p.loadURDF("plane.urdf", [0, 0, -0.3])
+kukaId = p.loadURDF("kuka_iiwa/model.urdf", [0, 0, 0])
+p.resetBasePositionAndOrientation(kukaId, [0, 0, 0], [0, 0, 0, 1])
+kukaEndEffectorIndex = 6
+numJoints = p.getNumJoints(kukaId)
+if (numJoints != 7):
+ exit()
+
+#lower limits for null space
+ll = [-.967, -2, -2.96, 0.19, -2.96, -2.09, -3.05]
+#upper limits for null space
+ul = [.967, 2, 2.96, 2.29, 2.96, 2.09, 3.05]
+#joint ranges for null space
+jr = [5.8, 4, 5.8, 4, 5.8, 4, 6]
+#restposes for null space
+rp = [0, 0, 0, 0.5 * math.pi, 0, -math.pi * 0.5 * 0.66, 0]
+#joint damping coefficents
+jd = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
+
+for i in range(numJoints):
+ p.resetJointState(kukaId, i, rp[i])
+
+p.setGravity(0, 0, 0)
+t = 0.
+prevPose = [0, 0, 0]
+prevPose1 = [0, 0, 0]
+hasPrevPose = 0
+useNullSpace = 1
+
+useOrientation = 1
+#If we set useSimulation=0, it sets the arm pose to be the IK result directly without using dynamic control.
+#This can be used to test the IK result accuracy.
+useSimulation = 1
+useRealTimeSimulation = 0
+ikSolver = 0
+p.setRealTimeSimulation(useRealTimeSimulation)
+#trailDuration is duration (in seconds) after debug lines will be removed automatically
+#use 0 for no-removal
+trailDuration = 15
+
+i=0
+while 1:
+ i+=1
+ #p.getCameraImage(320,
+ # 200,
+ # flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX,
+ # renderer=p.ER_BULLET_HARDWARE_OPENGL)
+ if (useRealTimeSimulation):
+ dt = datetime.now()
+ t = (dt.second / 60.) * 2. * math.pi
+ else:
+ t = t + 0.01
+
+ if (useSimulation and useRealTimeSimulation == 0):
+ p.stepSimulation()
+
+ for i in range(1):
+ pos = [-0.4, 0.2 * math.cos(t), 0. + 0.2 * math.sin(t)]
+ #end effector points down, not up (in case useOrientation==1)
+ orn = p.getQuaternionFromEuler([0, -math.pi, 0])
+
+ if (useNullSpace == 1):
+ if (useOrientation == 1):
+ jointPoses = p.calculateInverseKinematics(kukaId, kukaEndEffectorIndex, pos, orn, ll, ul,
+ jr, rp)
+ else:
+ jointPoses = p.calculateInverseKinematics(kukaId,
+ kukaEndEffectorIndex,
+ pos,
+ lowerLimits=ll,
+ upperLimits=ul,
+ jointRanges=jr,
+ restPoses=rp)
+ else:
+ if (useOrientation == 1):
+ jointPoses = p.calculateInverseKinematics(kukaId,
+ kukaEndEffectorIndex,
+ pos,
+ orn,
+ jointDamping=jd,
+ solver=ikSolver,
+ maxNumIterations=100,
+ residualThreshold=.01)
+ else:
+ jointPoses = p.calculateInverseKinematics(kukaId,
+ kukaEndEffectorIndex,
+ pos,
+ solver=ikSolver)
+
+ if (useSimulation):
+ for i in range(numJoints):
+ p.setJointMotorControl2(bodyIndex=kukaId,
+ jointIndex=i,
+ controlMode=p.POSITION_CONTROL,
+ targetPosition=jointPoses[i],
+ targetVelocity=0,
+ force=500,
+ positionGain=0.03,
+ velocityGain=1)
+ else:
+ #reset the joint state (ignoring all dynamics, not recommended to use during simulation)
+ for i in range(numJoints):
+ p.resetJointState(kukaId, i, jointPoses[i])
+
+ ls = p.getLinkState(kukaId, kukaEndEffectorIndex)
+ if (hasPrevPose):
+ p.addUserDebugLine(prevPose, pos, [0, 0, 0.3], 1, trailDuration)
+ p.addUserDebugLine(prevPose1, ls[4], [1, 0, 0], 1, trailDuration)
+ prevPose = pos
+ prevPose1 = ls[4]
+ hasPrevPose = 1
+p.disconnect()
diff --git a/examples/pybullet/gym/pybullet_examples/inverse_kinematics_husky_kuka.py b/examples/pybullet/gym/pybullet_examples/inverse_kinematics_husky_kuka.py
new file mode 100644
index 000000000..bb0e26eaa
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_examples/inverse_kinematics_husky_kuka.py
@@ -0,0 +1,202 @@
+import pybullet as p
+import time
+import math
+from datetime import datetime
+from datetime import datetime
+import pybullet_data
+
+clid = p.connect(p.SHARED_MEMORY)
+
+
+if (clid < 0):
+ p.connect(p.GUI)
+
+p.setPhysicsEngineParameter(enableConeFriction=0)
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+
+
+p.loadURDF("plane.urdf", [0, 0, -0.3])
+husky = p.loadURDF("husky/husky.urdf", [0.290388, 0.329902, -0.310270],
+ [0.002328, -0.000984, 0.996491, 0.083659])
+for i in range(p.getNumJoints(husky)):
+ print(p.getJointInfo(husky, i))
+kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf", 0.193749, 0.345564, 0.120208, 0.002327,
+ -0.000988, 0.996491, 0.083659)
+ob = kukaId
+jointPositions = [3.559609, 0.411182, 0.862129, 1.744441, 0.077299, -1.129685, 0.006001]
+for jointIndex in range(p.getNumJoints(ob)):
+ p.resetJointState(ob, jointIndex, jointPositions[jointIndex])
+
+#put kuka on top of husky
+
+cid = p.createConstraint(husky, -1, kukaId, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0], [0., 0., -.5],
+ [0, 0, 0, 1])
+
+baseorn = p.getQuaternionFromEuler([3.1415, 0, 0.3])
+baseorn = [0, 0, 0, 1]
+#[0, 0, 0.707, 0.707]
+
+#p.resetBasePositionAndOrientation(kukaId,[0,0,0],baseorn)#[0,0,0,1])
+kukaEndEffectorIndex = 6
+numJoints = p.getNumJoints(kukaId)
+if (numJoints != 7):
+ exit()
+
+#lower limits for null space
+ll = [-.967, -2, -2.96, 0.19, -2.96, -2.09, -3.05]
+#upper limits for null space
+ul = [.967, 2, 2.96, 2.29, 2.96, 2.09, 3.05]
+#joint ranges for null space
+jr = [5.8, 4, 5.8, 4, 5.8, 4, 6]
+#restposes for null space
+rp = [0, 0, 0, 0.5 * math.pi, 0, -math.pi * 0.5 * 0.66, 0]
+#joint damping coefficents
+jd = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
+
+for i in range(numJoints):
+ p.resetJointState(kukaId, i, rp[i])
+
+p.setGravity(0, 0, -10)
+t = 0.
+prevPose = [0, 0, 0]
+prevPose1 = [0, 0, 0]
+hasPrevPose = 0
+useNullSpace = 0
+
+useOrientation = 0
+#If we set useSimulation=0, it sets the arm pose to be the IK result directly without using dynamic control.
+#This can be used to test the IK result accuracy.
+useSimulation = 1
+useRealTimeSimulation = 1
+p.setRealTimeSimulation(useRealTimeSimulation)
+#trailDuration is duration (in seconds) after debug lines will be removed automatically
+#use 0 for no-removal
+trailDuration = 15
+basepos = [0, 0, 0]
+ang = 0
+ang = 0
+
+
+def accurateCalculateInverseKinematics(kukaId, endEffectorId, targetPos, threshold, maxIter):
+ closeEnough = False
+ iter = 0
+ dist2 = 1e30
+ while (not closeEnough and iter < maxIter):
+ jointPoses = p.calculateInverseKinematics(kukaId, kukaEndEffectorIndex, targetPos)
+ for i in range(numJoints):
+ p.resetJointState(kukaId, i, jointPoses[i])
+ ls = p.getLinkState(kukaId, kukaEndEffectorIndex)
+ newPos = ls[4]
+ diff = [targetPos[0] - newPos[0], targetPos[1] - newPos[1], targetPos[2] - newPos[2]]
+ dist2 = (diff[0] * diff[0] + diff[1] * diff[1] + diff[2] * diff[2])
+ closeEnough = (dist2 < threshold)
+ iter = iter + 1
+ #print ("Num iter: "+str(iter) + "threshold: "+str(dist2))
+ return jointPoses
+
+
+wheels = [2, 3, 4, 5]
+#(2, b'front_left_wheel', 0, 7, 6, 1, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'front_left_wheel_link')
+#(3, b'front_right_wheel', 0, 8, 7, 1, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'front_right_wheel_link')
+#(4, b'rear_left_wheel', 0, 9, 8, 1, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'rear_left_wheel_link')
+#(5, b'rear_right_wheel', 0, 10, 9, 1, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'rear_right_wheel_link')
+wheelVelocities = [0, 0, 0, 0]
+wheelDeltasTurn = [1, -1, 1, -1]
+wheelDeltasFwd = [1, 1, 1, 1]
+while 1:
+ keys = p.getKeyboardEvents()
+ shift = 0.01
+ wheelVelocities = [0, 0, 0, 0]
+ speed = 1.0
+ for k in keys:
+ if ord('s') in keys:
+ p.saveWorld("state.py")
+ if ord('a') in keys:
+ basepos = basepos = [basepos[0], basepos[1] - shift, basepos[2]]
+ if ord('d') in keys:
+ basepos = basepos = [basepos[0], basepos[1] + shift, basepos[2]]
+
+ if p.B3G_LEFT_ARROW in keys:
+ for i in range(len(wheels)):
+ wheelVelocities[i] = wheelVelocities[i] - speed * wheelDeltasTurn[i]
+ if p.B3G_RIGHT_ARROW in keys:
+ for i in range(len(wheels)):
+ wheelVelocities[i] = wheelVelocities[i] + speed * wheelDeltasTurn[i]
+ if p.B3G_UP_ARROW in keys:
+ for i in range(len(wheels)):
+ wheelVelocities[i] = wheelVelocities[i] + speed * wheelDeltasFwd[i]
+ if p.B3G_DOWN_ARROW in keys:
+ for i in range(len(wheels)):
+ wheelVelocities[i] = wheelVelocities[i] - speed * wheelDeltasFwd[i]
+
+ baseorn = p.getQuaternionFromEuler([0, 0, ang])
+ for i in range(len(wheels)):
+ p.setJointMotorControl2(husky,
+ wheels[i],
+ p.VELOCITY_CONTROL,
+ targetVelocity=wheelVelocities[i],
+ force=1000)
+ #p.resetBasePositionAndOrientation(kukaId,basepos,baseorn)#[0,0,0,1])
+ if (useRealTimeSimulation):
+ t = time.time() #(dt, micro) = datetime.utcnow().strftime('%Y-%m-%d %H:%M:%S.%f').split('.')
+ #t = (dt.second/60.)*2.*math.pi
+ else:
+ t = t + 0.001
+
+ if (useSimulation and useRealTimeSimulation == 0):
+ p.stepSimulation()
+
+ for i in range(1):
+ #pos = [-0.4,0.2*math.cos(t),0.+0.2*math.sin(t)]
+ pos = [0.2 * math.cos(t), 0, 0. + 0.2 * math.sin(t) + 0.7]
+ #end effector points down, not up (in case useOrientation==1)
+ orn = p.getQuaternionFromEuler([0, -math.pi, 0])
+
+ if (useNullSpace == 1):
+ if (useOrientation == 1):
+ jointPoses = p.calculateInverseKinematics(kukaId, kukaEndEffectorIndex, pos, orn, ll, ul,
+ jr, rp)
+ else:
+ jointPoses = p.calculateInverseKinematics(kukaId,
+ kukaEndEffectorIndex,
+ pos,
+ lowerLimits=ll,
+ upperLimits=ul,
+ jointRanges=jr,
+ restPoses=rp)
+ else:
+ if (useOrientation == 1):
+ jointPoses = p.calculateInverseKinematics(kukaId,
+ kukaEndEffectorIndex,
+ pos,
+ orn,
+ jointDamping=jd)
+ else:
+ threshold = 0.001
+ maxIter = 100
+ jointPoses = accurateCalculateInverseKinematics(kukaId, kukaEndEffectorIndex, pos,
+ threshold, maxIter)
+
+ if (useSimulation):
+ for i in range(numJoints):
+ p.setJointMotorControl2(bodyIndex=kukaId,
+ jointIndex=i,
+ controlMode=p.POSITION_CONTROL,
+ targetPosition=jointPoses[i],
+ targetVelocity=0,
+ force=500,
+ positionGain=1,
+ velocityGain=0.1)
+ else:
+ #reset the joint state (ignoring all dynamics, not recommended to use during simulation)
+ for i in range(numJoints):
+ p.resetJointState(kukaId, i, jointPoses[i])
+
+ ls = p.getLinkState(kukaId, kukaEndEffectorIndex)
+ if (hasPrevPose):
+ p.addUserDebugLine(prevPose, pos, [0, 0, 0.3], 1, trailDuration)
+ p.addUserDebugLine(prevPose1, ls[4], [1, 0, 0], 1, trailDuration)
+ prevPose = pos
+ prevPose1 = ls[4]
+ hasPrevPose = 1
diff --git a/examples/pybullet/gym/pybullet_examples/inverse_kinematics_pole.py b/examples/pybullet/gym/pybullet_examples/inverse_kinematics_pole.py
new file mode 100644
index 000000000..653581a0e
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_examples/inverse_kinematics_pole.py
@@ -0,0 +1,66 @@
+import pybullet as p
+import time
+import math
+from datetime import datetime
+import pybullet_data
+
+clid = p.connect(p.SHARED_MEMORY)
+if (clid < 0):
+ p.connect(p.GUI)
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+p.loadURDF("plane.urdf", [0, 0, -1.3])
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
+sawyerId = p.loadURDF("pole.urdf", [0, 0, 0])
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
+p.resetBasePositionAndOrientation(sawyerId, [0, 0, 0], [0, 0, 0, 1])
+
+sawyerEndEffectorIndex = 3
+numJoints = p.getNumJoints(sawyerId)
+#joint damping coefficents
+jd = [0.1, 0.1, 0.1, 0.1]
+
+p.setGravity(0, 0, 0)
+t = 0.
+prevPose = [0, 0, 0]
+prevPose1 = [0, 0, 0]
+hasPrevPose = 0
+
+ikSolver = 0
+useRealTimeSimulation = 0
+p.setRealTimeSimulation(useRealTimeSimulation)
+#trailDuration is duration (in seconds) after debug lines will be removed automatically
+#use 0 for no-removal
+trailDuration = 1
+
+while 1:
+ if (useRealTimeSimulation):
+ dt = datetime.now()
+ t = (dt.second / 60.) * 2. * math.pi
+ else:
+ t = t + 0.01
+ time.sleep(0.01)
+
+ for i in range(1):
+ pos = [2. * math.cos(t), 2. * math.cos(t), 0. + 2. * math.sin(t)]
+ jointPoses = p.calculateInverseKinematics(sawyerId,
+ sawyerEndEffectorIndex,
+ pos,
+ jointDamping=jd,
+ solver=ikSolver,
+ maxNumIterations=100)
+
+ #reset the joint state (ignoring all dynamics, not recommended to use during simulation)
+ for i in range(numJoints):
+ jointInfo = p.getJointInfo(sawyerId, i)
+ qIndex = jointInfo[3]
+ if qIndex > -1:
+ p.resetJointState(sawyerId, i, jointPoses[qIndex - 7])
+
+ ls = p.getLinkState(sawyerId, sawyerEndEffectorIndex)
+ if (hasPrevPose):
+ p.addUserDebugLine(prevPose, pos, [0, 0, 0.3], 1, trailDuration)
+ p.addUserDebugLine(prevPose1, ls[4], [1, 0, 0], 1, trailDuration)
+ prevPose = pos
+ prevPose1 = ls[4]
+ hasPrevPose = 1
diff --git a/examples/pybullet/gym/pybullet_examples/jacobian.py b/examples/pybullet/gym/pybullet_examples/jacobian.py
new file mode 100644
index 000000000..4a7e9725c
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_examples/jacobian.py
@@ -0,0 +1,102 @@
+import pybullet as p
+import pybullet_data
+
+
+
+def getJointStates(robot):
+ joint_states = p.getJointStates(robot, range(p.getNumJoints(robot)))
+ joint_positions = [state[0] for state in joint_states]
+ joint_velocities = [state[1] for state in joint_states]
+ joint_torques = [state[3] for state in joint_states]
+ return joint_positions, joint_velocities, joint_torques
+
+
+def getMotorJointStates(robot):
+ joint_states = p.getJointStates(robot, range(p.getNumJoints(robot)))
+ joint_infos = [p.getJointInfo(robot, i) for i in range(p.getNumJoints(robot))]
+ joint_states = [j for j, i in zip(joint_states, joint_infos) if i[3] > -1]
+ joint_positions = [state[0] for state in joint_states]
+ joint_velocities = [state[1] for state in joint_states]
+ joint_torques = [state[3] for state in joint_states]
+ return joint_positions, joint_velocities, joint_torques
+
+
+def setJointPosition(robot, position, kp=1.0, kv=0.3):
+ num_joints = p.getNumJoints(robot)
+ zero_vec = [0.0] * num_joints
+ if len(position) == num_joints:
+ p.setJointMotorControlArray(robot,
+ range(num_joints),
+ p.POSITION_CONTROL,
+ targetPositions=position,
+ targetVelocities=zero_vec,
+ positionGains=[kp] * num_joints,
+ velocityGains=[kv] * num_joints)
+ else:
+ print("Not setting torque. "
+ "Expected torque vector of "
+ "length {}, got {}".format(num_joints, len(torque)))
+
+
+def multiplyJacobian(robot, jacobian, vector):
+ result = [0.0, 0.0, 0.0]
+ i = 0
+ for c in range(len(vector)):
+ if p.getJointInfo(robot, c)[3] > -1:
+ for r in range(3):
+ result[r] += jacobian[r][i] * vector[c]
+ i += 1
+ return result
+
+
+clid = p.connect(p.SHARED_MEMORY)
+if (clid < 0):
+ p.connect(p.DIRECT)
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+
+time_step = 0.001
+gravity_constant = -9.81
+p.resetSimulation()
+p.setTimeStep(time_step)
+p.setGravity(0.0, 0.0, gravity_constant)
+
+p.loadURDF("plane.urdf", [0, 0, -0.3])
+
+kukaId = p.loadURDF("TwoJointRobot_w_fixedJoints.urdf", useFixedBase=True)
+#kukaId = p.loadURDF("TwoJointRobot_w_fixedJoints.urdf",[0,0,0])
+#kukaId = p.loadURDF("kuka_iiwa/model.urdf",[0,0,0])
+#kukaId = p.loadURDF("kuka_lwr/kuka.urdf",[0,0,0])
+#kukaId = p.loadURDF("humanoid/nao.urdf",[0,0,0])
+p.resetBasePositionAndOrientation(kukaId, [0, 0, 0], [0, 0, 0, 1])
+numJoints = p.getNumJoints(kukaId)
+kukaEndEffectorIndex = numJoints - 1
+
+# Set a joint target for the position control and step the sim.
+setJointPosition(kukaId, [0.1] * numJoints)
+p.stepSimulation()
+
+# Get the joint and link state directly from Bullet.
+pos, vel, torq = getJointStates(kukaId)
+mpos, mvel, mtorq = getMotorJointStates(kukaId)
+
+result = p.getLinkState(kukaId,
+ kukaEndEffectorIndex,
+ computeLinkVelocity=1,
+ computeForwardKinematics=1)
+link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = result
+# Get the Jacobians for the CoM of the end-effector link.
+# Note that in this example com_rot = identity, and we would need to use com_rot.T * com_trn.
+# The localPosition is always defined in terms of the link frame coordinates.
+
+zero_vec = [0.0] * len(mpos)
+jac_t, jac_r = p.calculateJacobian(kukaId, kukaEndEffectorIndex, com_trn, mpos, zero_vec, zero_vec)
+
+print("Link linear velocity of CoM from getLinkState:")
+print(link_vt)
+print("Link linear velocity of CoM from linearJacobian * q_dot:")
+print(multiplyJacobian(kukaId, jac_t, vel))
+print("Link angular velocity of CoM from getLinkState:")
+print(link_vr)
+print("Link angular velocity of CoM from angularJacobian * q_dot:")
+print(multiplyJacobian(kukaId, jac_r, vel))
diff --git a/examples/pybullet/gym/pybullet_examples/manyspheres.py b/examples/pybullet/gym/pybullet_examples/manyspheres.py
new file mode 100644
index 000000000..4c6e5197c
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_examples/manyspheres.py
@@ -0,0 +1,38 @@
+import pybullet as p
+import time
+import pybullet_data
+
+
+conid = p.connect(p.SHARED_MEMORY)
+if (conid < 0):
+ p.connect(p.GUI)
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+p.setInternalSimFlags(0)
+p.resetSimulation()
+
+p.loadURDF("plane.urdf", useMaximalCoordinates=True)
+p.loadURDF("tray/traybox.urdf", useMaximalCoordinates=True)
+
+gravXid = p.addUserDebugParameter("gravityX", -10, 10, 0)
+gravYid = p.addUserDebugParameter("gravityY", -10, 10, 0)
+gravZid = p.addUserDebugParameter("gravityZ", -10, 10, -10)
+p.setPhysicsEngineParameter(numSolverIterations=10)
+p.setPhysicsEngineParameter(contactBreakingThreshold=0.001)
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
+for i in range(10):
+ for j in range(10):
+ for k in range(10):
+ ob = p.loadURDF("sphere_1cm.urdf", [0.02 * i, 0.02 * j, 0.2 + 0.02 * k],
+ useMaximalCoordinates=True)
+
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
+p.setGravity(0, 0, -10)
+
+p.setRealTimeSimulation(1)
+while True:
+ gravX = p.readUserDebugParameter(gravXid)
+ gravY = p.readUserDebugParameter(gravYid)
+ gravZ = p.readUserDebugParameter(gravZid)
+ p.setGravity(gravX, gravY, gravZ)
+ time.sleep(0.01)
diff --git a/examples/pybullet/gym/pybullet_examples/mimicJointConstraint.py b/examples/pybullet/gym/pybullet_examples/mimicJointConstraint.py
new file mode 100644
index 000000000..7a54f5c10
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_examples/mimicJointConstraint.py
@@ -0,0 +1,50 @@
+#a mimic joint can act as a gear between two joints
+#you can control the gear ratio in magnitude and sign (>0 reverses direction)
+
+import pybullet as p
+import time
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+p.loadURDF("plane.urdf", 0, 0, -2)
+wheelA = p.loadURDF("differential/diff_ring.urdf", [0, 0, 0])
+for i in range(p.getNumJoints(wheelA)):
+ print(p.getJointInfo(wheelA, i))
+ p.setJointMotorControl2(wheelA, i, p.VELOCITY_CONTROL, targetVelocity=0, force=0)
+
+c = p.createConstraint(wheelA,
+ 1,
+ wheelA,
+ 3,
+ jointType=p.JOINT_GEAR,
+ jointAxis=[0, 1, 0],
+ parentFramePosition=[0, 0, 0],
+ childFramePosition=[0, 0, 0])
+p.changeConstraint(c, gearRatio=1, maxForce=10000)
+
+c = p.createConstraint(wheelA,
+ 2,
+ wheelA,
+ 4,
+ jointType=p.JOINT_GEAR,
+ jointAxis=[0, 1, 0],
+ parentFramePosition=[0, 0, 0],
+ childFramePosition=[0, 0, 0])
+p.changeConstraint(c, gearRatio=-1, maxForce=10000)
+
+c = p.createConstraint(wheelA,
+ 1,
+ wheelA,
+ 4,
+ jointType=p.JOINT_GEAR,
+ jointAxis=[0, 1, 0],
+ parentFramePosition=[0, 0, 0],
+ childFramePosition=[0, 0, 0])
+p.changeConstraint(c, gearRatio=-1, maxForce=10000)
+
+p.setRealTimeSimulation(1)
+while (1):
+ p.setGravity(0, 0, -10)
+ time.sleep(0.01)
+#p.removeConstraint(c)
diff --git a/examples/pybullet/gym/pybullet_examples/motorMaxVelocity.py b/examples/pybullet/gym/pybullet_examples/motorMaxVelocity.py
new file mode 100644
index 000000000..95a0c1e39
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_examples/motorMaxVelocity.py
@@ -0,0 +1,22 @@
+import pybullet as p
+import time
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+cartpole = p.loadURDF("cartpole.urdf")
+p.setRealTimeSimulation(1)
+p.setJointMotorControl2(cartpole,
+ 1,
+ p.POSITION_CONTROL,
+ targetPosition=1000,
+ targetVelocity=0,
+ force=1000,
+ positionGain=1,
+ velocityGain=0,
+ maxVelocity=0.5)
+while (1):
+ p.setGravity(0, 0, -10)
+ js = p.getJointState(cartpole, 1)
+ print("position=", js[0], "velocity=", js[1])
+ time.sleep(0.01)
diff --git a/examples/pybullet/gym/pybullet_examples/pdControl.py b/examples/pybullet/gym/pybullet_examples/pdControl.py
new file mode 100644
index 000000000..e9c902e23
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_examples/pdControl.py
@@ -0,0 +1,152 @@
+import pybullet as p
+from pdControllerExplicit import PDControllerExplicitMultiDof
+from pdControllerExplicit import PDControllerExplicit
+from pdControllerStable import PDControllerStable
+
+import time
+
+useMaximalCoordinates = False
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+pole = p.loadURDF("cartpole.urdf", [0, 0, 0], useMaximalCoordinates=useMaximalCoordinates)
+pole2 = p.loadURDF("cartpole.urdf", [0, 1, 0], useMaximalCoordinates=useMaximalCoordinates)
+pole3 = p.loadURDF("cartpole.urdf", [0, 2, 0], useMaximalCoordinates=useMaximalCoordinates)
+pole4 = p.loadURDF("cartpole.urdf", [0, 3, 0], useMaximalCoordinates=useMaximalCoordinates)
+
+exPD = PDControllerExplicitMultiDof(p)
+sPD = PDControllerStable(p)
+
+for i in range(p.getNumJoints(pole2)):
+ #disable default constraint-based motors
+ p.setJointMotorControl2(pole, i, p.POSITION_CONTROL, targetPosition=0, force=0)
+ p.setJointMotorControl2(pole2, i, p.POSITION_CONTROL, targetPosition=0, force=0)
+ p.setJointMotorControl2(pole3, i, p.POSITION_CONTROL, targetPosition=0, force=0)
+ p.setJointMotorControl2(pole4, i, p.POSITION_CONTROL, targetPosition=0, force=0)
+
+ #print("joint",i,"=",p.getJointInfo(pole2,i))
+
+timeStepId = p.addUserDebugParameter("timeStep", 0.001, 0.1, 0.01)
+desiredPosCartId = p.addUserDebugParameter("desiredPosCart", -10, 10, 2)
+desiredVelCartId = p.addUserDebugParameter("desiredVelCart", -10, 10, 0)
+kpCartId = p.addUserDebugParameter("kpCart", 0, 500, 1300)
+kdCartId = p.addUserDebugParameter("kdCart", 0, 300, 150)
+maxForceCartId = p.addUserDebugParameter("maxForceCart", 0, 5000, 1000)
+
+textColor = [1, 1, 1]
+shift = 0.05
+p.addUserDebugText("explicit PD", [shift, 0, .1],
+ textColor,
+ parentObjectUniqueId=pole,
+ parentLinkIndex=1)
+p.addUserDebugText("explicit PD plugin", [shift, 0, -.1],
+ textColor,
+ parentObjectUniqueId=pole2,
+ parentLinkIndex=1)
+p.addUserDebugText("stablePD", [shift, 0, .1],
+ textColor,
+ parentObjectUniqueId=pole4,
+ parentLinkIndex=1)
+p.addUserDebugText("position constraint", [shift, 0, -.1],
+ textColor,
+ parentObjectUniqueId=pole3,
+ parentLinkIndex=1)
+
+desiredPosPoleId = p.addUserDebugParameter("desiredPosPole", -10, 10, 0)
+desiredVelPoleId = p.addUserDebugParameter("desiredVelPole", -10, 10, 0)
+kpPoleId = p.addUserDebugParameter("kpPole", 0, 500, 1200)
+kdPoleId = p.addUserDebugParameter("kdPole", 0, 300, 100)
+maxForcePoleId = p.addUserDebugParameter("maxForcePole", 0, 5000, 1000)
+
+pd = p.loadPlugin("pdControlPlugin")
+
+p.setGravity(0, 0, -10)
+
+useRealTimeSim = False
+
+p.setRealTimeSimulation(useRealTimeSim)
+
+timeStep = 0.001
+
+while p.isConnected():
+ #p.getCameraImage(320,200)
+ timeStep = p.readUserDebugParameter(timeStepId)
+ p.setTimeStep(timeStep)
+
+ desiredPosCart = p.readUserDebugParameter(desiredPosCartId)
+ desiredVelCart = p.readUserDebugParameter(desiredVelCartId)
+ kpCart = p.readUserDebugParameter(kpCartId)
+ kdCart = p.readUserDebugParameter(kdCartId)
+ maxForceCart = p.readUserDebugParameter(maxForceCartId)
+
+ desiredPosPole = p.readUserDebugParameter(desiredPosPoleId)
+ desiredVelPole = p.readUserDebugParameter(desiredVelPoleId)
+ kpPole = p.readUserDebugParameter(kpPoleId)
+ kdPole = p.readUserDebugParameter(kdPoleId)
+ maxForcePole = p.readUserDebugParameter(maxForcePoleId)
+
+ basePos, baseOrn = p.getBasePositionAndOrientation(pole)
+
+ baseDof = 7
+ taus = exPD.computePD(pole, [0, 1], [
+ basePos[0], basePos[1], basePos[2], baseOrn[0], baseOrn[1], baseOrn[2], baseOrn[3],
+ desiredPosCart, desiredPosPole
+ ], [0, 0, 0, 0, 0, 0, 0, desiredVelCart, desiredVelPole], [0, 0, 0, 0, 0, 0, 0, kpCart, kpPole],
+ [0, 0, 0, 0, 0, 0, 0, kdCart, kdPole],
+ [0, 0, 0, 0, 0, 0, 0, maxForceCart, maxForcePole], timeStep)
+
+ for j in [0, 1]:
+ p.setJointMotorControlMultiDof(pole,
+ j,
+ controlMode=p.TORQUE_CONTROL,
+ force=[taus[j + baseDof]])
+ #p.setJointMotorControlArray(pole, [0,1], controlMode=p.TORQUE_CONTROL, forces=taus)
+
+ if (pd >= 0):
+ link = 0
+ p.setJointMotorControl2(bodyUniqueId=pole2,
+ jointIndex=link,
+ controlMode=p.PD_CONTROL,
+ targetPosition=desiredPosCart,
+ targetVelocity=desiredVelCart,
+ force=maxForceCart,
+ positionGain=kpCart,
+ velocityGain=kdCart)
+ link = 1
+ p.setJointMotorControl2(bodyUniqueId=pole2,
+ jointIndex=link,
+ controlMode=p.PD_CONTROL,
+ targetPosition=desiredPosPole,
+ targetVelocity=desiredVelPole,
+ force=maxForcePole,
+ positionGain=kpPole,
+ velocityGain=kdPole)
+
+ taus = sPD.computePD(pole4, [0, 1], [desiredPosCart, desiredPosPole],
+ [desiredVelCart, desiredVelPole], [kpCart, kpPole], [kdCart, kdPole],
+ [maxForceCart, maxForcePole], timeStep)
+ #p.setJointMotorControlArray(pole4, [0,1], controlMode=p.TORQUE_CONTROL, forces=taus)
+ for j in [0, 1]:
+ p.setJointMotorControlMultiDof(pole4, j, controlMode=p.TORQUE_CONTROL, force=[taus[j]])
+
+ p.setJointMotorControl2(pole3,
+ 0,
+ p.POSITION_CONTROL,
+ targetPosition=desiredPosCart,
+ targetVelocity=desiredVelCart,
+ positionGain=timeStep * (kpCart / 150.),
+ velocityGain=0.5,
+ force=maxForceCart)
+ p.setJointMotorControl2(pole3,
+ 1,
+ p.POSITION_CONTROL,
+ targetPosition=desiredPosPole,
+ targetVelocity=desiredVelPole,
+ positionGain=timeStep * (kpPole / 150.),
+ velocityGain=0.5,
+ force=maxForcePole)
+
+ if (not useRealTimeSim):
+ p.stepSimulation()
+ time.sleep(timeStep)
diff --git a/examples/pybullet/gym/pybullet_examples/pdControllerExplicit.py b/examples/pybullet/gym/pybullet_examples/pdControllerExplicit.py
new file mode 100644
index 000000000..20cd0a65e
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_examples/pdControllerExplicit.py
@@ -0,0 +1,98 @@
+import numpy as np
+
+
+class PDControllerExplicitMultiDof(object):
+
+ def __init__(self, pb):
+ self._pb = pb
+
+ def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds,
+ maxForces, timeStep):
+
+ numJoints = len(jointIndices) #self._pb.getNumJoints(bodyUniqueId)
+ curPos, curOrn = self._pb.getBasePositionAndOrientation(bodyUniqueId)
+ q1 = [curPos[0], curPos[1], curPos[2], curOrn[0], curOrn[1], curOrn[2], curOrn[3]]
+ baseLinVel, baseAngVel = self._pb.getBaseVelocity(bodyUniqueId)
+ qdot1 = [
+ baseLinVel[0], baseLinVel[1], baseLinVel[2], baseAngVel[0], baseAngVel[1], baseAngVel[2], 0
+ ]
+ qError = [0, 0, 0, 0, 0, 0, 0]
+ qIndex = 7
+ qdotIndex = 7
+ zeroAccelerations = [0, 0, 0, 0, 0, 0, 0]
+ for i in range(numJoints):
+ js = self._pb.getJointStateMultiDof(bodyUniqueId, jointIndices[i])
+
+ jointPos = js[0]
+ jointVel = js[1]
+ q1 += jointPos
+
+ if len(js[0]) == 1:
+ desiredPos = desiredPositions[qIndex]
+
+ qdiff = desiredPos - jointPos[0]
+ qError.append(qdiff)
+ zeroAccelerations.append(0.)
+ qdot1 += jointVel
+ qIndex += 1
+ qdotIndex += 1
+ if len(js[0]) == 4:
+ desiredPos = [
+ desiredPositions[qIndex], desiredPositions[qIndex + 1], desiredPositions[qIndex + 2],
+ desiredPositions[qIndex + 3]
+ ]
+ axis = self._pb.getAxisDifferenceQuaternion(desiredPos, jointPos)
+ jointVelNew = [jointVel[0], jointVel[1], jointVel[2], 0]
+ qdot1 += jointVelNew
+ qError.append(axis[0])
+ qError.append(axis[1])
+ qError.append(axis[2])
+ qError.append(0)
+ desiredVel = [
+ desiredVelocities[qdotIndex], desiredVelocities[qdotIndex + 1],
+ desiredVelocities[qdotIndex + 2]
+ ]
+ zeroAccelerations += [0., 0., 0., 0.]
+ qIndex += 4
+ qdotIndex += 4
+
+ q = np.array(q1)
+ qdot = np.array(qdot1)
+ qdotdesired = np.array(desiredVelocities)
+ qdoterr = qdotdesired - qdot
+ Kp = np.diagflat(kps)
+ Kd = np.diagflat(kds)
+ p = Kp.dot(qError)
+ d = Kd.dot(qdoterr)
+ forces = p + d
+ maxF = np.array(maxForces)
+ forces = np.clip(forces, -maxF, maxF)
+ return forces
+
+
+class PDControllerExplicit(object):
+
+ def __init__(self, pb):
+ self._pb = pb
+
+ def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds,
+ maxForces, timeStep):
+ numJoints = self._pb.getNumJoints(bodyUniqueId)
+ jointStates = self._pb.getJointStates(bodyUniqueId, jointIndices)
+ q1 = []
+ qdot1 = []
+ for i in range(numJoints):
+ q1.append(jointStates[i][0])
+ qdot1.append(jointStates[i][1])
+ q = np.array(q1)
+ qdot = np.array(qdot1)
+ qdes = np.array(desiredPositions)
+ qdotdes = np.array(desiredVelocities)
+ qError = qdes - q
+ qdotError = qdotdes - qdot
+ Kp = np.diagflat(kps)
+ Kd = np.diagflat(kds)
+ forces = Kp.dot(qError) + Kd.dot(qdotError)
+ maxF = np.array(maxForces)
+ forces = np.clip(forces, -maxF, maxF)
+ return forces
diff --git a/examples/pybullet/gym/pybullet_examples/pdControllerStable.py b/examples/pybullet/gym/pybullet_examples/pdControllerStable.py
new file mode 100644
index 000000000..bec3356c4
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_examples/pdControllerStable.py
@@ -0,0 +1,147 @@
+import numpy as np
+
+
+class PDControllerStableMultiDof(object):
+
+ def __init__(self, pb):
+ self._pb = pb
+
+ def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds,
+ maxForces, timeStep):
+
+ numJoints = len(jointIndices) #self._pb.getNumJoints(bodyUniqueId)
+ curPos, curOrn = self._pb.getBasePositionAndOrientation(bodyUniqueId)
+ #q1 = [desiredPositions[0],desiredPositions[1],desiredPositions[2],desiredPositions[3],desiredPositions[4],desiredPositions[5],desiredPositions[6]]
+ q1 = [curPos[0], curPos[1], curPos[2], curOrn[0], curOrn[1], curOrn[2], curOrn[3]]
+
+ #qdot1 = [0,0,0, 0,0,0,0]
+ baseLinVel, baseAngVel = self._pb.getBaseVelocity(bodyUniqueId)
+
+ qdot1 = [
+ baseLinVel[0], baseLinVel[1], baseLinVel[2], baseAngVel[0], baseAngVel[1], baseAngVel[2], 0
+ ]
+ qError = [0, 0, 0, 0, 0, 0, 0]
+
+ qIndex = 7
+ qdotIndex = 7
+ zeroAccelerations = [0, 0, 0, 0, 0, 0, 0]
+ for i in range(numJoints):
+ js = self._pb.getJointStateMultiDof(bodyUniqueId, jointIndices[i])
+
+ jointPos = js[0]
+ jointVel = js[1]
+ q1 += jointPos
+
+ if len(js[0]) == 1:
+ desiredPos = desiredPositions[qIndex]
+
+ qdiff = desiredPos - jointPos[0]
+ qError.append(qdiff)
+ zeroAccelerations.append(0.)
+ qdot1 += jointVel
+ qIndex += 1
+ qdotIndex += 1
+ if len(js[0]) == 4:
+ desiredPos = [
+ desiredPositions[qIndex], desiredPositions[qIndex + 1], desiredPositions[qIndex + 2],
+ desiredPositions[qIndex + 3]
+ ]
+ axis = self._pb.getAxisDifferenceQuaternion(desiredPos, jointPos)
+ jointVelNew = [jointVel[0], jointVel[1], jointVel[2], 0]
+ qdot1 += jointVelNew
+ qError.append(axis[0])
+ qError.append(axis[1])
+ qError.append(axis[2])
+ qError.append(0)
+ desiredVel = [
+ desiredVelocities[qdotIndex], desiredVelocities[qdotIndex + 1],
+ desiredVelocities[qdotIndex + 2]
+ ]
+ zeroAccelerations += [0., 0., 0., 0.]
+ qIndex += 4
+ qdotIndex += 4
+
+ q = np.array(q1)
+ qdot = np.array(qdot1)
+
+ qdotdesired = np.array(desiredVelocities)
+ qdoterr = qdotdesired - qdot
+
+ Kp = np.diagflat(kps)
+ Kd = np.diagflat(kds)
+
+ # Compute -Kp(q + qdot - qdes)
+ p_term = Kp.dot(qError - qdot*timeStep)
+ # Compute -Kd(qdot - qdotdes)
+ d_term = Kd.dot(qdoterr)
+
+ # Compute Inertia matrix M(q)
+ M = self._pb.calculateMassMatrix(bodyUniqueId, q1, flags=1)
+ M = np.array(M)
+ # Given: M(q) * qddot + C(q, qdot) = T_ext + T_int
+ # Compute Coriolis and External (Gravitational) terms G = C - T_ext
+ G = self._pb.calculateInverseDynamics(bodyUniqueId, q1, qdot1, zeroAccelerations, flags=1)
+ G = np.array(G)
+ # Obtain estimated generalized accelerations, considering Coriolis and Gravitational forces, and stable PD actions
+ qddot = np.linalg.solve(a=(M + Kd * timeStep),
+ b=p_term + d_term - G)
+ # Compute control generalized forces (T_int)
+ tau = p_term + d_term - Kd.dot(qddot) * timeStep
+ # Clip generalized forces to actuator limits
+ maxF = np.array(maxForces)
+ generalized_forces = np.clip(tau, -maxF, maxF)
+ return generalized_forces
+
+
+class PDControllerStable(object):
+ """
+ Implementation based on: Tan, J., Liu, K., & Turk, G. (2011). "Stable proportional-derivative controllers"
+ DOI: 10.1109/MCG.2011.30
+ """
+ def __init__(self, pb):
+ self._pb = pb
+
+ def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds,
+ maxForces, timeStep):
+ numJoints = self._pb.getNumJoints(bodyUniqueId)
+ jointStates = self._pb.getJointStates(bodyUniqueId, jointIndices)
+ q1 = []
+ qdot1 = []
+ zeroAccelerations = []
+ for i in range(numJoints):
+ q1.append(jointStates[i][0])
+ qdot1.append(jointStates[i][1])
+ zeroAccelerations.append(0)
+
+ q = np.array(q1)
+ qdot = np.array(qdot1)
+ qdes = np.array(desiredPositions)
+ qdotdes = np.array(desiredVelocities)
+
+ qError = qdes - q
+ qdotError = qdotdes - qdot
+
+ Kp = np.diagflat(kps)
+ Kd = np.diagflat(kds)
+
+ # Compute -Kp(q + qdot - qdes)
+ p_term = Kp.dot(qError - qdot*timeStep)
+ # Compute -Kd(qdot - qdotdes)
+ d_term = Kd.dot(qdotError)
+
+ # Compute Inertia matrix M(q)
+ M = self._pb.calculateMassMatrix(bodyUniqueId, q1)
+ M = np.array(M)
+ # Given: M(q) * qddot + C(q, qdot) = T_ext + T_int
+ # Compute Coriolis and External (Gravitational) terms G = C - T_ext
+ G = self._pb.calculateInverseDynamics(bodyUniqueId, q1, qdot1, zeroAccelerations)
+ G = np.array(G)
+ # Obtain estimated generalized accelerations, considering Coriolis and Gravitational forces, and stable PD actions
+ qddot = np.linalg.solve(a=(M + Kd * timeStep),
+ b=(-G + p_term + d_term))
+ # Compute control generalized forces (T_int)
+ tau = p_term + d_term - (Kd.dot(qddot) * timeStep)
+ # Clip generalized forces to actuator limits
+ maxF = np.array(maxForces)
+ generalized_forces = np.clip(tau, -maxF, maxF)
+ return generalized_forces
diff --git a/examples/pybullet/gym/pybullet_examples/pointCloudFromCameraImage.py b/examples/pybullet/gym/pybullet_examples/pointCloudFromCameraImage.py
new file mode 100644
index 000000000..65d88149c
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_examples/pointCloudFromCameraImage.py
@@ -0,0 +1,138 @@
+import pybullet as p
+import math
+import numpy as np
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+plane = p.loadURDF("plane100.urdf")
+cube = p.loadURDF("cube.urdf", [0, 0, 1])
+
+
+def getRayFromTo(mouseX, mouseY):
+ width, height, viewMat, projMat, cameraUp, camForward, horizon, vertical, _, _, dist, camTarget = p.getDebugVisualizerCamera(
+ )
+ camPos = [
+ camTarget[0] - dist * camForward[0], camTarget[1] - dist * camForward[1],
+ camTarget[2] - dist * camForward[2]
+ ]
+ farPlane = 10000
+ rayForward = [(camTarget[0] - camPos[0]), (camTarget[1] - camPos[1]), (camTarget[2] - camPos[2])]
+ lenFwd = math.sqrt(rayForward[0] * rayForward[0] + rayForward[1] * rayForward[1] +
+ rayForward[2] * rayForward[2])
+ invLen = farPlane * 1. / lenFwd
+ rayForward = [invLen * rayForward[0], invLen * rayForward[1], invLen * rayForward[2]]
+ rayFrom = camPos
+ oneOverWidth = float(1) / float(width)
+ oneOverHeight = float(1) / float(height)
+
+ dHor = [horizon[0] * oneOverWidth, horizon[1] * oneOverWidth, horizon[2] * oneOverWidth]
+ dVer = [vertical[0] * oneOverHeight, vertical[1] * oneOverHeight, vertical[2] * oneOverHeight]
+ rayToCenter = [
+ rayFrom[0] + rayForward[0], rayFrom[1] + rayForward[1], rayFrom[2] + rayForward[2]
+ ]
+ ortho = [
+ -0.5 * horizon[0] + 0.5 * vertical[0] + float(mouseX) * dHor[0] - float(mouseY) * dVer[0],
+ -0.5 * horizon[1] + 0.5 * vertical[1] + float(mouseX) * dHor[1] - float(mouseY) * dVer[1],
+ -0.5 * horizon[2] + 0.5 * vertical[2] + float(mouseX) * dHor[2] - float(mouseY) * dVer[2]
+ ]
+
+ rayTo = [
+ rayFrom[0] + rayForward[0] + ortho[0], rayFrom[1] + rayForward[1] + ortho[1],
+ rayFrom[2] + rayForward[2] + ortho[2]
+ ]
+ lenOrtho = math.sqrt(ortho[0] * ortho[0] + ortho[1] * ortho[1] + ortho[2] * ortho[2])
+ alpha = math.atan(lenOrtho / farPlane)
+ return rayFrom, rayTo, alpha
+
+
+width, height, viewMat, projMat, cameraUp, camForward, horizon, vertical, _, _, dist, camTarget = p.getDebugVisualizerCamera(
+)
+camPos = [
+ camTarget[0] - dist * camForward[0], camTarget[1] - dist * camForward[1],
+ camTarget[2] - dist * camForward[2]
+]
+farPlane = 10000
+rayForward = [(camTarget[0] - camPos[0]), (camTarget[1] - camPos[1]), (camTarget[2] - camPos[2])]
+lenFwd = math.sqrt(rayForward[0] * rayForward[0] + rayForward[1] * rayForward[1] +
+ rayForward[2] * rayForward[2])
+oneOverWidth = float(1) / float(width)
+oneOverHeight = float(1) / float(height)
+dHor = [horizon[0] * oneOverWidth, horizon[1] * oneOverWidth, horizon[2] * oneOverWidth]
+dVer = [vertical[0] * oneOverHeight, vertical[1] * oneOverHeight, vertical[2] * oneOverHeight]
+
+lendHor = math.sqrt(dHor[0] * dHor[0] + dHor[1] * dHor[1] + dHor[2] * dHor[2])
+lendVer = math.sqrt(dVer[0] * dVer[0] + dVer[1] * dVer[1] + dVer[2] * dVer[2])
+
+cornersX = [0, width, width, 0]
+cornersY = [0, 0, height, height]
+corners3D = []
+
+imgW = int(width / 4)
+imgH = int(height / 4)
+
+img = p.getCameraImage(imgW, imgH, renderer=p.ER_BULLET_HARDWARE_OPENGL)
+rgbBuffer = img[2]
+depthBuffer = img[3]
+print("rgbBuffer.shape=", rgbBuffer.shape)
+print("depthBuffer.shape=", depthBuffer.shape)
+
+#disable rendering temporary makes adding objects faster
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
+p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
+p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)
+visualShapeId = p.createVisualShape(shapeType=p.GEOM_SPHERE, rgbaColor=[1, 1, 1, 1], radius=0.03)
+collisionShapeId = -1 #p.createCollisionShape(shapeType=p.GEOM_MESH, fileName="duck_vhacd.obj", collisionFramePosition=shift,meshScale=meshScale)
+
+for i in range(4):
+ w = cornersX[i]
+ h = cornersY[i]
+ rayFrom, rayTo, _ = getRayFromTo(w, h)
+ rf = np.array(rayFrom)
+ rt = np.array(rayTo)
+ vec = rt - rf
+ l = np.sqrt(np.dot(vec, vec))
+ newTo = (0.01 / l) * vec + rf
+ #print("len vec=",np.sqrt(np.dot(vec,vec)))
+
+ p.addUserDebugLine(rayFrom, newTo, [1, 0, 0])
+ corners3D.append(newTo)
+count = 0
+
+stepX = 5
+stepY = 5
+for w in range(0, imgW, stepX):
+ for h in range(0, imgH, stepY):
+ count += 1
+ if ((count % 100) == 0):
+ print(count, "out of ", imgW * imgH / (stepX * stepY))
+ rayFrom, rayTo, alpha = getRayFromTo(w * (width / imgW), h * (height / imgH))
+ rf = np.array(rayFrom)
+ rt = np.array(rayTo)
+ vec = rt - rf
+ l = np.sqrt(np.dot(vec, vec))
+ depthImg = float(depthBuffer[h, w])
+ far = 1000.
+ near = 0.01
+ depth = far * near / (far - (far - near) * depthImg)
+ depth /= math.cos(alpha)
+ newTo = (depth / l) * vec + rf
+ #p.addUserDebugLine(rayFrom, newTo, [1, 0, 0])
+ mb = p.createMultiBody(baseMass=0,
+ baseCollisionShapeIndex=collisionShapeId,
+ baseVisualShapeIndex=visualShapeId,
+ basePosition=newTo,
+ useMaximalCoordinates=True)
+ color = rgbBuffer[h, w]
+ color = [color[0] / 255., color[1] / 255., color[2] / 255., 1]
+ p.changeVisualShape(mb, -1, rgbaColor=color)
+p.addUserDebugLine(corners3D[0], corners3D[1], [1, 0, 0])
+p.addUserDebugLine(corners3D[1], corners3D[2], [1, 0, 0])
+p.addUserDebugLine(corners3D[2], corners3D[3], [1, 0, 0])
+p.addUserDebugLine(corners3D[3], corners3D[0], [1, 0, 0])
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
+print("ready\n")
+#p.removeBody(plane)
+#p.removeBody(cube)
+while (1):
+ p.setGravity(0, 0, -10)
diff --git a/examples/pybullet/gym/pybullet_examples/profileTiming.py b/examples/pybullet/gym/pybullet_examples/profileTiming.py
new file mode 100644
index 000000000..96c25b300
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_examples/profileTiming.py
@@ -0,0 +1,25 @@
+import pybullet as p
+import time
+#you can visualize the timings using Google Chrome, visit about://tracing
+#and load the json file
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+
+t = time.time() + 3.1
+
+logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "chrome_about_tracing.json")
+while (time.time() < t):
+ p.stepSimulation()
+ p.submitProfileTiming("pythontest")
+ time.sleep(1./240.)
+ p.submitProfileTiming("nested")
+ for i in range (100):
+ p.submitProfileTiming("deep_nested")
+ p.submitProfileTiming()
+ time.sleep(1./1000.)
+ p.submitProfileTiming()
+ p.submitProfileTiming()
+
+p.stopStateLogging(logId)
diff --git a/examples/pybullet/gym/pybullet_examples/projective_texture.py b/examples/pybullet/gym/pybullet_examples/projective_texture.py
new file mode 100644
index 000000000..6116c59b0
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_examples/projective_texture.py
@@ -0,0 +1,41 @@
+import pybullet as p
+from time import sleep
+import matplotlib.pyplot as plt
+import numpy as np
+import pybullet_data
+
+physicsClient = p.connect(p.GUI)
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+p.setGravity(0, 0, 0)
+bearStartPos1 = [-3.3, 0, 0]
+bearStartOrientation1 = p.getQuaternionFromEuler([0, 0, 0])
+bearId1 = p.loadURDF("plane.urdf", bearStartPos1, bearStartOrientation1)
+bearStartPos2 = [0, 0, 0]
+bearStartOrientation2 = p.getQuaternionFromEuler([0, 0, 0])
+bearId2 = p.loadURDF("teddy_large.urdf", bearStartPos2, bearStartOrientation2)
+textureId = p.loadTexture("checker_grid.jpg")
+#p.changeVisualShape(objectUniqueId=0, linkIndex=-1, textureUniqueId=textureId)
+#p.changeVisualShape(objectUniqueId=1, linkIndex=-1, textureUniqueId=textureId)
+
+useRealTimeSimulation = 1
+
+if (useRealTimeSimulation):
+ p.setRealTimeSimulation(1)
+
+while 1:
+ if (useRealTimeSimulation):
+ camera = p.getDebugVisualizerCamera()
+ viewMat = camera[2]
+ projMat = camera[3]
+ #An example of setting the view matrix for the projective texture.
+ #viewMat = p.computeViewMatrix(cameraEyePosition=[7,0,0], cameraTargetPosition=[0,0,0], cameraUpVector=[0,0,1])
+ p.getCameraImage(300,
+ 300,
+ renderer=p.ER_BULLET_HARDWARE_OPENGL,
+ flags=p.ER_USE_PROJECTIVE_TEXTURE,
+ projectiveTextureView=viewMat,
+ projectiveTextureProj=projMat)
+ p.setGravity(0, 0, 0)
+ else:
+ p.stepSimulation()
diff --git a/examples/pybullet/gym/pybullet_examples/rollPitchYaw.py b/examples/pybullet/gym/pybullet_examples/rollPitchYaw.py
new file mode 100644
index 000000000..e5dac10ba
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_examples/rollPitchYaw.py
@@ -0,0 +1,28 @@
+import pybullet as p
+import time
+import pybullet_data
+
+cid = p.connect(p.SHARED_MEMORY)
+if (cid < 0):
+ p.connect(p.GUI)
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+q = p.loadURDF("quadruped/quadruped.urdf", useFixedBase=True)
+rollId = p.addUserDebugParameter("roll", -1.5, 1.5, 0)
+pitchId = p.addUserDebugParameter("pitch", -1.5, 1.5, 0)
+yawId = p.addUserDebugParameter("yaw", -1.5, 1.5, 0)
+fwdxId = p.addUserDebugParameter("fwd_x", -1, 1, 0)
+fwdyId = p.addUserDebugParameter("fwd_y", -1, 1, 0)
+fwdzId = p.addUserDebugParameter("fwd_z", -1, 1, 0)
+
+while True:
+ roll = p.readUserDebugParameter(rollId)
+ pitch = p.readUserDebugParameter(pitchId)
+ yaw = p.readUserDebugParameter(yawId)
+ x = p.readUserDebugParameter(fwdxId)
+ y = p.readUserDebugParameter(fwdyId)
+ z = p.readUserDebugParameter(fwdzId)
+
+ orn = p.getQuaternionFromEuler([roll, pitch, yaw])
+ p.resetBasePositionAndOrientation(q, [x, y, z], orn)
+ #p.stepSimulation()#not really necessary for this demo, no physics used
diff --git a/examples/pybullet/gym/pybullet_examples/satCollision.py b/examples/pybullet/gym/pybullet_examples/satCollision.py
new file mode 100644
index 000000000..af68e143f
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_examples/satCollision.py
@@ -0,0 +1,21 @@
+import pybullet as p
+import time
+
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+p.setGravity(0, 0, -10)
+p.setPhysicsEngineParameter(enableSAT=1)
+p.loadURDF("cube_concave.urdf", [0, 0, -25],
+ globalScaling=50,
+ useFixedBase=True,
+ flags=p.URDF_INITIALIZE_SAT_FEATURES)
+p.loadURDF("cube.urdf", [0, 0, 1], globalScaling=1, flags=p.URDF_INITIALIZE_SAT_FEATURES)
+p.loadURDF("duck_vhacd.urdf", [1, 0, 1], globalScaling=1, flags=p.URDF_INITIALIZE_SAT_FEATURES)
+
+while (p.isConnected()):
+ p.stepSimulation()
+ pts = p.getContactPoints()
+ #print("num contacts = ", len(pts))
+ time.sleep(1. / 240.)
diff --git a/examples/pybullet/gym/pybullet_examples/sceneAabb.py b/examples/pybullet/gym/pybullet_examples/sceneAabb.py
new file mode 100644
index 000000000..260fa4f64
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_examples/sceneAabb.py
@@ -0,0 +1,42 @@
+import pybullet as p
+import time
+import numpy as np
+
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+p.loadURDF("plane.urdf")
+p.loadURDF("sphere2.urdf",[0,0,2])
+
+dt = 1./240.
+p.setTimeStep(dt)
+
+def getSceneAABB():
+ aabbMins=[]
+ aabbMaxs=[]
+
+ for i in range(p.getNumBodies()):
+ uid = p.getBodyUniqueId(i)
+ aabb = p.getAABB(uid)
+ aabbMins.append(np.array(aabb[0]))
+ aabbMaxs.append(np.array(aabb[1]))
+
+ if len(aabbMins):
+ sceneAABBMin = aabbMins[0]
+ sceneAABBMax = aabbMaxs[0]
+
+ for aabb in aabbMins:
+ sceneAABBMin = np.minimum(sceneAABBMin,aabb)
+ for aabb in aabbMaxs:
+ sceneAABBMax = np.maximum(sceneAABBMax,aabb)
+
+ print("sceneAABBMin=",sceneAABBMin)
+ print("sceneAABBMax=",sceneAABBMax)
+
+getSceneAABB()
+
+while (1):
+ p.stepSimulation()
+ time.sleep(dt)
+
diff --git a/examples/pybullet/gym/pybullet_examples/signedDistanceField.py b/examples/pybullet/gym/pybullet_examples/signedDistanceField.py
new file mode 100644
index 000000000..e1f94fc00
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_examples/signedDistanceField.py
@@ -0,0 +1,17 @@
+import pybullet as p
+import pybullet
+import time
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+p.loadURDF("toys/concave_box.urdf")
+p.setGravity(0, 0, -10)
+for i in range(10):
+ p.loadURDF("sphere_1cm.urdf", [i * 0.02, 0, 0.5])
+p.loadURDF("duck_vhacd.urdf")
+timeStep = 1. / 240.
+p.setTimeStep(timeStep)
+while (1):
+ p.stepSimulation()
+ time.sleep(timeStep)
diff --git a/examples/pybullet/gym/pybullet_examples/snake.py b/examples/pybullet/gym/pybullet_examples/snake.py
new file mode 100644
index 000000000..d70a50e0a
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_examples/snake.py
@@ -0,0 +1,144 @@
+import pybullet as p
+import time
+import math
+
+# This simple snake logic is from some 15 year old Havok C++ demo
+# Thanks to Michael Ewert!
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+plane = p.createCollisionShape(p.GEOM_PLANE)
+
+p.createMultiBody(0, plane)
+
+useMaximalCoordinates = True
+sphereRadius = 0.25
+#colBoxId = p.createCollisionShapeArray([p.GEOM_BOX, p.GEOM_SPHERE],radii=[sphereRadius+0.03,sphereRadius+0.03], halfExtents=[[sphereRadius,sphereRadius,sphereRadius],[sphereRadius,sphereRadius,sphereRadius]])
+colBoxId = p.createCollisionShape(p.GEOM_BOX,
+ halfExtents=[sphereRadius, sphereRadius, sphereRadius])
+
+mass = 1
+visualShapeId = -1
+
+link_Masses = []
+linkCollisionShapeIndices = []
+linkVisualShapeIndices = []
+linkPositions = []
+linkOrientations = []
+linkInertialFramePositions = []
+linkInertialFrameOrientations = []
+indices = []
+jointTypes = []
+axis = []
+
+for i in range(36):
+ link_Masses.append(1)
+ linkCollisionShapeIndices.append(colBoxId)
+ linkVisualShapeIndices.append(-1)
+ linkPositions.append([0, sphereRadius * 2.0 + 0.01, 0])
+ linkOrientations.append([0, 0, 0, 1])
+ linkInertialFramePositions.append([0, 0, 0])
+ linkInertialFrameOrientations.append([0, 0, 0, 1])
+ indices.append(i)
+ jointTypes.append(p.JOINT_REVOLUTE)
+ axis.append([0, 0, 1])
+
+basePosition = [0, 0, 1]
+baseOrientation = [0, 0, 0, 1]
+sphereUid = p.createMultiBody(mass,
+ colBoxId,
+ visualShapeId,
+ basePosition,
+ baseOrientation,
+ linkMasses=link_Masses,
+ linkCollisionShapeIndices=linkCollisionShapeIndices,
+ linkVisualShapeIndices=linkVisualShapeIndices,
+ linkPositions=linkPositions,
+ linkOrientations=linkOrientations,
+ linkInertialFramePositions=linkInertialFramePositions,
+ linkInertialFrameOrientations=linkInertialFrameOrientations,
+ linkParentIndices=indices,
+ linkJointTypes=jointTypes,
+ linkJointAxis=axis,
+ useMaximalCoordinates=useMaximalCoordinates)
+
+p.setGravity(0, 0, -10)
+p.setRealTimeSimulation(0)
+
+anistropicFriction = [1, 0.01, 0.01]
+p.changeDynamics(sphereUid, -1, lateralFriction=2, anisotropicFriction=anistropicFriction)
+p.getNumJoints(sphereUid)
+for i in range(p.getNumJoints(sphereUid)):
+ p.getJointInfo(sphereUid, i)
+ p.changeDynamics(sphereUid, i, lateralFriction=2, anisotropicFriction=anistropicFriction)
+
+dt = 1. / 240.
+SNAKE_NORMAL_PERIOD = 0.1 #1.5
+m_wavePeriod = SNAKE_NORMAL_PERIOD
+
+m_waveLength = 4
+m_wavePeriod = 1.5
+m_waveAmplitude = 0.4
+m_waveFront = 0.0
+#our steering value
+m_steering = 0.0
+m_segmentLength = sphereRadius * 2.0
+forward = 0
+
+while (1):
+ keys = p.getKeyboardEvents()
+ for k, v in keys.items():
+
+ if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_TRIGGERED)):
+ m_steering = -.2
+ if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_RELEASED)):
+ m_steering = 0
+ if (k == p.B3G_LEFT_ARROW and (v & p.KEY_WAS_TRIGGERED)):
+ m_steering = .2
+ if (k == p.B3G_LEFT_ARROW and (v & p.KEY_WAS_RELEASED)):
+ m_steering = 0
+
+ amp = 0.2
+ offset = 0.6
+ numMuscles = p.getNumJoints(sphereUid)
+ scaleStart = 1.0
+
+ #start of the snake with smaller waves.
+ #I think starting the wave at the tail would work better ( while it still goes from head to tail )
+ if (m_waveFront < m_segmentLength * 4.0):
+ scaleStart = m_waveFront / (m_segmentLength * 4.0)
+
+ segment = numMuscles - 1
+
+ #we simply move a sin wave down the body of the snake.
+ #this snake may be going backwards, but who can tell ;)
+ for joint in range(p.getNumJoints(sphereUid)):
+ segment = joint #numMuscles-1-joint
+ #map segment to phase
+ phase = (m_waveFront - (segment + 1) * m_segmentLength) / m_waveLength
+ phase -= math.floor(phase)
+ phase *= math.pi * 2.0
+
+ #map phase to curvature
+ targetPos = math.sin(phase) * scaleStart * m_waveAmplitude
+
+ #// steer snake by squashing +ve or -ve side of sin curve
+ if (m_steering > 0 and targetPos < 0):
+ targetPos *= 1.0 / (1.0 + m_steering)
+
+ if (m_steering < 0 and targetPos > 0):
+ targetPos *= 1.0 / (1.0 - m_steering)
+
+ #set our motor
+ p.setJointMotorControl2(sphereUid,
+ joint,
+ p.POSITION_CONTROL,
+ targetPosition=targetPos + m_steering,
+ force=30)
+
+ #wave keeps track of where the wave is in time
+ m_waveFront += dt / m_wavePeriod * m_waveLength
+ p.stepSimulation()
+
+ time.sleep(dt)
diff --git a/examples/pybullet/gym/pybullet_examples/soccerball.py b/examples/pybullet/gym/pybullet_examples/soccerball.py
new file mode 100644
index 000000000..dae6acd8b
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_examples/soccerball.py
@@ -0,0 +1,17 @@
+import pybullet as p
+import pybullet_data as pd
+import time
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pd.getDataPath())
+offset = 0
+for scale in range (1,10,1):
+ ball = p.loadURDF("soccerball.urdf",[offset,0,1], globalScaling=scale*0.1)
+ p.changeDynamics(ball,-1,linearDamping=0, angularDamping=0, rollingFriction=0.001, spinningFriction=0.001)
+ p.changeVisualShape(ball,-1,rgbaColor=[0.8,0.8,0.8,1])
+ offset += 2*scale*0.1
+p.loadURDF("plane.urdf")
+p.setGravity(0,0,-10)
+p.setRealTimeSimulation(1)
+while p.isConnected():
+ #p.getCameraImage(320,200, renderer=p.ER_BULLET_HARDWARE_OPENGL)
+ time.sleep(0.5)
diff --git a/examples/pybullet/gym/pybullet_examples/switchConstraintSolver.py b/examples/pybullet/gym/pybullet_examples/switchConstraintSolver.py
new file mode 100644
index 000000000..b526789cc
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_examples/switchConstraintSolver.py
@@ -0,0 +1,32 @@
+import pybullet as p
+import time
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+#p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_PGS, globalCFM = 0.0001)
+p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_DANTZIG,
+ globalCFM=0.000001)
+#p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_PGS, globalCFM = 0.0001)
+
+p.loadURDF("plane.urdf")
+radius = 0.025
+distance = 1.86
+yaw = 135
+pitch = -11
+targetPos = [0, 0, 0]
+
+p.setPhysicsEngineParameter(solverResidualThreshold=0.001, numSolverIterations=200)
+p.resetDebugVisualizerCamera(distance, yaw, pitch, targetPos)
+objectId = -1
+
+for i in range(10):
+ objectId = p.loadURDF("cube_small.urdf", [1, 1, radius + i * 2 * radius])
+
+p.changeDynamics(objectId, -1, 100)
+
+timeStep = 1. / 240.
+p.setGravity(0, 0, -10)
+while (p.isConnected()):
+ p.stepSimulation()
+ time.sleep(timeStep)
diff --git a/examples/pybullet/gym/pybullet_examples/testrender_egl.py b/examples/pybullet/gym/pybullet_examples/testrender_egl.py
new file mode 100644
index 000000000..c511e3cdc
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_examples/testrender_egl.py
@@ -0,0 +1,102 @@
+
+#using the eglRendererPlugin (hardware OpenGL acceleration)
+#using EGL on Linux and default OpenGL window on Win32.
+
+#make sure to compile pybullet with PYBULLET_USE_NUMPY enabled
+#otherwise use testrender.py (slower but compatible without numpy)
+#you can also use GUI mode, for faster OpenGL rendering (instead of TinyRender CPU)
+
+import numpy as np
+import matplotlib.pyplot as plt
+import pybullet
+import time
+import pkgutil
+
+plt.ion()
+
+img = np.random.rand(200, 320)
+#img = [tandard_normal((50,100))
+image = plt.imshow(img, interpolation='none', animated=True, label="blah")
+ax = plt.gca()
+import pybullet_data
+
+
+pybullet.connect(pybullet.DIRECT)
+
+pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
+
+egl = pkgutil.get_loader('eglRenderer')
+if (egl):
+ pluginId = pybullet.loadPlugin(egl.get_filename(), "_eglRendererPlugin")
+else:
+ pluginId = pybullet.loadPlugin("eglRendererPlugin")
+print("pluginId=",pluginId)
+pybullet.loadURDF("plane.urdf", [0, 0, -1])
+pybullet.loadURDF("r2d2.urdf")
+
+camTargetPos = [0, 0, 0]
+cameraUp = [0, 0, 1]
+cameraPos = [1, 1, 1]
+pybullet.setGravity(0, 0, -10)
+
+pitch = -10.0
+
+roll = 0
+upAxisIndex = 2
+camDistance = 4
+pixelWidth = 320
+pixelHeight = 200
+nearPlane = 0.01
+farPlane = 100
+
+fov = 60
+
+main_start = time.time()
+while (1):
+ for yaw in range(0, 360, 10):
+ pybullet.stepSimulation()
+ start = time.time()
+
+ viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch,
+ roll, upAxisIndex)
+ aspect = pixelWidth / pixelHeight
+ projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane)
+ img_arr = pybullet.getCameraImage(pixelWidth,
+ pixelHeight,
+ viewMatrix,
+ projectionMatrix,
+ shadow=1,
+ lightDirection=[1, 1, 1],
+ renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
+ stop = time.time()
+ #print("renderImage %f" % (stop - start))
+
+ w = img_arr[0] #width of the image, in pixels
+ h = img_arr[1] #height of the image, in pixels
+ rgb = img_arr[2] #color data RGB
+ dep = img_arr[3] #depth data
+
+ #print('width = %d height = %d' % (w, h))
+
+ #note that sending the data to matplotlib is really slow
+
+ #reshape is not needed
+ np_img_arr = np.reshape(rgb, (h, w, 4))
+ np_img_arr = np_img_arr * (1. / 255.)
+
+ #show
+ #plt.imshow(np_img_arr,interpolation='none',extent=(0,1600,0,1200))
+ #image = plt.imshow(np_img_arr,interpolation='none',animated=True,label="blah")
+
+ image.set_data(np_img_arr)
+ ax.plot([0])
+ #plt.draw()
+ #plt.show()
+ plt.pause(0.01)
+ #image.draw()
+
+main_stop = time.time()
+
+print("Total time %f" % (main_stop - main_start))
+
+pybullet.resetSimulation()
diff --git a/examples/pybullet/gym/pybullet_examples/testrender_np.py b/examples/pybullet/gym/pybullet_examples/testrender_np.py
new file mode 100644
index 000000000..2c313a457
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_examples/testrender_np.py
@@ -0,0 +1,90 @@
+#make sure to compile pybullet with PYBULLET_USE_NUMPY enabled
+#otherwise use testrender.py (slower but compatible without numpy)
+#you can also use GUI mode, for faster OpenGL rendering (instead of TinyRender CPU)
+
+import numpy as np
+import matplotlib.pyplot as plt
+import pybullet
+import time
+import pybullet_data
+
+plt.ion()
+
+img = np.random.rand(200, 320)
+#img = [tandard_normal((50,100))
+image = plt.imshow(img, interpolation='none', animated=True, label="blah")
+ax = plt.gca()
+
+#pybullet.connect(pybullet.GUI)
+pybullet.connect(pybullet.DIRECT)
+
+pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
+pybullet.loadURDF("plane.urdf", [0, 0, -1])
+pybullet.loadURDF("r2d2.urdf")
+
+camTargetPos = [0, 0, 0]
+cameraUp = [0, 0, 1]
+cameraPos = [1, 1, 1]
+pybullet.setGravity(0, 0, -10)
+
+pitch = -10.0
+
+roll = 0
+upAxisIndex = 2
+camDistance = 4
+pixelWidth = 320
+pixelHeight = 200
+nearPlane = 0.01
+farPlane = 100
+
+fov = 60
+
+main_start = time.time()
+while (1):
+ for yaw in range(0, 360, 10):
+ pybullet.stepSimulation()
+ start = time.time()
+
+ viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch,
+ roll, upAxisIndex)
+ aspect = pixelWidth / pixelHeight
+ projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane)
+ img_arr = pybullet.getCameraImage(pixelWidth,
+ pixelHeight,
+ viewMatrix,
+ projectionMatrix,
+ shadow=1,
+ lightDirection=[1, 1, 1],
+ renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
+ stop = time.time()
+ print("renderImage %f" % (stop - start))
+
+ w = img_arr[0] #width of the image, in pixels
+ h = img_arr[1] #height of the image, in pixels
+ rgb = img_arr[2] #color data RGB
+ dep = img_arr[3] #depth data
+
+ print('width = %d height = %d' % (w, h))
+
+ #note that sending the data to matplotlib is really slow
+
+ #reshape is needed
+ np_img_arr = np.reshape(rgb, (h, w, 4))
+ np_img_arr = np_img_arr * (1. / 255.)
+
+ #show
+ #plt.imshow(np_img_arr,interpolation='none',extent=(0,1600,0,1200))
+ #image = plt.imshow(np_img_arr,interpolation='none',animated=True,label="blah")
+
+ image.set_data(np_img_arr)
+ ax.plot([0])
+ #plt.draw()
+ #plt.show()
+ plt.pause(0.01)
+ #image.draw()
+
+main_stop = time.time()
+
+print("Total time %f" % (main_stop - main_start))
+
+pybullet.resetSimulation()
diff --git a/examples/pybullet/gym/pybullet_examples/transparent.py b/examples/pybullet/gym/pybullet_examples/transparent.py
new file mode 100644
index 000000000..8a4831002
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_examples/transparent.py
@@ -0,0 +1,25 @@
+import pybullet as p
+import time
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+p.loadURDF("plane.urdf")
+sphereUid = p.loadURDF("sphere_transparent.urdf", [0, 0, 2])
+
+redSlider = p.addUserDebugParameter("red", 0, 1, 1)
+greenSlider = p.addUserDebugParameter("green", 0, 1, 0)
+blueSlider = p.addUserDebugParameter("blue", 0, 1, 0)
+alphaSlider = p.addUserDebugParameter("alpha", 0, 1, 0.5)
+
+while (1):
+ red = p.readUserDebugParameter(redSlider)
+ green = p.readUserDebugParameter(greenSlider)
+ blue = p.readUserDebugParameter(blueSlider)
+ alpha = p.readUserDebugParameter(alphaSlider)
+ p.changeVisualShape(sphereUid, -1, rgbaColor=[red, green, blue, alpha])
+ p.getCameraImage(320,
+ 200,
+ flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX,
+ renderer=p.ER_BULLET_HARDWARE_OPENGL)
+ time.sleep(0.01)
diff --git a/examples/pybullet/gym/pybullet_examples/vhacd.py b/examples/pybullet/gym/pybullet_examples/vhacd.py
new file mode 100644
index 000000000..8b4bcf77d
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_examples/vhacd.py
@@ -0,0 +1,13 @@
+import pybullet as p
+import pybullet_data as pd
+import os
+
+import pybullet_data
+
+p.connect(p.DIRECT)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+name_in = os.path.join(pd.getDataPath(), "duck.obj")
+name_out = "duck_vhacd.obj"
+name_log = "log.txt"
+p.vhacd(name_in, name_out, name_log)
+
diff --git a/examples/pybullet/gym/pybullet_examples/video_sync_mp4.py b/examples/pybullet/gym/pybullet_examples/video_sync_mp4.py
new file mode 100644
index 000000000..7a517b10e
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_examples/video_sync_mp4.py
@@ -0,0 +1,32 @@
+import pybullet as p
+import time
+import pybullet_data
+
+#Once the video is recorded, you can extract all individual frames using ffmpeg
+#mkdir frames
+#ffmpeg -i test.mp4 "frames/out-%03d.png"
+
+#by default, PyBullet runs at 240Hz
+p.connect(p.GUI, options="--width=320 --height=200 --mp4=\"test.mp4\" --mp4fps=240")
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
+p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING,1)
+p.loadURDF("plane.urdf")
+
+#in 3 seconds, the object travels about 0.5*g*t^2 meter ~ 45 meter.
+r2d2 = p.loadURDF("r2d2.urdf",[0,0,45])
+#disable linear damping
+p.changeDynamics(r2d2,-1, linearDamping=0)
+p.setGravity(0,0,-10)
+
+for i in range (3*240):
+ txt = "frame "+str(i)
+ item = p.addUserDebugText(txt, [0,1,0])
+ p.stepSimulation()
+ #synchronize the visualizer (rendering frames for the video mp4) with stepSimulation
+ p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING,1)
+ #print("r2d2 vel=", p.getBaseVelocity(r2d2)[0][2])
+ p.removeUserDebugItem(item)
+
+p.disconnect()
diff --git a/examples/pybullet/gym/pybullet_examples/vr_kuka_setup.py b/examples/pybullet/gym/pybullet_examples/vr_kuka_setup.py
new file mode 100644
index 000000000..ee94d5f30
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_examples/vr_kuka_setup.py
@@ -0,0 +1,184 @@
+import pybullet as p
+import time
+#p.connect(p.UDP,"192.168.86.100")
+import pybullet_data
+
+cid = p.connect(p.SHARED_MEMORY)
+
+if (cid < 0):
+ p.connect(p.GUI)
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+
+p.resetSimulation()
+#disable rendering during loading makes it much faster
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
+objects = [
+ p.loadURDF("plane.urdf", 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 1.000000)
+]
+objects = [
+ p.loadURDF("samurai.urdf", 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000,
+ 1.000000)
+]
+objects = [
+ p.loadURDF("pr2_gripper.urdf", 0.500000, 0.300006, 0.700000, -0.000000, -0.000000, -0.000031,
+ 1.000000)
+]
+pr2_gripper = objects[0]
+print("pr2_gripper=")
+print(pr2_gripper)
+
+jointPositions = [0.550569, 0.000000, 0.549657, 0.000000]
+for jointIndex in range(p.getNumJoints(pr2_gripper)):
+ p.resetJointState(pr2_gripper, jointIndex, jointPositions[jointIndex])
+
+pr2_cid = p.createConstraint(pr2_gripper, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0.2, 0, 0],
+ [0.500000, 0.300006, 0.700000])
+print("pr2_cid")
+print(pr2_cid)
+
+objects = [
+ p.loadURDF("kuka_iiwa/model_vr_limits.urdf", 1.400000, -0.200000, 0.600000, 0.000000, 0.000000,
+ 0.000000, 1.000000)
+]
+kuka = objects[0]
+jointPositions = [-0.000000, -0.000000, 0.000000, 1.570793, 0.000000, -1.036725, 0.000001]
+for jointIndex in range(p.getNumJoints(kuka)):
+ p.resetJointState(kuka, jointIndex, jointPositions[jointIndex])
+ p.setJointMotorControl2(kuka, jointIndex, p.POSITION_CONTROL, jointPositions[jointIndex], 0)
+
+objects = [
+ p.loadURDF("lego/lego.urdf", 1.000000, -0.200000, 0.700000, 0.000000, 0.000000, 0.000000,
+ 1.000000)
+]
+objects = [
+ p.loadURDF("lego/lego.urdf", 1.000000, -0.200000, 0.800000, 0.000000, 0.000000, 0.000000,
+ 1.000000)
+]
+objects = [
+ p.loadURDF("lego/lego.urdf", 1.000000, -0.200000, 0.900000, 0.000000, 0.000000, 0.000000,
+ 1.000000)
+]
+objects = p.loadSDF("gripper/wsg50_one_motor_gripper_new_free_base.sdf")
+kuka_gripper = objects[0]
+print("kuka gripper=")
+print(kuka_gripper)
+
+p.resetBasePositionAndOrientation(kuka_gripper, [0.923103, -0.200000, 1.250036],
+ [-0.000000, 0.964531, -0.000002, -0.263970])
+jointPositions = [
+ 0.000000, -0.011130, -0.206421, 0.205143, -0.009999, 0.000000, -0.010055, 0.000000
+]
+for jointIndex in range(p.getNumJoints(kuka_gripper)):
+ p.resetJointState(kuka_gripper, jointIndex, jointPositions[jointIndex])
+ p.setJointMotorControl2(kuka_gripper, jointIndex, p.POSITION_CONTROL, jointPositions[jointIndex],
+ 0)
+
+kuka_cid = p.createConstraint(kuka, 6, kuka_gripper, 0, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0.05],
+ [0, 0, 0])
+
+pr2_cid2 = p.createConstraint(kuka_gripper,
+ 4,
+ kuka_gripper,
+ 6,
+ jointType=p.JOINT_GEAR,
+ jointAxis=[1, 1, 1],
+ parentFramePosition=[0, 0, 0],
+ childFramePosition=[0, 0, 0])
+p.changeConstraint(pr2_cid2, gearRatio=-1, erp=0.5, relativePositionTarget=0, maxForce=100)
+
+objects = [
+ p.loadURDF("jenga/jenga.urdf", 1.300000, -0.700000, 0.750000, 0.000000, 0.707107, 0.000000,
+ 0.707107)
+]
+objects = [
+ p.loadURDF("jenga/jenga.urdf", 1.200000, -0.700000, 0.750000, 0.000000, 0.707107, 0.000000,
+ 0.707107)
+]
+objects = [
+ p.loadURDF("jenga/jenga.urdf", 1.100000, -0.700000, 0.750000, 0.000000, 0.707107, 0.000000,
+ 0.707107)
+]
+objects = [
+ p.loadURDF("jenga/jenga.urdf", 1.000000, -0.700000, 0.750000, 0.000000, 0.707107, 0.000000,
+ 0.707107)
+]
+objects = [
+ p.loadURDF("jenga/jenga.urdf", 0.900000, -0.700000, 0.750000, 0.000000, 0.707107, 0.000000,
+ 0.707107)
+]
+objects = [
+ p.loadURDF("jenga/jenga.urdf", 0.800000, -0.700000, 0.750000, 0.000000, 0.707107, 0.000000,
+ 0.707107)
+]
+objects = [
+ p.loadURDF("table/table.urdf", 1.000000, -0.200000, 0.000000, 0.000000, 0.000000, 0.707107,
+ 0.707107)
+]
+objects = [
+ p.loadURDF("teddy_vhacd.urdf", 1.050000, -0.500000, 0.700000, 0.000000, 0.000000, 0.707107,
+ 0.707107)
+]
+objects = [
+ p.loadURDF("cube_small.urdf", 0.950000, -0.100000, 0.700000, 0.000000, 0.000000, 0.707107,
+ 0.707107)
+]
+objects = [
+ p.loadURDF("sphere_small.urdf", 0.850000, -0.400000, 0.700000, 0.000000, 0.000000, 0.707107,
+ 0.707107)
+]
+objects = [
+ p.loadURDF("duck_vhacd.urdf", 0.850000, -0.400000, 0.900000, 0.000000, 0.000000, 0.707107,
+ 0.707107)
+]
+objects = p.loadSDF("kiva_shelf/model.sdf")
+ob = objects[0]
+p.resetBasePositionAndOrientation(ob, [0.000000, 1.000000, 1.204500],
+ [0.000000, 0.000000, 0.000000, 1.000000])
+objects = [
+ p.loadURDF("teddy_vhacd.urdf", -0.100000, 0.600000, 0.850000, 0.000000, 0.000000, 0.000000,
+ 1.000000)
+]
+objects = [
+ p.loadURDF("sphere_small.urdf", -0.100000, 0.955006, 1.169706, 0.633232, -0.000000, -0.000000,
+ 0.773962)
+]
+objects = [
+ p.loadURDF("cube_small.urdf", 0.300000, 0.600000, 0.850000, 0.000000, 0.000000, 0.000000,
+ 1.000000)
+]
+objects = [
+ p.loadURDF("table_square/table_square.urdf", -1.000000, 0.000000, 0.000000, 0.000000, 0.000000,
+ 0.000000, 1.000000)
+]
+ob = objects[0]
+jointPositions = [0.000000]
+for jointIndex in range(p.getNumJoints(ob)):
+ p.resetJointState(ob, jointIndex, jointPositions[jointIndex])
+
+objects = [
+ p.loadURDF("husky/husky.urdf", 2.000000, -5.000000, 1.000000, 0.000000, 0.000000, 0.000000,
+ 1.000000)
+]
+ob = objects[0]
+jointPositions = [
+ 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000,
+ 0.000000
+]
+for jointIndex in range(p.getNumJoints(ob)):
+ p.resetJointState(ob, jointIndex, jointPositions[jointIndex])
+
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
+
+p.setGravity(0.000000, 0.000000, 0.000000)
+p.setGravity(0, 0, -10)
+
+##show this for 10 seconds
+#now = time.time()
+#while (time.time() < now+10):
+# p.stepSimulation()
+p.setRealTimeSimulation(1)
+
+while (1):
+ p.setGravity(0, 0, -10)
+p.disconnect()
diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c
index 1ce848a0e..effdfb07c 100644
--- a/examples/pybullet/pybullet.c
+++ b/examples/pybullet/pybullet.c
@@ -51,9 +51,10 @@
#pragma message(B3_VAR_NAME_VALUE(PY_MAJOR_VERSION))
#pragma message(B3_VAR_NAME_VALUE(PY_MINOR_VERSION))
#endif
-
+//#define PYBULLET_USE_NUMPY
#ifdef PYBULLET_USE_NUMPY
#include
+//#include "C:/Python37/Lib/site-packages/numpy/core/include/numpy/arrayobject.h"
#endif
#if PY_MAJOR_VERSION >= 3
@@ -3252,17 +3253,29 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject*
PyObject* targetVelocityObj = 0;
PyObject* targetForceObj = 0;
- double kp = 0.1;
- double kd = 1.0;
+ double kpArray[3] = {0.1, 0.1, 0.1};
+ int kpSize = 0;
+ PyObject* kpObj = 0;
+ double kdArray[3] = {1.0, 1.0, 1.0};
+ int kdSize = 0;
+ PyObject* kdObj = 0;
double maxVelocity = -1;
+ double dampingArray[3] = {1.0, 1.0, 1.0};
+ int dampingSize = 0;
+ PyObject* dampingObj = 0;
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
- static char* kwlist[] = {"bodyUniqueId", "jointIndex", "controlMode", "targetPosition", "targetVelocity", "force", "positionGain", "velocityGain", "maxVelocity", "physicsClientId", NULL};
- if (!PyArg_ParseTupleAndKeywords(args, keywds, "iii|OOOdddi", kwlist, &bodyUniqueId, &jointIndex, &controlMode,
- &targetPositionObj, &targetVelocityObj, &targetForceObj, &kp, &kd, &maxVelocity, &physicsClientId))
+ static char* kwlist[] = {"bodyUniqueId", "jointIndex", "controlMode", "targetPosition", "targetVelocity", "force", "positionGain", "velocityGain", "maxVelocity", "damping", "physicsClientId", NULL};
+ if (!PyArg_ParseTupleAndKeywords(args, keywds, "iii|OOOddddi", kwlist, &bodyUniqueId, &jointIndex, &controlMode,
+ &targetPositionObj, &targetVelocityObj, &targetForceObj, &kpArray[0], &kdArray[0], &maxVelocity, &dampingArray[0], &physicsClientId))
{
- return NULL;
+ PyErr_Clear();
+ if (!PyArg_ParseTupleAndKeywords(args, keywds, "iii|OOOOOdOi", kwlist, &bodyUniqueId, &jointIndex, &controlMode,
+ &targetPositionObj, &targetVelocityObj, &targetForceObj, &kpObj, &kdObj, &maxVelocity, &dampingObj, &physicsClientId))
+ {
+ return NULL;
+ }
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
@@ -3346,6 +3359,81 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject*
}
}
+ if (kpObj)
+ {
+ int i = 0;
+ PyObject* kpSeq = 0;
+ kpSeq = PySequence_Fast(kpObj, "expected a kp sequence");
+ kpSize = PySequence_Size(kpObj);
+
+ if (kpSize < 0)
+ {
+ kpSize = 0;
+ }
+ if (kpSize > 3)
+ {
+ kpSize = 3;
+ }
+ if (kpSeq)
+ {
+ for (i = 0; i < kpSize; i++)
+ {
+ kpArray[i] = pybullet_internalGetFloatFromSequence(kpSeq, i);
+ }
+ Py_DECREF(kpSeq);
+ }
+ }
+
+ if (kdObj)
+ {
+ int i = 0;
+ PyObject* kdSeq = 0;
+ kdSeq = PySequence_Fast(kdObj, "expected a kd sequence");
+ kdSize = PySequence_Size(kdObj);
+
+ if (kdSize < 0)
+ {
+ kdSize = 0;
+ }
+ if (kdSize > 3)
+ {
+ kdSize = 3;
+ }
+ if (kdSeq)
+ {
+ for (i = 0; i < kdSize; i++)
+ {
+ kdArray[i] = pybullet_internalGetFloatFromSequence(kdSeq, i);
+ }
+ Py_DECREF(kdSeq);
+ }
+ }
+
+ if (dampingObj)
+ {
+ int i = 0;
+ PyObject* dampingSeq = 0;
+ dampingSeq = PySequence_Fast(dampingObj, "expected a damping sequence");
+ dampingSize = PySequence_Size(dampingObj);
+
+ if (dampingSize < 0)
+ {
+ dampingSize = 0;
+ }
+ if (dampingSize > 3)
+ {
+ dampingSize = 3;
+ }
+ if (dampingSeq)
+ {
+ for (i = 0; i < dampingSize; i++)
+ {
+ dampingArray[i] = pybullet_internalGetFloatFromSequence(dampingSeq, i);
+ }
+ Py_DECREF(dampingSeq);
+ }
+ }
+
//if (targetPositionSize == 0 && targetVelocitySize == 0)
//{
@@ -3418,7 +3506,15 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject*
//printf("Warning: targetPosition array size doesn't match joint position size (got %d, expected %d).",targetPositionSize, info.m_qSize);
}
- b3JointControlSetKp(commandHandle, info.m_uIndex, kp);
+ if (info.m_uSize == kpSize || kpSize == 1)
+ {
+ b3JointControlSetKpMultiDof(commandHandle, info.m_uIndex,
+ kpArray, kpSize);
+ }
+ else if (kpSize == 0)
+ {
+ b3JointControlSetKp(commandHandle, info.m_uIndex, kpArray[0]);
+ }
if (info.m_uSize == targetVelocitySize)
{
b3JointControlSetDesiredVelocityMultiDof(commandHandle, info.m_uIndex,
@@ -3428,12 +3524,30 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject*
{
//printf("Warning: targetVelocity array size doesn't match joint dimentions (got %d, expected %d).", targetVelocitySize, info.m_uSize);
}
- b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
+ if (info.m_uSize == kdSize || kdSize == 1)
+ {
+ b3JointControlSetKdMultiDof(commandHandle, info.m_uIndex,
+ kdArray, kdSize);
+ }
+ else if (kdSize == 0)
+ {
+ b3JointControlSetKd(commandHandle, info.m_uIndex, kdArray[0]);
+ }
if (info.m_uSize == targetForceSize || targetForceSize == 1)
{
b3JointControlSetDesiredForceTorqueMultiDof(commandHandle, info.m_uIndex,
targetForceArray, targetForceSize);
}
+ if (info.m_uSize == dampingSize || dampingSize == 1)
+ {
+ b3JointControlSetDampingMultiDof(commandHandle, info.m_uIndex,
+ dampingArray, dampingSize);
+ }
+ else if (dampingSize == 0)
+ {
+ b3JointControlSetDamping(commandHandle, info.m_uIndex,
+ dampingArray[0]);
+ }
break;
}
default:
diff --git a/setup.py b/setup.py
index 077f697c7..ef2748b4a 100644
--- a/setup.py
+++ b/setup.py
@@ -459,7 +459,7 @@ hh = setup_py_dir + "/" + datadir
for root, dirs, files in os.walk(hh):
for fn in files:
ext = os.path.splitext(fn)[1][1:]
- if ext and ext in 'yaml index meta data-00000-of-00001 png gif jpg urdf sdf obj txt mtl dae off stl STL xml '.split(
+ if ext and ext in 'yaml index meta data-00000-of-00001 png gif jpg urdf sdf obj txt mtl dae off stl STL xml gin npy '.split(
):
fn = root + "/" + fn
need_files.append(fn[1 + len(hh):])
@@ -501,7 +501,7 @@ if 'BT_USE_EGL' in EGL_CXX_FLAGS:
setup(
name='pybullet',
- version='3.1.0',
+ version='3.1.2',
description=
'Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning',
long_description=
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
index 1ba586114..00d5fd560 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
@@ -61,7 +61,8 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(btMultiBodySolverConstra
btScalar lowerLimit, btScalar upperLimit,
bool angConstraint,
btScalar relaxation,
- bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
+ bool isFriction, btScalar desiredVelocity, btScalar cfmSlip,
+ btScalar damping)
{
solverConstraint.m_multiBodyA = m_bodyA;
solverConstraint.m_multiBodyB = m_bodyB;
@@ -348,7 +349,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(btMultiBodySolverConstra
{
btScalar positionalError = 0.f;
- btScalar velocityError = desiredVelocity - rel_vel; // * damping;
+ btScalar velocityError = (desiredVelocity - rel_vel) * damping;
btScalar erp = infoGlobal.m_erp2;
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
index 4a6007ee3..1aaa07b69 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
+++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
@@ -94,7 +94,7 @@ protected:
bool angConstraint = false,
btScalar relaxation = 1.f,
- bool isFriction = false, btScalar desiredVelocity = 0, btScalar cfmSlip = 0);
+ bool isFriction = false, btScalar desiredVelocity = 0, btScalar cfmSlip = 0, btScalar damping = 1.0);
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
diff --git a/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp b/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp
index 25ddd539b..00a7ef357 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp
@@ -26,10 +26,13 @@ btMultiBodySphericalJointMotor::btMultiBodySphericalJointMotor(btMultiBody* body
: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 3, true, MULTIBODY_CONSTRAINT_SPHERICAL_MOTOR),
m_desiredVelocity(0, 0, 0),
m_desiredPosition(0,0,0,1),
- m_kd(1.),
- m_kp(0.2),
+ m_use_multi_dof_params(false),
+ m_kd(1., 1., 1.),
+ m_kp(0.2, 0.2, 0.2),
m_erp(1),
- m_rhsClamp(SIMD_INFINITY)
+ m_rhsClamp(SIMD_INFINITY),
+ m_maxAppliedImpulseMultiDof(maxMotorImpulse, maxMotorImpulse, maxMotorImpulse),
+ m_damping(1.0, 1.0, 1.0)
{
m_maxAppliedImpulse = maxMotorImpulse;
@@ -139,7 +142,8 @@ btQuaternion relRot = currentQuat.inverse() * desiredQuat;
btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
btScalar desiredVelocity = this->m_desiredVelocity[row];
- btScalar velocityError = desiredVelocity - currentVelocity;
+ double kd = m_use_multi_dof_params ? m_kd[row % 3] : m_kd[0];
+ btScalar velocityError = (desiredVelocity - currentVelocity) * kd;
btMatrix3x3 frameAworld;
frameAworld.setIdentity();
@@ -152,12 +156,16 @@ btQuaternion relRot = currentQuat.inverse() * desiredQuat;
case btMultibodyLink::eSpherical:
{
btVector3 constraintNormalAng = frameAworld.getColumn(row % 3);
- posError = m_kp*angleDiff[row % 3];
+ double kp = m_use_multi_dof_params ? m_kp[row % 3] : m_kp[0];
+ posError = kp*angleDiff[row % 3];
+ double max_applied_impulse = m_use_multi_dof_params ? m_maxAppliedImpulseMultiDof[row % 3] : m_maxAppliedImpulse;
fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
btVector3(0,0,0), dummy, dummy,
posError,
infoGlobal,
- -m_maxAppliedImpulse, m_maxAppliedImpulse, true);
+ -max_applied_impulse, max_applied_impulse, true,
+ 1.0, false, 0, 0,
+ m_damping[row % 3]);
constraintRow.m_orgConstraint = this;
constraintRow.m_orgDofIndex = row;
break;
diff --git a/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h b/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h
index 621beab5a..bdeccc2e2 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h
+++ b/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h
@@ -26,10 +26,13 @@ class btMultiBodySphericalJointMotor : public btMultiBodyConstraint
protected:
btVector3 m_desiredVelocity;
btQuaternion m_desiredPosition;
- btScalar m_kd;
- btScalar m_kp;
+ bool m_use_multi_dof_params;
+ btVector3 m_kd;
+ btVector3 m_kp;
btScalar m_erp;
btScalar m_rhsClamp; //maximum error
+ btVector3 m_maxAppliedImpulseMultiDof;
+ btVector3 m_damping;
public:
btMultiBodySphericalJointMotor(btMultiBody* body, int link, btScalar maxMotorImpulse);
@@ -44,16 +47,32 @@ public:
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal);
- virtual void setVelocityTarget(const btVector3& velTarget, btScalar kd = 1.f)
+ virtual void setVelocityTarget(const btVector3& velTarget, btScalar kd = 1.0)
+ {
+ m_desiredVelocity = velTarget;
+ m_kd = btVector3(kd, kd, kd);
+ m_use_multi_dof_params = false;
+ }
+
+ virtual void setVelocityTargetMultiDof(const btVector3& velTarget, const btVector3& kd = btVector3(1.0, 1.0, 1.0))
{
m_desiredVelocity = velTarget;
m_kd = kd;
+ m_use_multi_dof_params = true;
}
- virtual void setPositionTarget(const btQuaternion& posTarget, btScalar kp = 1.f)
+ virtual void setPositionTarget(const btQuaternion& posTarget, btScalar kp =1.f)
+ {
+ m_desiredPosition = posTarget;
+ m_kp = btVector3(kp, kp, kp);
+ m_use_multi_dof_params = false;
+ }
+
+ virtual void setPositionTargetMultiDof(const btQuaternion& posTarget, const btVector3& kp = btVector3(1.f, 1.f, 1.f))
{
m_desiredPosition = posTarget;
m_kp = kp;
+ m_use_multi_dof_params = true;
}
virtual void setErp(btScalar erp)
@@ -68,6 +87,28 @@ public:
{
m_rhsClamp = rhsClamp;
}
+
+ btScalar getMaxAppliedImpulseMultiDof(int i) const
+ {
+ return m_maxAppliedImpulseMultiDof[i];
+ }
+
+ void setMaxAppliedImpulseMultiDof(const btVector3& maxImp)
+ {
+ m_maxAppliedImpulseMultiDof = maxImp;
+ m_use_multi_dof_params = true;
+ }
+
+ btScalar getDamping(int i) const
+ {
+ return m_damping[i];
+ }
+
+ void setDamping(const btVector3& damping)
+ {
+ m_damping = damping;
+ }
+
virtual void debugDraw(class btIDebugDraw* drawer)
{
//todo(erwincoumans)