Merge branch 'master' of https://github.com/erwincoumans/bullet3
This commit is contained in:
Erwin Coumans
@@ -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"/>
|
||||
|
||||
@@ -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>
|
||||
|
||||
Binary file not shown.
@@ -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)
|
||||
{
|
||||
|
||||
92
examples/pybullet/kuka_with_cube.py
Normal file
92
examples/pybullet/kuka_with_cube.py
Normal file
@@ -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
|
||||
77
examples/pybullet/kuka_with_cube_playback.py
Normal file
77
examples/pybullet/kuka_with_cube_playback.py
Normal file
@@ -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)
|
||||
@@ -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
|
||||
|
||||
99
examples/pybullet/minitaur_evaluate.py
Normal file
99
examples/pybullet/minitaur_evaluate.py
Normal file
@@ -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
|
||||
@@ -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
|
||||
|
||||
p.stepSimulation()
|
||||
# print(minitaur.getBasePosition())
|
||||
time.sleep(timeStep)
|
||||
final_distance = np.linalg.norm(np.asarray(minitaur.getBasePosition()))
|
||||
print(final_distance)
|
||||
finalReturn = evaluate_params(evaluateFunc = evaluate_func, params=params, objectiveParams=[energy_weight], timeStep=timeStep, sleepTime=timeStep)
|
||||
|
||||
print(finalReturn)
|
||||
|
||||
main(0)
|
||||
|
||||
@@ -484,6 +484,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)
|
||||
{
|
||||
|
||||
|
||||
@@ -476,6 +476,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)
|
||||
{
|
||||
if (index<size())
|
||||
|
||||
Reference in New Issue
Block a user