diff options
author | Erwin Coumans <erwincoumans@google.com> | 2017-02-21 17:38:32 -0800 |
---|---|---|
committer | Erwin Coumans <erwincoumans@google.com> | 2017-02-21 17:38:32 -0800 |
commit | 3988d363b38373d597990255966c2a81b1f8513b (patch) | |
tree | 18eadeb6e7c40721e3d3875b3e59b62a51257d10 /examples | |
parent | 37890e5a4dc597c15ad2769d9e3dfb0e8a26ec2f (diff) | |
parent | 1cd65a324ca820031725264821989a8382f0b873 (diff) | |
download | bullet3-3988d363b38373d597990255966c2a81b1f8513b.tar.gz |
Merge branch 'master' of https://github.com/erwincoumans/bullet3
Diffstat (limited to 'examples')
-rw-r--r-- | examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 187 | ||||
-rw-r--r-- | examples/pybullet/kuka_with_cube.py | 92 | ||||
-rw-r--r-- | examples/pybullet/kuka_with_cube_playback.py | 77 | ||||
-rw-r--r-- | examples/pybullet/minitaur.py | 73 | ||||
-rw-r--r-- | examples/pybullet/minitaur_evaluate.py | 99 | ||||
-rw-r--r-- | examples/pybullet/minitaur_test.py | 26 |
6 files changed, 520 insertions, 34 deletions
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 3a6341e36..1da9d24fe 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/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) { diff --git a/examples/pybullet/kuka_with_cube.py b/examples/pybullet/kuka_with_cube.py new file mode 100644 index 000000000..309449138 --- /dev/null +++ b/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 diff --git a/examples/pybullet/kuka_with_cube_playback.py b/examples/pybullet/kuka_with_cube_playback.py new file mode 100644 index 000000000..5b3aade9b --- /dev/null +++ b/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)
\ No newline at end of file diff --git a/examples/pybullet/minitaur.py b/examples/pybullet/minitaur.py index 39827071c..58aab4a38 100644 --- a/examples/pybullet/minitaur.py +++ b/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): - self.quadruped = p.loadURDF("quadruped/quadruped.urdf",0,0,.3) - self.kp = 1 - self.kd = 0.1 - self.maxForce = 100 + def buildJointNameToIdDict(self): 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]) - self.setMotorAngleByName('motor_front_rightL_joint', motorCommands[1]) - self.setMotorAngleByName('motor_front_leftR_joint', motorCommands[2]) - self.setMotorAngleByName('motor_front_leftL_joint', motorCommands[3]) - self.setMotorAngleByName('motor_back_rightR_joint', motorCommands[4]) - self.setMotorAngleByName('motor_back_rightL_joint', motorCommands[5]) - self.setMotorAngleByName('motor_back_leftR_joint', motorCommands[6]) - self.setMotorAngleByName('motor_back_leftL_joint', motorCommands[7]) + motorCommandsWithDir = np.multiply(motorCommands, self.motorDir) + for i in range(self.nMotors): + self.setMotorAngleById(self.motorIdList[i], motorCommandsWithDir[i]) + + def getMotorAngles(self): + motorAngles = [] + for i in range(self.nMotors): + 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 diff --git a/examples/pybullet/minitaur_evaluate.py b/examples/pybullet/minitaur_evaluate.py new file mode 100644 index 000000000..3c3e65280 --- /dev/null +++ b/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 diff --git a/examples/pybullet/minitaur_test.py b/examples/pybullet/minitaur_test.py index 8f4591086..e98cd8cde 100644 --- a/examples/pybullet/minitaur_test.py +++ b/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() - amplitude = 0.24795664427 - speed = 0.2860877729434 - 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) + params = [0.1903581461951056, 0.0006732219568880068, 0.05018085615283363, 3.219916795483583, 6.2406418167980595, 4.189869754607539] + evaluate_func = 'evaluate_desired_motorAngle_2Amplitude4Phase' + energy_weight = 0.01 + + finalReturn = evaluate_params(evaluateFunc = evaluate_func, params=params, objectiveParams=[energy_weight], timeStep=timeStep, sleepTime=timeStep) - p.stepSimulation() -# print(minitaur.getBasePosition()) - time.sleep(timeStep) - final_distance = np.linalg.norm(np.asarray(minitaur.getBasePosition())) - print(final_distance) + print(finalReturn) main(0) |