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 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 setupWorld():
p.resetSimulation()
p.loadURDF("planeMesh.urdf")
kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf",[0,0,10])
for i in range (p.getNumJoints(kukaId)):
p.setJointMotorControl2(kukaId,i,p.POSITION_CONTROL,force=0)
for i in range (numObjects):
cube = p.loadURDF("cube_small.urdf",[0,i*0.02,(i+1)*0.2])
#p.changeDynamics(cube,-1,mass=100)
p.stepSimulation()
p.setGravity(0,0,-10)
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 _step(self, action):
p.stepSimulation()
# time.sleep(self.timeStep)
self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
theta, theta_dot, x, x_dot = self.state
force = action
p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, targetVelocity=(action + self.state[3]))
done = x < -self.x_threshold \
or x > self.x_threshold \
or theta < -self.theta_threshold_radians \
or theta > self.theta_threshold_radians
reward = 1.0
return np.array(self.state), reward, done, {}
def _step(self, action):
force = self.force_mag if action==1 else -self.force_mag
p.setJointMotorControl2(self.cartpole, 0, p.TORQUE_CONTROL, force=force)
p.stepSimulation()
self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
theta, theta_dot, x, x_dot = self.state
done = x < -self.x_threshold \
or x > self.x_threshold \
or theta < -self.theta_threshold_radians \
or theta > self.theta_threshold_radians
done = bool(done)
reward = 1.0
#print("state=",self.state)
return np.array(self.state), reward, done, {}
def setupWorld(self):
numObjects = 50
maximalCoordinates = False
p.resetSimulation()
p.setPhysicsEngineParameter(deterministicOverlappingPairs=1)
p.loadURDF("planeMesh.urdf", useMaximalCoordinates=maximalCoordinates)
kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf", [0, 0, 10],
useMaximalCoordinates=maximalCoordinates)
for i in range(p.getNumJoints(kukaId)):
p.setJointMotorControl2(kukaId, i, p.POSITION_CONTROL, force=0)
for i in range(numObjects):
cube = p.loadURDF("cube_small.urdf", [0, i * 0.02, (i + 1) * 0.2])
#p.changeDynamics(cube,-1,mass=100)
p.stepSimulation()
p.setGravity(0, 0, -10)
def _step(self, action):
p.stepSimulation()
# time.sleep(self.timeStep)
self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
theta, theta_dot, x, x_dot = self.state
dv = 0.1
deltav = [-10.*dv,-5.*dv, -2.*dv, -0.1*dv, 0, 0.1*dv, 2.*dv,5.*dv, 10.*dv][action]
p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, targetVelocity=(deltav + self.state[3]))
done = x < -self.x_threshold \
or x > self.x_threshold \
or theta < -self.theta_threshold_radians \
or theta > self.theta_threshold_radians
reward = 1.0
return np.array(self.state), reward, done, {}
def _reset(self):
# print("-----------reset simulation---------------")
p.resetSimulation()
self.cartpole = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"cartpole.urdf"),[0,0,0])
self.timeStep = 0.01
p.setJointMotorControl2(self.cartpole, 1, p.VELOCITY_CONTROL, force=0)
p.setGravity(0,0, -10)
p.setTimeStep(self.timeStep)
p.setRealTimeSimulation(0)
initialCartPos = self.np_random.uniform(low=-0.5, high=0.5, size=(1,))
initialAngle = self.np_random.uniform(low=-0.5, high=0.5, size=(1,))
p.resetJointState(self.cartpole, 1, initialAngle)
p.resetJointState(self.cartpole, 0, initialCartPos)
self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
return np.array(self.state)
def _reset(self):
# print("-----------reset simulation---------------")
p.resetSimulation()
self.cartpole = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"cartpole.urdf"),[0,0,0])
p.changeDynamics(self.cartpole, -1, linearDamping=0, angularDamping=0)
p.changeDynamics(self.cartpole, 0, linearDamping=0, angularDamping=0)
p.changeDynamics(self.cartpole, 1, linearDamping=0, angularDamping=0)
self.timeStep = 0.02
p.setJointMotorControl2(self.cartpole, 1, p.VELOCITY_CONTROL, force=0)
p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, force=0)
p.setGravity(0,0, -9.8)
p.setTimeStep(self.timeStep)
p.setRealTimeSimulation(0)
randstate = self.np_random.uniform(low=-0.05, high=0.05, size=(4,))
p.resetJointState(self.cartpole, 1, randstate[0], randstate[1])
p.resetJointState(self.cartpole, 0, randstate[2], randstate[3])
#print("randstate=",randstate)
self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
#print("self.state=", self.state)
return np.array(self.state)
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 set_torque(self, torque): p.setJointMotorControl2(bodyIndex=self.bodies[self.bodyIndex], jointIndex=self.jointIndex, controlMode=p.TORQUE_CONTROL, force=torque) #, positionGain=0.1, velocityGain=0.1)
jointDamping=jd,
solver=ikSolver,
maxNumIterations=100,
residualThreshold=.01)
else:
jointPoses = p.calculateInverseKinematics(kukaId,
kukaEndEffectorIndex,
pos,
solver=ikSolver)
if (useSimulation):
for i in range(numJoints):
p.setJointMotorControl2(bodyIndex=kukaId,
jointIndex=i,
controlMode=p.POSITION_CONTROL,
targetPosition=jointPoses[i],
targetVelocity=0,
force=500,
positionGain=0.03,
velocityGain=1)
else:
#reset the joint state (ignoring all dynamics, not recommended to use during simulation)
for i in range(numJoints):
p.resetJointState(kukaId, i, jointPoses[i])
ls = p.getLinkState(kukaId, kukaEndEffectorIndex)
if (hasPrevPose):
p.addUserDebugLine(prevPose, pos, [0, 0, 0.3], 1, trailDuration)
p.addUserDebugLine(prevPose1, ls[4], [1, 0, 0], 1, trailDuration)
prevPose = pos
prevPose1 = ls[4]
hasPrevPose = 1
def step(self, action, stepnum):
done = False
# Use this for better rendering
p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING)
# Velocity control
# Action = [tt_dot1, tt_dot2, ..., tt_dot7]
# action = np.clip(action, self.min_theta_prime, self.max_theta_prime)[0]
for i in range(self.numJoints):
p.setJointMotorControl2(bodyUniqueId=self.downscaleUid,
jointIndex=i,
controlMode=p.VELOCITY_CONTROL,
# targetPosition=self.rest_pose[i] + action[i],
targetVelocity=action[i])
# Step simulation 1kHz
p.stepSimulation()
# Get new state data including: joint positions, velocities, torques
self.state_robot = p.getLinkState(self.downscaleUid,
linkIndex=self.downscaleEndEffectorIndex,
computeForwardKinematics=True)[0]
self.state_robot = self.state_robot - np.array([0, 0, 0.2]) # 0.2 0.17
# print(self.state_robot)
# print(self.target_obj)
joint_states = p.getJointStates(self.downscaleUid,
range(self.numJoints))
joint_info = [p.getJointInfo(self.downscaleUid, i) for i in range(self.numJoints)]
joint_states = [j for j, i in zip(joint_states, joint_info) 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]
# self.state_robot = robotorydownscaleforwardkinematics(joint_positions)
# print(self.state_robot)
w1 = 1
w2 = 0.001
w3 = 0
target_2D = np.array([self.target_obj[0], self.target_obj[1]])
current_2D = np.array([self.state_robot[0], self.state_robot[1]])
z_distance = np.abs(self.target_obj[2] - self.state_robot[2])
self.cartesian_distance = np.linalg.norm(self.target_obj - np.array(self.state_robot), 2)
self.cartesian_2D = np.linalg.norm(target_2D - current_2D, 2)
self.torque_norm = np.linalg.norm(np.array(joint_torques), 2)
# xy_threshold = self.cartesian_2D_threshold_lv2
# z_threshold = self.z_distance_threshold_lv2
# Gradually narrow down the Learning Termination Conditions based on the current number of episode:
if stepnum < 800000:
xy_threshold = self.cartesian_2D_threshold_lv1
z_threshold = self.z_distance_threshold_lv1
else:
xy_threshold = self.cartesian_2D_threshold_lv2
z_threshold = self.z_distance_threshold_lv2
if self.cartesian_distance < xy_threshold and z_distance < z_threshold:
done = True
w3 = 25
if self.step_counter == MAX_ESPISODE_LEN:
done = True
reward = - w1 * self.cartesian_distance - \
w2 * self.torque_norm + \
w3
info = self.state_robot
self.observation = np.concatenate((joint_positions, joint_velocities, self.target_obj, self.state_robot),
axis=0)
return self.observation, reward, done, info
def position_ctl(val):
pos = val * 2 * np.pi
p.setJointMotorControl2(robot, 0, p.POSITION_CONTROL, pos, force=10)
def load_racecar_urdf(self, cwd):
"""Load the URDF of the racecar into the environment
The racecar URDF comes with its own dimensions and
textures, collidables.
"""
# Load robot
self.car = p.loadURDF(Utilities.gen_urdf_path(
"racecar/racecar_differential.urdf", cwd), [0, 0, 2],
useFixedBase=False,
globalScaling=0.5)
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)
# Constraints
#p.setJointMotorControl2(car,10,p.VELOCITY_CONTROL,targetVelocity=1,force=10)
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)
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)
steeringAngle = p.readUserDebugParameter(steeringSlider)
#print(targetVelocity)
for wheel in wheels:
joint_count = p.getNumJoints(hand)
print("num of joints: ", joint_count)
pre = re.compile('index')
for id in range(joint_count):
info = p.getJointInfo(hand, id)
if pre.match(info[1].decode('utf-8')):
print("joint info:", info)
while (1):
pink = convertSensor(pinkId)
middle = convertSensor(middleId)
index = convertSensor(indexId)
thumb = convertSensor(thumbId)
ring = convertSensor(ring_id)
p.setJointMotorControl2(hand, 7, p.POSITION_CONTROL, pi / 4.)
p.setJointMotorControl2(hand, 9, p.POSITION_CONTROL, thumb + pi / 10)
p.setJointMotorControl2(hand, 11, p.POSITION_CONTROL, thumb)
p.setJointMotorControl2(hand, 13, p.POSITION_CONTROL, thumb)
# That's index finger parts
p.setJointMotorControl2(hand, 17, p.POSITION_CONTROL, index)
p.setJointMotorControl2(hand, 19, p.POSITION_CONTROL, index)
p.setJointMotorControl2(hand, 21, p.POSITION_CONTROL, index)
p.setJointMotorControl2(hand, 24, p.POSITION_CONTROL, middle)
p.setJointMotorControl2(hand, 26, p.POSITION_CONTROL, middle)
p.setJointMotorControl2(hand, 28, p.POSITION_CONTROL, middle)
p.setJointMotorControl2(hand, 40, p.POSITION_CONTROL, pink)
p.setJointMotorControl2(hand, 42, p.POSITION_CONTROL, pink)
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']
hip_back_rightR_joint = jointNamesToIndex['hip_back_rightR_joint']
knee_back_rightR_joint = jointNamesToIndex['knee_back_rightR_joint']
motor_back_rightL_joint = jointNamesToIndex['motor_back_rightL_joint']
chassis_left_center = jointNamesToIndex['chassis_left_center']
def set_torque(self, torque):
p.setJointMotorControl2(
bodyIndex=self.bodies[self.bodyIndex],
jointIndex=self.jointIndex,
controlMode=p.TORQUE_CONTROL,
force=torque) #, positionGain=0.1, velocityGain=0.1)
def disable_motor(self):
p.setJointMotorControl2(self.bodies[self.bodyIndex],
self.jointIndex,
controlMode=p.VELOCITY_CONTROL,
force=0)
def set_velocity(self, velocity):
p.setJointMotorControl2(self.bodies[self.bodyIndex],
self.jointIndex,
p.VELOCITY_CONTROL,
targetVelocity=velocity)
def set_position(self, position):
p.setJointMotorControl2(self.bodies[self.bodyIndex],
self.jointIndex,
p.POSITION_CONTROL,
targetPosition=position)
flags=p.URDF_USE_INERTIA_FROM_FILE)
# {'right_rear_wheel_joint': 3, 'right_front_wheel_joint': 7, 'left_rear_wheel_joint': 2, 'left_front_wheel_joint': 5}
wheel_id = [3, 7, 2, 5]
wheel_name = ['BR', 'FR', 'BL', 'BF']
wheel_velocity = [-20, -10, 0, 10]
for j in range(p.getNumJoints(robotId)):
info = p.getJointInfo(robotId, j)
jid = info[0]
jname = info[1].decode("utf-8")
jtype = info[2]
if jtype == p.JOINT_REVOLUTE:
print(jname)
p.setJointMotorControl2(bodyUniqueId=robotId,
jointIndex=jid,
controlMode=p.VELOCITY_CONTROL,
force=0)
p.setPhysicsEngineParameter(enableConeFriction=0)
for wid, wvec in zip(wheel_id, wheel_velocity):
p.setJointMotorControl2(bodyUniqueId=robotId,
jointIndex=wid,
controlMode=p.VELOCITY_CONTROL,
targetVelocity=wvec,
force=50)
for i in range(10000):
p.stepSimulation()
time.sleep(1. / 240.)
org2 = p.connect(p.DIRECT)
org = p.connect(p.SHARED_MEMORY)
if (org<0):
org = p.connect(p.DIRECT)
gui = p.connect(p.GUI)
p.resetSimulation(physicsClientId=org)
#door.urdf, hinge.urdf, duck_vhacd.urdf, r2d2.urdf, quadruped/quadruped.urdf
mb = p.loadURDF("r2d2.urdf", flags=p.URDF_USE_IMPLICIT_CYLINDER, physicsClientId=org)
for i in range(p.getNumJoints(mb,physicsClientId=org)):
p.setJointMotorControl2(mb,i,p.VELOCITY_CONTROL,force=0,physicsClientId=org)
#print("numJoints:",p.getNumJoints(mb,physicsClientId=org))
#print("base name:",p.getBodyInfo(mb,physicsClientId=org))
#for i in range(p.getNumJoints(mb,physicsClientId=org)):
# print("jointInfo(",i,"):",p.getJointInfo(mb,i,physicsClientId=org))
# print("linkState(",i,"):",p.getLinkState(mb,i,physicsClientId=org))
parser = urdfEditor.UrdfEditor()
parser.initializeFromBulletBody(mb,physicsClientId=org)
parser.saveUrdf("test.urdf")
if (1):
print("\ncreatingMultiBody...................n")
def keep_other_joints_fixed(self):
"""Fix other joints pose."""
p.setJointMotorControl2(self.id, TORSO_LIFT_JNT, p.POSITION_CONTROL, 0)
p.setJointMotorControl2(self.id, ARM_FLEX_JNT, p.POSITION_CONTROL, 0)
p.setJointMotorControl2(self.id, ARM_LIFT_JNT, p.POSITION_CONTROL, 0)
p.setJointMotorControl2(self.id, ARM_ROLL_JNT, p.POSITION_CONTROL, 0)
p.setJointMotorControl2(self.id, WRIST_FLEX_JNT, p.POSITION_CONTROL, 0)
p.setJointMotorControl2(self.id, WRIST_ROLL_JNT, p.POSITION_CONTROL, 0)
p.setJointMotorControl2(self.id, HAND_MOTOR_JNT, p.POSITION_CONTROL, 0)
p.setJointMotorControl2(self.id, HAND_L_PROXIMAL_JNT, p.POSITION_CONTROL, 0)
p.setJointMotorControl2(self.id, HAND_L_SPRING_PROXIMAL_JNT, p.POSITION_CONTROL, 0)
p.setJointMotorControl2(self.id, HAND_L_MIMIC_DISTAL_JNT, p.POSITION_CONTROL, 0)
p.setJointMotorControl2(self.id, HAND_L_DISTAL_JNT, p.POSITION_CONTROL, 0)
p.setJointMotorControl2(self.id, HAND_R_PROXIMAL_JNT, p.POSITION_CONTROL, 0)
p.setJointMotorControl2(self.id, HAND_R_SPRING_PROXIMAL_JNT, p.POSITION_CONTROL, 0)
p.setJointMotorControl2(self.id, HAND_R_MIMIC_DISTAL_JNT, p.POSITION_CONTROL, 0)
p.setJointMotorControl2(self.id, HAND_R_DISTAL_JNT, p.POSITION_CONTROL, 0)
def gripper(self, cmd, mode=pb.POSITION_CONTROL):
'''
Gripper commands need to be mirrored to simulate behavior of the actual
UR5.
'''
pb.setJointMotorControl2(self.handle, self.left_knuckle, mode, -cmd)
pb.setJointMotorControl2(self.handle, self.left_inner_knuckle, mode,
-cmd)
pb.setJointMotorControl2(self.handle, self.left_finger, mode, cmd)
pb.setJointMotorControl2(self.handle, self.left_fingertip, mode, cmd)
pb.setJointMotorControl2(self.handle, self.right_knuckle, mode, -cmd)
pb.setJointMotorControl2(self.handle, self.right_inner_knuckle, mode,
-cmd)
pb.setJointMotorControl2(self.handle, self.right_finger, mode, cmd)
pb.setJointMotorControl2(self.handle, self.right_fingertip, mode, cmd)
rhis = RandomPointInHalfSphere(0.0,
0.0369,
0.0437,
radius=0.2022,
height=0.2610,
min_dist=0.0477)
dist = DistanceBetweenObjects(bodyA=robot, bodyB=ball.id, linkA=13, linkB=-1)
for i in range(frequency * 30):
motorPos = []
for m in range(len(motors)):
pos = (math.pi / 2) * p.readUserDebugParameter(debugParams[m])
motorPos.append(pos)
p.setJointMotorControl2(robot,
motors[m],
p.POSITION_CONTROL,
targetPosition=pos)
p.stepSimulation()
time.sleep(1. / frequency)
link_state = p.getLinkState(robot, 13)
read_pos_val = p.readUserDebugParameter(read_pos)
if read_pos_val >= 0.9 and read_pos_once:
print(link_state[0])
print(link_state[4])
read_pos_once = False
if read_pos_val <= 0.1:
read_pos_once = True
def resetPose(self):
kneeFrictionForce = 0
halfpi = 1.57079632679
kneeangle = -2.1834 #halfpi - acos(upper_leg_length / lower_leg_length)
#left front leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftL_joint'],self.motorDir[0]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftL_link'],self.motorDir[0]*kneeangle)
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftR_joint'],self.motorDir[1]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.motorDir[1]*kneeangle)
p.createConstraint(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.quadruped,self.jointNameToId['knee_front_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
self.setMotorAngleByName('motor_front_leftL_joint', self.motorDir[0]*halfpi)
self.setMotorAngleByName('motor_front_leftR_joint', self.motorDir[1]*halfpi)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_leftL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_leftR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
#left back leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftL_joint'],self.motorDir[2]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftL_link'],self.motorDir[2]*kneeangle)
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftR_joint'],self.motorDir[3]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.motorDir[3]*kneeangle)
p.createConstraint(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.quadruped,self.jointNameToId['knee_back_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
self.setMotorAngleByName('motor_back_leftL_joint',self.motorDir[2]*halfpi)
self.setMotorAngleByName('motor_back_leftR_joint',self.motorDir[3]*halfpi)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_leftL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_leftR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
#right front leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightL_joint'],self.motorDir[4]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightL_link'],self.motorDir[4]*kneeangle)
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightR_joint'],self.motorDir[5]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.motorDir[5]*kneeangle)
p.createConstraint(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.quadruped,self.jointNameToId['knee_front_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
self.setMotorAngleByName('motor_front_rightL_joint',self.motorDir[4]*halfpi)
self.setMotorAngleByName('motor_front_rightR_joint',self.motorDir[5]*halfpi)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_rightL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_rightR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
#right back leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightL_joint'],self.motorDir[6]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightL_link'],self.motorDir[6]*kneeangle)
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightR_joint'],self.motorDir[7]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.motorDir[7]*kneeangle)
p.createConstraint(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.quadruped,self.jointNameToId['knee_back_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
self.setMotorAngleByName('motor_back_rightL_joint',self.motorDir[6]*halfpi)
self.setMotorAngleByName('motor_back_rightR_joint',self.motorDir[7]*halfpi)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_rightL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_rightR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
def simulate(self, theta=0.1, withRandom=False):
self.resetSim(withRandom)
pushDir = self.getPushDir(theta, withRandom)
pushStep = self.getPushStep()
x = self.robotInitPoseCart[0]
y = self.robotInitPoseCart[1]
z = self.robotInitPoseCart[2]
for simTime in range(self.simLength):
p.stepSimulation()
# set end effector pose
d = pushDir * pushStep
x += d[0]
y += d[1]
z += d[2]
eePos = [x, y, z]
# compute the inverse kinematics
jointPoses = p.calculateInverseKinematics(
self.kukaId,
self.kukaEndEffectorIndex,
eePos,
self.orn,
jointDamping=self.jd)
for i in range(self.numJoints):
p.setJointMotorControl2(bodyIndex=self.kukaId,
jointIndex=i,
controlMode=p.POSITION_CONTROL,
targetPosition=jointPoses[i],
targetVelocity=0,
force=500,
positionGain=0.3,
velocityGain=1)
# get joint states
ls = p.getLinkState(self.kukaId, self.kukaEndEffectorIndex)
blockPose = p.getBasePositionAndOrientation(self.blockId)
xb = blockPose[0][0]
yb = blockPose[0][1]
roll, pitch, yaw = p.getEulerFromQuaternion(blockPose[1])
# get contact information
contactInfo = p.getContactPoints(self.kukaId, self.blockId)
# get the net contact force between robot and block
if len(contactInfo) > 0:
f_c_temp = 0
for i in range(len(contactInfo)):
f_c_temp += contactInfo[i][9]
self.contactForce[simTime] = f_c_temp
self.contactCount[simTime] = len(contactInfo)
self.traj[simTime, :] = np.array([x, y, xb, yb, yaw])
# contact force mask - get rid of trash in the beginning
self.contactForce[:300] = 0
return self.traj
positionGain=0,
velocityGain=1,
force=[31, 31, 31])
p.setJointMotorControlMultiDof(humanoid4,
j,
p.POSITION_CONTROL,
targetPosition,
targetVelocity=[0, 0, 0],
positionGain=0,
velocityGain=1,
force=[1, 1, 1])
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
p.setJointMotorControl2(humanoid,
j,
p.VELOCITY_CONTROL,
targetVelocity=0,
force=0)
p.setJointMotorControl2(humanoid3,
j,
p.VELOCITY_CONTROL,
targetVelocity=0,
force=31)
p.setJointMotorControl2(humanoid4,
j,
p.VELOCITY_CONTROL,
targetVelocity=0,
force=10)
#print(ji)
print("joint[", j, "].type=", jointTypes[ji[2]])
jointRanges=jr,
restPoses=rp)
else:
if (useOrientation == 1):
jointPoses = p.calculateInverseKinematics(
kukaId, kukaEndEffectorIndex, pos, orn)
else:
jointPoses = p.calculateInverseKinematics(
kukaId, kukaEndEffectorIndex, pos)
if (useSimulation):
for i in range(numJoints):
p.setJointMotorControl2(bodyIndex=kukaId,
jointIndex=i,
controlMode=p.POSITION_CONTROL,
targetPosition=jointPoses[i],
targetVelocity=0,
force=500,
positionGain=0.03,
velocityGain=1)
else:
#reset the joint state (ignoring all dynamics, not recommended to use during simulation)
for i in range(numJoints):
p.resetJointState(kukaId, i, jointPoses[i])
ls = p.getLinkState(kukaId, kukaEndEffectorIndex)
if (hasPrevPose):
p.addUserDebugLine(prevPose, pos, [0, 0, 0.3], 1, trailDuration)
p.addUserDebugLine(prevPose1, ls[4], [1, 0, 0], 1, trailDuration)
prevPose = pos
prevPose1 = ls[4]
hasPrevPose = 1
def moveJoint(pos):
p.setJointMotorControl2(bodyUniqueId=TheArm,
jointIndex=1,
controlMode=p.POSITION_CONTROL,
targetPosition=pos,
maxVelocity=maxSpeed)
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])
p.changeConstraint(c,gearRatio=-1, maxForce=10000)
def torque_ctl(val):
torque = val * 0.01
p.setJointMotorControl2(robot, 0, p.TORQUE_CONTROL, force=torque)
# 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
#for i in range(p.getNumJoints(kuka_gripper)):
i = 4
p.setJointMotorControl2(kuka_gripper,
i,
p.POSITION_CONTROL,
targetPosition=e[ANALOG] * 0.05,
force=10)
i = 6
p.setJointMotorControl2(kuka_gripper,
i,
p.POSITION_CONTROL,
targetPosition=e[ANALOG] * 0.05,
force=10)
if sq_len < THRESHOLD * THRESHOLD:
eef_pos = e[POSITION]
eef_orn = p.getQuaternionFromEuler([0, -math.pi, 0])
joint_pos = p.calculateInverseKinematics(kuka,
6,
eef_pos,
button = p.addUserDebugParameter("value", -1, 1, 1)
# 加载urdf文件
robot = p.loadURDF(
"/home/wsh/Documents/my_DRL_sim/urdf_files/single_joint_urdf/single_joint.urdf",
startPos,
useFixedBase=1,
flags=p.URDF_USE_INERTIA_FROM_FILE)
p.resetDebugVisualizerCamera(0.3, 0, -60, [0, 0, 0])
p.changeVisualShape(
robot, 0, rgbaColor=[174.0 / 255.0, 187.0 / 255.0, 143.0 / 255.0, 0.7])
time_step = 0.001 # 1.0 / 240.0
p.setTimeStep(time_step)
p.setJointMotorControl2(robot, 0, p.POSITION_CONTROL, 0, force=0)
aa = p.getNumConstraints()
def update_obs():
joint_state = p.getJointState(robot, 0)
pos = joint_state[0]
vel = joint_state[1]
return pos, vel
def position_ctl(val):
pos = val * 2 * np.pi
p.setJointMotorControl2(robot, 0, p.POSITION_CONTROL, pos, force=10)
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)
a = 0.4
b = 0.2
c = 0.01
omega1 = 0
omega2 = 2
omega3 = 0
omega4 = 2
## Simulate the thing
maxstep = 10000
all_target_1 = numpy.zeros((maxstep, 2)) # joint 0
all_target_2 = numpy.zeros((maxstep, 2)) # joint 3
def disable_motor(self): p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,controlMode=p.POSITION_CONTROL, targetPosition=0, targetVelocity=0, positionGain=0.1, velocityGain=0.1, force=0)
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']
hip_back_rightR_joint = jointNamesToIndex['hip_back_rightR_joint']
knee_back_rightR_joint = jointNamesToIndex['knee_back_rightR_joint']
motor_back_rightL_joint = jointNamesToIndex['motor_back_rightL_joint']
chassis_left_center = jointNamesToIndex['chassis_left_center']
motor_front_leftL_joint = jointNamesToIndex['motor_front_leftL_joint']
import time
useMaximalCoordinates = False
p.connect(p.GUI)
pole = p.loadURDF("cartpole.urdf", [0, 0, 0], useMaximalCoordinates=useMaximalCoordinates)
pole2 = p.loadURDF("cartpole.urdf", [0, 1, 0], useMaximalCoordinates=useMaximalCoordinates)
pole3 = p.loadURDF("cartpole.urdf", [0, 2, 0], useMaximalCoordinates=useMaximalCoordinates)
pole4 = p.loadURDF("cartpole.urdf", [0, 3, 0], useMaximalCoordinates=useMaximalCoordinates)
exPD = PDControllerExplicitMultiDof(p)
sPD = PDControllerStable(p)
for i in range(p.getNumJoints(pole2)):
#disable default constraint-based motors
p.setJointMotorControl2(pole, i, p.POSITION_CONTROL, targetPosition=0, force=0)
p.setJointMotorControl2(pole2, i, p.POSITION_CONTROL, targetPosition=0, force=0)
p.setJointMotorControl2(pole3, i, p.POSITION_CONTROL, targetPosition=0, force=0)
p.setJointMotorControl2(pole4, i, p.POSITION_CONTROL, targetPosition=0, force=0)
#print("joint",i,"=",p.getJointInfo(pole2,i))
timeStepId = p.addUserDebugParameter("timeStep", 0.001, 0.1, 0.01)
desiredPosCartId = p.addUserDebugParameter("desiredPosCart", -10, 10, 2)
desiredVelCartId = p.addUserDebugParameter("desiredVelCart", -10, 10, 0)
kpCartId = p.addUserDebugParameter("kpCart", 0, 500, 1300)
kdCartId = p.addUserDebugParameter("kdCart", 0, 300, 150)
maxForceCartId = p.addUserDebugParameter("maxForceCart", 0, 5000, 1000)
textColor = [1, 1, 1]
shift = 0.05
import pybullet as p
import time
p.connect(p.GUI)
cartpole=p.loadURDF("cartpole.urdf")
p.setRealTimeSimulation(1)
p.setJointMotorControl2(cartpole,1,p.POSITION_CONTROL,targetPosition=1000,targetVelocity=0,force=1000, positionGain=1, velocityGain=0, maxVelocity=0.5)
while (1):
p.setGravity(0,0,-10)
js = p.getJointState(cartpole,1)
print("position=",js[0],"velocity=",js[1])
time.sleep(0.01)
#this snake may be going backwards, but who can tell ;)
for joint in range(p.getNumJoints(sphereUid)):
segment = joint #numMuscles-1-joint
#map segment to phase
phase = (m_waveFront - (segment + 1) * m_segmentLength) / m_waveLength
phase -= math.floor(phase)
phase *= math.pi * 2.0
#map phase to curvature
targetPos = math.sin(phase) * scaleStart * m_waveAmplitude
#// steer snake by squashing +ve or -ve side of sin curve
if (m_steering > 0 and targetPos < 0):
targetPos *= 1.0 / (1.0 + m_steering)
if (m_steering < 0 and targetPos > 0):
targetPos *= 1.0 / (1.0 - m_steering)
#set our motor
p.setJointMotorControl2(sphereUid,
joint,
p.POSITION_CONTROL,
targetPosition=targetPos + m_steering,
force=30)
#wave keeps track of where the wave is in time
m_waveFront += dt / m_wavePeriod * m_waveLength
p.stepSimulation()
time.sleep(dt)
with open(pd.getDataPath()+"/laikago/data1.txt","r") as filestream:
for line in filestream:
print("line=",line)
maxForce = p.readUserDebugParameter(maxForceId)
currentline = line.split(",")
#print (currentline)
#print("-----")
frame = currentline[0]
t = currentline[1]
#print("frame[",frame,"]")
joints=currentline[2:14]
#print("joints=",joints)
for j in range (12):
targetPos = float(joints[j])
p.setJointMotorControl2(quadruped,jointIds[j],p.POSITION_CONTROL,jointDirections[j]*targetPos+jointOffsets[j], force=maxForce)
p.stepSimulation()
for lower_leg in lower_legs:
#print("points for ", quadruped, " link: ", lower_leg)
pts = p.getContactPoints(quadruped,-1, lower_leg)
#print("num points=",len(pts))
#for pt in pts:
# print(pt[9])
time.sleep(1./500.)
for j in range (p.getNumJoints(quadruped)):
p.changeDynamics(quadruped,j,linearDamping=0, angularDamping=0)
info = p.getJointInfo(quadruped,j)
js = p.getJointState(quadruped,j)
#print(info)
for line in filestream:
print("line=", line)
maxForce = p.readUserDebugParameter(maxForceId)
currentline = line.split(",")
#print (currentline)
#print("-----")
frame = currentline[0]
t = currentline[1]
#print("frame[",frame,"]")
joints = currentline[2:14]
#print("joints=",joints)
for j in range(12):
targetPos = float(joints[j])
p.setJointMotorControl2(quadruped,
jointIds[j],
p.POSITION_CONTROL,
jointDirections[j] * targetPos +
jointOffsets[j],
force=maxForce)
p.stepSimulation()
for lower_leg in lower_legs:
#print("points for ", quadruped, " link: ", lower_leg)
pts = p.getContactPoints(quadruped, -1, lower_leg)
#print("num points=",len(pts))
#for pt in pts:
# print(pt[9])
time.sleep(1. / 500.)
for j in range(p.getNumJoints(quadruped)):
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(quadruped, j)
js = p.getJointState(quadruped, j)
def parse_robot(self, bodies):
"""
Parse the robot to get properties including joint information and mass
:param bodies: body ids in pybullet
:return: parts, joints, ordered_joints, robot_body, robot_mass
"""
assert len(bodies) == 1, 'robot body has length > 1'
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 = []
robot_mass = 0.0
part_name, robot_name = p.getBodyInfo(bodies[0])
part_name = part_name.decode("utf8")
parts[part_name] = BodyPart(part_name,
bodies,
0,
-1,
self.scale,
model_type=self.model_type)
# By default, use base_link as self.robot_body
if self.robot_name == part_name:
self.robot_body = parts[part_name]
for j in range(p.getNumJoints(bodies[0])):
robot_mass += p.getDynamicsInfo(bodies[0], j)[0]
p.setJointMotorControl2(bodies[0],
j,
p.POSITION_CONTROL,
positionGain=0.1,
velocityGain=0.1,
force=0)
_, joint_name, joint_type, _, _, _, _, _, _, _, _, _, part_name, _, _, _, _ = p.getJointInfo(
bodies[0], j)
joint_name = joint_name.decode("utf8")
part_name = part_name.decode("utf8")
parts[part_name] = BodyPart(part_name,
bodies,
0,
j,
self.scale,
model_type=self.model_type)
# If self.robot_name is not base_link, but a body part, use it as self.robot_body
if self.robot_name == part_name:
self.robot_body = parts[part_name]
if joint_name[:6] == "ignore":
Joint(joint_name,
bodies,
0,
j,
self.scale,
model_type=self.model_type).disable_motor()
continue
if joint_name[:8] != "jointfix" and joint_type != p.JOINT_FIXED:
joints[joint_name] = Joint(joint_name,
bodies,
0,
j,
self.scale,
model_type=self.model_type)
ordered_joints.append(joints[joint_name])
joints[joint_name].power_coef = 100.0
if self.robot_body is None:
raise Exception('robot body not initialized.')
return parts, joints, ordered_joints, self.robot_body, robot_mass
#python script with hardcoded values, assumes that you run the vr_kuka_setup.py first
import pybullet as p
p.connect(p.SHARED_MEMORY)
pr2_gripper = 2
pr2_cid = 1
CONTROLLER_ID = 0
POSITION = 1
ORIENTATION = 2
ANALOG = 3
BUTTONS = 6
gripper_max_joint = 0.550569
while True:
events = p.getVREvents()
for e in (events):
if e[CONTROLLER_ID] == 3: # To make sure we only get the value for one of the remotes
p.changeConstraint(pr2_cid, e[POSITION], e[ORIENTATION], maxForce=500)
p.setJointMotorControl2(pr2_gripper,
0,
controlMode=p.POSITION_CONTROL,
targetPosition=gripper_max_joint - e[ANALOG] * gripper_max_joint,
force=1.0)
p.setJointMotorControl2(pr2_gripper,
2,
controlMode=p.POSITION_CONTROL,
targetPosition=gripper_max_joint - e[ANALOG] * gripper_max_joint,
force=1.1)
p.changeDynamics(quadruped, -1, localInertiaDiagonal=localInertiaDiagonal)
#for i in range (nJoints):
# p.changeDynamics(quadruped,i,localInertiaDiagonal=[0.000001,0.000001,0.000001])
drawInertiaBox(quadruped, -1, [1, 0, 0])
#drawInertiaBox(quadruped,motor_front_rightR_joint, [1,0,0])
for i in range(nJoints):
drawInertiaBox(quadruped, i, [0, 1, 0])
if (useMaximalCoordinates):
steps = 400
for aa in range(steps):
p.setJointMotorControl2(quadruped, motor_front_leftL_joint,
p.POSITION_CONTROL,
motordir[0] * halfpi * float(aa) / steps)
p.setJointMotorControl2(quadruped, motor_front_leftR_joint,
p.POSITION_CONTROL,
motordir[1] * halfpi * float(aa) / steps)
p.setJointMotorControl2(quadruped, motor_back_leftL_joint,
p.POSITION_CONTROL,
motordir[2] * halfpi * float(aa) / steps)
p.setJointMotorControl2(quadruped, motor_back_leftR_joint,
p.POSITION_CONTROL,
motordir[3] * halfpi * float(aa) / steps)
p.setJointMotorControl2(quadruped, motor_front_rightL_joint,
p.POSITION_CONTROL,
motordir[4] * halfpi * float(aa) / steps)
p.setJointMotorControl2(quadruped, motor_front_rightR_joint,
p.POSITION_CONTROL,
p.resetJointState(door,1,angle)
angleread = p.getJointState(door,1)
print("angleread = ",angleread)
prevTime = time.time()
angle = math.pi*0.5
count=0
while (1):
count+=1
if (count==12):
p.stopStateLogging(logId)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
curTime = time.time()
diff = curTime - prevTime
#every second, swap target angle
if (diff>1):
angle = - angle
prevTime = curTime
if position_control:
p.setJointMotorControl2(door,1,p.POSITION_CONTROL, targetPosition = angle, positionGain=10.1, velocityGain=1, force=11.001)
else:
p.setJointMotorControl2(door,1,p.VELOCITY_CONTROL, targetVelocity=1, force=1011)
#contacts = p.getContactPoints()
#print("contacts=",contacts)
p.stepSimulation()
#time.sleep(1./240.)
targetVelocitySliderr = p.addUserDebugParameter("wheelVelocityright", -30, 30,
0)
targetPositonSlider = p.addUserDebugParameter('joint10Position', -2.7, 2.7, 0)
maxForceSlider = p.addUserDebugParameter("maxForce", -10, 10, 10)
#steeringSlider = p.addUserDebugParameter("steering",-0.5,0.5,0)
while (True):
maxForce = p.readUserDebugParameter(maxForceSlider)
targetVelocityl = p.readUserDebugParameter(targetVelocitySliderl)
targetVelocityr = p.readUserDebugParameter(targetVelocitySliderr)
#steeringAngle = p.readUserDebugParameter(steeringSlider)
targetArmPosition = p.readUserDebugParameter(targetPositonSlider)
p.setJointMotorControl2(mm,
1,
controlMode=p.VELOCITY_CONTROL,
targetVelocity=targetVelocityl,
force=maxForce)
p.setJointMotorControl2(mm,
2,
controlMode=p.VELOCITY_CONTROL,
targetVelocity=targetVelocityr,
force=maxForce)
#p.setJointMotorControl2(mm, 9, controlMode=p.POSITION_CONTROL, targetPosition = targetArmPosition)
keys = p.getKeyboardEvents()
shift = 0.01
wheelVelocities = [0, 0]
speed = 1.0
for k in keys:
def applyAction(self, motorCommands):
#print ("self.numJoints")
#print (self.numJoints)
if (self.useInverseKinematics):
dx = motorCommands[0]
dy = motorCommands[1]
dz = motorCommands[2]
da = motorCommands[3]
fingerAngle = motorCommands[4]
state = p.getLinkState(self.kukaUid,self.kukaEndEffectorIndex)
actualEndEffectorPos = state[0]
#print("pos[2] (getLinkState(kukaEndEffectorIndex)")
#print(actualEndEffectorPos[2])
self.endEffectorPos[0] = self.endEffectorPos[0]+dx
if (self.endEffectorPos[0]>0.65):
self.endEffectorPos[0]=0.65
if (self.endEffectorPos[0]<0.50):
self.endEffectorPos[0]=0.50
self.endEffectorPos[1] = self.endEffectorPos[1]+dy
if (self.endEffectorPos[1]<-0.17):
self.endEffectorPos[1]=-0.17
if (self.endEffectorPos[1]>0.22):
self.endEffectorPos[1]=0.22
#print ("self.endEffectorPos[2]")
#print (self.endEffectorPos[2])
#print("actualEndEffectorPos[2]")
#print(actualEndEffectorPos[2])
#if (dz<0 or actualEndEffectorPos[2]<0.5):
self.endEffectorPos[2] = self.endEffectorPos[2]+dz
self.endEffectorAngle = self.endEffectorAngle + da
pos = self.endEffectorPos
orn = p.getQuaternionFromEuler([0,-math.pi,0]) # -math.pi,yaw])
if (self.useNullSpace==1):
if (self.useOrientation==1):
jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos,orn,self.ll,self.ul,self.jr,self.rp)
else:
jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos,lowerLimits=self.ll, upperLimits=self.ul, jointRanges=self.jr, restPoses=self.rp)
else:
if (self.useOrientation==1):
jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos,orn,jointDamping=self.jd)
else:
jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos)
#print("jointPoses")
#print(jointPoses)
#print("self.kukaEndEffectorIndex")
#print(self.kukaEndEffectorIndex)
if (self.useSimulation):
for i in range (self.kukaEndEffectorIndex+1):
#print(i)
p.setJointMotorControl2(bodyUniqueId=self.kukaUid,jointIndex=i,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[i],targetVelocity=0,force=self.maxForce,maxVelocity=self.maxVelocity, positionGain=0.3,velocityGain=1)
else:
#reset the joint state (ignoring all dynamics, not recommended to use during simulation)
for i in range (self.numJoints):
p.resetJointState(self.kukaUid,i,jointPoses[i])
#fingers
p.setJointMotorControl2(self.kukaUid,7,p.POSITION_CONTROL,targetPosition=self.endEffectorAngle,force=self.maxForce)
p.setJointMotorControl2(self.kukaUid,8,p.POSITION_CONTROL,targetPosition=-fingerAngle,force=self.fingerAForce)
p.setJointMotorControl2(self.kukaUid,11,p.POSITION_CONTROL,targetPosition=fingerAngle,force=self.fingerBForce)
p.setJointMotorControl2(self.kukaUid,10,p.POSITION_CONTROL,targetPosition=0,force=self.fingerTipForce)
p.setJointMotorControl2(self.kukaUid,13,p.POSITION_CONTROL,targetPosition=0,force=self.fingerTipForce)
else:
for action in range (len(motorCommands)):
motor = self.motorIndices[action]
p.setJointMotorControl2(self.kukaUid,motor,p.POSITION_CONTROL,targetPosition=motorCommands[action],force=self.maxForce)
targets= [(-0.2,0.2,0.2, -0.2,0.2,0.2, 0.2,0.2,0.2, 0.2,0.2,0.2),
(-0.3,0.3,0.3, -0.3,0.3,0.3, 0.3,0.3,0.3, 0.3,0.3,0.3)]
ti = 0
while time.time() < t_end:
pybullet.stepSimulation()
if co == 2000:
print( co )
ti = (ti+1)%len(targets)
co=0
for j in range(12):
pybullet.setJointMotorControl2(bodyUniqueId=obj,
jointIndex=j,
controlMode=pybullet.POSITION_CONTROL,
targetPosition =targets[ti][j],
force = maxForce)
co = co+1
#time.sleep(0.1)
print ("finished")
#remove all objects
pybullet.resetSimulation()
#disconnect from the physics server
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])
# apply pid law to calulate this value,use Kp and Kd variable above
# and tune em properly.
speed_correction = 10
error = turn(speed_correction, last_error, hsv_lower, hsv_upper)
last_error=error #initialize accordingly
for k, v in keys.items():
if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_IS_DOWN) and PID_CONTROL==False):
targetVel = 2
for joint in range(1,3):
p.setJointMotorControl2(husky,2*joint, p.VELOCITY_CONTROL, targetVelocity =targetVel,force = maxForce)
for joint in range(1,3):
p.setJointMotorControl2(husky,2*joint+1, p.VELOCITY_CONTROL,targetVelocity =-1*targetVel,force = maxForce)
p.stepSimulation()
if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_RELEASED)):
targetVel = 0
for joint in range(2, 6):
p.setJointMotorControl2(husky, joint, p.VELOCITY_CONTROL,targetVelocity = targetVel,force = maxForce)
if (k == p.B3G_LEFT_ARROW and (v & p.KEY_IS_DOWN)and PID_CONTROL==False):
targetVel = 2
for joint in range(1,3):
p.setJointMotorControl2(husky,2* joint+1, p.VELOCITY_CONTROL,targetVelocity = targetVel,force = maxForce)
for joint in range(1,3):
p.setJointMotorControl2(husky,2* joint, p.VELOCITY_CONTROL,targetVelocity =-1* targetVel,force = maxForce)
objects = [p.loadURDF("jenga/jenga.urdf", 0.800000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("table/table.urdf", 1.000000,-0.200000,0.000000,0.000000,0.000000,0.707107,0.707107)]
objects = [p.loadURDF("cube_small.urdf", 0.950000,-0.100000,0.700000,0.000000,0.000000,0.707107,0.707107)]
objects = [p.loadURDF("sphere_small.urdf", 0.850000,-0.400000,0.700000,0.000000,0.000000,0.707107,0.707107)]
#load the MuJoCo MJCF hand
objects = p.loadMJCF("MPL/mpl2.xml")
hand=objects[0]
ho = p.getQuaternionFromEuler([3.14,-3.14/2,0])
hand_cid = p.createConstraint(hand,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[-0.05,0,0.02],[0.500000,0.300006,0.700000],ho)
print ("hand_cid")
print (hand_cid)
for i in range (p.getNumJoints(hand)):
p.setJointMotorControl2(hand,i,p.POSITION_CONTROL,0,0)
#clamp in range 400-600
#minV = 400
#maxV = 600
minV = 250
maxV = 450
POSITION=1
ORIENTATION=2
BUTTONS=6
p.setRealTimeSimulation(1)
def turn(error, prev_error, hsv_lower, hsv_upper):
baseSpeed = 0
p.changeVisualShape(husky, -1, rgbaColor=[1, 1, 1, 0.1])
width = 512
height = 512
fov = 60
aspect = width / height
near = 0.02
far = 5
#targetVel_L = speed
#targetVel_R = speed
x = p.getLinkState(husky,3)
y = p.getLinkState(husky,5)
z = p.getLinkState(husky,8)
# print(x)
# print(y)
# print(z)
view_matrix = p.computeViewMatrix( z[0], [z[0][0] + 10*(x[0][0] - y[0][0]) , z[0][1] + 10*(x[0][1] - y[0][1]), z[0][2] + 10*(x[0][2] - y[0][2])], [0, 0, 1])
# view_matrix = p.computeViewMatrix( z[0], [d1, d2, d3] , [0, 0, 1])
projection_matrix = p.computeProjectionMatrixFOV(fov, aspect, near, far)
# Get depth values using the OpenGL renderer
images = p.getCameraImage(width,
height,
view_matrix,
projection_matrix,
shadow=True,
renderer=p.ER_BULLET_HARDWARE_OPENGL)
rgb_opengl = np.reshape(images[2][:,:,:3], (height, width, 3))
cv2.imwrite("cam.jpg",rgb_opengl)
rgb_opengl = cv2.imread("cam.jpg")
rgb_opengl = cv2.resize(rgb_opengl,(50,50))
hsv = cv2.cvtColor(rgb_opengl,cv2.COLOR_BGR2HSV)
#hsv = cv2.medianBlur(hsv,3)
final = cv2.inRange(hsv, hsv_lower, hsv_upper)
# # print(rgb_opengl.shape," ",rgb_opengl.dtype)
# cv2.imshow("segment",final)
# cv2.waitKey(1)
det_color = cv2.bitwise_and(rgb_opengl,rgb_opengl,mask=final)
contours, _1 = cv2.findContours(final, cv2.RETR_LIST, cv2.CHAIN_APPROX_NONE)
targetVel_R = baseSpeed + error
targetVel_L = baseSpeed - error
if(len(contours)>=1):
contours = sorted(contours, key=lambda x:cv2.contourArea(x), reverse=True)
if (cv2.contourArea(contours[0])>100):
M = cv2.moments(contours[0])
if(M['m00']!=0):
cx = int(M['m10']/M['m00'])
print(cx, cv2.contourArea(contours[0]), len(contours))
error = (cx - 25)
pid_val = Kp*error + Kd*(error-prev_error)
print(Kd*(error-prev_error))
targetVel_R = baseSpeed + pid_val
targetVel_L = baseSpeed - pid_val
for joint in range(1,3):
p.setJointMotorControl2(husky,2*joint, p.VELOCITY_CONTROL, targetVelocity =targetVel_R,force = maxForce)
for joint in range(1,3):
p.setJointMotorControl2(husky,(2*joint) +1, p.VELOCITY_CONTROL,targetVelocity =targetVel_L,force = maxForce)
return error
linkPositions=linkPositions,
linkOrientations=linkOrientations,
linkInertialFramePositions=linkInertialFramePositions,
linkInertialFrameOrientations=linkInertialFrameOrientations,
linkParentIndices=indices,
linkJointTypes=jointTypes,
linkJointAxis=axis)
p.changeDynamics(boxId, -1, spinningFriction=0.001, rollingFriction=0.001, linearDamping=0.0)
print(p.getNumJoints(boxId))
for joint in range(p.getNumJoints(boxId)):
targetVelocity = 10
if (i % 2):
targetVelocity = -10
p.setJointMotorControl2(boxId,
joint,
p.VELOCITY_CONTROL,
targetVelocity=targetVelocity,
force=100)
segmentStart = segmentStart - 1.1
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
while (1):
camData = p.getDebugVisualizerCamera()
viewMat = camData[2]
projMat = camData[3]
p.getCameraImage(256,
256,
viewMatrix=viewMat,
projectionMatrix=projMat,
renderer=p.ER_BULLET_HARDWARE_OPENGL)
keys = p.getKeyboardEvents()
steps = 0
while True:
time.sleep(0.01)
steps = steps + 1
gravX = p.readUserDebugParameter(gravXid)
gravY = p.readUserDebugParameter(gravYid)
gravZ = p.readUserDebugParameter(gravZid)
baseAngle = p.readUserDebugParameter(baseAngleid)
neckAngle = p.readUserDebugParameter(neckAngleid)
p.setGravity(gravX, gravY, gravZ)
if (steps < 100):
p.setJointMotorControl2(luxo,
BASE_JOINT_ID,
p.POSITION_CONTROL,
targetPosition=BASE_ANGLE,
force=maxForce)
p.setJointMotorControl2(luxo,
NECK_JOINT_ID,
p.POSITION_CONTROL,
targetPosition=NECK_ANGLE,
force=maxForce)
else:
p.setJointMotorControl2(luxo,
NECK_JOINT_ID,
p.TORQUE_CONTROL,
force=neckAngle)
p.setJointMotorControl2(luxo,
BASE_JOINT_ID,
p.TORQUE_CONTROL,
print (pr2_gripper)
jointPositions=[ 0.550569, 0.000000, 0.549657, 0.000000 ]
for jointIndex in range (p.getNumJoints(pr2_gripper)):
p.resetJointState(pr2_gripper,jointIndex,jointPositions[jointIndex])
pr2_cid = p.createConstraint(pr2_gripper,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0.2,0,0],[0.500000,0.300006,0.700000])
print ("pr2_cid")
print (pr2_cid)
objects = [p.loadURDF("kuka_iiwa/model_vr_limits.urdf", 1.400000,-0.200000,0.600000,0.000000,0.000000,0.000000,1.000000)]
kuka = objects[0]
jointPositions=[ -0.000000, -0.000000, 0.000000, 1.570793, 0.000000, -1.036725, 0.000001 ]
for jointIndex in range (p.getNumJoints(kuka)):
p.resetJointState(kuka,jointIndex,jointPositions[jointIndex])
p.setJointMotorControl2(kuka,jointIndex,p.POSITION_CONTROL,jointPositions[jointIndex],0)
objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.700000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.800000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.900000,0.000000,0.000000,0.000000,1.000000)]
objects = p.loadSDF("gripper/wsg50_one_motor_gripper_new_free_base.sdf")
kuka_gripper = objects[0]
print ("kuka gripper=")
print(kuka_gripper)
p.resetBasePositionAndOrientation(kuka_gripper,[0.923103,-0.200000,1.250036],[-0.000000,0.964531,-0.000002,-0.263970])
jointPositions=[ 0.000000, -0.011130, -0.206421, 0.205143, -0.009999, 0.000000, -0.010055, 0.000000 ]
for jointIndex in range (p.getNumJoints(kuka_gripper)):
p.resetJointState(kuka_gripper,jointIndex,jointPositions[jointIndex])
p.setJointMotorControl2(kuka_gripper,jointIndex,p.POSITION_CONTROL,jointPositions[jointIndex],0)
gravId = p.addUserDebugParameter("gravity", -10, 10, -10)
jointIds = []
paramIds = []
p.setPhysicsEngineParameter(numSolverIterations=10)
p.changeDynamics(humanoid, -1, linearDamping=0, angularDamping=0)
for j in range(p.getNumJoints(humanoid)):
p.changeDynamics(humanoid, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(humanoid, 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))
p.setRealTimeSimulation(1)
while (1):
p.setGravity(0, 0, p.readUserDebugParameter(gravId))
for i in range(len(paramIds)):
c = paramIds[i]
targetPos = p.readUserDebugParameter(c)
p.setJointMotorControl2(humanoid,
jointIds[i],
p.POSITION_CONTROL,
targetPos,
force=5 * 240.)
time.sleep(0.01)
