Merge branch 'master' into SIblock

Extras/InverseDynamics/DillCreator.cpp

@@ -109,7 +109,7 @@ int DillCreator::recurseDill(const int level, const int parent, const idScalar d
 			m_body_T_parent_ref[body](i, j) = 0.0;
 		}
 	}
-	const idScalar size_5 = pow(size, 5);
+        const idScalar size_5 = std::pow(size, 5);
         m_body_I_body[body](0, 0) = size_5 / 0.2e6;
         m_body_I_body[body](1, 1) = size_5 * 403 / 1.2e6;
 	m_body_I_body[body](2, 2) = m_body_I_body[body](1, 1);

examples/Constraints/ConstraintDemo.cpp

@@ -19,6 +19,7 @@ subject to the following restrictions:
 #include "LinearMath/btIDebugDraw.h"
 
 #include <stdio.h>  //printf debugging
+#include <cmath>
 
 #include "../CommonInterfaces/CommonRigidBodyBase.h"
 
@@ -114,8 +115,8 @@ void AllConstraintDemo::initPhysics()
 	///gear constraint demo
 
 #define THETA SIMD_PI / 4.f
-#define L_1 (2 - tan(THETA))
-#define L_2 (1 / cos(THETA))
+#define L_1 (2 - std::tan(THETA))
+#define L_2 (1 / std::cos(THETA))
 #define RATIO L_2 / L_1
 
 	btRigidBody* bodyA = 0;

examples/ExampleBrowser/ExampleEntries.cpp

@@ -5,11 +5,6 @@
 #include "../BlockSolver/RigidBodyBoxes.h"
 #include "LinearMath/btAlignedObjectArray.h"
 #include "EmptyExample.h"
-#include "../BulletRobotics/BoxStack.h"
-#include "../BulletRobotics/FixJointBoxes.h"
-#include "../BulletRobotics/JointLimit.h"
-// #include "../BulletRobotics/GraspBox.h"
-#include "../BulletRobotics/FixJointBoxes.h"
 #include "../RenderingExamples/RenderInstancingDemo.h"
 #include "../RenderingExamples/CoordinateSystemDemo.h"
 #include "../RenderingExamples/RaytracerSetup.h"
@@ -136,12 +131,6 @@ static ExampleEntry gDefaultExamples[] =
 
 		ExampleEntry(1, "Soft Contact", "Using the error correction parameter (ERP) and constraint force mixing (CFM) values for contacts to simulate compliant contact.", RigidBodySoftContactCreateFunc),
 
-		ExampleEntry(0, "Bullet Robotics"),
-		ExampleEntry(1, "Box Stack", "Create a stack of boxes of large mass ratio.", BoxStackExampleCreateFunc),
-		ExampleEntry(1, "Joint Limit", "Create three objects joint together", JointLimitCreateFunc),
-		// ExampleEntry(1, "Grasp Box", "A robot arm of large mass tries to grasp a box of small mass", GraspBoxCreateFunc),
-		ExampleEntry(1, "FixJoint Boxes", "FixJoint Boxes", FixJointBoxesCreateFunc),
-
 		ExampleEntry(0, "MultiBody"),
 		ExampleEntry(1, "MultiDof", "Create a basic btMultiBody with 3-DOF spherical joints (mobilizers). The demo uses a fixed base or a floating base at restart.", MultiDofCreateFunc),
 		ExampleEntry(1, "TestJointTorque", "Apply a torque to a btMultiBody with 1-DOF joints (mobilizers). This setup is similar to API/TestHingeTorque.", TestJointTorqueCreateFunc),

examples/SharedMemory/BodyJointInfoUtility.h

@@ -21,6 +21,12 @@ inline char* strDup(const char* const str)
 template <typename T, typename U>
 void addJointInfoFromMultiBodyData(const T* mb, U* bodyJoints, bool verboseOutput)
 {
+	int numDofs = 0;
+	if (mb->m_baseMass>0)
+	{
+		numDofs = 6;
+	}
+
 	if (mb->m_baseName)
 	{
 		if (verboseOutput)
@@ -107,7 +113,9 @@ void addJointInfoFromMultiBodyData(const T* mb, U* bodyJoints, bool verboseOutpu
 		}
 		qOffset += mb->m_links[link].m_posVarCount;
 		uOffset += mb->m_links[link].m_dofCount;
+		numDofs += mb->m_links[link].m_dofCount;
 	}
+	bodyJoints->m_numDofs = numDofs;
 }
 
 #endif  //BODY_JOINT_INFO_UTILITY_H

