Merge branch 'master' of https://github.com/erwincoumans/bullet3

data/plane.urdf

@@ -2,7 +2,7 @@
 <robot name="cube.urdf">
   <link name="planeLink">
   <contact>
-      <lateral_friction value="1.5"/>
+      <lateral_friction value="2"/>
   </contact>
     <inertial>
       <origin rpy="0 0 0" xyz="0 0 0"/>

data/quadruped/quadruped.urdf

@@ -15,7 +15,7 @@
       </geometry>
     </collision>
     <inertial>
-      <mass value="1."/>
+      <mass value="3.2"/>
       <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
     </inertial>
   </link>

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -448,7 +448,7 @@ struct MinitaurStateLogger : public InternalStateLogger
 	btMultiBody* m_minitaurMultiBody;
 	btAlignedObjectArray<int> m_motorIdList;
 
-	MinitaurStateLogger(int loggingUniqueId, std::string fileName, btMultiBody* minitaurMultiBody, btAlignedObjectArray<int>& motorIdList)
+	MinitaurStateLogger(int loggingUniqueId, const std::string& fileName, btMultiBody* minitaurMultiBody, btAlignedObjectArray<int>& motorIdList)
 		:m_loggingTimeStamp(0),
 		m_logFileHandle(0),
 		m_minitaurMultiBody(minitaurMultiBody)
@@ -558,6 +558,171 @@ struct MinitaurStateLogger : public InternalStateLogger
 	}
 };
 
