Merge pull request #965 from jietan/pullRequest

Pull request

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/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)