allow user to specify the maximum number of dofs to log in GenericRobotStateLogger (default = 12)
add minitaur quadruped playback of minitaur log files (real robot and simulated create the same log files) add improved minitaur.urdf file, see https://youtu.be/lv7lybtOzeo for a preview.
This commit is contained in:
erwincoumans
@@ -512,7 +512,8 @@ struct MinitaurStateLogger : public InternalStateLogger
|
||||
|
||||
MinitaurLogRecord logData;
|
||||
//'t', 'r', 'p', 'y', 'q0', 'q1', 'q2', 'q3', 'q4', 'q5', 'q6', 'q7', 'u0', 'u1', 'u2', 'u3', 'u4', 'u5', 'u6', 'u7', 'xd', 'mo'
|
||||
btScalar motorDir[8] = {1, -1, 1, -1, -1, 1, -1, 1};
|
||||
btScalar motorDir[8] = {1, 1, 1, 1, 1, 1, 1, 1};
|
||||
|
||||
|
||||
btQuaternion orn = m_minitaurMultiBody->getBaseWorldTransform().getRotation();
|
||||
btMatrix3x3 mat(orn);
|
||||
@@ -568,12 +569,14 @@ struct GenericRobotStateLogger : public InternalStateLogger
|
||||
btMultiBodyDynamicsWorld* m_dynamicsWorld;
|
||||
btAlignedObjectArray<int> m_bodyIdList;
|
||||
bool m_filterObjectUniqueId;
|
||||
|
||||
GenericRobotStateLogger(int loggingUniqueId, const std::string& fileName, btMultiBodyDynamicsWorld* dynamicsWorld)
|
||||
int m_maxLogDof;
|
||||
|
||||
GenericRobotStateLogger(int loggingUniqueId, const std::string& fileName, btMultiBodyDynamicsWorld* dynamicsWorld, int maxLogDof)
|
||||
:m_loggingTimeStamp(0),
|
||||
m_logFileHandle(0),
|
||||
m_dynamicsWorld(dynamicsWorld),
|
||||
m_filterObjectUniqueId(false)
|
||||
m_filterObjectUniqueId(false),
|
||||
m_maxLogDof(maxLogDof)
|
||||
{
|
||||
m_loggingType = STATE_LOGGING_GENERIC_ROBOT;
|
||||
|
||||
@@ -595,32 +598,24 @@ struct GenericRobotStateLogger : public InternalStateLogger
|
||||
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 = "IfIfffffffffffffI";
|
||||
|
||||
for (int i=0;i<m_maxLogDof;i++)
|
||||
{
|
||||
m_structTypes.append("f");
|
||||
char jointName[256];
|
||||
sprintf(jointName,"q%d",i);
|
||||
structNames.push_back(jointName);
|
||||
}
|
||||
for (int i=0;i<m_maxLogDof;i++)
|
||||
{
|
||||
m_structTypes.append("f");
|
||||
char jointName[256];
|
||||
sprintf(jointName,"u%d",i);
|
||||
structNames.push_back(jointName);
|
||||
}
|
||||
|
||||
m_structTypes = "IfIfffffffffffffIffffffffffffffffffffffff";
|
||||
const char* fileNameC = fileName.c_str();
|
||||
|
||||
m_logFileHandle = createMinitaurLogFile(fileNameC, structNames, m_structTypes);
|
||||
@@ -699,7 +694,7 @@ struct GenericRobotStateLogger : public InternalStateLogger
|
||||
logData.m_values.push_back(q);
|
||||
}
|
||||
}
|
||||
for (int j = numDofs; j < 12; ++j)
|
||||
for (int j = numDofs; j < m_maxLogDof; ++j)
|
||||
{
|
||||
float q = 0.0;
|
||||
logData.m_values.push_back(q);
|
||||
@@ -713,7 +708,7 @@ struct GenericRobotStateLogger : public InternalStateLogger
|
||||
logData.m_values.push_back(u);
|
||||
}
|
||||
}
|
||||
for (int j = numDofs; j < 12; ++j)
|
||||
for (int j = numDofs; j < m_maxLogDof; ++j)
|
||||
{
|
||||
float u = 0.0;
|
||||
logData.m_values.push_back(u);
|
||||
@@ -1865,7 +1860,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
{
|
||||
maxLogDof = clientCmd.m_stateLoggingArguments.m_maxLogDof;
|
||||
}
|
||||
GenericRobotStateLogger* logger = new GenericRobotStateLogger(loggerUid,fileName,m_data->m_dynamicsWorld);
|
||||
GenericRobotStateLogger* logger = new GenericRobotStateLogger(loggerUid,fileName,m_data->m_dynamicsWorld,maxLogDof);
|
||||
|
||||
if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID) && (clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds>0))
|
||||
{
|
||||
|
||||
@@ -2,32 +2,35 @@ import pybullet as p
|
||||
import time
|
||||
import math
|
||||
|
||||
useRealTime = 1
|
||||
useRealTime = 0
|
||||
fixedTimeStep = 0.001
|
||||
speed = 10
|
||||
amplitude = 1.3
|
||||
amplitude = 0.8
|
||||
jump_amp = 0.5
|
||||
maxForce = 3.5
|
||||
kp = .6
|
||||
kd = 1
|
||||
kneeFrictionForce = 0.00
|
||||
kp = .05
|
||||
kd = .5
|
||||
|
||||
|
||||
physId = p.connect(p.SHARED_MEMORY)
|
||||
if (physId<0):
|
||||
p.connect(p.GUI)
|
||||
|
||||
p.loadURDF("plane.urdf")
|
||||
p.setGravity(0,0,-10)
|
||||
#p.resetSimulation()
|
||||
p.loadURDF("plane.urdf",0,0,0.1)
|
||||
#p.loadSDF("kitchens/1.sdf")
|
||||
p.setGravity(0,0,0)
|
||||
p.setTimeStep(fixedTimeStep)
|
||||
|
||||
p.setRealTimeSimulation(1)
|
||||
quadruped = p.loadURDF("quadruped/quadruped.urdf",1,-2,0.3)
|
||||
p.setRealTimeSimulation(0)
|
||||
quadruped = p.loadURDF("quadruped/minitaur.urdf",[0,0,0.2],useFixedBase=False)
|
||||
nJoints = p.getNumJoints(quadruped)
|
||||
|
||||
jointNameToId = {}
|
||||
for i in range(nJoints):
|
||||
jointInfo = p.getJointInfo(quadruped, i)
|
||||
jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
|
||||
jointInfo = p.getJointInfo(quadruped, i)
|
||||
jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
|
||||
|
||||
|
||||
motor_front_rightR_joint = jointNameToId['motor_front_rightR_joint']
|
||||
hip_front_rightR_link = jointNameToId['hip_front_rightR_link']
|
||||
@@ -56,93 +59,70 @@ knee_back_leftL_link = jointNameToId['knee_back_leftL_link']
|
||||
|
||||
|
||||
#p.getNumJoints(1)
|
||||
#right front leg
|
||||
p.resetJointState(quadruped,0,1.57)
|
||||
p.resetJointState(quadruped,2,-2.2)
|
||||
p.resetJointState(quadruped,3,-1.57)
|
||||
p.resetJointState(quadruped,5,2.2)
|
||||
p.createConstraint(quadruped,2,quadruped,5,p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2])
|
||||
p.resetJointState(quadruped,motor_front_rightR_joint,1.57)
|
||||
p.resetJointState(quadruped,knee_front_rightR_link,-2.2)
|
||||
p.resetJointState(quadruped,motor_front_rightL_joint,1.57)
|
||||
p.resetJointState(quadruped,knee_front_rightL_link,-2.2)
|
||||
p.createConstraint(quadruped,knee_front_rightR_link,quadruped,knee_front_rightL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
|
||||
p.setJointMotorControl(quadruped,knee_front_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
|
||||
p.setJointMotorControl(quadruped,knee_front_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
|
||||
|
||||
p.setJointMotorControl(quadruped,0,p.POSITION_CONTROL,1.57,1)
|
||||
p.setJointMotorControl(quadruped,1,p.VELOCITY_CONTROL,0,0)
|
||||
p.setJointMotorControl(quadruped,2,p.VELOCITY_CONTROL,0,0)
|
||||
p.setJointMotorControl(quadruped,3,p.POSITION_CONTROL,-1.57,1)
|
||||
p.setJointMotorControl(quadruped,4,p.VELOCITY_CONTROL,0,0)
|
||||
p.setJointMotorControl(quadruped,5,p.VELOCITY_CONTROL,0,0)
|
||||
p.resetJointState(quadruped,motor_front_leftR_joint,-1.57)
|
||||
p.resetJointState(quadruped,knee_front_leftR_link,2.2)
|
||||
p.resetJointState(quadruped,motor_front_leftL_joint,-1.57)
|
||||
p.resetJointState(quadruped,knee_front_leftL_link,2.2)
|
||||
p.createConstraint(quadruped,knee_front_leftR_link,quadruped,knee_front_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
|
||||
p.setJointMotorControl(quadruped,knee_front_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
|
||||
p.setJointMotorControl(quadruped,knee_front_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
|
||||
|
||||
#left front leg
|
||||
p.resetJointState(quadruped,6,1.57)
|
||||
p.resetJointState(quadruped,8,-2.2)
|
||||
p.resetJointState(quadruped,9,-1.57)
|
||||
p.resetJointState(quadruped,11,2.2)
|
||||
p.createConstraint(quadruped,8,quadruped,11,p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2])
|
||||
p.resetJointState(quadruped,motor_back_rightR_joint,1.57)
|
||||
p.resetJointState(quadruped,knee_back_rightR_link,-2.2)
|
||||
p.resetJointState(quadruped,motor_back_rightL_joint,1.57)
|
||||
p.resetJointState(quadruped,knee_back_rightL_link,-2.2)
|
||||
p.createConstraint(quadruped,knee_back_rightR_link,quadruped,knee_back_rightL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
|
||||
p.setJointMotorControl(quadruped,knee_back_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
|
||||
p.setJointMotorControl(quadruped,knee_back_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
|
||||
|
||||
p.setJointMotorControl(quadruped,6,p.POSITION_CONTROL,1.57,1)
|
||||
p.setJointMotorControl(quadruped,7,p.VELOCITY_CONTROL,0,0)
|
||||
p.setJointMotorControl(quadruped,8,p.VELOCITY_CONTROL,0,0)
|
||||
p.setJointMotorControl(quadruped,9,p.POSITION_CONTROL,-1.57,1)
|
||||
p.setJointMotorControl(quadruped,10,p.VELOCITY_CONTROL,0,0)
|
||||
p.setJointMotorControl(quadruped,11,p.VELOCITY_CONTROL,0,0)
|
||||
|
||||
|
||||
#right back leg
|
||||
p.resetJointState(quadruped,12,1.57)
|
||||
p.resetJointState(quadruped,14,-2.2)
|
||||
p.resetJointState(quadruped,15,-1.57)
|
||||
p.resetJointState(quadruped,17,2.2)
|
||||
p.createConstraint(quadruped,14,quadruped,17,p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2])
|
||||
|
||||
p.setJointMotorControl(quadruped,12,p.POSITION_CONTROL,1.57,1)
|
||||
p.setJointMotorControl(quadruped,13,p.VELOCITY_CONTROL,0,0)
|
||||
p.setJointMotorControl(quadruped,14,p.VELOCITY_CONTROL,0,0)
|
||||
p.setJointMotorControl(quadruped,15,p.POSITION_CONTROL,-1.57,1)
|
||||
p.setJointMotorControl(quadruped,16,p.VELOCITY_CONTROL,0,0)
|
||||
p.setJointMotorControl(quadruped,17,p.VELOCITY_CONTROL,0,0)
|
||||
|
||||
#left back leg
|
||||
p.resetJointState(quadruped,18,1.57)
|
||||
p.resetJointState(quadruped,20,-2.2)
|
||||
p.resetJointState(quadruped,21,-1.57)
|
||||
p.resetJointState(quadruped,23,2.2)
|
||||
p.createConstraint(quadruped,20,quadruped,23,p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2])
|
||||
|
||||
p.setJointMotorControl(quadruped,18,p.POSITION_CONTROL,1.57,1)
|
||||
p.setJointMotorControl(quadruped,19,p.VELOCITY_CONTROL,0,0)
|
||||
p.setJointMotorControl(quadruped,20,p.VELOCITY_CONTROL,0,0)
|
||||
p.setJointMotorControl(quadruped,21,p.POSITION_CONTROL,-1.57,1)
|
||||
p.setJointMotorControl(quadruped,22,p.VELOCITY_CONTROL,0,0)
|
||||
p.setJointMotorControl(quadruped,23,p.VELOCITY_CONTROL,0,0)
|
||||
|
||||
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_front_rightR_joint, controlMode=p.POSITION_CONTROL, targetPosition=1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_front_rightL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_front_leftR_joint, controlMode=p.POSITION_CONTROL, targetPosition=1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_front_leftL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_rightR_joint, controlMode=p.POSITION_CONTROL, targetPosition=1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_rightL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_leftR_joint, controlMode=p.POSITION_CONTROL, targetPosition=1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_leftL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.resetJointState(quadruped,motor_back_leftR_joint,-1.57)
|
||||
p.resetJointState(quadruped,knee_back_leftR_link,2.2)
|
||||
p.resetJointState(quadruped,motor_back_leftL_joint,-1.57)
|
||||
p.resetJointState(quadruped,knee_back_leftL_link,2.2)
|
||||
p.createConstraint(quadruped,knee_back_leftR_link,quadruped,knee_back_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
|
||||
p.setJointMotorControl(quadruped,motor_back_leftR_joint,p.POSITION_CONTROL,-1.57,maxForce)
|
||||
p.setJointMotorControl(quadruped,knee_back_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
|
||||
p.setJointMotorControl(quadruped,motor_back_leftL_joint,p.POSITION_CONTROL,-1.57,maxForce)
|
||||
p.setJointMotorControl(quadruped,knee_back_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
|
||||
|
||||
|
||||
|
||||
#stand still
|
||||
p.setRealTimeSimulation(useRealTime)
|
||||
motordir=[-1,-1,-1,-1,1,1,1,1]
|
||||
legnumbering=[motor_front_leftR_joint,motor_front_leftL_joint,motor_back_leftR_joint,motor_back_leftL_joint,motor_front_rightR_joint,motor_front_rightL_joint,motor_back_rightR_joint,motor_back_rightL_joint]
|
||||
|
||||
#use the Minitaur leg numbering
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[0],controlMode=p.POSITION_CONTROL,targetPosition=motordir[0]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[1],controlMode=p.POSITION_CONTROL,targetPosition=motordir[1]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[2],controlMode=p.POSITION_CONTROL,targetPosition=motordir[2]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[3],controlMode=p.POSITION_CONTROL,targetPosition=motordir[3]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[4],controlMode=p.POSITION_CONTROL,targetPosition=motordir[4]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[5],controlMode=p.POSITION_CONTROL,targetPosition=motordir[5]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[6],controlMode=p.POSITION_CONTROL,targetPosition=motordir[6]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[7],controlMode=p.POSITION_CONTROL,targetPosition=motordir[7]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
|
||||
|
||||
t=0.0
|
||||
ref_time = time.time()
|
||||
t_end = t + 4
|
||||
while t < t_end:
|
||||
if (useRealTime==0):
|
||||
t = t+fixedTimeStep
|
||||
p.stepSimulation()
|
||||
else:
|
||||
t = time.time()-ref_time
|
||||
p.setGravity(0,0,-10)
|
||||
p.stepSimulation()
|
||||
|
||||
|
||||
print("quadruped Id = ")
|
||||
print(quadruped)
|
||||
p.saveWorld("quadru.py")
|
||||
logId = p.startStateLogging(p.STATE_LOGGING_MINITAUR,"quadrupedLog.txt",[quadruped])
|
||||
|
||||
p.setGravity(0,0,-10)
|
||||
|
||||
|
||||
|
||||
#stand still
|
||||
p.setRealTimeSimulation(1)
|
||||
|
||||
#jump
|
||||
t = 0.0
|
||||
t_end = t + 10
|
||||
@@ -163,49 +143,17 @@ while t < t_end:
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_rightL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-math.sin(t*speed)*jump_amp-1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_leftR_joint, controlMode=p.POSITION_CONTROL, targetPosition=math.sin(t*speed)*jump_amp+1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_leftL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-math.sin(t*speed)*jump_amp-1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
|
||||
target = math.sin(t*speed)*jump_amp+1.57;
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[0],controlMode=p.POSITION_CONTROL,targetPosition=motordir[0]*target,positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[1],controlMode=p.POSITION_CONTROL,targetPosition=motordir[1]*target,positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[2],controlMode=p.POSITION_CONTROL,targetPosition=motordir[2]*target,positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[3],controlMode=p.POSITION_CONTROL,targetPosition=motordir[3]*target,positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[4],controlMode=p.POSITION_CONTROL,targetPosition=motordir[4]*target,positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[5],controlMode=p.POSITION_CONTROL,targetPosition=motordir[5]*target,positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[6],controlMode=p.POSITION_CONTROL,targetPosition=motordir[6]*target,positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[7],controlMode=p.POSITION_CONTROL,targetPosition=motordir[7]*target,positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
|
||||
if (useRealTime==0):
|
||||
p.stepSimulation()
|
||||
|
||||
#hop forward
|
||||
t_end = 20
|
||||
i=0
|
||||
while t < t_end:
|
||||
if (useRealTime):
|
||||
t = time.time()-ref_time
|
||||
else:
|
||||
t = t+fixedTimeStep
|
||||
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_front_rightR_joint, controlMode=p.POSITION_CONTROL, targetPosition=math.sin(t*speed)*amplitude+1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_front_rightL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_front_leftR_joint, controlMode=p.POSITION_CONTROL, targetPosition=math.sin(t*speed)*amplitude+1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_front_leftL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_rightR_joint, controlMode=p.POSITION_CONTROL, targetPosition=math.sin(t*speed+3.14)*amplitude+1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_rightL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_leftR_joint, controlMode=p.POSITION_CONTROL, targetPosition=math.sin(t*speed+3.14)*amplitude+1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_leftL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
|
||||
if (useRealTime==0):
|
||||
p.stepSimulation()
|
||||
|
||||
#walk
|
||||
t_end = 100
|
||||
i=0
|
||||
while t < t_end:
|
||||
if (useRealTime):
|
||||
t = time.time()-ref_time
|
||||
else:
|
||||
t = t+fixedTimeStep
|
||||
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_front_rightR_joint, controlMode=p.POSITION_CONTROL, targetPosition=math.sin(t*3)*.3+1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_front_rightL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_front_leftR_joint, controlMode=p.POSITION_CONTROL, targetPosition=math.sin(t*3+0.5*3.14)*.3+1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_front_leftL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_rightR_joint, controlMode=p.POSITION_CONTROL, targetPosition=math.sin(t*3+3.14)*.3+1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_rightL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_leftR_joint, controlMode=p.POSITION_CONTROL, targetPosition=math.sin(t*3+1.5*3.14)*.3+1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_leftL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
|
||||
|
||||
p.stepSimulation()
|
||||
p.setRealTimeSimulation(1)
|
||||
|
||||
126
examples/pybullet/quadruped_playback.py
Normal file
126
examples/pybullet/quadruped_playback.py
Normal file
@@ -0,0 +1,126 @@
|
||||
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().decode('utf8').rstrip('\n').split(',')
|
||||
fmt = f.readline().decode('utf8').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(b'\xaa\xbb')
|
||||
print ("num chunks")
|
||||
print (len(chunks))
|
||||
|
||||
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)
|
||||
|
||||
log = readLogFile("LOG00076.TXT");
|
||||
|
||||
recordNum = len(log)
|
||||
print('record num:'),
|
||||
print(recordNum)
|
||||
itemNum = len(log[0])
|
||||
print('item num:'),
|
||||
print(itemNum)
|
||||
|
||||
useRealTime = 0
|
||||
fixedTimeStep = 0.001
|
||||
speed = 10
|
||||
amplitude = 0.8
|
||||
jump_amp = 0.5
|
||||
maxForce = 3.5
|
||||
kp = .05
|
||||
kd = .5
|
||||
|
||||
|
||||
|
||||
quadruped = 1
|
||||
nJoints = p.getNumJoints(quadruped)
|
||||
jointNameToId = {}
|
||||
for i in range(nJoints):
|
||||
jointInfo = p.getJointInfo(quadruped, i)
|
||||
jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
|
||||
|
||||
motor_front_rightR_joint = jointNameToId['motor_front_rightR_joint']
|
||||
hip_front_rightR_link = jointNameToId['hip_front_rightR_link']
|
||||
knee_front_rightR_link = jointNameToId['knee_front_rightR_link']
|
||||
motor_front_rightL_joint = jointNameToId['motor_front_rightL_joint']
|
||||
motor_front_rightL_link = jointNameToId['motor_front_rightL_link']
|
||||
knee_front_rightL_link = jointNameToId['knee_front_rightL_link']
|
||||
motor_front_leftR_joint = jointNameToId['motor_front_leftR_joint']
|
||||
hip_front_leftR_link = jointNameToId['hip_front_leftR_link']
|
||||
knee_front_leftR_link = jointNameToId['knee_front_leftR_link']
|
||||
motor_front_leftL_joint = jointNameToId['motor_front_leftL_joint']
|
||||
motor_front_leftL_link = jointNameToId['motor_front_leftL_link']
|
||||
knee_front_leftL_link = jointNameToId['knee_front_leftL_link']
|
||||
motor_back_rightR_joint = jointNameToId['motor_back_rightR_joint']
|
||||
hip_rightR_link = jointNameToId['hip_rightR_link']
|
||||
knee_back_rightR_link = jointNameToId['knee_back_rightR_link']
|
||||
motor_back_rightL_joint = jointNameToId['motor_back_rightL_joint']
|
||||
motor_back_rightL_link = jointNameToId['motor_back_rightL_link']
|
||||
knee_back_rightL_link = jointNameToId['knee_back_rightL_link']
|
||||
motor_back_leftR_joint = jointNameToId['motor_back_leftR_joint']
|
||||
hip_leftR_link = jointNameToId['hip_leftR_link']
|
||||
knee_back_leftR_link = jointNameToId['knee_back_leftR_link']
|
||||
motor_back_leftL_joint = jointNameToId['motor_back_leftL_joint']
|
||||
motor_back_leftL_link = jointNameToId['motor_back_leftL_link']
|
||||
knee_back_leftL_link = jointNameToId['knee_back_leftL_link']
|
||||
|
||||
motorDir = [1, 1, 1, 1, 1, 1, 1, 1];
|
||||
legnumbering=[motor_front_leftR_joint,motor_front_leftL_joint,motor_back_leftR_joint,motor_back_leftL_joint,motor_front_rightR_joint,motor_front_rightL_joint,motor_back_rightR_joint,motor_back_rightL_joint]
|
||||
|
||||
|
||||
for record in log:
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[0], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[0]*record[7], positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[1], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[1]*record[8], positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[2], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[2]*record[9], positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[3], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[3]*record[10], positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[4], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[4]*record[11], positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[5], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[5]*record[12], positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[6], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[6]*record[13], positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[7], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[7]*record[14], positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setGravity(0.000000,0.000000,-10.000000)
|
||||
p.stepSimulation()
|
||||
p.stepSimulation()
|
||||
sleep(0.01)
|
||||
|
||||
21
examples/pybullet/quadruped_setup_playback.py
Normal file
21
examples/pybullet/quadruped_setup_playback.py
Normal file
@@ -0,0 +1,21 @@
|
||||
import pybullet as p
|
||||
p.connect(p.SHARED_MEMORY)
|
||||
objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,-.300000,0.000000,0.000000,0.000000,1.000000)]
|
||||
objects = [p.loadURDF("quadruped/minitaur.urdf", [-0.000046,-0.000068,0.200774],[-0.000701,0.000387,-0.000252,1.000000],useFixedBase=False)]
|
||||
ob = objects[0]
|
||||
jointPositions=[ 0.000000, 1.531256, 0.000000, -2.240112, 1.527979, 0.000000, -2.240646, 1.533105, 0.000000, -2.238254, 1.530335, 0.000000, -2.238298, 0.000000, -1.528038, 0.000000, 2.242656, -1.525193, 0.000000, 2.244008, -1.530011, 0.000000, 2.240683, -1.528687, 0.000000, 2.240517 ]
|
||||
for ji in range (p.getNumJoints(ob)):
|
||||
p.resetJointState(ob,ji,jointPositions[ji])
|
||||
p.setJointMotorControl2(bodyIndex=ob, jointIndex=ji, controlMode=p.VELOCITY_CONTROL, force=0)
|
||||
|
||||
cid0 = p.createConstraint(1,3,1,6,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.200000],[0.000000,0.010000,0.200000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000])
|
||||
p.changeConstraint(cid0,maxForce=500.000000)
|
||||
cid1 = p.createConstraint(1,16,1,19,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.200000],[0.000000,0.010000,0.200000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000])
|
||||
p.changeConstraint(cid1,maxForce=500.000000)
|
||||
cid2 = p.createConstraint(1,9,1,12,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.200000],[0.000000,0.010000,0.200000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000])
|
||||
p.changeConstraint(cid2,maxForce=500.000000)
|
||||
cid3 = p.createConstraint(1,22,1,25,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.200000],[0.000000,0.010000,0.200000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000])
|
||||
p.changeConstraint(cid3,maxForce=500.000000)
|
||||
p.setGravity(0.000000,0.000000,0.000000)
|
||||
p.stepSimulation()
|
||||
p.disconnect()
|
||||
Reference in New Issue
Block a user