+struct GenericRobotStateLogger : public InternalStateLogger
+{
+    float m_loggingTimeStamp;
+    std::string m_fileName;
+    FILE* m_logFileHandle;
+    std::string m_structTypes;
+    btMultiBodyDynamicsWorld* m_dynamicsWorld;
+    btAlignedObjectArray<int> m_bodyIdList;
+    bool m_filterObjectUniqueId;
+    
+    GenericRobotStateLogger(int loggingUniqueId, std::string fileName, btMultiBodyDynamicsWorld* dynamicsWorld)
+    :m_loggingTimeStamp(0),
+    m_logFileHandle(0),
+    m_filterObjectUniqueId(false),
+    m_dynamicsWorld(dynamicsWorld)
+    {
+        m_loggingType = STATE_LOGGING_GENERIC_ROBOT;
+        
+        btAlignedObjectArray<std::string> structNames;
+        structNames.push_back("timeStamp");
+        structNames.push_back("objectId");
+        structNames.push_back("posX");
+        structNames.push_back("posY");
+        structNames.push_back("posZ");
+        structNames.push_back("oriX");
+        structNames.push_back("oriY");
+        structNames.push_back("oriZ");
+        structNames.push_back("oriW");
+        structNames.push_back("velX");
+        structNames.push_back("velY");
+        structNames.push_back("velZ");
+        structNames.push_back("omegaX");
+        structNames.push_back("omegaY");
+        structNames.push_back("omegaZ");
+        structNames.push_back("qNum");
+        structNames.push_back("q0");
+        structNames.push_back("q1");
+        structNames.push_back("q2");
+        structNames.push_back("q3");
+        structNames.push_back("q4");
+        structNames.push_back("q5");
+        structNames.push_back("q6");
+        structNames.push_back("q7");
+        structNames.push_back("q8");
+        structNames.push_back("q9");
+        structNames.push_back("q10");
+        structNames.push_back("q11");
+        structNames.push_back("u0");
+        structNames.push_back("u1");
+        structNames.push_back("u2");
+        structNames.push_back("u3");
+        structNames.push_back("u4");
+        structNames.push_back("u5");
+        structNames.push_back("u6");
+        structNames.push_back("u7");
+        structNames.push_back("u8");
+        structNames.push_back("u9");
+        structNames.push_back("u10");
+        structNames.push_back("u11");
+        
+        m_structTypes = "fIfffffffffffffIffffffffffffffffffffffff";
+        const char* fileNameC = fileName.c_str();
+        
+        m_logFileHandle = createMinitaurLogFile(fileNameC, structNames, m_structTypes);
+    }
+    virtual void stop()
+    {
+        if (m_logFileHandle)
+        {
+            closeMinitaurLogFile(m_logFileHandle);
+            m_logFileHandle = 0;
+        }
+    }
+    
+    virtual void logState(btScalar timeStep)
+    {
+        if (m_logFileHandle)
+        {
+            for (int i=0;i<m_dynamicsWorld->getNumMultibodies();i++)
+            {
+                btMultiBody* mb = m_dynamicsWorld->getMultiBody(i);
+                int objectUniqueId = mb->getUserIndex2();
+                if (m_filterObjectUniqueId && m_bodyIdList.findLinearSearch2(objectUniqueId) < 0)
+                {
+                    continue;
+                }
+                
+                MinitaurLogRecord logData;
+                logData.m_values.push_back(m_loggingTimeStamp);
+                
+                btVector3 pos = mb->getBasePos();
+                btQuaternion ori = mb->getWorldToBaseRot();
+                btVector3 vel = mb->getBaseVel();
+                btVector3 omega = mb->getBaseOmega();
+                
+                float posX = pos[0];
+                float posY = pos[1];
+                float posZ = pos[2];
+                float oriX = ori.x();
+                float oriY = ori.y();
+                float oriZ = ori.z();
+                float oriW = ori.w();
+                float velX = vel[0];
+                float velY = vel[1];
+                float velZ = vel[2];
+                float omegaX = omega[0];
+                float omegaY = omega[1];
+                float omegaZ = omega[2];
+                
+                logData.m_values.push_back(objectUniqueId);
+                logData.m_values.push_back(posX);
+                logData.m_values.push_back(posY);
+                logData.m_values.push_back(posZ);
+                logData.m_values.push_back(oriX);
+                logData.m_values.push_back(oriY);
+                logData.m_values.push_back(oriZ);
+                logData.m_values.push_back(oriW);
+                logData.m_values.push_back(velX);
+                logData.m_values.push_back(velY);
+                logData.m_values.push_back(velZ);
+                logData.m_values.push_back(omegaX);
+                logData.m_values.push_back(omegaY);
+                logData.m_values.push_back(omegaZ);
+                
+                int numDofs = mb->getNumDofs();
+                logData.m_values.push_back(numDofs);
+                
+                for (int j = 0; j < 12; ++j)
+                {
+                    if (j < numDofs)
+                    {
+                        float q = mb->getJointPos(j);
+                        logData.m_values.push_back(q);
+                    }
+                    else
+                    {
+                        float q = 0.0;
+                        logData.m_values.push_back(q);
+                    }
+                }
+                
+                for (int j = 0; j < 12; ++j)
+                {
+                    if (j < numDofs)
+                    {
+                        float u = mb->getJointVel(j);
+                        logData.m_values.push_back(u);
+                    }
+                    else
+                    {
+                        float u = 0.0;
+                        logData.m_values.push_back(u);
+                    }
+                }
+                
+                //at the moment, appendMinitaurLogData will directly write to disk (potential delay)
+                //better to fill a huge memory buffer and once in a while write it to disk
+                appendMinitaurLogData(m_logFileHandle, m_structTypes, logData);
+                fflush(m_logFileHandle);
+            }
+            
+            m_loggingTimeStamp++;
+        }
+    }
+};
 
 struct PhysicsServerCommandProcessorInternalData
 {
@@ -1683,6 +1848,26 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 								}
 							}
 						}
