def initializeFromBulletBody(self, bodyUid, physicsClientId):
self.initialize()
#always create a base link
baseLink = UrdfLink()
baseLinkIndex = -1
self.convertLinkFromMultiBody(bodyUid, baseLinkIndex, baseLink, physicsClientId)
baseLink.link_name = p.getBodyInfo(bodyUid, physicsClientId=physicsClientId)[0].decode("utf-8")
self.linkNameToIndex[baseLink.link_name]=len(self.urdfLinks)
self.urdfLinks.append(baseLink)
#print(visualShapes)
#optionally create child links and joints
for j in range(p.getNumJoints(bodyUid,physicsClientId=physicsClientId)):
jointInfo = p.getJointInfo(bodyUid,j,physicsClientId=physicsClientId)
urdfLink = UrdfLink()
self.convertLinkFromMultiBody(bodyUid, j, urdfLink,physicsClientId)
urdfLink.link_name = jointInfo[12].decode("utf-8")
self.linkNameToIndex[urdfLink.link_name]=len(self.urdfLinks)
self.urdfLinks.append(urdfLink)
urdfJoint = UrdfJoint()
urdfJoint.link = urdfLink
urdfJoint.joint_name = jointInfo[1].decode("utf-8")
urdfJoint.joint_type = jointInfo[2]
urdfJoint.joint_axis_xyz = jointInfo[13]
orgParentIndex = jointInfo[16]
if (orgParentIndex<0):
urdfJoint.parent_name = baseLink.link_name
else:
parentJointInfo = p.getJointInfo(bodyUid,orgParentIndex,physicsClientId=physicsClientId)
urdfJoint.parent_name = parentJointInfo[12].decode("utf-8")
urdfJoint.child_name = urdfLink.link_name
#todo, compensate for inertia/link frame offset
dynChild = p.getDynamicsInfo(bodyUid,j,physicsClientId=physicsClientId)
childInertiaPos = dynChild[3]
childInertiaOrn = dynChild[4]
parentCom2JointPos=jointInfo[14]
parentCom2JointOrn=jointInfo[15]
tmpPos,tmpOrn = p.multiplyTransforms(childInertiaPos,childInertiaOrn,parentCom2JointPos,parentCom2JointOrn)
tmpPosInv,tmpOrnInv = p.invertTransform(tmpPos,tmpOrn)
dynParent = p.getDynamicsInfo(bodyUid,orgParentIndex,physicsClientId=physicsClientId)
parentInertiaPos = dynParent[3]
parentInertiaOrn = dynParent[4]
pos,orn = p.multiplyTransforms(parentInertiaPos,parentInertiaOrn, tmpPosInv, tmpOrnInv)
pos,orn_unused=p.multiplyTransforms(parentInertiaPos,parentInertiaOrn, parentCom2JointPos,[0,0,0,1])
urdfJoint.joint_origin_xyz = pos
urdfJoint.joint_origin_rpy = p.getEulerFromQuaternion(orn)
self.urdfJoints.append(urdfJoint)
def buildJointNameToIdDict(self):
nJoints = p.getNumJoints(self.quadruped)
self.jointNameToId = {}
for i in range(nJoints):
jointInfo = p.getJointInfo(self.quadruped, i)
self.jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
self.resetPose()
def __init__(self, joint_name, bodies, bodyIndex, jointIndex): self.bodies = bodies self.bodyIndex = bodyIndex self.jointIndex = jointIndex self.joint_name = joint_name _,_,_,_,_,_,_,_,self.lowerLimit, self.upperLimit,_,_,_ = p.getJointInfo(self.bodies[self.bodyIndex], self.jointIndex) self.power_coeff = 0
def reset(self):
objects = p.loadSDF(os.path.join(self.urdfRootPath,"kuka_iiwa/kuka_with_gripper2.sdf"))
self.kukaUid = objects[0]
#for i in range (p.getNumJoints(self.kukaUid)):
# print(p.getJointInfo(self.kukaUid,i))
p.resetBasePositionAndOrientation(self.kukaUid,[-0.100000,0.000000,0.070000],[0.000000,0.000000,0.000000,1.000000])
self.jointPositions=[ 0.006418, 0.413184, -0.011401, -1.589317, 0.005379, 1.137684, -0.006539, 0.000048, -0.299912, 0.000000, -0.000043, 0.299960, 0.000000, -0.000200 ]
self.numJoints = p.getNumJoints(self.kukaUid)
for jointIndex in range (self.numJoints):
p.resetJointState(self.kukaUid,jointIndex,self.jointPositions[jointIndex])
p.setJointMotorControl2(self.kukaUid,jointIndex,p.POSITION_CONTROL,targetPosition=self.jointPositions[jointIndex],force=self.maxForce)
self.trayUid = p.loadURDF(os.path.join(self.urdfRootPath,"tray/tray.urdf"), 0.640000,0.075000,-0.190000,0.000000,0.000000,1.000000,0.000000)
self.endEffectorPos = [0.537,0.0,0.5]
self.endEffectorAngle = 0
self.motorNames = []
self.motorIndices = []
for i in range (self.numJoints):
jointInfo = p.getJointInfo(self.kukaUid,i)
qIndex = jointInfo[3]
if qIndex > -1:
#print("motorname")
#print(jointInfo[1])
self.motorNames.append(str(jointInfo[1]))
self.motorIndices.append(i)
def getMotorJointStates(robot): joint_states = p.getJointStates(robot, range(p.getNumJoints(robot))) joint_infos = [p.getJointInfo(robot, i) for i in range(p.getNumJoints(robot))] joint_states = [j for j, i in zip(joint_states, joint_infos) if i[3] > -1] joint_positions = [state[0] for state in joint_states] joint_velocities = [state[1] for state in joint_states] joint_torques = [state[3] for state in joint_states] return joint_positions, joint_velocities, joint_torques
def multiplyJacobian(robot, jacobian, vector): result = [0.0, 0.0, 0.0] i = 0 for c in range(len(vector)): if p.getJointInfo(robot, c)[3] > -1: for r in range(3): result[r] += jacobian[r][i] * vector[c] i += 1 return result
def addToScene(self, bodies):
if self.parts is not None:
parts = self.parts
else:
parts = {}
if self.jdict is not None:
joints = self.jdict
else:
joints = {}
if self.ordered_joints is not None:
ordered_joints = self.ordered_joints
else:
ordered_joints = []
dump = 0
for i in range(len(bodies)):
if p.getNumJoints(bodies[i]) == 0:
part_name, robot_name = p.getBodyInfo(bodies[i], 0)
robot_name = robot_name.decode("utf8")
part_name = part_name.decode("utf8")
parts[part_name] = BodyPart(part_name, bodies, i, -1)
for j in range(p.getNumJoints(bodies[i])):
p.setJointMotorControl2(bodies[i],j,p.POSITION_CONTROL,positionGain=0.1,velocityGain=0.1,force=0)
_,joint_name,_,_,_,_,_,_,_,_,_,_,part_name = p.getJointInfo(bodies[i], j)
joint_name = joint_name.decode("utf8")
part_name = part_name.decode("utf8")
if dump: print("ROBOT PART '%s'" % part_name)
if dump: print("ROBOT JOINT '%s'" % joint_name) # limits = %+0.2f..%+0.2f effort=%0.3f speed=%0.3f" % ((joint_name,) + j.limits()) )
parts[part_name] = BodyPart(part_name, bodies, i, j)
if part_name == self.robot_name:
self.robot_body = parts[part_name]
if i == 0 and j == 0 and self.robot_body is None: # if nothing else works, we take this as robot_body
parts[self.robot_name] = BodyPart(self.robot_name, bodies, 0, -1)
self.robot_body = parts[self.robot_name]
if joint_name[:6] == "ignore":
Joint(joint_name, bodies, i, j).disable_motor()
continue
if joint_name[:8] != "jointfix":
joints[joint_name] = Joint(joint_name, bodies, i, j)
ordered_joints.append(joints[joint_name])
joints[joint_name].power_coef = 100.0
return parts, joints, ordered_joints, self.robot_body
def reset(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):
jointInfo = p.getJointInfo(self.quadruped, i)
self.jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
self.resetPose()
for i in range(100):
p.stepSimulation()
def Step(stepIndex): for objectId in range(objectNum): record = log[stepIndex*objectNum+objectId] Id = record[2] pos = [record[3],record[4],record[5]] orn = [record[6],record[7],record[8],record[9]] p.resetBasePositionAndOrientation(Id,pos,orn) numJoints = p.getNumJoints(Id) for i in range (numJoints): jointInfo = p.getJointInfo(Id,i) qIndex = jointInfo[3] if qIndex > -1: p.resetJointState(Id,i,record[qIndex-7+17])
def reset(self):
self.initial_z = None
objs = p.loadMJCF(os.path.join(self.urdfRootPath,"mjcf/humanoid_symmetric_no_ground.xml"),flags = p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
self.human = objs[0]
self.jdict = {}
self.ordered_joints = []
self.ordered_joint_indices = []
for j in range( p.getNumJoints(self.human) ):
info = p.getJointInfo(self.human, j)
link_name = info[12].decode("ascii")
if link_name=="left_foot": self.left_foot = j
if link_name=="right_foot": self.right_foot = j
self.ordered_joint_indices.append(j)
if info[2] != p.JOINT_REVOLUTE: continue
jname = info[1].decode("ascii")
self.jdict[jname] = j
lower, upper = (info[8], info[9])
self.ordered_joints.append( (j, lower, upper) )
p.setJointMotorControl2(self.human, j, controlMode=p.VELOCITY_CONTROL, force=0)
self.motor_names = ["abdomen_z", "abdomen_y", "abdomen_x"]
self.motor_power = [100, 100, 100]
self.motor_names += ["right_hip_x", "right_hip_z", "right_hip_y", "right_knee"]
self.motor_power += [100, 100, 300, 200]
self.motor_names += ["left_hip_x", "left_hip_z", "left_hip_y", "left_knee"]
self.motor_power += [100, 100, 300, 200]
self.motor_names += ["right_shoulder1", "right_shoulder2", "right_elbow"]
self.motor_power += [75, 75, 75]
self.motor_names += ["left_shoulder1", "left_shoulder2", "left_elbow"]
self.motor_power += [75, 75, 75]
self.motors = [self.jdict[n] for n in self.motor_names]
print("self.motors")
print(self.motors)
print("num motors")
print(len(self.motors))
def spawn_sdf(self, verbose=False):
"""Spawn sdf"""
if verbose:
pylog.debug(self.sdf)
self._identity = pybullet.loadSDF(self.sdf,
useMaximalCoordinates=0,
globalScaling=1)[0]
initial_pose(self._identity, self.options.spawn, self.units)
for joint_i in range(pybullet.getNumJoints(self.identity())):
joint_info = pybullet.getJointInfo(self.identity(), joint_i)
self._links[joint_info[12].decode('UTF-8')] = joint_i
self._joints[joint_info[1].decode('UTF-8')] = joint_i
if self.options.morphology.links is not None:
for link in self.options.morphology.links:
if link not in self._links:
self._links[link] = -1
break
for link in self.options.morphology.links:
assert link in self._links, 'Link {} not in {}'.format(
link,
self._links,
)
if verbose:
self.print_information()
def get_control_id_by_name(self, names):
"""
get joint/link ID by name
"""
nbJoint = pb.getNumJoints(self.robotID)
jointNames = {}
ctlID = 0
for i in range(nbJoint):
jointInfo = pb.getJointInfo(self.robotID, i)
name = jointInfo[1].decode("utf-8")
# skip fixed joint
if jointInfo[2] == 4:
continue
# skip base joint
if jointInfo[-1] == -1:
continue
jointNames[name] = ctlID
ctlID += 1
return [jointNames[name] for name in names]
def getJointRanges(bodyId, includeFixed=False):
"""
Parameters
----------
bodyId : int
includeFixed : bool
Returns
-------
lowerLimits : [ float ] * numDofs
upperLimits : [ float ] * numDofs
jointRanges : [ float ] * numDofs
restPoses : [ float ] * numDofs
"""
lowerLimits, upperLimits, jointRanges, restPoses = [], [], [], []
numJoints = p.getNumJoints(bodyId)
# loop through all joints
for i in range(numJoints):
jointInfo = p.getJointInfo(bodyId, i)
print(jointInfo[0], jointInfo[1], jointInfo[2], jointInfo[3], jointInfo[8:10], jointInfo[12])
if includeFixed or jointInfo[3] > -1:
# jointInfo[3] > -1 means that the joint is not fixed
ll, ul = jointInfo[8:10]
jr = ul - ll
# For simplicity, assume resting state == initial state
rp = p.getJointState(bodyId, i)[0]
lowerLimits.append(ll)
upperLimits.append(ul)
jointRanges.append(jr)
restPoses.append(rp)
return lowerLimits, upperLimits, jointRanges, restPoses
def initialize(self):
ur5_urdf_filepath = os.path.join(
self.root_dir, 'simulators/urdf/ur5/ur5_simple_gripper.urdf')
self.id = pb.loadURDF(ur5_urdf_filepath, [0, 0, 0], [0, 0, 0, 1])
self.gripper_closed = False
self.holding_obj = None
self.num_joints = pb.getNumJoints(self.id)
[
pb.resetJointState(self.id, idx, self.home_positions[idx])
for idx in range(self.num_joints)
]
self.arm_joint_names = list()
self.arm_joint_indices = list()
self.gripper_joint_names = list()
self.gripper_joint_indices = list()
for i in range(self.num_joints):
joint_info = pb.getJointInfo(self.id, i)
if i in range(1, 7):
self.arm_joint_names.append(str(joint_info[1]))
self.arm_joint_indices.append(i)
elif i in range(10, 12):
self.gripper_joint_names.append(str(joint_info[1]))
self.gripper_joint_indices.append(i)
def reset(self):
robot = p.loadURDF("../Gazebo_arm/urdf/tm700_robot_clean.urdf")
self.tm700Uid = robot
p.resetBasePositionAndOrientation(self.tm700Uid, [0.0, 0.0, -0.0], # position of robot, GREEN IS Y AXIS
[0.000000, 0.000000, 1.000000, 0.000000]) # direction of robot
self.jointPositions = [
0.0, -0, -1.5, -0.0, -1.6, -0, -0, 1.5, -0.02,0.02] # position 6 is actually gripper joint
self.numJoints = p.getNumJoints(self.tm700Uid)
for jointIndex in range(self.numJoints):
p.resetJointState(self.tm700Uid, jointIndex, self.jointPositions[jointIndex])
p.setJointMotorControl2(self.tm700Uid,
jointIndex,
p.POSITION_CONTROL,
targetPosition=self.jointPositions[jointIndex],
force=self.maxForce)
# print(p.getJointInfo(robot, jointIndex))
# print(p.getLinkState(robot, jointIndex))
# self.trayUid = p.loadURDF(os.path.join(self.urdfRootPath, "tray/tray.urdf"), 0.6400, #first 3: position, last 4: quaternions
# 0.0000, -0.19, 0.000000, 0.000000, 1.000000, 0.000000)
self.endEffectorPos = [0.4317596244807792, 0.1470447615125933, 0.2876258566462587]
self.endEffectorAngle = 0.02
self.motorNames = []
self.motorIndices = []
for i in range(self.numJoints):
jointInfo = p.getJointInfo(self.tm700Uid, i)
qIndex = jointInfo[3]
if qIndex > -1:
#print("motorname")
#print(jointInfo[1])
self.motorNames.append(str(jointInfo[1]))
self.motorIndices.append(i)
def initialize(self):
''''''
ur5_urdf_filepath = os.path.join(
self.root_dir, 'simulators/urdf/kuka/kuka_with_gripper2.sdf')
self.id = pb.loadSDF(ur5_urdf_filepath)[0]
pb.resetBasePositionAndOrientation(self.id, [-0.2, 0, 0], [0, 0, 0, 1])
# self.is_holding = False
self.gripper_closed = False
self.holding_obj = None
self.num_joints = pb.getNumJoints(self.id)
[
pb.resetJointState(self.id, idx, self.home_positions[idx])
for idx in range(self.num_joints)
]
self.openGripper()
self.arm_joint_names = list()
self.arm_joint_indices = list()
for i in range(self.num_joints):
joint_info = pb.getJointInfo(self.id, i)
if i in range(7):
self.arm_joint_names.append(str(joint_info[1]))
self.arm_joint_indices.append(i)
def getJointRanges(bodyId, includeFixed=False):
"""
Parameters
----------
bodyId : int
includeFixed : bool
Returns
-------
lowerLimits : [ float ] * numDofs
upperLimits : [ float ] * numDofs
jointRanges : [ float ] * numDofs
restPoses : [ float ] * numDofs
"""
lowerLimits, upperLimits, jointRanges, restPoses = [], [], [], []
numJoints = p.getNumJoints(bodyId)
for i in range(numJoints):
jointInfo = p.getJointInfo(bodyId, i)
if includeFixed or jointInfo[3] > -1:
ll, ul = jointInfo[8:10]
jr = ul - ll
# For simplicity, assume resting state == initial state
rp = p.getJointState(bodyId, i)[0]
lowerLimits.append(-2) # What do these vals mean?
upperLimits.append(2)
jointRanges.append(2)
restPoses.append(rp)
return lowerLimits, upperLimits, jointRanges, restPoses
def __init__(self, parent, config):
super(JointController, self).__init__(parent, config)
self.uid = parent.uid
self.position_gain = config.get('position_gain', 0.015)
self.velocity_gain = config.get('velocity_gain', 1.0)
self.scaling = config.get('action_scaling', 1.0)
self.control_mode = {
'position': p.POSITION_CONTROL,
'velocity': p.VELOCITY_CONTROL,
'torque': p.TORQUE_CONTROL
}[config.get('control_mode', 'velocity')]
joint_info = [
p.getJointInfo(self.uid, i)
for i in range(p.getNumJoints(self.uid))
]
if 'joint' in config:
joints = [config.get('joint')]
elif 'joints' in config:
joints = config.get('joints')
else:
joints = [info[1].decode('UTF-8') for info in joint_info]
self.joint_ids = [
info[0] for info in joint_info
if info[1].decode('UTF-8') in joints and info[3] > -1
]
self.rest_position = config.get('rest_position',
[0] * len(self.joint_ids))
self.torque_limit = np.array([
p.getJointInfo(self.uid, joint_id)[10]
for joint_id in self.joint_ids
])
self.vel_limit = np.array([
p.getJointInfo(self.uid, joint_id)[11]
for joint_id in self.joint_ids
])
if self.control_mode == p.TORQUE_CONTROL:
self.action_space = spaces.Box(-self.torque_limit / self.scaling,
self.torque_limit / self.scaling,
dtype='float32')
elif self.control_mode == p.VELOCITY_CONTROL:
self.action_space = spaces.Box(-self.vel_limit / self.scaling,
self.vel_limit / self.scaling,
dtype='float32')
else:
low = np.array([
p.getJointInfo(self.uid, joint_id)[8]
for joint_id in self.joint_ids
]) / self.scaling
high = np.array([
p.getJointInfo(self.uid, joint_id)[9]
for joint_id in self.joint_ids
]) / self.scaling
self.action_space = spaces.Box(low,
high,
shape=(len(low), ),
dtype='float32')
self.random_reset = config.get('reset_range',
[0.] * len(self.joint_ids))
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[2]
pos = [record[3],record[4],record[5]]
orn = [record[6],record[7],record[8],record[9]]
p.resetBasePositionAndOrientation(Id,pos,orn)
numJoints = p.getNumJoints(Id)
for i in range (numJoints):
jointInfo = p.getJointInfo(Id,i)
qIndex = jointInfo[3]
if qIndex > -1:
p.resetJointState(Id,i,record[qIndex-7+17])
sleep(0.0005)
def addToScene(self, bodies):
if self.parts is not None:
parts = self.parts
else:
parts = {}
if self.jdict is not None:
joints = self.jdict
else:
joints = {}
if self.ordered_joints is not None:
ordered_joints = self.ordered_joints
else:
ordered_joints = []
dump = 0
for i in range(len(bodies)):
if p.getNumJoints(bodies[i]) == 0:
part_name, robot_name = p.getBodyInfo(bodies[i], 0)
robot_name = robot_name.decode("utf8")
part_name = part_name.decode("utf8")
parts[part_name] = BodyPart(part_name, bodies, i, -1)
for j in range(p.getNumJoints(bodies[i])):
p.setJointMotorControl2(bodies[i],
j,
p.POSITION_CONTROL,
positionGain=0.1,
velocityGain=0.1,
force=0)
jointInfo = p.getJointInfo(bodies[i], j)
joint_name = jointInfo[1]
part_name = jointInfo[12]
joint_name = joint_name.decode("utf8")
part_name = part_name.decode("utf8")
if dump: print("ROBOT PART '%s'" % part_name)
if dump:
print(
"ROBOT JOINT '%s'" % joint_name
) # limits = %+0.2f..%+0.2f effort=%0.3f speed=%0.3f" % ((joint_name,) + j.limits()) )
parts[part_name] = BodyPart(part_name, bodies, i, j)
if part_name == self.robot_name:
self.robot_body = parts[part_name]
if i == 0 and j == 0 and self.robot_body is None: # if nothing else works, we take this as robot_body
parts[self.robot_name] = BodyPart(self.robot_name, bodies,
0, -1)
self.robot_body = parts[self.robot_name]
if joint_name[:6] == "ignore":
Joint(joint_name, bodies, i, j).disable_motor()
continue
if joint_name[:8] != "jointfix":
joints[joint_name] = Joint(joint_name, bodies, i, j)
ordered_joints.append(joints[joint_name])
joints[joint_name].power_coef = 100.0
return parts, joints, ordered_joints, self.robot_body
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.resetSimulation()
StartPos = [0,0,1]
StartOrientation = p.getQuaternionFromEuler([0,0,0])
temp = p.loadURDF(robot_path4, StartPos, StartOrientation, useFixedBase = 1)
position, orientation = p.getBasePositionAndOrientation(temp) #(x,y,z,w) in quaternions
num_joints = p.getNumJoints(temp)
#gets information about joint number 2
joint_index = 2
_, name, joint_type, _, _, _, _, _, lower_limit, upper_limit, _, _, _ ,_,_,_,_= \
p.getJointInfo(temp, joint_index)
name, joint_type, lower_limit, upper_limit
#gets position of all joints
joint_positions = [j[0] for j in p.getJointStates(temp, range(num_joints))]
joint_positions
#get world position of joints
#world_position, world_orientation = p.getLinkState(temp, 2)[:2]
#world_position
p.setGravity(0, 0, -9.81) # everything should fall down
p.setTimeStep(0.0001) # this slows everything down, but let's be accurate...
p.setRealTimeSimulation(0) # we want to be faster than real time :)
def getJointNames(botId):
for i in range(p.getNumJoints(botId)):
print(p.getJointInfo(botId, i)[0:2])
linkVisualShapeIndices=linkVisualShapeIndices,
linkPositions=linkPositions,
linkOrientations=linkOrientations,
linkInertialFramePositions=linkInertialFramePositions,
linkInertialFrameOrientations=linkInertialFrameOrientations,
linkParentIndices=indices,
linkJointTypes=jointTypes,
linkJointAxis=axis,
useMaximalCoordinates=useMaximalCoordinates)
p.setGravity(0, 0, -10)
p.setRealTimeSimulation(0)
p.getNumJoints(sphereUid)
for i in range(p.getNumJoints(sphereUid)):
p.getJointInfo(sphereUid, i)
p.changeDynamics(sphereUid,
i,
lateralFriction=2,
anisotropicFriction=[1, 0.01,
0.01]) #0,0,0])#1,0.01,0.01])
dt = 1. / 240.
SNAKE_NORMAL_PERIOD = 0.1 #1.5
m_wavePeriod = SNAKE_NORMAL_PERIOD
m_waveLength = 4
m_wavePeriod = 1.5
m_waveAmplitude = 0.4
m_waveFront = 0.0
#our steering value
import time
import math
from datetime import datetime
from datetime import datetime
import pybullet_data
clid = p.connect(p.SHARED_MEMORY)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
if (clid < 0):
p.connect(p.GUI)
p.loadURDF("plane.urdf", [0, 0, -0.3])
husky = p.loadURDF("husky/husky.urdf", [0.290388, 0.329902, -0.310270],
[0.002328, -0.000984, 0.996491, 0.083659])
for i in range(p.getNumJoints(husky)):
print(p.getJointInfo(husky, i))
kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf", 0.193749, 0.345564,
0.120208, 0.002327, -0.000988, 0.996491, 0.083659)
ob = kukaId
jointPositions = [
3.559609, 0.411182, 0.862129, 1.744441, 0.077299, -1.129685, 0.006001
]
for jointIndex in range(p.getNumJoints(ob)):
p.resetJointState(ob, jointIndex, jointPositions[jointIndex])
#put kuka on top of husky
cid = p.createConstraint(husky, -1, kukaId, -1, p.JOINT_FIXED, [0, 0, 0],
[0, 0, 0], [0., 0., -.5], [0, 0, 0, 1])
baseorn = p.getQuaternionFromEuler([3.1415, 0, 0.3])
import pybullet as p
import time
useMaximalCoordinates=False
p.connect(p.GUI)
pole = p.loadURDF("cartpole.urdf", useMaximalCoordinates=useMaximalCoordinates)
for i in range (p.getNumJoints(pole)):
#disable default constraint-based motors
p.setJointMotorControl2(pole,i,p.POSITION_CONTROL,targetPosition=0,force=0)
print("joint",i,"=",p.getJointInfo(pole,i))
desiredPosCartId = p.addUserDebugParameter("desiredPosCart",-10,10,2)
desiredVelCartId = p.addUserDebugParameter("desiredVelCart",-10,10,0)
kpCartId = p.addUserDebugParameter("kpCart",0,500,300)
kdCartId = p.addUserDebugParameter("kdCart",0,300,150)
maxForceCartId = p.addUserDebugParameter("maxForceCart",0,5000,1000)
desiredPosPoleId = p.addUserDebugParameter("desiredPosPole",-10,10,0)
desiredVelPoleId = p.addUserDebugParameter("desiredVelPole",-10,10,0)
kpPoleId = p.addUserDebugParameter("kpPole",0,500,200)
kdPoleId = p.addUserDebugParameter("kdPole",0,300,100)
maxForcePoleId = p.addUserDebugParameter("maxForcePole",0,5000,1000)
pd = p.loadPlugin("pdControlPlugin")
p.setGravity(0,0,-10)
p.loadURDF("plane_transparent.urdf", useMaximalCoordinates=True)
#disable rendering during creation.
#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION,1)
jointNamesToIndex={}
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
vision = p.loadURDF("vision60.urdf",[0,0,0.4],useFixedBase=False)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
for j in range(p.getNumJoints(vision)):
jointInfo = p.getJointInfo(vision,j)
jointInfoName = jointInfo[1].decode("utf-8")
print("joint ",j," = ",jointInfoName, "type=",jointTypeNames[jointInfo[2]])
jointNamesToIndex[jointInfoName ]=j
#print("jointNamesToIndex[..]=",jointNamesToIndex[jointInfoName])
p.setJointMotorControl2(vision,j,p.VELOCITY_CONTROL,targetVelocity=0, force=jointFriction)
chassis_right_center = jointNamesToIndex['chassis_right_center']
motor_front_rightR_joint = jointNamesToIndex['motor_front_rightR_joint']
motor_front_rightS_joint = jointNamesToIndex['motor_front_rightS_joint']
hip_front_rightR_joint = jointNamesToIndex['hip_front_rightR_joint']
knee_front_rightR_joint = jointNamesToIndex['knee_front_rightR_joint']
motor_front_rightL_joint = jointNamesToIndex['motor_front_rightL_joint']
motor_back_rightR_joint = jointNamesToIndex['motor_back_rightR_joint']
motor_back_rightS_joint = jointNamesToIndex['motor_back_rightS_joint']
p.setAdditionalSearchPath(pd.getDataPath())
plane = p.loadURDF("plane.urdf")
p.setGravity(0, 0, -9.8)
p.setTimeStep(1. / 500)
#p.setDefaultContactERP(0)
#urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
urdfFlags = p.URDF_USE_SELF_COLLISION
quadruped = p.loadURDF("laikago/laikago.urdf", [0, 0, .5], [0, 0.5, 0.5, 0],
flags=urdfFlags,
useFixedBase=False)
#enable collision between lower legs
for j in range(p.getNumJoints(quadruped)):
print(p.getJointInfo(quadruped, j))
#2,5,8 and 11 are the lower legs
lower_legs = [2, 5, 8, 11]
for l0 in lower_legs:
for l1 in lower_legs:
if (l1 > l0):
enableCollision = 1
print("collision for pair", l0, l1,
p.getJointInfo(quadruped, l0)[12],
p.getJointInfo(quadruped, l1)[12], "enabled=", enableCollision)
p.setCollisionFilterPair(quadruped, quadruped, 2, 5, enableCollision)
jointIds = []
paramIds = []
jointOffsets = []
p.connect(p.GUI)
p.resetSimulation()
p.setGravity(0,0,-10)
useRealTimeSim = 1
#for video recording (works best on Mac and Linux, not well on Windows)
#p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "racecar.mp4")
p.setRealTimeSimulation(useRealTimeSim) # either this
#p.loadURDF("plane.urdf")
p.loadSDF(os.path.join(pybullet_data.getDataPath(),"stadium.sdf"))
car = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"racecar/racecar.urdf"))
for i in range (p.getNumJoints(car)):
print (p.getJointInfo(car,i))
inactive_wheels = [3,5,7]
wheels = [2]
for wheel in inactive_wheels:
p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=0,force=0)
steering = [4,6]
targetVelocitySlider = p.addUserDebugParameter("wheelVelocity",-10,10,0)
maxForceSlider = p.addUserDebugParameter("maxForce",0,10,10)
steeringSlider = p.addUserDebugParameter("steering",-0.5,0.5,0)
while (True):
maxForce = p.readUserDebugParameter(maxForceSlider)
targetVelocity = p.readUserDebugParameter(targetVelocitySlider)
0.1013909845,-0.05515999155,0.143618978,0.9659421276,0.1884590249,-0.1422460188,0.105854014,0.581348]
startVel = [1.235314324,-0.008525509087,0.1515293946,-1.161516553,0.1866449799,-0.1050802848,0,0.935706195,0.08277326387,0.3002461862,0,0,0,0,0,1.114409628,0.3618553952,
-0.4505575061,0,-1.725374735,-0.5052852598,-0.8555179722,-0.2221173515,0,-0.1837617357,0.00171895706,0.03912837591,0,0.147945294,1.837653345,0.1534535548,1.491385941,0,
-4.632454387,-0.9111172777,-1.300648184,-1.345694622,0,-1.084238535,0.1313680236,-0.7236998534,0,-0.5278312973]
p.resetBasePositionAndOrientation(humanoid, startLocations[0],[0,0,0,1])
p.resetBasePositionAndOrientation(humanoid2, startLocations[1],[0,0,0,1])
p.resetBasePositionAndOrientation(humanoid3, startLocations[2],[0,0,0,1])
p.resetBasePositionAndOrientation(humanoid4, startLocations[3],[0,0,0,1])
index0=7
for j in range (p.getNumJoints(humanoid)):
ji = p.getJointInfo(humanoid,j)
targetPosition=[0]
jointType = ji[2]
if (jointType == p.JOINT_SPHERICAL):
targetPosition=[startPose[index0+1],startPose[index0+2],startPose[index0+3],startPose[index0+0]]
targetVel = [startVel[index0+0],startVel[index0+1],startVel[index0+2]]
index0+=4
print("spherical position: ",targetPosition)
print("spherical velocity: ",targetVel)
p.resetJointStateMultiDof(humanoid,j,targetValue=targetPosition,targetVelocity=targetVel)
p.resetJointStateMultiDof(humanoid2,j,targetValue=targetPosition,targetVelocity=targetVel)
if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE):
targetPosition=[startPose[index0]]
targetVel = [startVel[index0]]
index0+=1
print("revolute:", targetPosition)
import pybullet as p
import time
import math
from datetime import datetime
from datetime import datetime
clid = p.connect(p.SHARED_MEMORY)
if (clid<0):
p.connect(p.GUI)
p.loadURDF("plane.urdf",[0,0,-0.3])
husky = p.loadURDF("husky/husky.urdf",[0.290388,0.329902,-0.310270],[0.002328,-0.000984,0.996491,0.083659])
for i in range (p.getNumJoints(husky)):
print(p.getJointInfo(husky,i))
kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf", 0.193749,0.345564,0.120208,0.002327,-0.000988,0.996491,0.083659)
ob = kukaId
jointPositions=[ 3.559609, 0.411182, 0.862129, 1.744441, 0.077299, -1.129685, 0.006001 ]
for jointIndex in range (p.getNumJoints(ob)):
p.resetJointState(ob,jointIndex,jointPositions[jointIndex])
#put kuka on top of husky
cid = p.createConstraint(husky,-1,kukaId,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0.,0.,-.5],[0,0,0,1])
baseorn = p.getQuaternionFromEuler([3.1415,0,0.3])
baseorn = [0,0,0,1]
#[0, 0, 0.707, 0.707]
#p.resetBasePositionAndOrientation(kukaId,[0,0,0],baseorn)#[0,0,0,1])
kukaEndEffectorIndex = 6
import pybullet as p
p.connect(p.GUI) #or p.SHARED_MEMORY or p.DIRECT
p.loadURDF("plane.urdf")
p.setGravity(0,0,-10)
huskypos = [0,0,0.1]
husky = p.loadURDF("husky/husky.urdf",huskypos[0],huskypos[1],huskypos[2])
numJoints = p.getNumJoints(husky)
for joint in range (numJoints) :
print (p.getJointInfo(husky,joint))
targetVel = 10 #rad/s
maxForce = 100 #Newton
for joint in range (2,6) :
p.setJointMotorControl(husky,joint,p.VELOCITY_CONTROL,targetVel,maxForce)
for step in range (300):
p.stepSimulation()
targetVel=-10
for joint in range (2,6) :
p.setJointMotorControl(husky,joint,p.VELOCITY_CONTROL,targetVel,maxForce)
for step in range (400):
p.stepSimulation()
p.getContactPointData(husky)
p.disconnect()
import pybullet as p
import time
p.connect(p.GUI)
door = p.loadURDF("door.urdf")
#linear/angular damping for base and all children=0
p.changeDynamics(door, -1, linearDamping=0, angularDamping=0)
for j in range(p.getNumJoints(door)):
p.changeDynamics(door, j, linearDamping=0, angularDamping=0)
print(p.getJointInfo(door, j))
frictionId = p.addUserDebugParameter("jointFriction", 0, 20, 10)
torqueId = p.addUserDebugParameter("joint torque", 0, 20, 5)
while (1):
frictionForce = p.readUserDebugParameter(frictionId)
jointTorque = p.readUserDebugParameter(torqueId)
#set the joint friction
p.setJointMotorControl2(door, 1, p.VELOCITY_CONTROL, targetVelocity=0, force=frictionForce)
#apply a joint torque
p.setJointMotorControl2(door, 1, p.TORQUE_CONTROL, force=jointTorque)
p.stepSimulation()
time.sleep(0.01)
p.connect(p.GUI)
p.setAdditionalSearchPath(pd.getDataPath())
plane = p.loadURDF("plane.urdf")
p.setGravity(0,0,-9.8)
p.setTimeStep(1./500)
#p.setDefaultContactERP(0)
#urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
urdfFlags = p.URDF_USE_SELF_COLLISION
quadruped = p.loadURDF("laikago/laikago.urdf",[0,0,.5],[0,0.5,0.5,0], flags = urdfFlags,useFixedBase=False)
#enable collision between lower legs
for j in range (p.getNumJoints(quadruped)):
print(p.getJointInfo(quadruped,j))
#2,5,8 and 11 are the lower legs
lower_legs = [2,5,8,11]
for l0 in lower_legs:
for l1 in lower_legs:
if (l1>l0):
enableCollision = 1
print("collision for pair",l0,l1, p.getJointInfo(quadruped,l0)[12],p.getJointInfo(quadruped,l1)[12], "enabled=",enableCollision)
p.setCollisionFilterPair(quadruped, quadruped, 2,5,enableCollision)
jointIds=[]
paramIds=[]
jointOffsets=[]
jointDirections=[-1,1,1,1,1,1,-1,1,1,1,1,1]
jointAngles=[0,0,0,0,0,0,0,0,0,0,0,0]
#a mimic joint can act as a gear between two joints
#you can control the gear ratio in magnitude and sign (>0 reverses direction)
import pybullet as p
import time
p.connect(p.GUI)
p.loadURDF("plane.urdf", 0, 0, -2)
wheelA = p.loadURDF("differential/diff_ring.urdf", [0, 0, 0])
for i in range(p.getNumJoints(wheelA)):
print(p.getJointInfo(wheelA, i))
p.setJointMotorControl2(wheelA, i, p.VELOCITY_CONTROL, targetVelocity=0, force=0)
c = p.createConstraint(wheelA,
1,
wheelA,
3,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=1, maxForce=10000)
c = p.createConstraint(wheelA,
2,
wheelA,
4,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=-1, maxForce=10000)
## Loading
physicsClient = p.connect(p.GUI) #or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
p.setGravity(0, 0, -10)
planeId = p.loadURDF("plane.urdf")
RobotStartPosition = [0, 0, 0.75]
RobotStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
RoboBoi = p.loadURDF("Robot.urdf", RobotStartPosition, RobotStartOrientation)
# Create Parent-Child Map
parent_child_map = numpy.array([[0, 1], [1, 2], [0, 3], [3, 4], [4, 5], \
[3, 6], [6, 7], [7, 8], [0, 9], [9, 10], [10, 11]])
## Print all the links
for i in range(p.getNumJoints(RoboBoi)):
link = p.getJointInfo(RoboBoi, i)
print(link)
print("Here")
## Let's try to move the robot legs so it can stand
## Try moving back legs
p.setJointMotorControl2(RoboBoi, 9, p.POSITION_CONTROL, -0.4)
p.setJointMotorControl2(RoboBoi, 6, p.POSITION_CONTROL, -0.4)
INITSTATE = p.saveState()
originalPos, originalOrientation = p.getBasePositionAndOrientation(RoboBoi)
a = numpy.zeros(p.getNumJoints(RoboBoi))
b = numpy.zeros(p.getNumJoints(RoboBoi))
c = numpy.zeros(p.getNumJoints(RoboBoi))
omega = numpy.zeros(p.getNumJoints(RoboBoi))
def addToScene(self, bodies):
if self.parts is not None:
parts = self.parts
else:
parts = {}
if self.jdict is not None:
joints = self.jdict
else:
joints = {}
if self.ordered_joints is not None:
ordered_joints = self.ordered_joints
else:
ordered_joints = []
dump = 0
for i in range(len(bodies)):
if p.getNumJoints(bodies[i]) == 0:
part_name, robot_name = p.getBodyInfo(bodies[i], 0)
robot_name = robot_name.decode("utf8")
part_name = part_name.decode("utf8")
parts[part_name] = BodyPart(part_name,
bodies,
i,
-1,
self.scale,
model_type=self.model_type)
for j in range(p.getNumJoints(bodies[i])):
p.setJointMotorControl2(bodies[i],
j,
p.POSITION_CONTROL,
positionGain=0.1,
velocityGain=0.1,
force=0)
## TODO (hzyjerry): the following is diabled due to pybullet update
# _,joint_name,joint_type, _,_,_, _,_,_,_, _,_, part_name = p.getJointInfo(bodies[i], j)
_, joint_name, joint_type, _, _, _, _, _, _, _, _, _, part_name, _, _, _, _ = p.getJointInfo(
bodies[i], j)
joint_name = joint_name.decode("utf8")
part_name = part_name.decode("utf8")
if dump: print("ROBOT PART '%s'" % part_name)
if dump:
print(
"ROBOT JOINT '%s'" % joint_name
) # limits = %+0.2f..%+0.2f effort=%0.3f speed=%0.3f" % ((joint_name,) + j.limits()) )
parts[part_name] = BodyPart(part_name,
bodies,
i,
j,
self.scale,
model_type=self.model_type)
if part_name == self.robot_name:
self.robot_body = parts[part_name]
if i == 0 and j == 0 and self.robot_body is None: # if nothing else works, we take this as robot_body
parts[self.robot_name] = BodyPart(
self.robot_name,
bodies,
0,
-1,
self.scale,
model_type=self.model_type)
self.robot_body = parts[self.robot_name]
if joint_name[:6] == "ignore":
Joint(joint_name, bodies, i, j, self.scale).disable_motor()
continue
if joint_name[:8] != "jointfix" and joint_type != p.JOINT_FIXED:
joints[joint_name] = Joint(joint_name,
bodies,
i,
j,
self.scale,
model_type=self.model_type)
ordered_joints.append(joints[joint_name])
joints[joint_name].power_coef = 100.0
debugmode = 0
if debugmode:
for j in ordered_joints:
print(j, j.power_coef)
return parts, joints, ordered_joints, self.robot_body
def _build_model(self):
if self.render:
self.physicsClient = p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
# p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0) # disable rendering during creation
cameraDistance = 10 # at 11m distance
cameraYaw = 0
cameraPitch = -45 # -30 deg (down angle)
cameraTargetPosition = [0, 0, -3] # focusing (0, 0, 1)
p.resetDebugVisualizerCamera(cameraDistance, cameraYaw,
cameraPitch, cameraTargetPosition)
# add camera follows the unicycle let an agent could get visual state
# draw ring of radius = 9 m
# n_polygon = 40
# point_x = self.floor_r * np.cos(2 * np.pi * np.arange(n_polygon) / n_polygon)
# point_y = self.floor_r * np.sin(2 * np.pi * np.arange(n_polygon) / n_polygon)
# points = list(zip(point_x, point_y, np.ones(n_polygon)/20))
# points_end = [points[(i + 1) % n_polygon] for i in range(n_polygon)]
#
# for i in range(n_polygon):
# p.addUserDebugLine(points[i], points_end[i], lineColorRGB=[0.5, 0.5, 0.05], lineWidth=3)
else:
self.physicsClient = p.connect(p.DIRECT)
# Define ground area
p.addUserDebugLine([-7, -7, 1], [-7, 7, 1],
lineColorRGB=[0.5, 0.5, 0.05],
lineWidth=30)
p.addUserDebugLine([-7, 7, 1], [7, 7, 1],
lineColorRGB=[0.5, 0.5, 0.05],
lineWidth=30)
p.addUserDebugLine([7, 7, 1], [7, -7, 1],
lineColorRGB=[0.5, 0.5, 0.05],
lineWidth=30)
p.addUserDebugLine([7, -7, 1], [-7, -7, 1],
lineColorRGB=[0.5, 0.5, 0.05],
lineWidth=30)
p.setRealTimeSimulation(
enableRealTimeSimulation=0
) # p.TORQUE_CONTROL works only at non-real-time sim mode
p.setAdditionalSearchPath(
pybullet_data.getDataPath()) # optional. to load preexisting data
p.setGravity(0, 0, -10) # g= 10 kg m / s^2
p.setTimeStep(self.time_step)
planeId = p.loadURDF("plane.urdf")
#planeId = p.loadURDF("plane100.urdf", useMaximalCoordinates=True)
C_Radius = 0.5 # Radius of wheels = 50 cm
C_Height = 0.01 # width of wheels = 1 cm
# Building blocks
colCylinderId = p.createCollisionShape(p.GEOM_CYLINDER,
radius=C_Radius,
height=C_Height) # Wheels
colLodId = p.createCollisionShape(p.GEOM_CYLINDER,
radius=0.03,
height=1.5) # center column
colSphereId = p.createCollisionShape(
p.GEOM_SPHERE,
radius=0.05) # indicator to check that wheels are rolling
# link information [upper wheel, lower wheel, wheel indicator, wheel indicator]
link_Masses = [3, 3, 0, 0] # mass of the Wheels = 3 kg
linkCollisionShapeIndices = [
colCylinderId, colCylinderId, colSphereId, colSphereId
]
linkVisualShapeIndices = [-1, -1, -1, -1]
linkPositions = [[0, 0, 1.5 / 2], [0, 0, -1.5 / 2], [0, 0.3, 0],
[0, 0.3, 0]]
linkOrientations = [[0, 0, 0, 1], [1, 0, 0, 1], [1, 0, 0, 1],
[1, 0, 0, 1]]
linkInertialFramePositions = [[0, 0, 0], [0, 0, 0], [0, 0, 0],
[0, 0, 0]]
linkInertialFrameOrientations = [[0, 0, 0, 1], [0, 0, 0, 1],
[0, 0, 0, 1], [0, 0, 0, 1]]
indices = [0, 0, 1,
2] # linked to [ Lod, Lod, upper wheel, lower wheel ]
jointTypes = [
p.JOINT_REVOLUTE, p.JOINT_REVOLUTE, p.JOINT_FIXED, p.JOINT_FIXED
] # rolling,rolling,fixed,fixed
axis = [[0, 0, 1], [0, 0, 1], [0, 0, 1], [0, 0, 1]]
# position and orientation of the center of column
basePosition = [0, 0, 0.5 + 1.5 / 2 + 1e-10]
baseOrientation = [0, 0, 0, 1]
visualShapeId = -1
self.unicycleUid = \
p.createMultiBody(6, colLodId,
visualShapeId,
basePosition,
baseOrientation,
linkMasses=link_Masses,
linkCollisionShapeIndices=linkCollisionShapeIndices,
linkVisualShapeIndices=linkVisualShapeIndices,
linkPositions=linkPositions,
linkOrientations=linkOrientations,
linkInertialFramePositions=linkInertialFramePositions,
linkInertialFrameOrientations=linkInertialFrameOrientations,
linkParentIndices=indices,
linkJointTypes=jointTypes,
linkJointAxis=axis
)
#p.changeDynamics(self.unicycleUid, -1, spinningFriction=0.0, rollingFriction=0.0, lateralFriction=0.02, linearDamping=0.001, angularDamping=0.001)
# indices of "moving" joint
self.jointinfo = []
for i in range(p.getNumJoints(self.unicycleUid)):
if p.getJointInfo(self.unicycleUid, i)[2] == 0:
self.jointinfo.append(i)
p.changeDynamics(self.unicycleUid,
linkIndex=self.jointinfo[0],
angularDamping=0.7)
# video flag
recordVideo = True
prefix = time.strftime('%Y%m%d%H%M%S', time.localtime(time.time()))
p.connect(p.GUI)
p.setGravity(0.0, 0.0, -10.0)
startPos = [-16.0, 0.0, 0.0]
endPos = [14.0, 0.0, 0.0]
env = Env(startPos, endPos)
plan = RobotControl.generateTraj(env.robotId)
for jointId in range(p.getNumJoints(env.robotId)):
print(p.getJointInfo(env.robotId, jointId))
p.enableJointForceTorqueSensor(env.robotId, jointId, 1)
if recordVideo:
videoFile = Helper.findLog(prefix + '.mp4')
videoLogId = p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, videoFile)
t = 0
n = 0
while True:
p.stepSimulation()
time.sleep(1 / 240)
controlSignal = RobotControl.realTimeControl(env.robotId, plan, n)
env.control(controlSignal)
n = n + 1
def load_urdf(self,
id,
filename,
start_pose,
remove_friction=False,
static=False,
label="thing",
description=""):
""" """
try:
use_fixed_base = 1 if static is True else 0
base_link_sim_id = p.loadURDF(
filename,
start_pose.position().to_array(),
start_pose.quaternion(),
useFixedBase=use_fixed_base,
flags=p.URDF_ENABLE_SLEEPING
or p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES)
self.entity_id_map[id] = base_link_sim_id
# Create a joint map to ease exploration
self.reverse_entity_id_map[base_link_sim_id] = id
self.joint_id_map[base_link_sim_id] = {}
self.reverse_joint_id_map[base_link_sim_id] = {}
for i in range(0, p.getNumJoints(base_link_sim_id)):
info = p.getJointInfo(base_link_sim_id, i)
self.joint_id_map[base_link_sim_id][info[1]] = info[0]
self.reverse_joint_id_map[base_link_sim_id][info[0]] = info[1]
# If file successfully loaded
if base_link_sim_id < 0:
raise RuntimeError("Invalid URDF")
scene_node = SceneNode(pose=start_pose, is_static=True)
scene_node.id = id
scene_node.label = label
scene_node.description = description
sim_id = self.entity_id_map[id]
visual_shapes = p.getVisualShapeData(sim_id)
for shape in visual_shapes:
link_id = shape[1]
type = shape[2]
dimensions = shape[3]
mesh_file_path = shape[4]
position = shape[5]
orientation = shape[6]
rgba_color = shape[7]
if link_id != -1:
link_state = p.getLinkState(sim_id, link_id)
t_link = link_state[0]
q_link = link_state[1]
t_inertial = link_state[2]
q_inertial = link_state[3]
tf_world_link = np.dot(translation_matrix(t_link),
quaternion_matrix(q_link))
tf_inertial_link = np.dot(translation_matrix(t_inertial),
quaternion_matrix(q_inertial))
world_transform = np.dot(tf_world_link,
np.linalg.inv(tf_inertial_link))
else:
t_link, q_link = p.getBasePositionAndOrientation(sim_id)
world_transform = np.dot(translation_matrix(t_link),
quaternion_matrix(q_link))
if type == p.GEOM_SPHERE:
primitive_shape = Sphere(dimensions[0] * 2.0)
elif type == p.GEOM_BOX:
primitive_shape = Box(dimensions[0], dimensions[1],
dimensions[2])
elif type == p.GEOM_CYLINDER:
primitive_shape = Cylinder(dimensions[1] * 2.0,
dimensions[0])
elif type == p.GEOM_PLANE:
primitive_shape = Box(dimensions[0], dimensions[1], 0.0001)
elif type == p.GEOM_MESH:
primitive_shape = Mesh("file://" + mesh_file_path,
scale_x=dimensions[0],
scale_y=dimensions[1],
scale_z=dimensions[2])
else:
raise NotImplementedError(
"Shape capsule not supported at the moment")
if link_id != -1:
shape_transform = np.dot(translation_matrix(position),
quaternion_matrix(orientation))
shape_transform = np.dot(world_transform, shape_transform)
shape_transform = np.linalg.inv(
np.dot(np.linalg.inv(shape_transform),
scene_node.pose.transform()))
position = translation_from_matrix(shape_transform)
orientation = quaternion_from_matrix(shape_transform)
primitive_shape.pose.pos.x = position[0]
primitive_shape.pose.pos.y = position[1]
primitive_shape.pose.pos.z = position[2]
primitive_shape.pose.from_quaternion(
orientation[0], orientation[1], orientation[2],
orientation[3])
else:
shape_transform = np.dot(translation_matrix(position),
quaternion_matrix(orientation))
shape_transform = np.dot(world_transform, shape_transform)
position = translation_from_matrix(shape_transform)
orientation = quaternion_from_matrix(shape_transform)
scene_node.pose.pos.x = position[0]
scene_node.pose.pos.y = position[1]
scene_node.pose.pos.z = position[2]
scene_node.pose.from_quaternion(orientation[0],
orientation[1],
orientation[2],
orientation[3])
if len(rgba_color) > 0:
primitive_shape.color[0] = rgba_color[0]
primitive_shape.color[1] = rgba_color[1]
primitive_shape.color[2] = rgba_color[2]
primitive_shape.color[3] = rgba_color[3]
scene_node.shapes.append(primitive_shape)
self.entity_map[id] = scene_node
return True, scene_node
rospy.loginfo(
"[simulation] '{}' File successfully loaded".format(filename))
except Exception as e:
rospy.logwarn("[simulation] Error loading URDF '{}': {}".format(
filename, e))
return False, None
import pybullet_data
from time import sleep, time
import numpy as np
def magnitude(a):
return (a[0]**2+a[1]**2)**0.5
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.loadURDF("plane.urdf")
botpos=[0,0,0.1]
bot = p.loadURDF("urdf/Paucibot.urdf",*botpos)
p.setGravity(0,0,-10)
numJoints = p.getNumJoints(bot)
for joint in range(numJoints):
print(p.getJointInfo(bot,joint))
wheels = [ 2, 5 ]
targetVel = 15
maxForce = 6
kp, kd, ki = 0.005, 0.005, 0.000
init = time()
target_x = 0
target_pitch =0
prev_error = 0
prev_error_pitch = 0
encoder_pos = [0,0]
encoder_vel = [0,0]
while(1):
posi = p.getBasePositionAndOrientation(bot)[0]
orie = p.getBasePositionAndOrientation(bot)[1]
euler = p.getEulerFromQuaternion(orie)
def buildJointNameToIdDict(self):
nJoints = p.getNumJoints(self.robot)
self.jointNameToId = {}
for i in range(nJoints):
jointInfo = p.getJointInfo(self.robot, i)
self.jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
l8 = 0.090
l9 = 0.092
l10 = 0.330
l11 = 0.300
l12 = 0.123005
l13 = 0.146
l14 = 0.018
l15 = 0.026
l16 = 0.0175
n_joints = p.getNumJoints(teoId)
name2id={}
for i in range(n_joints):
name2id[p.getJointInfo(teoId,i)[1]]=i
n_joints = p.getNumJoints(teoId)
jointIndices = [0] * n_joints
targetPos = [0.0] * n_joints
maxVelocities = [radians(1)] * n_joints
maxForces = [0.0] * n_joints
targetPosOffset = [0.0]*n_joints
targetPosModified = [0.0]*n_joints
for i in range(n_joints):
jointIndices[i] = i
useFixedBase = True
flags = p.URDF_INITIALIZE_SAT_FEATURES#0#p.URDF_USE_SELF_COLLISION
#plane_pos = [0,0,0]
#plane = p.loadURDF("plane.urdf", plane_pos, flags = flags, useFixedBase=useFixedBase)
table_pos = [0,0,-0.625]
table = p.loadURDF("table/table.urdf", table_pos, flags = flags, useFixedBase=useFixedBase)
xarm = p.loadURDF("xarm/xarm6_robot.urdf", flags = flags, useFixedBase=useFixedBase)
jointIds = []
paramIds = []
for j in range(p.getNumJoints(xarm)):
p.changeDynamics(xarm, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(xarm, j)
#print(info)
jointName = info[1]
jointType = info[2]
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
jointIds.append(j)
paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4, 0))
skip_cam_frames = 10
while (1):
p.stepSimulation()
for i in range(len(paramIds)):
c = paramIds[i]
targetPos = p.readUserDebugParameter(c)
p.setJointMotorControl2(xarm, jointIds[i], p.POSITION_CONTROL, targetPos, force=5 * 240.)
def initializeFromBulletBody(self, bodyUid, physicsClientId):
self.initialize()
#always create a base link
baseLink = UrdfLink()
baseLinkIndex = -1
self.convertLinkFromMultiBody(bodyUid, baseLinkIndex, baseLink,
physicsClientId)
baseLink.link_name = p.getBodyInfo(
bodyUid, physicsClientId=physicsClientId)[0].decode("utf-8")
self.linkNameToIndex[baseLink.link_name] = len(self.urdfLinks)
self.urdfLinks.append(baseLink)
#optionally create child links and joints
for j in range(p.getNumJoints(bodyUid,
physicsClientId=physicsClientId)):
jointInfo = p.getJointInfo(bodyUid,
j,
physicsClientId=physicsClientId)
urdfLink = UrdfLink()
self.convertLinkFromMultiBody(bodyUid, j, urdfLink,
physicsClientId)
urdfLink.link_name = jointInfo[12].decode("utf-8")
self.linkNameToIndex[urdfLink.link_name] = len(self.urdfLinks)
self.urdfLinks.append(urdfLink)
urdfJoint = UrdfJoint()
urdfJoint.link = urdfLink
urdfJoint.joint_name = jointInfo[1].decode("utf-8")
urdfJoint.joint_type = jointInfo[2]
urdfJoint.joint_axis_xyz = jointInfo[13]
orgParentIndex = jointInfo[16]
if (orgParentIndex < 0):
urdfJoint.parent_name = baseLink.link_name
else:
parentJointInfo = p.getJointInfo(
bodyUid, orgParentIndex, physicsClientId=physicsClientId)
urdfJoint.parent_name = parentJointInfo[12].decode("utf-8")
urdfJoint.child_name = urdfLink.link_name
#todo, compensate for inertia/link frame offset
dynChild = p.getDynamicsInfo(bodyUid,
j,
physicsClientId=physicsClientId)
childInertiaPos = dynChild[3]
childInertiaOrn = dynChild[4]
parentCom2JointPos = jointInfo[14]
parentCom2JointOrn = jointInfo[15]
tmpPos, tmpOrn = p.multiplyTransforms(childInertiaPos,
childInertiaOrn,
parentCom2JointPos,
parentCom2JointOrn)
tmpPosInv, tmpOrnInv = p.invertTransform(tmpPos, tmpOrn)
dynParent = p.getDynamicsInfo(bodyUid,
orgParentIndex,
physicsClientId=physicsClientId)
parentInertiaPos = dynParent[3]
parentInertiaOrn = dynParent[4]
pos, orn = p.multiplyTransforms(parentInertiaPos, parentInertiaOrn,
tmpPosInv, tmpOrnInv)
pos, orn_unused = p.multiplyTransforms(parentInertiaPos,
parentInertiaOrn,
parentCom2JointPos,
[0, 0, 0, 1])
urdfJoint.joint_origin_xyz = pos
urdfJoint.joint_origin_rpy = p.getEulerFromQuaternion(orn)
self.urdfJoints.append(urdfJoint)
def main():
# ------------------------ #
# --- Setup simulation --- #
# ------------------------ #
# Create pybullet GUI
physics_client_id = p.connect(p.GUI)
p.resetDebugVisualizerCamera(1.8, 120, -50, [0.0, -0.0, -0.0])
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
sim_timestep = 1.0 / 240
p.setTimeStep(sim_timestep)
# Load plane contained in pybullet_data
p.loadURDF(os.path.join(pybullet_data.getDataPath(), "plane.urdf"))
# Set gravity for simulation
p.setGravity(0, 0, -9.8)
# ------------------- #
# --- Setup robot --- #
# ------------------- #
robot = iCubHandsEnv(physics_client_id, use_IK=1, control_arm='r')
p.stepSimulation()
# -------------------------- #
# --- Load other objects --- #
# -------------------------- #
p.loadURDF(os.path.join(pybullet_data.getDataPath(), "table/table.urdf"),
[1, 0.0, 0.0])
p.loadURDF(
os.path.join(ycb_objects.getDataPath(), 'YcbFoamBrick', "model.urdf"),
[0.5, -0.03, 0.7])
# Run the world for a bit
for _ in range(100):
p.stepSimulation()
# ------------------ #
# --- Start Demo --- #
# ------------------ #
robot.pre_grasp()
for _ in range(10):
p.stepSimulation()
time.sleep(sim_timestep)
# 1: go above the object
pos_1 = [0.49, 0.0, 0.8]
quat_1 = p.getQuaternionFromEuler([0, 0, m.pi / 2])
robot.apply_action(pos_1 + list(quat_1), max_vel=5)
robot.pre_grasp()
for _ in range(60):
p.stepSimulation()
time.sleep(sim_timestep)
# 2: turn hand above the object
pos_2 = [0.485, 0.0, 0.72]
quat_2 = p.getQuaternionFromEuler([m.pi / 2, 1 / 3 * m.pi, -m.pi])
robot.apply_action(pos_2 + list(quat_2), max_vel=5)
robot.pre_grasp()
for _ in range(60):
p.stepSimulation()
time.sleep(sim_timestep)
# 3: close fingers
pos_cl = [
0, 0.6, 0.8, 1.0, 0, 0.6, 0.8, 1.0, 0, 0.6, 0.8, 1.0, 0, 0.6, 0.8, 1.0,
1.57, 0.8, 0.5, 0.8
]
robot.grasp(pos_cl)
for _ in range(60):
p.stepSimulation()
time.sleep(sim_timestep)
# 4: go up
pos_4 = [0.45, 0, 0.9]
quat_4 = p.getQuaternionFromEuler([m.pi / 2, 1 / 3 * m.pi, -m.pi])
robot.apply_action(pos_4 + list(quat_4), max_vel=5)
robot.grasp(pos_cl)
for _ in range(60):
p.stepSimulation()
time.sleep(sim_timestep)
# 5: go right
pos_5 = [0.3, -0.2, 0.9]
quat_5 = p.getQuaternionFromEuler([0.0, 0.0, m.pi / 2])
robot.apply_action(pos_5 + list(quat_5), max_vel=5)
robot.grasp(pos_cl)
for _ in range(60):
p.stepSimulation()
time.sleep(sim_timestep)
# 6: open hand
robot.pre_grasp()
for _ in range(50):
p.stepSimulation()
time.sleep(sim_timestep)
# ------------------------ #
# --- Play with joints --- #
# ------------------------ #
param_ids = []
joint_ids = []
num_joints = p.getNumJoints(robot.robot_id)
joint_states = p.getJointStates(robot.robot_id, range(0, num_joints))
joint_poses = [x[0] for x in joint_states]
idx = 0
for i in range(num_joints):
joint_info = p.getJointInfo(robot.robot_id, i)
joint_name = joint_info[1]
joint_type = joint_info[2]
if joint_type is p.JOINT_REVOLUTE or joint_type is p.JOINT_PRISMATIC:
param_ids.append(
p.addUserDebugParameter(joint_name.decode("utf-8"),
joint_info[8], joint_info[9],
joint_poses[i]))
joint_ids.append(i)
idx += 1
while True:
new_pos = []
for i in param_ids:
new_pos.append(p.readUserDebugParameter(i))
p.setJointMotorControlArray(robot.robot_id,
joint_ids,
p.POSITION_CONTROL,
targetPositions=new_pos)
p.stepSimulation()
time.sleep(sim_timestep)
import pybullet as p
p.connect(p.GUI)
r2d2 = p.loadURDF("r2d2.urdf", [0, 0, 1])
for l in range(p.getNumJoints(r2d2)):
print(p.getJointInfo(r2d2, l))
p.loadURDF("r2d2.urdf", [2, 0, 1])
p.loadURDF("r2d2.urdf", [4, 0, 1])
p.getCameraImage(320, 200, flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX)
segLinkIndex = 1
verbose = 0
while (1):
keys = p.getKeyboardEvents()
#for k in keys:
# print("key=",k,"state=", keys[k])
if ord('1') in keys:
state = keys[ord('1')]
if (state & p.KEY_WAS_RELEASED):
verbose = 1 - verbose
if ord('s') in keys:
state = keys[ord('s')]
if (state & p.KEY_WAS_RELEASED):
segLinkIndex = 1 - segLinkIndex
#print("segLinkIndex=",segLinkIndex)
flags = 0
if (segLinkIndex):
flags = p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX
img = p.getCameraImage(320, 200, flags=flags)
plane = URDF(get_scene("plane-big.urdf.xml")).get_path()
planeId = p.loadURDF(plane)
startPos = [0, 0, 0] # RGB = xyz
startOrientation = p.getQuaternionFromEuler(
[0, 0, 0]) # rotated around which axis? # np.deg2rad(90)
# rotating a standing cylinder around the y axis, puts it flat onto the x axis
xml_path = get_scene("ergojr-penholder-heavy")
robot_file = URDF(xml_path, force_recompile=True).get_path()
robot = p.loadURDF(robot_file, startPos, startOrientation, useFixedBase=1)
for i in range(p.getNumJoints(robot)):
print(p.getJointInfo(robot, i))
#
# c = p.createConstraint(robot,6,robot,7,jointType=p.JOINT_GEAR,jointAxis =[1,0,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
# p.changeConstraint(c, gearRatio=.5, maxForce=100000)
# leftWheels = [6,7]
# motors = [3, 5, 8]
# motors = [3, 5, 8, 11, 14, 17]
motors = [3, 6, 9, 12, 15, 18]
backlash = [4, 5, 8, 11, 14, 17]
runtime = frequency * 10
debugParams = []
p.setGravity(0, 0, 0)
p.setTimeStep(fixedTimeStep)
orn = p.getQuaternionFromEuler([0, 0, 0.4])
p.setRealTimeSimulation(0)
quadruped = p.loadURDF("quadruped/minitaur_v1.urdf", [1, -1, .3],
orn,
useFixedBase=False,
useMaximalCoordinates=useMaximalCoordinates,
flags=p.URDF_USE_IMPLICIT_CYLINDER)
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']
motor_front_rightL_joint = jointNameToId['motor_front_rightL_joint']
knee_front_rightL_link = jointNameToId['knee_front_rightL_link']
hip_front_rightR_link = jointNameToId['hip_front_rightR_link']
knee_front_rightR_link = jointNameToId['knee_front_rightR_link']
motor_front_rightL_link = jointNameToId['motor_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']
def __init__(self, joint_name, bodies, bodyIndex, jointIndex, scale,
model_type):
self.bodies = bodies
self.bodyIndex = bodyIndex
self.jointIndex = jointIndex
self.joint_name = joint_name
_, _, self.jointType, _, _, _, _, _, self.lowerLimit, self.upperLimit, _, _, _, _, _, _, _ = p.getJointInfo(
self.bodies[self.bodyIndex], self.jointIndex)
self.power_coeff = 0
if model_type == "MJCF":
self.scale = scale
else:
self.scale = 1
if self.jointType == p.JOINT_PRISMATIC:
self.upperLimit *= self.scale
self.lowerLimit *= self.scale
]
def euc_dist(posA, posB):
dist = 0.
for i in range(len(posA)):
dist += (posA[i] - posB[i])**2
return dist
p.setRealTimeSimulation(1)
controllers = [e[0] for e in p.getVREvents()]
for j in range(p.getNumJoints(kuka_gripper)):
print(p.getJointInfo(kuka_gripper, j))
while True:
events = p.getVREvents()
for e in (events):
# Only use one controller
###########################################
# This is important: make sure there's only one VR Controller!
if e[0] == controllers[0]:
break
sq_len = euc_dist(p.getLinkState(kuka, 6)[0], e[POSITION])
# A simplistic version of gripper control
#@TO-DO: Add slider for the gripper
p.setGravity(0,0,-10)
useRealTimeSim = 0
p.setTimeStep(1./120.)
p.setRealTimeSimulation(useRealTimeSim) # either this
#track = p.loadURDF("plane.urdf")
track = p.loadSDF("f10_racecar/meshes/barca_track.sdf", globalScaling=1)
otherCar = p.loadURDF("f10_racecar/racecar_differential.urdf", [0,1,.3])
car = p.loadURDF("f10_racecar/racecar_differential.urdf", [0,0,.3])
for wheel in range(p.getNumJoints(car)):
print("joint[",wheel,"]=", p.getJointInfo(car,wheel))
p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=0,force=0)
p.getJointInfo(car,wheel)
wheels = [8,15]
print("----------------")
#p.setJointMotorControl2(car,10,p.VELOCITY_CONTROL,targetVelocity=1,force=10)
c = p.createConstraint(car,9,car,11,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=1, maxForce=10000)
c = p.createConstraint(car,10,car,13,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=-1, maxForce=10000)
c = p.createConstraint(car,9,car,13,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=-1, maxForce=10000)
parser = urdfEditor.UrdfEditor()
parser.initializeFromBulletBody(mb,physicsClientId=org)
parser.saveUrdf("test.urdf")
if (1):
print("\ncreatingMultiBody...................n")
obUid = parser.createMultiBody(physicsClientId=gui)
parser2 = urdfEditor.UrdfEditor()
print("\n###########################################\n")
parser2.initializeFromBulletBody(obUid,physicsClientId=gui)
parser2.saveUrdf("test2.urdf")
for i in range (p.getNumJoints(obUid, physicsClientId=gui)):
p.setJointMotorControl2(obUid,i,p.VELOCITY_CONTROL,targetVelocity=0,force=1,physicsClientId=gui)
print(p.getJointInfo(obUid,i,physicsClientId=gui))
parser=0
p.setRealTimeSimulation(1,physicsClientId=gui)
while (p.getConnectionInfo(physicsClientId=gui)["isConnected"]):
p.stepSimulation(physicsClientId=gui)
time.sleep(0.01)
def run(self):
scores = deque(maxlen=100)
if self.type_of_env == 1:
physicsClient = p.connect(
p.DIRECT) # or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(
pybullet_data.getDataPath()) # optionally
p.setGravity(0, 0, -9.81)
planeId = p.loadURDF("plane.urdf")
#cartStartPos = [0, 0, 0]
#cartStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
cartPoleId = p.loadURDF("cartPole.urdf")
if self.debug:
print('=========================')
print(p.getNumJoints(cartPoleId))
print(
'jointIndex jointName jointType qIndex uIndex flags jointDamping jointFriction jointLowerLimit jointUpperLimit jointMaxForce jointMaxVelocity linkName jointAxis parentFramePos parentFrameOrn parentIndex'
)
print(p.getJointInfo(cartPoleId, 0))
print(p.getJointInfo(cartPoleId, 1))
print('=========================')
reward_for_each_episode = [0 for x in range(self.n_episodes)]
start_time = datetime.datetime.now()
print("timestamp =" + str(start_time))
for e in range(self.n_episodes):
# Initialization
state = self.preprocess_state(self.env.reset())
i = 0
cumulative_reward = 0
time = 0
done = False
start_time_episode = datetime.datetime.now()
for t in range(self.max_t):
if self.render:
self.env.render()
action = self.choose_action(state)
if self.debug:
print()
if action == 1:
print("Action: " + "RIGHT")
else:
print("Action: " + "LEFT")
next_state, reward, done, _ = self.env.step(action)
if self.debug:
print("Reward: " + str(reward))
pole_angle_rad = next_state[2]
pole_angle_degrees = pole_angle_rad * 180 / math.pi
print("New cart position: " + str(next_state[0]))
print("New cart velocity: " + str(next_state[1]))
print("New pole position [degrees]: " +
str(pole_angle_degrees))
print("New pole velocity: " + str(next_state[3]))
print("")
if done:
end_time_episode = datetime.datetime.now()
print(
str(e) + "/" + str(self.n_episodes) +
"; episode steps: " + str(time) + "; duration: " +
str((end_time_episode -
start_time_episode).total_seconds()) + "s "
"; episode reward: " + str(cumulative_reward))
break
next_state = self.preprocess_state(next_state)
self.remember(state, action, reward, next_state, done)
state = next_state
cumulative_reward += reward
reward_for_each_episode[e] = cumulative_reward
time += 1
scores.append(i)
mean_score = np.mean(scores)
self.replay(self.batch_size)
end_time = datetime.datetime.now()
print("timestamp =" + str(end_time))
if self.plot:
fig = plt.figure()
ax = fig.add_subplot(111)
fig.subplots_adjust(top=0.85)
ax.text(0.95,
0.01,
'Execution time ' + str((end_time - start_time)),
verticalalignment='bottom',
horizontalalignment='right',
transform=ax.transAxes,
color='black',
fontsize=12)
plt.plot([x for x in range(self.n_episodes)],
reward_for_each_episode,
'b',
label='Reward')
ysmoothed = gaussian_filter1d(reward_for_each_episode, sigma=2)
plt.plot([x for x in range(self.n_episodes)],
ysmoothed,
'r',
label='Reward smoothed')
plt.axhline(y=self.n_win_ticks,
color='g',
linestyle='-',
label='Threshold ticks')
plt.xlabel('Episode')
plt.ylabel('Reward')
plt.legend()
plt.show()
return e
linkOrientations=linkOrientations,
linkInertialFramePositions=linkInertialFramePositions,
linkInertialFrameOrientations=linkInertialFrameOrientations,
linkParentIndices=indices,
linkJointTypes=jointTypes,
linkJointAxis=axis,
useMaximalCoordinates=useMaximalCoordinates)
p.setGravity(0, 0, -10)
p.setRealTimeSimulation(0)
anistropicFriction = [1, 0.01, 0.01]
p.changeDynamics(sphereUid, -1, lateralFriction=2, anisotropicFriction=anistropicFriction)
p.getNumJoints(sphereUid)
for i in range(p.getNumJoints(sphereUid)):
p.getJointInfo(sphereUid, i)
p.changeDynamics(sphereUid, i, lateralFriction=2, anisotropicFriction=anistropicFriction)
dt = 1. / 240.
SNAKE_NORMAL_PERIOD = 0.1 #1.5
m_wavePeriod = SNAKE_NORMAL_PERIOD
m_waveLength = 4
m_wavePeriod = 1.5
m_waveAmplitude = 0.4
m_waveFront = 0.0
#our steering value
m_steering = 0.0
m_segmentLength = sphereRadius * 2.0
forward = 0
def createWorld(self, model_name, track_name):
self.resetCounter = 0
if model_name is None:
model_name = 'racecar_differential.urdf'
if track_name is None:
track_name = 'barca_track.sdf'
model_path = os.path.join(os.path.dirname(__file__), 'f10_racecar', model_name)
if not os.path.exists(model_path):
raise IOError('Model file {} does not exist'.format(model_path))
track_path = os.path.join(os.path.dirname(__file__), 'f10_racecar/meshes', track_name)
if not os.path.exists(track_path):
raise IOError('Track file {} does not exist'.format(track_path))
self.track = p.loadSDF(track_path, globalScaling=1)
carPosition = [0, 0, 0.15]
carOrientation = p.getQuaternionFromEuler([0, 0, np.pi/3])
self.car = p.loadURDF(model_path, carPosition, carOrientation)
for wheel in range(p.getNumJoints(self.car)):
p.setJointMotorControl2(self.car, wheel, p.VELOCITY_CONTROL, targetVelocity=0, force=0)
p.getJointInfo(self.car, wheel)
c = p.createConstraint(self.car, 9, self.car, 11, jointType=p.JOINT_GEAR, jointAxis=[0,1,0], parentFramePosition=[0,0,0], childFramePosition=[0,0,0])
p.changeConstraint(c, gearRatio=1, maxForce=10000)
c = p.createConstraint(self.car, 10, self.car, 13, jointType=p.JOINT_GEAR, jointAxis=[0,1,0], parentFramePosition=[0,0,0], childFramePosition=[0,0,0])
p.changeConstraint(c, gearRatio=-1, maxForce=10000)
c = p.createConstraint(self.car, 9, self.car, 13, jointType=p.JOINT_GEAR, jointAxis=[0,1,0], parentFramePosition=[0,0,0], childFramePosition=[0,0,0])
p.changeConstraint(c, gearRatio=-1, maxForce=10000)
c = p.createConstraint(self.car, 16, self.car, 18, jointType=p.JOINT_GEAR, jointAxis=[0,1,0], parentFramePosition=[0,0,0], childFramePosition=[0,0,0])
p.changeConstraint(c, gearRatio=1, maxForce=10000)
c = p.createConstraint(self.car, 16, self.car, 19, jointType=p.JOINT_GEAR, jointAxis=[0,1,0], parentFramePosition=[0,0,0], childFramePosition=[0,0,0])
p.changeConstraint(c, gearRatio=-1, maxForce=10000)
c = p.createConstraint(self.car, 17, self.car, 19, jointType=p.JOINT_GEAR, jointAxis=[0,1,0], parentFramePosition=[0,0,0], childFramePosition=[0,0,0])
p.changeConstraint(c, gearRatio=-1, maxForce=10000)
c = p.createConstraint(self.car, 1, self.car, 18, jointType=p.JOINT_GEAR, jointAxis=[0,1,0], parentFramePosition=[0,0,0], childFramePosition=[0,0,0])
p.changeConstraint(c, gearRatio=-1, gearAuxLink=15, maxForce=10000)
c = p.createConstraint(self.car, 3, self.car, 19, jointType=p.JOINT_GEAR, jointAxis=[0,1,0], parentFramePosition=[0,0,0], childFramePosition=[0,0,0])
p.changeConstraint(c, gearRatio=-1, gearAuxLink=15, maxForce=10000)
rayStartLen = 0.25
for i in range(self.numRays):
self.rayFrom.append([rayStartLen*np.sin(-0.5*0.25*2.*np.pi+0.75*2.*np.pi*float(i)/self.numRays),
rayStartLen *
np.cos(-0.5*0.25*2.*np.pi+0.75*2. *
np.pi*float(i)/self.numRays),
0
])
self.rayTo.append([self.rayLen*np.sin(-0.5*0.25*2.*np.pi+0.75*2.*np.pi*float(i)/self.numRays),
self.rayLen *
np.cos(-0.5*0.25*2.*np.pi+0.75*2. *
np.pi*float(i)/self.numRays),
0
])
self.rayIds.append(p.addUserDebugLine(
self.rayFrom[i], self.rayTo[i], self.rayMissColor, parentObjectUniqueId=self.car, parentLinkIndex=self.lidar_joint))
if (cid<0):
p.connect(p.GUI)
p.resetSimulation()
p.setGravity(0,0,-10)
useRealTimeSim = 1
#for video recording (works best on Mac and Linux, not well on Windows)
#p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "racecar.mp4")
p.setRealTimeSimulation(useRealTimeSim) # either this
p.loadURDF("plane.urdf")
#p.loadSDF("stadium.sdf")
car = p.loadURDF("racecar/racecar_differential.urdf") #, [0,0,2],useFixedBase=True)
for i in range (p.getNumJoints(car)):
print (p.getJointInfo(car,i))
for wheel in range(p.getNumJoints(car)):
p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=0,force=0)
p.getJointInfo(car,wheel)
wheels = [8,15]
print("----------------")
#p.setJointMotorControl2(car,10,p.VELOCITY_CONTROL,targetVelocity=1,force=10)
c = p.createConstraint(car,9,car,11,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=1, maxForce=10000)
c = p.createConstraint(car,10,car,13,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=-1, maxForce=10000)
c = p.createConstraint(car,9,car,13,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
def reset_sim(com_0 = np.array([0,0,l9+l10+l11+l12-0.001]), orientation=np.array([0,0,0]), g=-9.79983):
p.resetSimulation()
p.setTimeStep(timestep)
p.setGravity(0,0,g)
planeId = p.loadURDF("plane.urdf")
teoStartOrientation = p.getQuaternionFromEuler(-orientation)
teoStartPosition = [0,0,com_0[2]]
urdf_path = os.path.join(urdf_root,"TEO_wobble.urdf")
teoId = p.loadURDF(urdf_path,teoStartPosition ,teoStartOrientation)
#print("TEO pos =",p.getBasePositionAndOrientation(teoId)[0])
n_joints = p.getNumJoints(teoId)
name2id={}
for i in range(n_joints):
name2id[p.getJointInfo(teoId,i)[1]]=i
n_joints = p.getNumJoints(teoId)
jointIndices = [0] * n_joints
targetPos = [0.0] * n_joints
maxVelocities = [radians(1)] * n_joints
maxForces = [0.0] * n_joints
for i in range(n_joints):
jointIndices[i] = i
maxForces[i] = p.getJointInfo(teoId,i)[10]
targetPos[i] = 0
rightLegAngles= ik.rl_com_from_foot(com_0 , orientation)
leftLegAngles = ik.ll_com_from_foot(com_0 , orientation)
'''
print("com =",com_0)
print("com_f =",com_0+np.array([0,l16,-l12]))
print()
print("Hst0_r =",[0,-l16,l9+l10+l11+l12])
print("rightLegP(com_0) =",rightLegP(com_0))
print("rightLegAngles =",[degrees(q) for q in rightLegAngles])
print()
print("Hst0_l =",[0,l16,l9+l10+l11+l12])
print("leftLegP(com_0) =",leftLegP(com_0))
print("leftLegAngles =",[degrees(q) for q in leftLegAngles])
'''
for i, index in enumerate(rightLegIndices):
if (not np.isnan(rightLegAngles[i])) and (rightLegAngles[i] > p.getJointInfo(teoId, index)[8] and rightLegAngles[i] < p.getJointInfo(teoId, index)[9]):
targetPos[index]=rightLegAngles[i]
for i, index in enumerate(leftLegIndices):
if (not np.isnan(leftLegAngles[i])) and ( leftLegAngles[i] > p.getJointInfo(teoId, index)[8] and leftLegAngles[i] < p.getJointInfo(teoId, index)[9]):
targetPos[index]=leftLegAngles[i]
for jointIndex in range(n_joints):
p.resetJointState(teoId,
jointIndex,
targetPos[jointIndex])
mode = p.POSITION_CONTROL
p.setJointMotorControlArray(teoId,
jointIndices,
controlMode=mode,
forces=maxForces,
targetPositions = targetPos
)
return teoId
if (useRealTimeSimulation):
dt = datetime.now()
t = (dt.second / 60.) * 2. * math.pi
else:
t = t + 0.01
time.sleep(0.01)
for i in range(1):
pos = [2. * math.cos(t), 2. * math.cos(t), 0. + 2. * math.sin(t)]
jointPoses = p.calculateInverseKinematics(sawyerId,
sawyerEndEffectorIndex,
pos,
jointDamping=jd,
solver=ikSolver,
maxNumIterations=100)
#reset the joint state (ignoring all dynamics, not recommended to use during simulation)
for i in range(numJoints):
jointInfo = p.getJointInfo(sawyerId, i)
qIndex = jointInfo[3]
if qIndex > -1:
p.resetJointState(sawyerId, i, jointPoses[qIndex - 7])
ls = p.getLinkState(sawyerId, sawyerEndEffectorIndex)
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