examples/SharedMemory/PhysicsClient.h

@@ -33,6 +33,8 @@ public:
 
 	virtual int getNumJoints(int bodyUniqueId) const = 0;
 
+	virtual int getNumDofs(int bodyUniqueId) const = 0;
+
 	virtual bool getJointInfo(int bodyUniqueId, int jointIndex, struct b3JointInfo& info) const = 0;
 
 	virtual int getNumUserConstraints() const = 0;

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -2526,6 +2526,12 @@ B3_SHARED_API int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyUniqu
 	return cl->getNumJoints(bodyUniqueId);
 }
 
+B3_SHARED_API int b3GetNumDofs(b3PhysicsClientHandle physClient, int bodyUniqueId)
+{
+        PhysicsClient* cl = (PhysicsClient*)physClient;
+        return cl->getNumDofs(bodyUniqueId);
+}
+
 B3_SHARED_API int b3ComputeDofCount(b3PhysicsClientHandle physClient, int bodyUniqueId)
 {
 	int nj = b3GetNumJoints(physClient, bodyUniqueId);
@@ -4642,8 +4648,9 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3Physi
 	command->m_calculateJacobianArguments.m_localPosition[0] = localPosition[0];
 	command->m_calculateJacobianArguments.m_localPosition[1] = localPosition[1];
 	command->m_calculateJacobianArguments.m_localPosition[2] = localPosition[2];
-	int numJoints = cl->getNumJoints(bodyUniqueId);
-	for (int i = 0; i < numJoints; i++)
+
+	int numDofs = cl->getNumDofs(bodyUniqueId);
+	for (int i = 0; i < numDofs; i++)
 	{
 		command->m_calculateJacobianArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
 		command->m_calculateJacobianArguments.m_jointVelocitiesQdot[i] = jointVelocitiesQdot[i];

examples/SharedMemory/PhysicsClientC_API.h

@@ -118,6 +118,8 @@ extern "C"
 
 	///give a unique body index (after loading the body) return the number of joints.
 	B3_SHARED_API int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyUniqueId);
+	  ///give a unique body index (after loading the body) return the number of degrees of freedom (DoF).
+        B3_SHARED_API int b3GetNumDofs(b3PhysicsClientHandle physClient, int bodyUniqueId);
 	
 	///compute the number of degrees of freedom for this body.
 	///Return -1 for unsupported spherical joint, -2 for unsupported planar joint.

examples/SharedMemory/PhysicsClientSharedMemory.cpp

@@ -20,7 +20,7 @@ struct BodyJointInfoCache
 	b3AlignedObjectArray<b3JointInfo> m_jointInfo;
 	std::string m_bodyName;
 	btAlignedObjectArray<int> m_userDataIds;
-
+	int m_numDofs;
 	~BodyJointInfoCache()
 	{
 	}
@@ -144,6 +144,17 @@ int PhysicsClientSharedMemory::getNumJoints(int bodyUniqueId) const
 	return 0;
 }
 
+int PhysicsClientSharedMemory::getNumDofs(int bodyUniqueId) const
+{
+        BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
+        if (bodyJointsPtr && *bodyJointsPtr)
+        {
+                BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
+                return bodyJoints->m_numDofs;
+        }
+        return 0;
+}
+
 bool PhysicsClientSharedMemory::getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo& info) const
 {
 	BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];

examples/SharedMemory/PhysicsClientSharedMemory.h

@@ -45,6 +45,8 @@ public:
 
 	virtual int getNumJoints(int bodyUniqueId) const;
 
+	virtual int getNumDofs(int bodyUniqueId) const;
+
 	virtual bool getJointInfo(int bodyUniqueId, int jointIndex, struct b3JointInfo& info) const;
 
 	virtual int getNumUserConstraints() const;

examples/SharedMemory/PhysicsDirect.cpp

@@ -22,6 +22,7 @@ struct BodyJointInfoCache2
 	btAlignedObjectArray<b3JointInfo> m_jointInfo;
 	std::string m_bodyName;
 	btAlignedObjectArray<int> m_userDataIds;
+	int m_numDofs;
 
 	~BodyJointInfoCache2()
 	{
@@ -1275,9 +1276,9 @@ bool PhysicsDirect::getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const
 	return false;
 }
 