+                        if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_GENERIC_ROBOT)
+                        {
+                            std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName;
+                            
+                            int loggerUid = m_data->m_stateLoggersUniqueId++;
+                            GenericRobotStateLogger* logger = new GenericRobotStateLogger(loggerUid,fileName,m_data->m_dynamicsWorld);
+                            
+                            if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID) && (clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds>0))
+                            {
+                                logger->m_filterObjectUniqueId = true;
+                                for (int i = 0; i < clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds; ++i)
+                                {
+                                    logger->m_bodyIdList.push_back(clientCmd.m_stateLoggingArguments.m_bodyUniqueIds[i]);
+                                }
+                            }
+                            
+                            m_data->m_stateLoggers.push_back(logger);
+                            serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED;
+                            serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid;
+                        }
 					}
 					if ((clientCmd.m_updateFlags & STATE_LOGGING_STOP_LOG) && clientCmd.m_stateLoggingArguments.m_loggingUniqueId>=0)
 					{

examples/pybullet/kuka_with_cube.py

@@ -0,0 +1,92 @@
+import pybullet as p
+import time
+import math
+from datetime import datetime
+
+#clid = p.connect(p.SHARED_MEMORY)
+p.connect(p.GUI)
+p.loadURDF("plane.urdf",[0,0,-0.3],useFixedBase=True)
+kukaId = p.loadURDF("kuka_iiwa/model.urdf",[0,0,0],useFixedBase=True)
+p.resetBasePositionAndOrientation(kukaId,[0,0,0],[0,0,0,1])
+kukaEndEffectorIndex = 6
+numJoints = p.getNumJoints(kukaId)
+if (numJoints!=7):
+	exit()
+
+p.loadURDF("cube.urdf",[2,2,5])
+p.loadURDF("cube.urdf",[-2,-2,5])
+p.loadURDF("cube.urdf",[2,-2,5])
+
+#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
+
+count = 0
+useOrientation = 1
+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
+
+logId = p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT,"LOG0001.txt",[0,1,2])
+	
+while 1:
+	if (useRealTimeSimulation):
+		dt = datetime.now()
+		t = (dt.second/60.)*2.*math.pi
+	else:
+		t=t+0.1
+	
+	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)
+			else:
+				jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos)
+	
+		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

examples/pybullet/kuka_with_cube_playback.py

@@ -0,0 +1,77 @@
+import pybullet as p
+import time
+import math
+from datetime import datetime
+from numpy import *
+from pylab import *
+import struct
+import sys
+import os, fnmatch
+import argparse
+from time import sleep
+
+def readLogFile(filename, verbose = True):
+  f = open(filename, 'rb')
+  
+  print('Opened'),
+  print(filename)
+
+  keys = f.readline().rstrip('\n').split(',')
+  fmt = f.readline().rstrip('\n')
+  
+  # The byte number of one record
+  sz = struct.calcsize(fmt)
+  # The type number of one record
+  ncols = len(fmt)
+
+  if verbose:
+    print('Keys:'),
+    print(keys)
+    print('Format:'),
+    print(fmt)
+    print('Size:'),
+    print(sz)
+    print('Columns:'),
+    print(ncols)
+
+  # Read data
+  wholeFile = f.read()
+  # split by alignment word
+  chunks = wholeFile.split('\xaa\xbb')
+  log = list()
+  for chunk in chunks:
+    if len(chunk) == sz:
+      values = struct.unpack(fmt, chunk)
+      record = list()
+      for i in range(ncols):
+        record.append(values[i])
+      log.append(record)
+
+  return log
+
+#clid = p.connect(p.SHARED_MEMORY)
+p.connect(p.GUI)
+p.loadURDF("plane.urdf",[0,0,-0.3])
+p.loadURDF("kuka_iiwa/model.urdf",[0,0,0])
+p.loadURDF("cube.urdf",[2,2,5])
+p.loadURDF("cube.urdf",[-2,-2,5])
+p.loadURDF("cube.urdf",[2,-2,5])
+
+log = readLogFile("LOG0001.txt")
+
+recordNum = len(log)
+itemNum = len(log[0])
+print('record num:'),
+print(recordNum)
+print('item num:'),
+print(itemNum)
+
+for record in log:
+    Id = record[1]
+    pos = [record[2],record[3],record[4]]
+    orn = [record[5],record[6],record[7],record[8]]
+    p.resetBasePositionAndOrientation(Id,pos,orn)
+    numJoints = record[15]
+    for i in range (numJoints):
+    	p.resetJointState(Id,i,record[i+16])
+    sleep(0.0005)

examples/pybullet/minitaur.py

@@ -1,14 +1,12 @@
 import pybullet as p
