本文整理汇总了Python中pybullet.getQuaternionFromEuler方法的典型用法代码示例。如果您正苦于以下问题:Python pybullet.getQuaternionFromEuler方法的具体用法?Python pybullet.getQuaternionFromEuler怎么用?Python pybullet.getQuaternionFromEuler使用的例子?那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类pybullet
的用法示例。
在下文中一共展示了pybullet.getQuaternionFromEuler方法的15个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: _reset
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getQuaternionFromEuler [as 别名]
def _reset(self):
#print("KukaGymEnv _reset")
self.terminated = 0
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(self._timeStep)
p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"),[0,0,-1])
p.loadURDF(os.path.join(self._urdfRoot,"table/table.urdf"), 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0)
xpos = 0.55 +0.12*random.random()
ypos = 0 +0.2*random.random()
ang = 3.14*0.5+3.1415925438*random.random()
orn = p.getQuaternionFromEuler([0,0,ang])
self.blockUid =p.loadURDF(os.path.join(self._urdfRoot,"block.urdf"), xpos,ypos,-0.15,orn[0],orn[1],orn[2],orn[3])
p.setGravity(0,0,-10)
self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
self._envStepCounter = 0
p.stepSimulation()
self._observation = self.getExtendedObservation()
return np.array(self._observation)
示例2: _reset
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getQuaternionFromEuler [as 别名]
def _reset(self):
self.terminated = 0
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(self._timeStep)
p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"),[0,0,-1])
p.loadURDF(os.path.join(self._urdfRoot,"table/table.urdf"), 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0)
xpos = 0.5 +0.2*random.random()
ypos = 0 +0.25*random.random()
ang = 3.1415925438*random.random()
orn = p.getQuaternionFromEuler([0,0,ang])
self.blockUid =p.loadURDF(os.path.join(self._urdfRoot,"block.urdf"), xpos,ypos,-0.1,orn[0],orn[1],orn[2],orn[3])
p.setGravity(0,0,-10)
self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
self._envStepCounter = 0
p.stepSimulation()
self._observation = self.getExtendedObservation()
return np.array(self._observation)
示例3: __init__
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getQuaternionFromEuler [as 别名]
def __init__(self, robot_name, print_debug, email_cred_file='', log_file='', control_rate=100, gripper_attached='default'):
super(WidowXController, self).__init__(robot_name, print_debug, email_cred_file, log_file, control_rate, gripper_attached)
self._redist_rate = rospy.Rate(50)
self._arbotix = ArbotiX('/dev/ttyUSB0')
assert self._arbotix.syncWrite(MAX_TORQUE_L, [[servo_id, 255, 0] for servo_id in SERVO_IDS]) != -1, "failure during servo configuring"
assert self._arbotix.syncWrite(TORQUE_LIMIT, [[servo_id, 255, 0] for servo_id in SERVO_IDS]) != -1, "failure during servo configuring"
self._joint_lock = Lock()
self._angles, self._velocities = {}, {}
rospy.Subscriber("/joint_states", JointState, self._joint_callback)
time.sleep(1)
self._joint_pubs = [rospy.Publisher('/{}/command'.format(name), Float64, queue_size=1) for name in JOINT_NAMES[:-1]]
self._gripper_pub = rospy.Publisher('/gripper_prismatic_joint/command', Float64, queue_size=1)
p.connect(p.DIRECT)
widow_x_urdf = '/'.join(__file__.split('/')[:-1]) + '/widowx/widowx.urdf'
self._armID = p.loadURDF(widow_x_urdf, useFixedBase=True)
p.resetBasePositionAndOrientation(self._armID, [0, 0, 0], p.getQuaternionFromEuler([np.pi, np.pi, np.pi]))
self._n_errors = 0
示例4: reset
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getQuaternionFromEuler [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: ik_random_restarts
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getQuaternionFromEuler [as 别名]
def ik_random_restarts(self, body, target_joint, target_pos, target_orient, world_creation, robot_arm_joint_indices, robot_lower_limits, robot_upper_limits, ik_indices=range(29, 29+7), max_iterations=1000, max_ik_random_restarts=50, random_restart_threshold=0.01, half_range=False, step_sim=False, check_env_collisions=False):
orient_orig = target_orient
best_ik_joints = None
best_ik_distance = 0
for r in range(max_ik_random_restarts):
target_joint_positions = self.ik(body, target_joint, target_pos, target_orient, ik_indices=ik_indices, max_iterations=max_iterations, half_range=half_range)
world_creation.setup_robot_joints(body, robot_arm_joint_indices, robot_lower_limits, robot_upper_limits, randomize_joint_positions=False, default_positions=np.array(target_joint_positions), tool=None)
if step_sim:
for _ in range(5):
p.stepSimulation(physicsClientId=self.id)
if len(p.getContactPoints(bodyA=body, bodyB=body, physicsClientId=self.id)) > 0 and orient_orig is not None:
# The robot's arm is in contact with itself. Continually randomize end effector orientation until a solution is found
target_orient = p.getQuaternionFromEuler(p.getEulerFromQuaternion(orient_orig, physicsClientId=self.id) + np.deg2rad(self.np_random.uniform(-45, 45, size=3)), physicsClientId=self.id)
if check_env_collisions:
for _ in range(25):
p.stepSimulation(physicsClientId=self.id)
gripper_pos, gripper_orient = p.getLinkState(body, target_joint, computeForwardKinematics=True, physicsClientId=self.id)[:2]
if np.linalg.norm(target_pos - np.array(gripper_pos)) < random_restart_threshold and (target_orient is None or np.linalg.norm(target_orient - np.array(gripper_orient)) < random_restart_threshold or np.isclose(np.linalg.norm(target_orient - np.array(gripper_orient)), 2, atol=random_restart_threshold)):
return True, np.array(target_joint_positions)
if best_ik_joints is None or np.linalg.norm(target_pos - np.array(gripper_pos)) < best_ik_distance:
best_ik_joints = target_joint_positions
best_ik_distance = np.linalg.norm(target_pos - np.array(gripper_pos))
world_creation.setup_robot_joints(body, robot_arm_joint_indices, robot_lower_limits, robot_upper_limits, randomize_joint_positions=False, default_positions=np.array(best_ik_joints), tool=None)
return False, np.array(best_ik_joints)
示例6: _setup
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getQuaternionFromEuler [as 别名]
def _setup(self):
'''
Create the mug at a random position on the ground, handle facing
roughly towards the robot. Robot's job is to grab and lift.
'''
rospack = rospkg.RosPack()
path = rospack.get_path('costar_objects')
sdf_dir = os.path.join(path, self.sdf_dir)
obj_to_add = os.path.join(sdf_dir, self.model, self.model_file_name)
identity_orientation = pb.getQuaternionFromEuler([0, 0, 0])
try:
obj_id_list = pb.loadSDF(obj_to_add)
for obj_id in obj_id_list:
random_position = np.random.rand(
3) * self.spawn_pos_delta + self.spawn_pos_min
pb.resetBasePositionAndOrientation(
obj_id, random_position, identity_orientation)
except Exception, e:
print e
示例7: _reset
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getQuaternionFromEuler [as 别名]
def _reset(self):
# reset is called once at initialization of simulation
self.vt = 0
self.vd = 0
self.maxV = 24.6 # 235RPM = 24,609142453 rad/sec
self._envStepCounter = 0
p.resetSimulation()
p.setGravity(0,0,-10) # m/s^2
p.setTimeStep(0.01) # sec
planeId = p.loadURDF("plane.urdf")
cubeStartPos = [0,0,0.001]
cubeStartOrientation = p.getQuaternionFromEuler([0,0,0])
path = os.path.abspath(os.path.dirname(__file__))
self.botId = p.loadURDF(os.path.join(path, "balancebot_simple.xml"),
cubeStartPos,
cubeStartOrientation)
# you *have* to compute and return the observation from reset()
self._observation = self._compute_observation()
return np.array(self._observation)
示例8: _randomly_place_objects
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getQuaternionFromEuler [as 别名]
def _randomly_place_objects(self, urdfList):
"""Randomly places the objects in the bin.
Args:
urdfList: The list of urdf files to place in the bin.
Returns:
The list of object unique ID's.
"""
# Randomize positions of each object urdf.
objectUids = []
for urdf_name in urdfList:
xpos = 0.4 +self._blockRandom*random.random()
ypos = self._blockRandom*(random.random()-.5)
angle = np.pi/2 + self._blockRandom * np.pi * random.random()
orn = p.getQuaternionFromEuler([0, 0, angle])
urdf_path = os.path.join(self._urdfRoot, urdf_name)
uid = p.loadURDF(urdf_path, [xpos, ypos, .15],
[orn[0], orn[1], orn[2], orn[3]])
objectUids.append(uid)
# Let each object fall to the tray individual, to prevent object
# intersection.
for _ in range(500):
p.stepSimulation()
return objectUids
示例9: __init__
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getQuaternionFromEuler [as 别名]
def __init__(self, path, position, orientation):
self.orientation = orientation
self.position = position
self.path = path
self.plane = p.loadURDF(self.path, basePosition=self.position,
baseOrientation=p.getQuaternionFromEuler(self.orientation))
示例10: setBallPos
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getQuaternionFromEuler [as 别名]
def setBallPos(self, x, y):
"""Sets the ball position on the field"""
if self.ball is not None:
# Putting the ball on the ground at given position
p.resetBasePositionAndOrientation(
self.ball, [x, y, 0.06], p.getQuaternionFromEuler([0, 0, 0]))
# Stopping the ball speed
p.changeDynamics(self.ball, 0,
linearDamping=0, angularDamping=0.1)
示例11: quat_from_euler
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getQuaternionFromEuler [as 别名]
def quat_from_euler(euler):
euler = list(euler)
quat = p.getQuaternionFromEuler(euler)
return np.array(quat, dtype=np.float32)
示例12: mat33_from_euler
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getQuaternionFromEuler [as 别名]
def mat33_from_euler(euler):
euler = list(euler)
quat = p.getQuaternionFromEuler(euler)
mat33 = p.getMatrixFromQuaternion(quat)
return np.reshape(mat33, [3, 3])
示例13: pos_in_frame
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getQuaternionFromEuler [as 别名]
def pos_in_frame(pos, frame):
frame_xyz = frame[0]
frame_rpy = frame[1]
quat = p.getQuaternionFromEuler(frame_rpy)
mat33 = p.getMatrixFromQuaternion(quat)
mat33 = np.reshape(mat33, [3, 3])
pos_in_frame = frame_xyz + np.dot(pos, mat33.T)
return pos_in_frame
示例14: set_cstr_dof
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getQuaternionFromEuler [as 别名]
def set_cstr_dof(cstr, pos, euler, max_force):
pos = list(pos)
euler = list(euler)
quat = p.getQuaternionFromEuler(euler)
p.changeConstraint(
userConstraintUniqueId=cstr,
jointChildPivot=pos,
jointChildFrameOrientation=quat,
maxForce=max_force)
示例15: step
# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getQuaternionFromEuler [as 别名]
def step(self, action):
self.take_step(action, robot_arm='right', gains=self.config('robot_gains'), forces=self.config('robot_forces'), human_gains=0.0005)
robot_force_on_human, cup_force_on_human = self.get_total_force()
total_force_on_human = robot_force_on_human + cup_force_on_human
reward_water, water_mouth_velocities, water_hit_human_reward = self.get_water_rewards()
end_effector_velocity = np.linalg.norm(p.getBaseVelocity(self.cup, physicsClientId=self.id)[0])
obs = self._get_obs([cup_force_on_human], [robot_force_on_human, cup_force_on_human])
# Get human preferences
preferences_score = self.human_preferences(end_effector_velocity=end_effector_velocity, total_force_on_human=robot_force_on_human, tool_force_at_target=cup_force_on_human, food_hit_human_reward=water_hit_human_reward, food_mouth_velocities=water_mouth_velocities)
cup_pos, cup_orient = p.getBasePositionAndOrientation(self.cup, physicsClientId=self.id)
cup_pos, cup_orient = p.multiplyTransforms(cup_pos, cup_orient, [0, 0.06, 0], p.getQuaternionFromEuler([np.pi/2.0, 0, 0], physicsClientId=self.id), physicsClientId=self.id)
cup_top_center_pos, _ = p.multiplyTransforms(cup_pos, cup_orient, self.cup_top_center_offset, [0, 0, 0, 1], physicsClientId=self.id)
reward_distance = -np.linalg.norm(self.target_pos - np.array(cup_top_center_pos)) # Penalize distances between top of cup and mouth
reward_action = -np.sum(np.square(action)) # Penalize actions
# Encourage robot to have a tilted end effector / cup
cup_euler = p.getEulerFromQuaternion(cup_orient, physicsClientId=self.id)
reward_tilt = -abs(cup_euler[0] + np.pi/2) if self.robot_type == 'jaco' else -abs(cup_euler[0] - np.pi/2)
reward = self.config('distance_weight')*reward_distance + self.config('action_weight')*reward_action + self.config('cup_tilt_weight')*reward_tilt + self.config('drinking_reward_weight')*reward_water + preferences_score
if self.gui and reward_water != 0:
print('Task success:', self.task_success, 'Water reward:', reward_water)
info = {'total_force_on_human': total_force_on_human, 'task_success': int(self.task_success >= self.total_water_count*self.config('task_success_threshold')), 'action_robot_len': self.action_robot_len, 'action_human_len': self.action_human_len, 'obs_robot_len': self.obs_robot_len, 'obs_human_len': self.obs_human_len}
done = False
return obs, reward, done, info