-int PhysicsDirect::getNumJoints(int bodyIndex) const
+int PhysicsDirect::getNumJoints(int bodyUniqueId) const
 {
-	BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex];
+	BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
 	if (bodyJointsPtr && *bodyJointsPtr)
 	{
 		BodyJointInfoCache2* bodyJoints = *bodyJointsPtr;
@@ -1287,6 +1288,18 @@ int PhysicsDirect::getNumJoints(int bodyIndex) const
 	return 0;
 }
 
+int PhysicsDirect::getNumDofs(int bodyUniqueId) const
+{
+        BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
+        if (bodyJointsPtr && *bodyJointsPtr)
+        {
+                BodyJointInfoCache2* bodyJoints = *bodyJointsPtr;
+                return bodyJoints->m_numDofs;
+        }
+        btAssert(0);
+        return 0;
+}
+
 bool PhysicsDirect::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const
 {
 	BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex];

examples/SharedMemory/PhysicsDirect.h

@@ -61,7 +61,9 @@ public:
 
 	virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const;
 
-	virtual int getNumJoints(int bodyIndex) const;
+	virtual int getNumJoints(int bodyUniqueId) const;
+
+	virtual int getNumDofs(int bodyUniqueId) const;
 
 	virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
 

examples/SharedMemory/PhysicsLoopBack.cpp

@@ -107,9 +107,14 @@ bool PhysicsLoopBack::getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) con
 	return m_data->m_physicsClient->getBodyInfo(bodyUniqueId, info);
 }
 
-int PhysicsLoopBack::getNumJoints(int bodyIndex) const
+int PhysicsLoopBack::getNumJoints(int bodyUniqueId) const
 {
-	return m_data->m_physicsClient->getNumJoints(bodyIndex);
+	return m_data->m_physicsClient->getNumJoints(bodyUniqueId);
+}
+
+int PhysicsLoopBack::getNumDofs(int bodyUniqueId) const
+{
+        return m_data->m_physicsClient->getNumDofs(bodyUniqueId);
 }
 
 bool PhysicsLoopBack::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const

examples/SharedMemory/PhysicsLoopBack.h

@@ -42,7 +42,9 @@ public:
 
 	virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const;
 
-	virtual int getNumJoints(int bodyIndex) const;
+	virtual int getNumJoints(int bodyUniqueId) const;
+
+	virtual int getNumDofs(int bodyUniqueId) const;
 
 	virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
 

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -1699,6 +1699,7 @@ struct PhysicsServerCommandProcessorInternalData
 		  m_logPlaybackUid(-1),
 		  m_physicsDeltaTime(1. / 240.),
 		  m_numSimulationSubSteps(0),
+                  m_simulationTimestamp(0),
 		  m_userConstraintUIDGenerator(1),
 		  m_broadphaseCollisionFilterCallback(0),
 		  m_pairCache(0),

examples/ThirdPartyLibs/Gwen/Anim.cpp

@@ -5,8 +5,9 @@
 */
 
 #include "Gwen/Anim.h"
-#include "Gwen/Utility.h"
 #include <math.h>
+#include <cmath>
+#include "Gwen/Utility.h"
 
 using namespace Gwen;
 
@@ -123,10 +124,9 @@ void Gwen::Anim::TimedAnimation::Think()
 	if (fDelta < 0.0f) fDelta = 0.0f;
 	if (fDelta > 1.0f) fDelta = 1.0f;
 
-	Run(pow(fDelta, m_fEase));
+        Run(std::pow(fDelta, m_fEase));
 
-	if (fDelta == 1.0f)
-	{
+        if (fDelta == 1.0f) {
           m_bFinished = true;
           OnFinish();
         }

examples/ThirdPartyLibs/Gwen/Controls/TextBox.cpp

@@ -11,6 +11,7 @@
 #include "Gwen/Platform.h"
 
 #include <math.h>
+#include <cmath>
 
 using namespace Gwen;
 using namespace Gwen::Controls;
@@ -89,7 +90,8 @@ void TextBox::Render(Skin::Base* skin)
 	}
 
 	// Draw caret
-	if (fmod(Gwen::Platform::GetTimeInSeconds() - m_fLastInputTime, 1.0f) > 0.5f)
+        if (std::fmod(Gwen::Platform::GetTimeInSeconds() - m_fLastInputTime,
+                      1.0f) > 0.5f)
           skin->GetRender()->SetDrawColor(Gwen::Color(255, 255, 255, 255));
         else
 		skin->GetRender()->SetDrawColor(Gwen::Color(0, 0, 0, 255));