+import numpy as np
 
 class Minitaur:
-  def __init__(self):
+  def __init__(self, urdfRootPath=''):
+    self.urdfRootPath = urdfRootPath
     self.reset()
 
-  def reset(self):
+  def buildJointNameToIdDict(self):
-    self.quadruped = p.loadURDF("quadruped/quadruped.urdf",0,0,.3)
-    self.kp = 1
-    self.kd = 0.1
-    self.maxForce = 100
     nJoints = p.getNumJoints(self.quadruped)
     self.jointNameToId = {}
     for i in range(nJoints):
@@ -18,13 +16,39 @@ class Minitaur:
     for i in range(100):
       p.stepSimulation()
 
+  def buildMotorIdList(self):
+    self.motorIdList.append(self.jointNameToId['motor_front_leftR_joint'])
+    self.motorIdList.append(self.jointNameToId['motor_front_leftL_joint'])
+    self.motorIdList.append(self.jointNameToId['motor_back_leftR_joint'])
+    self.motorIdList.append(self.jointNameToId['motor_back_leftL_joint'])
+    self.motorIdList.append(self.jointNameToId['motor_front_rightL_joint'])
+    self.motorIdList.append(self.jointNameToId['motor_front_rightR_joint'])
+    self.motorIdList.append(self.jointNameToId['motor_back_rightL_joint'])
+    self.motorIdList.append(self.jointNameToId['motor_back_rightR_joint'])
+
+
+  def reset(self):
+    self.quadruped = p.loadURDF("%s/quadruped/quadruped.urdf" % self.urdfRootPath,0,0,.3)
+    self.kp = 1
+    self.kd = 0.1
+    self.maxForce = 3.5
+    self.nMotors = 8
+    self.motorIdList = []
+    self.motorDir = [1, -1, 1, -1, -1, 1, -1, 1]
+    self.buildJointNameToIdDict()
+    self.buildMotorIdList()
+
+
   def disableAllMotors(self):
     nJoints = p.getNumJoints(self.quadruped)
     for i in range(nJoints):
       p.setJointMotorControl2(bodyIndex=self.quadruped, jointIndex=i, controlMode=p.VELOCITY_CONTROL, force=0)
 
+  def setMotorAngleById(self, motorId, desiredAngle):
+    p.setJointMotorControl2(bodyIndex=self.quadruped, jointIndex=motorId, controlMode=p.POSITION_CONTROL, targetPosition=desiredAngle, positionGain=self.kp, velocityGain=self.kd, force=self.maxForce)
+
   def setMotorAngleByName(self, motorName, desiredAngle):
-    p.setJointMotorControl2(bodyIndex=self.quadruped, jointIndex=self.jointNameToId[motorName], controlMode=p.POSITION_CONTROL, targetPosition=desiredAngle, positionGain=self.kp, velocityGain=self.kd, force=self.maxForce)
+    self.setMotorAngleById(self.jointNameToId[motorName], desiredAngle)
 
   def resetPose(self):
     #right front leg
@@ -73,11 +97,30 @@ class Minitaur:
     return orientation
 
   def applyAction(self, motorCommands):
