本文整理汇总了Python中pybullet.getJointStates方法的典型用法代码示例。如果您正苦于以下问题:Python pybullet.getJointStates方法的具体用法?Python pybullet.getJointStates怎么用?Python pybullet.getJointStates使用的例子?那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类pybullet
的用法示例。
在下文中一共展示了pybullet.getJointStates方法的15个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: _get_obs
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getJointStates [as 别名]
def _get_obs(self, forces, forces_human):
torso_pos = np.array(p.getLinkState(self.robot, 15 if self.robot_type == 'pr2' else 0, computeForwardKinematics=True, physicsClientId=self.id)[0])
tool_pos, tool_orient = p.getBasePositionAndOrientation(self.cup, physicsClientId=self.id)
robot_joint_states = p.getJointStates(self.robot, jointIndices=self.robot_right_arm_joint_indices, physicsClientId=self.id)
robot_joint_positions = np.array([x[0] for x in robot_joint_states])
robot_pos, robot_orient = p.getBasePositionAndOrientation(self.robot, physicsClientId=self.id)
if self.human_control:
human_pos = np.array(p.getBasePositionAndOrientation(self.human, physicsClientId=self.id)[0])
human_joint_states = p.getJointStates(self.human, jointIndices=self.human_controllable_joint_indices, physicsClientId=self.id)
human_joint_positions = np.array([x[0] for x in human_joint_states])
head_pos, head_orient = p.getLinkState(self.human, 23, computeForwardKinematics=True, physicsClientId=self.id)[:2]
robot_obs = np.concatenate([tool_pos-torso_pos, tool_orient, tool_pos - self.target_pos, robot_joint_positions, head_pos-torso_pos, head_orient, forces]).ravel()
if self.human_control:
human_obs = np.concatenate([tool_pos-human_pos, tool_orient, tool_pos - self.target_pos, human_joint_positions, head_pos-human_pos, head_orient, forces_human]).ravel()
else:
human_obs = []
return np.concatenate([robot_obs, human_obs]).ravel()
示例2: _get_obs
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getJointStates [as 别名]
def _get_obs(self, forces, forces_human):
torso_pos = np.array(p.getLinkState(self.robot, 15 if self.robot_type == 'pr2' else 0, computeForwardKinematics=True, physicsClientId=self.id)[0])
spoon_pos, spoon_orient = p.getBasePositionAndOrientation(self.spoon, physicsClientId=self.id)
robot_right_joint_states = p.getJointStates(self.robot, jointIndices=self.robot_right_arm_joint_indices, physicsClientId=self.id)
robot_right_joint_positions = np.array([x[0] for x in robot_right_joint_states])
robot_pos, robot_orient = p.getBasePositionAndOrientation(self.robot, physicsClientId=self.id)
if self.human_control:
human_pos = np.array(p.getBasePositionAndOrientation(self.human, physicsClientId=self.id)[0])
human_joint_states = p.getJointStates(self.human, jointIndices=self.human_controllable_joint_indices, physicsClientId=self.id)
human_joint_positions = np.array([x[0] for x in human_joint_states])
head_pos, head_orient = p.getLinkState(self.human, 23, computeForwardKinematics=True, physicsClientId=self.id)[:2]
robot_obs = np.concatenate([spoon_pos-torso_pos, spoon_orient, spoon_pos-self.target_pos, robot_right_joint_positions, head_pos-torso_pos, head_orient, forces]).ravel()
if self.human_control:
human_obs = np.concatenate([spoon_pos-human_pos, spoon_orient, spoon_pos-self.target_pos, human_joint_positions, head_pos-human_pos, head_orient, forces_human]).ravel()
else:
human_obs = []
return np.concatenate([robot_obs, human_obs]).ravel()
示例3: enforce_joint_limits
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getJointStates [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
示例4: enforce_hard_human_joint_limits
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getJointStates [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)
示例5: _get_obs
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getJointStates [as 别名]
def _get_obs(self, forces, forces_human):
torso_pos = np.array(p.getLinkState(self.robot, 15 if self.robot_type == 'pr2' else 0, computeForwardKinematics=True, physicsClientId=self.id)[0])
state = p.getLinkState(self.tool, 1, computeForwardKinematics=True, physicsClientId=self.id)
tool_pos = np.array(state[0])
tool_orient = np.array(state[1]) # Quaternions
robot_joint_states = p.getJointStates(self.robot, jointIndices=self.robot_left_arm_joint_indices, physicsClientId=self.id)
robot_joint_positions = np.array([x[0] for x in robot_joint_states])
robot_pos, robot_orient = p.getBasePositionAndOrientation(self.robot, physicsClientId=self.id)
if self.human_control:
human_pos = np.array(p.getBasePositionAndOrientation(self.human, physicsClientId=self.id)[0])
human_joint_states = p.getJointStates(self.human, jointIndices=self.human_controllable_joint_indices, physicsClientId=self.id)
human_joint_positions = np.array([x[0] for x in human_joint_states])
# Human shoulder, elbow, and wrist joint locations
shoulder_pos, shoulder_orient = p.getLinkState(self.human, 5, computeForwardKinematics=True, physicsClientId=self.id)[:2]
elbow_pos, elbow_orient = p.getLinkState(self.human, 7, computeForwardKinematics=True, physicsClientId=self.id)[:2]
wrist_pos, wrist_orient = p.getLinkState(self.human, 9, computeForwardKinematics=True, physicsClientId=self.id)[:2]
robot_obs = np.concatenate([tool_pos-torso_pos, tool_orient, robot_joint_positions, shoulder_pos-torso_pos, elbow_pos-torso_pos, wrist_pos-torso_pos, forces]).ravel()
if self.human_control:
human_obs = np.concatenate([tool_pos-human_pos, tool_orient, human_joint_positions, shoulder_pos-human_pos, elbow_pos-human_pos, wrist_pos-human_pos, forces_human]).ravel()
else:
human_obs = []
return np.concatenate([robot_obs, human_obs]).ravel()
示例6: _get_obs
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getJointStates [as 别名]
def _get_obs(self, forces, forces_human):
torso_pos = np.array(p.getLinkState(self.robot, 15 if self.robot_type == 'pr2' else 0, computeForwardKinematics=True, physicsClientId=self.id)[0])
state = p.getLinkState(self.tool, 1, computeForwardKinematics=True, physicsClientId=self.id)
tool_pos = np.array(state[0])
tool_orient = np.array(state[1]) # Quaternions
robot_joint_states = p.getJointStates(self.robot, jointIndices=self.robot_left_arm_joint_indices, physicsClientId=self.id)
robot_joint_positions = np.array([x[0] for x in robot_joint_states])
robot_pos, robot_orient = p.getBasePositionAndOrientation(self.robot, physicsClientId=self.id)
if self.human_control:
human_pos = np.array(p.getBasePositionAndOrientation(self.human, physicsClientId=self.id)[0])
human_joint_states = p.getJointStates(self.human, jointIndices=self.human_controllable_joint_indices, physicsClientId=self.id)
human_joint_positions = np.array([x[0] for x in human_joint_states])
# Human shoulder, elbow, and wrist joint locations
shoulder_pos, shoulder_orient = p.getLinkState(self.human, 5, computeForwardKinematics=True, physicsClientId=self.id)[:2]
elbow_pos, elbow_orient = p.getLinkState(self.human, 7, computeForwardKinematics=True, physicsClientId=self.id)[:2]
wrist_pos, wrist_orient = p.getLinkState(self.human, 9, computeForwardKinematics=True, physicsClientId=self.id)[:2]
robot_obs = np.concatenate([tool_pos-torso_pos, tool_orient, tool_pos - self.target_pos, self.target_pos-torso_pos, robot_joint_positions, shoulder_pos-torso_pos, elbow_pos-torso_pos, wrist_pos-torso_pos, forces]).ravel()
if self.human_control:
human_obs = np.concatenate([tool_pos-human_pos, tool_orient, tool_pos - self.target_pos, self.target_pos-human_pos, human_joint_positions, shoulder_pos-human_pos, elbow_pos-human_pos, wrist_pos-human_pos, forces_human]).ravel()
else:
human_obs = []
return np.concatenate([robot_obs, human_obs]).ravel()
示例7: getAnglesPosition
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getJointStates [as 别名]
def getAnglesPosition(self, joint_names):
"""
Gets the position of the robot's joints in radians. If one of the joint
doesn't exist, the method will raise a KeyError.
Parameters:
joint_names - List of string containing the names of the joints
Returns:
joint_positions - List of floats containing the joint's positions
"""
indexes = [self.joint_dict[name].getIndex() for name in joint_names]
return [state[0] for state in pybullet.getJointStates(
self.robot_model,
indexes,
physicsClientId=self.physics_client)]
示例8: getAnglesVelocity
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getJointStates [as 别名]
def getAnglesVelocity(self, joint_names):
"""
Gets the velocity of the robot's joints in rad/s. If one of the joint
doesn't exist, the method will raise a KeyError.
Parameters:
joint_names - List of string containing the names of the joints
Returns:
joint_velocities - List of floats containing the joint's velocities
"""
indexes = [self.joint_dict[name].getIndex() for name in joint_names]
return [state[1] for state in pybullet.getJointStates(
self.robot_model,
indexes,
physicsClientId=self.physics_client)]
示例9: getJointStates
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getJointStates [as 别名]
def getJointStates(robot):
joint_states = p.getJointStates(robot, range(p.getNumJoints(robot)))
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
示例10: getMotorJointStates
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getJointStates [as 别名]
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
示例11: getMotorJointStates
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getJointStates [as 别名]
def getMotorJointStates(self, robot):
import pybullet as p
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
示例12: setup_human_joints
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getJointStates [as 别名]
def setup_human_joints(self, human, joints_positions, controllable_joints, use_static_joints=True, human_reactive_force=None, human_reactive_gain=0.05):
if self.human_impairment != 'tremor':
self.human_tremors = np.zeros(len(controllable_joints))
elif len(controllable_joints) == 4:
self.human_tremors = self.np_random.uniform(np.deg2rad(-20), np.deg2rad(20), size=len(controllable_joints))
else:
self.human_tremors = self.np_random.uniform(np.deg2rad(-10), np.deg2rad(10), size=len(controllable_joints))
# Set starting joint positions
human_joint_states = p.getJointStates(human, jointIndices=list(range(p.getNumJoints(human, physicsClientId=self.id))), physicsClientId=self.id)
human_joint_positions = np.array([x[0] for x in human_joint_states])
for j in range(p.getNumJoints(human, physicsClientId=self.id)):
set_position = None
for j_index, j_angle in joints_positions:
if j == j_index:
p.resetJointState(human, jointIndex=j, targetValue=j_angle, targetVelocity=0, physicsClientId=self.id)
set_position = j_angle
break
if use_static_joints and j not in controllable_joints:
# Make all non controllable joints on the person static by setting mass of each link (joint) to 0
p.changeDynamics(human, j, mass=0, physicsClientId=self.id)
# Set velocities to 0
p.resetJointState(human, jointIndex=j, targetValue=human_joint_positions[j] if set_position is None else set_position, targetVelocity=0, physicsClientId=self.id)
# By default, all joints have motors enabled by default that prevent free motion. Disable these motors in human
for j in range(p.getNumJoints(human, physicsClientId=self.id)):
p.setJointMotorControl2(human, jointIndex=j, controlMode=p.VELOCITY_CONTROL, force=0, physicsClientId=self.id)
self.enforce_joint_limits(human)
if human_reactive_force is not None:
# NOTE: This runs a Position / Velocity PD controller for each joint motor on the human
human_joint_states = p.getJointStates(human, jointIndices=controllable_joints, physicsClientId=self.id)
target_human_joint_positions = np.array([x[0] for x in human_joint_states])
forces = [human_reactive_force * self.human_strength] * len(target_human_joint_positions)
p.setJointMotorControlArray(human, jointIndices=controllable_joints, controlMode=p.POSITION_CONTROL, targetPositions=target_human_joint_positions, positionGains=np.array([human_reactive_gain]*len(forces)), forces=forces, physicsClientId=self.id)
示例13: get_motor_joint_states
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getJointStates [as 别名]
def get_motor_joint_states(self, robot):
num_joints = p.getNumJoints(robot, physicsClientId=self.id)
joint_states = p.getJointStates(robot, range(num_joints), physicsClientId=self.id)
joint_infos = [p.getJointInfo(robot, i, physicsClientId=self.id) for i in range(num_joints)]
joint_states = [j for j, i in zip(joint_states, joint_infos) if i[2] != p.JOINT_FIXED]
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
示例14: _get_obs
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getJointStates [as 别名]
def _get_obs(self, forces, forces_human):
torso_pos = np.array(p.getLinkState(self.robot, 15 if self.robot_type == 'pr2' else 0, computeForwardKinematics=True, physicsClientId=self.id)[0])
tool_left_pos, tool_left_orient = p.getLinkState(self.robot, 78 if self.robot_type=='pr2' else 24 if self.robot_type=='sawyer' else 54 if self.robot_type=='baxter' else 9 if self.robot_type=='jaco' else 7, computeForwardKinematics=True, physicsClientId=self.id)[:2]
tool_right_pos, tool_right_orient = p.getLinkState(self.robot, 55 if self.robot_type=='pr2' else 24 if self.robot_type=='sawyer' else 31 if self.robot_type=='baxter' else 9 if self.robot_type=='jaco' else 7, computeForwardKinematics=True, physicsClientId=self.id)[:2]
robot_joint_states = p.getJointStates(self.robot, jointIndices=self.robot_both_arm_joint_indices, physicsClientId=self.id)
robot_joint_positions = np.array([x[0] for x in robot_joint_states])
robot_pos, robot_orient = p.getBasePositionAndOrientation(self.robot, physicsClientId=self.id)
if self.human_control:
human_pos = np.array(p.getBasePositionAndOrientation(self.human, physicsClientId=self.id)[0])
human_joint_states = p.getJointStates(self.human, jointIndices=self.human_controllable_joint_indices, physicsClientId=self.id)
human_joint_positions = np.array([x[0] for x in human_joint_states])
# Human shoulder, elbow, and wrist joint locations
shoulder_pos, shoulder_orient = p.getLinkState(self.human, 5, computeForwardKinematics=True, physicsClientId=self.id)[:2]
elbow_pos, elbow_orient = p.getLinkState(self.human, 7, computeForwardKinematics=True, physicsClientId=self.id)[:2]
wrist_pos, wrist_orient = p.getLinkState(self.human, 9, computeForwardKinematics=True, physicsClientId=self.id)[:2]
waist_pos = np.array(p.getLinkState(self.human, 24, computeForwardKinematics=True, physicsClientId=self.id)[0])
hips_pos = np.array(p.getLinkState(self.human, 27, computeForwardKinematics=True, physicsClientId=self.id)[0])
shoulder_pos, elbow_pos, wrist_pos, waist_pos, hips_pos = np.array(shoulder_pos), np.array(elbow_pos), np.array(wrist_pos), np.array(waist_pos), np.array(hips_pos)
robot_obs = np.concatenate([tool_left_pos-torso_pos, tool_left_orient, tool_right_pos-torso_pos, tool_right_orient, robot_joint_positions, shoulder_pos-torso_pos, elbow_pos-torso_pos, wrist_pos-torso_pos, waist_pos-torso_pos, hips_pos-torso_pos, forces]).ravel()
if self.human_control:
human_obs = np.concatenate([tool_left_pos-human_pos, tool_left_orient, tool_right_pos-human_pos, tool_right_orient, human_joint_positions, shoulder_pos-human_pos, elbow_pos-human_pos, wrist_pos-human_pos, waist_pos-human_pos, hips_pos-human_pos, forces_human]).ravel()
else:
human_obs = []
return np.concatenate([robot_obs, human_obs]).ravel()
示例15: _getGripper
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getJointStates [as 别名]
def _getGripper(self):
vs = [v[0] for v in pb.getJointStates(self.handle,
self.stable_gripper_indices)]
return np.array([np.round(-np.mean(vs),1)])