本文整理汇总了Python中pybullet.resetJointState方法的典型用法代码示例。如果您正苦于以下问题:Python pybullet.resetJointState方法的具体用法?Python pybullet.resetJointState怎么用?Python pybullet.resetJointState使用的例子?那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类pybullet
的用法示例。
在下文中一共展示了pybullet.resetJointState方法的15个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: accurateCalculateInverseKinematics
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import resetJointState [as 别名]
def accurateCalculateInverseKinematics( kukaId, endEffectorId, targetPos, threshold, maxIter):
closeEnough = False
iter = 0
dist2 = 1e30
while (not closeEnough and iter<maxIter):
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,targetPos)
for i in range (numJoints):
p.resetJointState(kukaId,i,jointPoses[i])
ls = p.getLinkState(kukaId,kukaEndEffectorIndex)
newPos = ls[4]
diff = [targetPos[0]-newPos[0],targetPos[1]-newPos[1],targetPos[2]-newPos[2]]
dist2 = (diff[0]*diff[0] + diff[1]*diff[1] + diff[2]*diff[2])
closeEnough = (dist2 < threshold)
iter=iter+1
#print ("Num iter: "+str(iter) + "threshold: "+str(dist2))
return jointPoses
示例2: _reset
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import resetJointState [as 别名]
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)
示例3: _calculate_ik
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import resetJointState [as 别名]
def _calculate_ik(self, targetPos, targetQuat, threshold=1e-5, maxIter=1000, nJoints=6):
closeEnough = False
iter_count = 0
dist2 = None
best_ret, best_dist = None, float('inf')
while (not closeEnough and iter_count < maxIter):
jointPoses = list(p.calculateInverseKinematics(self._armID, 5, targetPos, targetQuat, JOINT_MIN, JOINT_MAX))
for i in range(nJoints):
jointPoses[i] = max(min(jointPoses[i], JOINT_MAX[i]), JOINT_MIN[i])
p.resetJointState(self._armID, i, jointPoses[i])
ls = p.getLinkState(self._armID, 5, computeForwardKinematics=1)
newPos, newQuat = ls[4], ls[5]
dist2 = sum([(targetPos[i] - newPos[i]) ** 2 for i in range(3)])
closeEnough = dist2 < threshold
iter_count += 1
if dist2 < best_dist:
best_ret, best_dist = (jointPoses, newPos, newQuat), dist2
return best_ret
示例4: reset
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import resetJointState [as 别名]
def reset(self, height=0.5, orientation='straight'):
"""Resets the robot for experiment (joints, robot position, ball position, simulator time)
Keyword Arguments:
height {float} -- height of the reset (m) (default: {0.55})
orientation {str} -- orientation (straight, front or back) of the robot (default: {'straight'})
"""
self.lines = []
self.t = 0
self.start = time.time()
# Resets the robot position
orn = [0, 0, 0]
if orientation == 'front':
orn = [0, math.pi/2, 0]
elif orientation == 'back':
orn = [0, -math.pi/2, 0]
self.resetPose([0, 0, height], p.getQuaternionFromEuler(orn))
# Reset the joints to 0
for entry in self.joints.values():
p.resetJointState(self.robot, entry, 0)
示例5: enforce_joint_limits
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import resetJointState [as 别名]
def enforce_joint_limits(self, body):
# Enforce joint limits
joint_states = p.getJointStates(body, jointIndices=list(range(p.getNumJoints(body, physicsClientId=self.id))), physicsClientId=self.id)
joint_positions = np.array([x[0] for x in joint_states])
lower_limits = []
upper_limits = []
for j in range(p.getNumJoints(body, physicsClientId=self.id)):
joint_info = p.getJointInfo(body, j, physicsClientId=self.id)
joint_name = joint_info[1]
joint_pos = joint_positions[j]
lower_limit = joint_info[8]
upper_limit = joint_info[9]
if lower_limit == 0 and upper_limit == -1:
lower_limit = -1e10
upper_limit = 1e10
lower_limits.append(lower_limit)
upper_limits.append(upper_limit)
# print(joint_name, joint_pos, lower_limit, upper_limit)
if joint_pos < lower_limit:
p.resetJointState(body, jointIndex=j, targetValue=lower_limit, targetVelocity=0, physicsClientId=self.id)
elif joint_pos > upper_limit:
p.resetJointState(body, jointIndex=j, targetValue=upper_limit, targetVelocity=0, physicsClientId=self.id)
lower_limits = np.array(lower_limits)
upper_limits = np.array(upper_limits)
return lower_limits, upper_limits
示例6: set_gripper_open_position
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import resetJointState [as 别名]
def set_gripper_open_position(self, robot, position=0, left=True, set_instantly=False, indices=None):
if self.robot_type == 'pr2':
indices_new = [79, 80, 81, 82] if left else [57, 58, 59, 60]
positions = [position]*len(indices_new)
elif self.robot_type == 'baxter':
indices_new = [49, 51] if left else [27, 29]
positions = [position, -position]
elif self.robot_type == 'sawyer':
indices_new = [20, 22]
positions = [position, -position]
elif self.robot_type == 'jaco':
indices_new = [9, 11, 13]
positions = [position, position, position]
if indices is None:
indices = indices_new
if set_instantly:
for i, j in enumerate(indices):
p.resetJointState(robot, jointIndex=j, targetValue=positions[i], targetVelocity=0, physicsClientId=self.id)
p.setJointMotorControlArray(robot, jointIndices=indices, controlMode=p.POSITION_CONTROL, targetPositions=positions, positionGains=np.array([0.05]*len(indices)), forces=[500]*len(indices), physicsClientId=self.id)
示例7: setup_robot_joints
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import resetJointState [as 别名]
def setup_robot_joints(self, robot, robot_joint_indices, lower_limits, upper_limits, randomize_joint_positions=False, default_positions=[1, 1, 0, -1.75, 0, -1.1, -0.5], tool=None):
if randomize_joint_positions:
# Randomize arm joint positions
# Keep trying random joint positions until the end effector is not colliding with anything
retry = True
while retry:
for j, lower_limit, upper_limit in zip(robot_joint_indices, lower_limits, upper_limits):
if lower_limit == -1e10:
lower_limit = -np.pi
upper_limit = np.pi
joint_range = upper_limit - lower_limit
p.resetJointState(robot, jointIndex=j, targetValue=self.np_random.uniform(lower_limit + joint_range/6.0, upper_limit - joint_range/6.0), targetVelocity=0, physicsClientId=self.id)
p.stepSimulation(physicsClientId=self.id)
retry = len(p.getContactPoints(bodyA=robot, physicsClientId=self.id)) > 0
if tool is not None:
retry = retry or (len(p.getContactPoints(bodyA=tool, physicsClientId=self.id)) > 0)
else:
default_positions[default_positions < lower_limits] = lower_limits[default_positions < lower_limits]
default_positions[default_positions > upper_limits] = upper_limits[default_positions > upper_limits]
for i, j in enumerate(robot_joint_indices):
p.resetJointState(robot, jointIndex=j, targetValue=default_positions[i], targetVelocity=0, physicsClientId=self.id)
示例8: step
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import resetJointState [as 别名]
def step(self, action):
yaw = 0
while True:
yaw += -0.75
p.resetDebugVisualizerCamera(cameraDistance=1.2, cameraYaw=yaw, cameraPitch=-20, cameraTargetPosition=[0, 0, 1.0], physicsClientId=self.id)
indices = [4, 5, 6]
# indices = [14, 15, 16]
deltas = [0.01, 0.01, -0.01]
indices = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9]
deltas = [0, 0, 0, 0, 0.01, 0.01, -0.01, 0, 0, 0]
# indices = []
# deltas = []
# indices = np.array([0, 1, 2, 3, 4, 5, 6, 7, 8, 9]) + 10
# deltas = [0, 0, 0, 0, -0.01, 0.01, -0.01, 0, 0, 0]
for i, d in zip(indices, deltas):
joint_position = p.getJointState(self.human, jointIndex=i, physicsClientId=self.id)[0]
if joint_position + d > self.human_lower_limits[i] and joint_position + d < self.human_upper_limits[i]:
p.resetJointState(self.human, jointIndex=i, targetValue=joint_position+d, targetVelocity=0, physicsClientId=self.id)
p.stepSimulation(physicsClientId=self.id)
print('cameraYaw=%.2f, cameraPitch=%.2f, distance=%.2f' % p.getDebugVisualizerCamera(physicsClientId=self.id)[-4:-1])
self.enforce_realistic_human_joint_limits()
time.sleep(0.05)
return [], None, None, None
示例9: enforce_hard_human_joint_limits
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import resetJointState [as 别名]
def enforce_hard_human_joint_limits(self):
if not self.human_controllable_joint_indices:
return
# Enforce joint limits. Sometimes, external forces and break the person's hard joint limits.
joint_states = p.getJointStates(self.human, jointIndices=self.human_controllable_joint_indices, physicsClientId=self.id)
joint_positions = np.array([x[0] for x in joint_states])
if self.human_joint_lower_limits is None:
self.human_joint_lower_limits = []
self.human_joint_upper_limits = []
for i, j in enumerate(self.human_controllable_joint_indices):
joint_info = p.getJointInfo(self.human, j, physicsClientId=self.id)
joint_name = joint_info[1]
joint_pos = joint_positions[i]
lower_limit = joint_info[8]
upper_limit = joint_info[9]
self.human_joint_lower_limits.append(lower_limit)
self.human_joint_upper_limits.append(upper_limit)
# print(joint_name, joint_pos, lower_limit, upper_limit)
for i, j in enumerate(self.human_controllable_joint_indices):
if joint_positions[i] < self.human_joint_lower_limits[i]:
p.resetJointState(self.human, jointIndex=j, targetValue=self.human_joint_lower_limits[i], targetVelocity=0, physicsClientId=self.id)
elif joint_positions[i] > self.human_joint_upper_limits[i]:
p.resetJointState(self.human, jointIndex=j, targetValue=self.human_joint_upper_limits[i], targetVelocity=0, physicsClientId=self.id)
示例10: place
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import resetJointState [as 别名]
def place(self, pos, rot, joints):
pb.resetBasePositionAndOrientation(self.handle, pos, rot)
pb.createConstraint(
self.handle, -1, -1, -1, pb.JOINT_FIXED, pos, [0, 0, 0], rot)
for i, q in enumerate(joints):
pb.resetJointState(self.handle, i, q)
# gripper
pb.resetJointState(self.handle, self.left_knuckle, 0)
pb.resetJointState(self.handle, self.right_knuckle, 0)
pb.resetJointState(self.handle, self.left_finger, 0)
pb.resetJointState(self.handle, self.right_finger, 0)
pb.resetJointState(self.handle, self.left_fingertip, 0)
pb.resetJointState(self.handle, self.right_fingertip, 0)
self.arm(joints,)
self.gripper(0)
示例11: _reset
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import resetJointState [as 别名]
def _reset(self):
# reset state
self.steps = 0
self.done = False
# reset pole on cart in starting poses
p.resetBasePositionAndOrientation(self.cartpole, (0,0,0.08), (0,0,0,1)) # reset cart position and orientation
p.resetJointState(self.cartpole, 0, 0) # reset joint position of pole
for _ in xrange(100): p.stepSimulation()
# give a fixed force push in a random direction to get things going...
theta = (np.random.random()*2-1) if self.random_theta else 0.0
for _ in xrange(self.initial_force_steps):
p.stepSimulation()
p.applyExternalForce(self.cartpole, 0, (theta, 0 , 0), (0, 0, 0), p.WORLD_FRAME)
if self.delay > 0:
time.sleep(self.delay)
# bootstrap state by running for all repeats
for i in xrange(self.repeats):
self.set_state_element_for_repeat(i)
# return this state
return np.copy(self.state)
示例12: Step
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import resetJointState [as 别名]
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])
示例13: reset
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import resetJointState [as 别名]
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)
示例14: sync_ik_robot
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import resetJointState [as 别名]
def sync_ik_robot(self, joint_positions, simulate=False, sync_last=True):
"""
Force the internal robot model to match the provided joint angles.
Args:
joint_positions (list): a list or flat numpy array of joint positions.
simulate (bool): If True, actually use physics simulation, else
write to physics state directly.
sync_last (bool): If False, don't sync the last joint angle. This
is useful for directly controlling the roll at the end effector.
"""
num_joints = len(joint_positions)
if not sync_last:
num_joints -= 1
for i in range(num_joints):
if simulate:
p.setJointMotorControl2(
self.ik_robot,
self.actual[i],
p.POSITION_CONTROL,
targetVelocity=0,
targetPosition=joint_positions[i],
force=500,
positionGain=0.5,
velocityGain=1.,
)
else:
# Note that we use self.actual[i], and not i
p.resetJointState(self.ik_robot, self.actual[i], joint_positions[i])
示例15: sync_ik_robot
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import resetJointState [as 别名]
def sync_ik_robot(self, joint_positions, simulate=False, sync_last=True):
"""
Force the internal robot model to match the provided joint angles.
Args:
joint_positions (list): a list or flat numpy array of joint positions.
simulate (bool): If True, actually use physics simulation, else
write to physics state directly.
sync_last (bool): If False, don't sync the last joint angle. This
is useful for directly controlling the roll at the end effector.
"""
num_joints = len(joint_positions)
if not sync_last:
num_joints -= 1
for i in range(num_joints):
if simulate:
p.setJointMotorControl2(
self.ik_robot,
i,
p.POSITION_CONTROL,
targetVelocity=0,
targetPosition=joint_positions[i],
force=500,
positionGain=0.5,
velocityGain=1.,
)
else:
p.resetJointState(self.ik_robot, i, joint_positions[i], 0)