-    self.setMotorAngleByName('motor_front_rightR_joint', motorCommands[0])
+    motorCommandsWithDir = np.multiply(motorCommands, self.motorDir)
-    self.setMotorAngleByName('motor_front_rightL_joint', motorCommands[1])
+    for i in range(self.nMotors):
-    self.setMotorAngleByName('motor_front_leftR_joint', motorCommands[2])
+      self.setMotorAngleById(self.motorIdList[i], motorCommandsWithDir[i])
-    self.setMotorAngleByName('motor_front_leftL_joint', motorCommands[3])
+
-    self.setMotorAngleByName('motor_back_rightR_joint', motorCommands[4])
+  def getMotorAngles(self):
-    self.setMotorAngleByName('motor_back_rightL_joint', motorCommands[5])
+    motorAngles = []
-    self.setMotorAngleByName('motor_back_leftR_joint', motorCommands[6])
+    for i in range(self.nMotors):
-    self.setMotorAngleByName('motor_back_leftL_joint', motorCommands[7])
+      jointState = p.getJointState(self.quadruped, self.motorIdList[i])
+      motorAngles.append(jointState[0])
+    motorAngles = np.multiply(motorAngles, self.motorDir)
+    return motorAngles
+
+  def getMotorVelocities(self):
+    motorVelocities = []
+    for i in range(self.nMotors):
+      jointState = p.getJointState(self.quadruped, self.motorIdList[i])
+      motorVelocities.append(jointState[1])
+    motorVelocities = np.multiply(motorVelocities, self.motorDir)
+    return motorVelocities
+
+  def getMotorTorques(self):
+    motorTorques = []
+    for i in range(self.nMotors):
+      jointState = p.getJointState(self.quadruped, self.motorIdList[i])
+      motorTorques.append(jointState[3])
+    motorTorques = np.multiply(motorTorques, self.motorDir)
+    return motorTorques

examples/pybullet/minitaur_evaluate.py

@@ -0,0 +1,99 @@
+from minitaur import Minitaur
+import pybullet as p
+import numpy as np
+import time
+import sys
+import math
+
+minitaur = None
+
+evaluate_func_map = dict()
+
+
+def current_position():
+  global minitaur
+  position = minitaur.getBasePosition()
+  return np.asarray(position)
+
+def is_fallen():
+  global minitaur
+  orientation = minitaur.getBaseOrientation()
+  rotMat = p.getMatrixFromQuaterion(orientation)
+  localUp = rotMat[6:]
+  return np.dot(np.asarray([0, 0, 1]), np.asarray(localUp)) < 0
+
+def evaluate_desired_motorAngle_8Amplitude8Phase(i, params):
+  nMotors = 8
+  speed = 0.35
+  for jthMotor in range(nMotors):
+    joint_values[jthMotor] = math.sin(i*speed + params[nMotors + jthMotor])*params[jthMotor]*+1.57
+  return joint_values
+
+def evaluate_desired_motorAngle_2Amplitude4Phase(i, params):
+  speed = 0.35
+  phaseDiff = params[2]
+  a0 = math.sin(i * speed) * params[0] + 1.57
+  a1 = math.sin(i * speed + phaseDiff) * params[1] + 1.57
+  a2 = math.sin(i * speed + params[3]) * params[0] + 1.57
+  a3 = math.sin(i * speed + params[3] + phaseDiff) * params[1] + 1.57
+  a4 = math.sin(i * speed + params[4] + phaseDiff) * params[1] + 1.57
+  a5 = math.sin(i * speed + params[4]) * params[0] + 1.57
+  a6 = math.sin(i * speed + params[5] + phaseDiff) * params[1] + 1.57
+  a7 = math.sin(i * speed + params[5]) * params[0] + 1.57
+  joint_values = [a0, a1, a2, a3, a4, a5, a6, a7]
+  return joint_values
+
+def evaluate_desired_motorAngle_hop(i, params):
+  amplitude = params[0]
+  speed = params[1]
+  a1 = math.sin(i*speed)*amplitude+1.57
+  a2 = math.sin(i*speed+3.14)*amplitude+1.57
+  joint_values = [a1, 1.57, a2, 1.57, 1.57, a1, 1.57, a2]
+  return joint_values
+
+
+evaluate_func_map['evaluate_desired_motorAngle_8Amplitude8Phase'] = evaluate_desired_motorAngle_8Amplitude8Phase
+evaluate_func_map['evaluate_desired_motorAngle_2Amplitude4Phase'] = evaluate_desired_motorAngle_2Amplitude4Phase
+evaluate_func_map['evaluate_desired_motorAngle_hop'] = evaluate_desired_motorAngle_hop
+
+
+
+def evaluate_params(evaluateFunc, params, objectiveParams, urdfRoot='', timeStep=0.01, maxNumSteps=1000, sleepTime=0):
+  print('start evaluation')
+  beforeTime = time.time()
+  p.resetSimulation()
+
+  p.setTimeStep(timeStep)
+  p.loadURDF("%s/plane.urdf" % urdfRoot)
+  p.setGravity(0,0,-10)
+
+  global minitaur
+  minitaur = Minitaur(urdfRoot)
+  start_position = current_position()
+  last_position = None  # for tracing line
+  total_energy = 0
+
+  for i in range(maxNumSteps):
+    torques = minitaur.getMotorTorques()
+    velocities = minitaur.getMotorVelocities()
+    total_energy += np.dot(np.fabs(torques), np.fabs(velocities)) * timeStep
+
+    joint_values = evaluate_func_map[evaluateFunc](i, params)
+    minitaur.applyAction(joint_values)
+    p.stepSimulation()
+    if (is_fallen()):
+      break
+
+    if i % 100 == 0:
+      sys.stdout.write('.')
+      sys.stdout.flush()
+    time.sleep(sleepTime)
+
+  print(' ')
+
+  alpha = objectiveParams[0]
+  final_distance = np.linalg.norm(start_position - current_position())
+  finalReturn = final_distance - alpha * total_energy
+  elapsedTime = time.time() - beforeTime
+  print ("trial for ", params, " final_distance", final_distance, "total_energy", total_energy, "finalReturn", finalReturn, "elapsed_time", elapsedTime)
+  return finalReturn