examples/TinyRenderer/TinyRenderer.cpp

@@ -1,20 +1,21 @@
 #include "TinyRenderer.h"
 
-#include <vector>
-#include <limits>
+#include <cmath>
 #include <iostream>
-#include "tgaimage.h"
-#include "model.h"
-#include "geometry.h"
-#include "our_gl.h"
-#include "../Utils/b3ResourcePath.h"
-#include "Bullet3Common/b3MinMax.h"
+#include <limits>
+#include <vector>
+#include "../CommonInterfaces/CommonFileIOInterface.h"
 #include "../OpenGLWindow/ShapeData.h"
+#include "../Utils/b3BulletDefaultFileIO.h"
+#include "../Utils/b3ResourcePath.h"
+#include "Bullet3Common/b3Logging.h"
+#include "Bullet3Common/b3MinMax.h"
 #include "LinearMath/btAlignedObjectArray.h"
 #include "LinearMath/btVector3.h"
-#include "Bullet3Common/b3Logging.h"
-#include "../CommonInterfaces/CommonFileIOInterface.h"
-#include "../Utils/b3BulletDefaultFileIO.h"
+#include "geometry.h"
+#include "model.h"
+#include "our_gl.h"
+#include "tgaimage.h"
 
 struct DepthShader : public IShader
 {
@@ -161,7 +162,8 @@ struct Shader : public IShader
 		Vec2f uv = varying_uv * bar;
 
 		Vec3f reflection_direction = (bn * (bn * m_light_dir_local * 2.f) - m_light_dir_local).normalize();
-		float specular = pow(b3Max(reflection_direction.z, 0.f), m_model->specular(uv));
+                float specular = std::pow(b3Max(reflection_direction.z, 0.f),
+                                          m_model->specular(uv));
                 float diffuse = b3Max(0.f, bn * m_light_dir_local);
 
                 color = m_model->diffuse(uv);

examples/TinyRenderer/model.cpp

@@ -1,10 +1,11 @@
 
-#include <iostream>
-#include <fstream>
-#include <sstream>
 #include "model.h"
-#include "Bullet3Common/b3Logging.h"
 #include <string.h>  // memcpy
+#include <cmath>
+#include <fstream>
+#include <iostream>
+#include <sstream>
+#include "Bullet3Common/b3Logging.h"
 Model::Model(const char *filename) : verts_(), faces_(), norms_(), uv_(), diffusemap_(), normalmap_(), specularmap_()
 {
 	std::ifstream in;
@@ -159,8 +160,8 @@ TGAColor Model::diffuse(Vec2f uvf)
 		//		bool repeat = true;
 		//		if (repeat)
 		{
-			uvf[0] = modf(uvf[0], &val);
-			uvf[1] = modf(uvf[1], &val);
+                  uvf[0] = std::modf(uvf[0], &val);
+                  uvf[1] = std::modf(uvf[1], &val);
                 }
                 Vec2i uv(uvf[0] * diffusemap_.get_width(), uvf[1] * diffusemap_.get_height());
 		return diffusemap_.get(uv[0], uv[1]);

examples/pybullet/gym/pybullet_data/__init__.py

@@ -1,4 +1,3 @@
-import pybullet as p
 import os
 
 def getDataPath():

examples/pybullet/gym/pybullet_envs/__init__.py

@@ -25,14 +25,14 @@ register(
 register(
     id='MinitaurBulletEnv-v0',
     entry_point='pybullet_envs.bullet:MinitaurBulletEnv',
-    timestep_limit=1000,
+    max_episode_steps=1000,
     reward_threshold=15.0,
 )
 
 register(
     id='MinitaurBulletDuckEnv-v0',
     entry_point='pybullet_envs.bullet:MinitaurBulletDuckEnv',
-    timestep_limit=1000,
+    max_episode_steps=1000,
     reward_threshold=5.0,
 )
 
@@ -40,7 +40,7 @@ register(
 register(
     id='MinitaurReactiveEnv-v0',
     entry_point='pybullet_envs.minitaur.envs:MinitaurReactiveEnv',
-    timestep_limit=1000,
+    max_episode_steps=1000,
     reward_threshold=5.0,
 )
 
@@ -48,7 +48,7 @@ register(
 register(
     id='MinitaurBallGymEnv-v0',
     entry_point='pybullet_envs.minitaur.envs:MinitaurBallGymEnv',
-    timestep_limit=1000,
+    max_episode_steps=1000,
     reward_threshold=5.0,
 )
 
@@ -56,28 +56,28 @@ register(
 register(
     id='MinitaurTrottingEnv-v0',
     entry_point='pybullet_envs.minitaur.envs:MinitaurTrottingEnv',
-    timestep_limit=1000,
+    max_episode_steps=1000,
     reward_threshold=5.0,
 )
 
 register(
     id='MinitaurStandGymEnv-v0',
     entry_point='pybullet_envs.minitaur.envs:MinitaurStandGymEnv',
-    timestep_limit=1000,
+    max_episode_steps=1000,
     reward_threshold=5.0,
 )
 
 register(
     id='MinitaurAlternatingLegsEnv-v0',
     entry_point='pybullet_envs.minitaur.envs:MinitaurAlternatingLegsEnv',
-    timestep_limit=1000,
+    max_episode_steps=1000,
     reward_threshold=5.0,
 )
 
 register(
     id='MinitaurFourLegStandEnv-v0',
     entry_point='pybullet_envs.minitaur.envs:MinitaurFourLegStandEnv',
-    timestep_limit=1000,
+    max_episode_steps=1000,
     reward_threshold=5.0,
 )
 
@@ -86,14 +86,14 @@ register(
 register(
     id='RacecarBulletEnv-v0',
     entry_point='pybullet_envs.bullet:RacecarGymEnv',
-    timestep_limit=1000,
+    max_episode_steps=1000,
     reward_threshold=5.0,
 )
 
 register(
 	id='RacecarZedBulletEnv-v0',
 	entry_point='pybullet_envs.bullet:RacecarZEDGymEnv',
-	timestep_limit=1000,
+	max_episode_steps=1000,
 	reward_threshold=5.0,
 )
 
@@ -101,21 +101,21 @@ register(
 register(
 	id='KukaBulletEnv-v0',
 	entry_point='pybullet_envs.bullet:KukaGymEnv',
-	timestep_limit=1000,
+	max_episode_steps=1000,
 	reward_threshold=5.0,
 )
 
 register(
 	id='KukaCamBulletEnv-v0',
 	entry_point='pybullet_envs.bullet:KukaCamGymEnv',
-	timestep_limit=1000,
+	max_episode_steps=1000,
 	reward_threshold=5.0,
 )
 
 register(
 	id='KukaDiverseObjectGrasping-v0',
 	entry_point='pybullet_envs.bullet:KukaDiverseObjectEnv',
-	timestep_limit=1000,
+	max_episode_steps=1000,
 	reward_threshold=5.0,
 )
 

examples/pybullet/gym/pybullet_envs/agents/utility.py

@@ -149,7 +149,7 @@ def save_config(config, logdir=None):
     tf.logging.info(message.format(config.logdir))
     tf.gfile.MakeDirs(config.logdir)
     config_path = os.path.join(config.logdir, 'config.yaml')
-    with tf.gfile.FastGFile(config_path, 'w') as file_:
+    with tf.gfile.GFile(config_path, 'w') as file_:
       yaml.dump(config, file_, default_flow_style=False)
   else:
     message = (

examples/pybullet/gym/pybullet_envs/minitaur/actuatornet/__init__.py

@@ -0,0 +1,2 @@
+
+

examples/pybullet/gym/pybullet_envs/minitaur/actuatornet/actuatornet_keras.py

@@ -0,0 +1,70 @@
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+from absl import app
+from absl import flags
+from keras.callbacks import ModelCheckpoint
+from keras.layers import Dense
+from keras.models import Sequential
+from keras import optimizers
+import numpy
+
+FLAGS = flags.FLAGS
+
+flags.DEFINE_string(
+    "input_filename", "data/minitaur_log_latency_0.01.csv", "The name of the input CSV file."
+    "Each line in the CSV file will contain the motor position, the "
+    "motor speed, action and torques.")
+
+
+def main(unused_argv):
+  # fix random seed for reproducibility
+  numpy.random.seed(7)
+  # load pima indians dataset
+  dataset = numpy.loadtxt(
+      FLAGS.input_filename,
+      delimiter=",")
+  # split into input (X) and output (Y) variables
+  x = dataset[:, 0:3]
+  y = dataset[:, 3]
+  print("x=", x)
+  print("y=", y)
+
+  # create model
+  model = Sequential()
+  model.add(Dense(12, input_dim=3, activation="relu"))
+  model.add(Dense(8, activation="sigmoid"))
+  model.add(Dense(1, activation="linear"))
+
+  # Compile model (use adam or sgd)
+  model.compile(
+      loss="mean_squared_error",
+      optimizer="adam",
+      metrics=["mean_squared_error"])
+
+  # checkpoint
+  filepath = "/tmp/keras/weights-improvement-{epoch:02d}-{val_loss:.2f}.hdf5"
+  checkpoint = ModelCheckpoint(
+      filepath, monitor="val_loss", verbose=1, save_best_only=True, mode="min")
+  callbacks_list = [checkpoint]
+
+  # Fit the model
+  # model.fit(X, Y, epochs=150, batch_size=10)
+  # model.fit(X, Y, epochs=150, batch_size=10, callbacks=callbacks_list)
+  model.fit(
+      x,
+      y,
+      validation_split=0.34,
+      epochs=4500,
+      batch_size=1024,
+      callbacks=callbacks_list,
+      verbose=0)
+
+  # evaluate the model
+  scores = model.evaluate(x, y)
+  print("\n%s: %.2f%%" % (model.metrics_names[1], scores[1] * 100))
+
+
+if __name__ == "__main__":
+  app.run(main)

examples/pybullet/gym/pybullet_envs/minitaur/actuatornet/actuatornet_keras_lstm.py

@@ -0,0 +1,166 @@
+"""see https://machinelearningmastery.com/multivariate-time-series-forecasting-lstms-keras/"""
+
+import matplotlib
+matplotlib.use('TkAgg')
+import numpy as np
+from matplotlib import pyplot
+from pandas import read_csv
+from pandas import DataFrame
+from pandas import concat
+from sklearn.preprocessing import MinMaxScaler
+from sklearn.preprocessing import LabelEncoder
+from sklearn.metrics import mean_squared_error
+from keras.models import Sequential
+from keras.layers import Dropout
+from keras.layers import Dense
+from keras.layers import LSTM
+from keras.callbacks import ModelCheckpoint
+from matplotlib import pyplot
+
+from pandas import read_csv
+
+
+# convert series to supervised learning
+def series_to_supervised(data, n_in=1, n_out=1, dropnan=True):
+  n_vars = 1 if type(data) is list else data.shape[1]
+  df = DataFrame(data)
+  cols, names = list(), list()
+  # input sequence (t-n, ... t-1)
+  for i in range(n_in, 0, -1):
+    cols.append(df.shift(i))
+    names += [('var%d(t-%d)' % (j + 1, i)) for j in range(n_vars)]
+  # forecast sequence (t, t+1, ... t+n)
+  for i in range(0, n_out):
+    cols.append(df.shift(-i))
+    if i == 0:
+      names += [('var%d(t)' % (j + 1)) for j in range(n_vars)]
+    else:
+      names += [('var%d(t+%d)' % (j + 1, i)) for j in range(n_vars)]
+  # put it all together
+  agg = concat(cols, axis=1)
+  agg.columns = names
+  # drop rows with NaN values
+  if dropnan:
+    agg.dropna(inplace=True)
+  return agg
+
+
+values = [x for x in range(10)]
+data = series_to_supervised(values, 2)
+print(data)
+
+# load dataset
+#dataset = read_csv('./data/minitaur_log_latency_0.01.csv')
+#dataset = read_csv('./data/minitaur_log_latency_0.003.csv')
+dataset = read_csv('./data/minitaur_log_latency_0.006.csv')
+
+values = dataset.values
+# integer encode direction
+#encoder = LabelEncoder()
+#values[:,3] = encoder.fit_transform(values[:,3])
+# ensure all data is float
+values = values.astype('float32')
+
+# normalize features
+useNormalization = False
+if useNormalization:
+  scaler = MinMaxScaler(feature_range=(0, 1))
+  scaled = scaler.fit_transform(values)
+else:
+  scaled = values
+
+# frame as supervised learning
+lag_steps = 5
+reframed = series_to_supervised(scaled, lag_steps, 1)
+print("reframed before drop=", reframed)
+
+# drop columns we don't want to predict
+reframed.drop(reframed.columns[[3,7,11,15,19]], axis=1, inplace=True)
+print("after drop=",reframed.head())
+
+#dummy = scaler.inverse_transform(reframed)
+#print(dummy)
+
+groups = [0, 1, 2, 3]
+
+i = 1
+# plot each column
+doPlot = False
+if doPlot:
+  pyplot.figure()
+  for group in groups:
+    pyplot.subplot(len(groups), 1, i)
+    pyplot.plot(values[0:25, group])
+    pyplot.title(dataset.columns[group], y=0.5, loc='right')
+    i += 1
+  pyplot.show()
+
+# split into train and test sets
+values = reframed.values
+n_train_hours = 6000
+train = values[:n_train_hours, :]
+test = values[n_train_hours:, :]
+# split into input and outputs
+train_X, train_y = train[:, :-1], train[:, -1]
+test_X, test_y = test[:, :-1], test[:, -1]
+
+print("train_X.shape[1]=",train_X.shape[1])
+
+
+# design network
+useLSTM=True
+if useLSTM:
+  # reshape input to be 3D [samples, timesteps, features]
+  train_X = train_X.reshape((train_X.shape[0], lag_steps+1, int(train_X.shape[1]/(lag_steps+1))))
+  test_X = test_X.reshape((test_X.shape[0], lag_steps+1, int(test_X.shape[1]/(lag_steps+1))))
+  model = Sequential()
+  model.add(LSTM(40,  input_shape=(train_X.shape[1], train_X.shape[2])))
+  model.add(Dropout(0.05))
+  model.add(Dense(8, activation='sigmoid'))
+  model.add(Dense(8, activation='sigmoid'))
+  model.add(Dropout(0.05))
+  model.add(Dense(1, activation='linear'))
+else:
+  # create model
+  model = Sequential()
+  model.add(Dense(12, input_dim=train_X.shape[1], activation="relu"))
+  model.add(Dense(8, activation="sigmoid"))
+  model.add(Dense(1, activation="linear"))
+
+#model.compile(loss='mae', optimizer='adam')
+model.compile(
+    loss='mean_squared_error', optimizer='adam', metrics=['mean_squared_error'])
+
+# checkpoint
+filepath = '/tmp/keras/weights-improvement-{epoch:02d}-{val_loss:.2f}.hdf5'
+checkpoint = ModelCheckpoint(
+    filepath, monitor='val_loss', verbose=1, save_best_only=True, mode='min')
+callbacks_list = [checkpoint]
+
+# fit network
+history = model.fit(
+    train_X,
+    train_y,
+    epochs=1500,
+    batch_size=32,
+    callbacks=callbacks_list,
+    validation_data=(test_X, test_y),
+    verbose=2,
+    shuffle=False)
+# plot history
+
+
+data = np.array([[[1.513535008329887299,3.234624992847829894e-01,1.731481043119239782,1.741165415165205399,
+1.534267104753672228e+00,1.071354965017878635e+00,1.712386127673626302e+00]]])
+
+
+#prediction = model.predict(data)
+#print("prediction=",prediction)
+
+
+pyplot.plot(history.history['loss'], label='train')
+pyplot.plot(history.history['val_loss'], label='test')
+pyplot.legend()
+pyplot.show()
+
+

examples/pybullet/gym/pybullet_envs/minitaur/actuatornet/minitaur_raibert_controller_example.py

@@ -0,0 +1,67 @@
+#The example to run the raibert controller in a Minitaur gym env.
+
+#blaze run :minitaur_raibert_controller_example  -- --log_path=/tmp/logs
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import tensorflow as tf
+from pybullet_envs.minitaur.envs import minitaur_raibert_controller
+from pybullet_envs.minitaur.envs import minitaur_gym_env
+
+flags = tf.app.flags
+FLAGS = tf.app.flags.FLAGS
+
+flags.DEFINE_float("motor_kp", 1.0, "The position gain of the motor.")
+flags.DEFINE_float("motor_kd", 0.015, "The speed gain of the motor.")
+flags.DEFINE_float(
+    "control_latency", 0.006, "The latency between sensor measurement and action"
+    " execution the robot.")
+flags.DEFINE_string("log_path", ".", "The directory to write the log file.")
+
+
+def speed(t):
+  max_speed = 0.35
+  t1 = 3
+  if t < t1:
+    return t / t1 * max_speed
+  else:
+    return -max_speed
+
+
+def main(argv):
+    del argv
+    env = minitaur_gym_env.MinitaurGymEnv(
+        urdf_version=minitaur_gym_env.RAINBOW_DASH_V0_URDF_VERSION,
+        control_time_step=0.006,
+        action_repeat=6,
+        pd_latency=0.0,
+        control_latency=FLAGS.control_latency,
+        motor_kp=FLAGS.motor_kp,
+        motor_kd=FLAGS.motor_kd,
+        remove_default_joint_damping=True,
+        leg_model_enabled=False,
+        render=True,
+        on_rack=False,
+        accurate_motor_model_enabled=True,
+        log_path=FLAGS.log_path)
+    env.reset()
+
+    controller = minitaur_raibert_controller.MinitaurRaibertTrottingController(
+        env.minitaur)
+
+    tstart = env.minitaur.GetTimeSinceReset()
+    for _ in range(1000):
+      t = env.minitaur.GetTimeSinceReset() - tstart
+      controller.behavior_parameters = (
+          minitaur_raibert_controller.BehaviorParameters(
+              desired_forward_speed=speed(t)))
+      controller.update(t)
+      env.step(controller.get_action())
+
+    #env.close()
+
+if __name__ == "__main__":
+  tf.app.run(main)
+

examples/pybullet/gym/pybullet_envs/minitaur/actuatornet/proto2csv.py

@@ -0,0 +1,46 @@
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+#python proto2csv.py --proto_file=/tmp/logs/minitaur_log_2019-01-27-12-59-31 --csv_file=/tmp/logs/out.csv
+#each line in csv contains: angle, velocity, action, torque
+
+import tensorflow as tf
+import argparse
+import numpy
+from pybullet_envs.minitaur.envs import minitaur_logging
+
+flags = tf.app.flags
+FLAGS = tf.app.flags.FLAGS
+
+flags.DEFINE_string("proto_file", "logs", "path to protobuf input file")
+flags.DEFINE_string("csv_file", "file.csv", "poth to csv output file")
+
+
+def main(argv):
+  del argv
+
+  logging = minitaur_logging.MinitaurLogging()
+  episode = logging.restore_episode(FLAGS.proto_file)
+  #print(dir (episode))
+  #print("episode=",episode)
+  fields = episode.ListFields()
+
+  recs = []
+
+  for rec in fields[0][1]:
+    #print(rec.time)
+    for motorState in rec.motor_states:
+      #print("motorState.angle=",motorState.angle)
+      #print("motorState.velocity=",motorState.velocity)
+      #print("motorState.action=",motorState.action)
+      #print("motorState.torque=",motorState.torque)
+      recs.append([motorState.angle,motorState.velocity,motorState.action,motorState.torque])
+		
+  a = numpy.array(recs)
+  numpy.savetxt(FLAGS.csv_file, a, delimiter=",")
+
+if __name__ == "__main__":
+  tf.app.run(main)
+
+

examples/pybullet/gym/pybullet_envs/minitaur/actuatornet/requirements.txt

@@ -0,0 +1,6 @@
+pybullet
+tensorflow
+gym
+pandas
+matplotlib
+

examples/pybullet/gym/pybullet_envs/minitaur/agents/scripts/utility.py

@@ -172,7 +172,7 @@ def save_config(config, logdir=None):
     tf.logging.info(message.format(config.logdir))
     tf.gfile.MakeDirs(config.logdir)
     config_path = os.path.join(config.logdir, 'config.yaml')
-    with tf.gfile.FastGFile(config_path, 'w') as file_:
+    with tf.gfile.GFile(config_path, 'w') as file_:
       yaml.dump(config, file_, default_flow_style=False)
   else:
     message = (

examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_gym_env.py

@@ -249,6 +249,7 @@ class MinitaurGymEnv(gym.Env):
     self._hard_reset = hard_reset  # This assignment need to be after reset()
 
   def close(self):
+    if self._env_step_counter>0:
       self.logging.save_episode(self._episode_proto)
     self.minitaur.Terminate()
 
@@ -258,6 +259,7 @@ class MinitaurGymEnv(gym.Env):
   def reset(self, initial_motor_angles=None, reset_duration=1.0):
     self._pybullet_client.configureDebugVisualizer(
         self._pybullet_client.COV_ENABLE_RENDERING, 0)
+    if self._env_step_counter>0:
       self.logging.save_episode(self._episode_proto)
     self._episode_proto = minitaur_logging_pb2.MinitaurEpisode()
     minitaur_logging.preallocate_episode_proto(self._episode_proto,