examples/pybullet/minitaur_test.py

@@ -1,6 +1,7 @@
 import pybullet as p
-
 from minitaur import Minitaur
+from minitaur_evaluate import *
+
 import time
 import math
 import numpy as np
@@ -10,24 +11,13 @@ def main(unused_args):
   c = p.connect(p.SHARED_MEMORY)
   if (c<0):
       c = p.connect(p.GUI)
-  p.resetSimulation()
-  p.setTimeStep(timeStep)
-  p.loadURDF("plane.urdf")
-  p.setGravity(0,0,-10)
 
-  minitaur = Minitaur()
+  params = [0.1903581461951056, 0.0006732219568880068, 0.05018085615283363, 3.219916795483583, 6.2406418167980595, 4.189869754607539]
-  amplitude = 0.24795664427
+  evaluate_func = 'evaluate_desired_motorAngle_2Amplitude4Phase'
-  speed = 0.2860877729434
+  energy_weight = 0.01
-  for i in range(1000):
-    a1 = math.sin(i*speed)*amplitude+1.57
-    a2 = math.sin(i*speed+3.14)*amplitude+1.57
-    joint_values = [a1, -1.57, a1, -1.57, a2, -1.57, a2, -1.57]
-    minitaur.applyAction(joint_values)
 
-    p.stepSimulation()
+  finalReturn = evaluate_params(evaluateFunc = evaluate_func, params=params, objectiveParams=[energy_weight], timeStep=timeStep, sleepTime=timeStep)
-#    print(minitaur.getBasePosition())
+
-    time.sleep(timeStep)
+  print(finalReturn)
-  final_distance = np.linalg.norm(np.asarray(minitaur.getBasePosition()))
-  print(final_distance)
 
 main(0)

src/Bullet3Common/b3AlignedObjectArray.h

@@ -483,6 +483,22 @@ protected:
 		}
 		return index;
 	}
+    
+    int	findLinearSearch2(const T& key) const
+    {
+        int index=-1;
+        int i;
+        
+        for (i=0;i<size();i++)
+        {
+            if (m_data[i] == key)
+            {
+                index = i;
+                break;
+            }
+        }
+        return index;
+    }
 
 	void	remove(const T& key)
 	{

src/LinearMath/btAlignedObjectArray.h

@@ -475,6 +475,24 @@ protected:
 		}
 		return index;
 	}
+    
+    // If the key is not in the array, return -1 instead of 0,
+    // since 0 also means the first element in the array.
+    int	findLinearSearch2(const T& key) const
+    {
+        int index=-1;
+        int i;
+        
+        for (i=0;i<size();i++)
+        {
+            if (m_data[i] == key)
+            {
+                index = i;
+                break;
+            }
+        }
+        return index;
+    }
 
     void removeAtIndex(int index)
     {