Python pybullet.POSITION_CONTROL Examples
The following are 30
code examples of pybullet.POSITION_CONTROL().
You can vote up the ones you like or vote down the ones you don't like,
and go to the original project or source file by following the links above each example.
You may also want to check out all available functions/classes of the module
pybullet
, or try the search function
.
.
Example #1
| Source File: panda_env.py From pybullet-robot-envs with GNU Lesser General Public License v2.1 | 6 votes |
|
def apply_action_fingers(self, action, obj_id=None):
# move finger joints in position control
assert len(action) == 2, ('finger joints are 2! The number of actions you passed is ', len(action))
idx_fingers = [self._joint_name_to_ids['panda_finger_joint1'], self._joint_name_to_ids['panda_finger_joint2']]
# use object id to check contact force and eventually stop the finger motion
if obj_id is not None:
_, forces = self.check_contact_fingertips(obj_id)
# print("contact forces {}".format(forces))
if forces[0] >= 20.0:
action[0] = p.getJointState(self.robot_id, idx_fingers[0], physicsClientId=self._physics_client_id)[0]
if forces[1] >= 20.0:
action[1] = p.getJointState(self.robot_id, idx_fingers[1], physicsClientId=self._physics_client_id)[0]
for i, idx in enumerate(idx_fingers):
p.setJointMotorControl2(self.robot_id,
idx,
p.POSITION_CONTROL,
targetPosition=action[i],
force=10,
maxVelocity=1,
physicsClientId=self._physics_client_id)
Example #2
| Source File: icub_env_with_hands.py From pybullet-robot-envs with GNU Lesser General Public License v2.1 | 6 votes |
|
def pre_grasp(self):
# move fingers to pre-grasp configuration
if self._control_arm is 'l':
idx_thumb = self._joint_name_to_ids['l_hand::l_tj2']
idx_fingers = [self._joint_name_to_ids[jn] for jn in self.joint_groups['l_hand']]
else:
idx_thumb = self._joint_name_to_ids['r_hand::r_tj2']
idx_fingers = [self._joint_name_to_ids[jn] for jn in self.joint_groups['r_hand']]
pos = [0.0] * len(idx_fingers)
for i, idx in enumerate(idx_fingers):
if idx == idx_thumb:
pos[i] = 1.57
p.setJointMotorControlArray(self.robot_id, idx_fingers, p.POSITION_CONTROL,
targetPositions=pos,
positionGains=[0.1] * len(idx_fingers),
velocityGains=[1.0] * len(idx_fingers),
physicsClientId=self._physics_client_id)
Example #3
| Source File: motorController.py From Boston_Dynamics_Atlas_Explained with MIT License | 6 votes |
|
def setMotorsAngleInRealTimestep(self, motorTargetAngles, motorTargetTime, delayTime):
if(motorTargetTime == 0):
self._joint_targetPos = np.array(motorTargetAngles)
for i in range(self._joint_number):
p.setJointMotorControl2(bodyIndex=self._robot, jointIndex=self._joint_id[i], controlMode=p.POSITION_CONTROL,
targetPosition=self._joint_targetPos[i],
positionGain=self._kp, velocityGain=self._kd, force=self._torque, maxVelocity=self._max_velocity, physicsClientId=self._physicsClientId)
time.sleep(delayTime)
else:
self._joint_currentPos = self._joint_targetPos
self._joint_targetPos = np.array(motorTargetAngles)
for i in range(self._joint_number):
dydt = (self._joint_targetPos-self._joint_currentPos)/motorTargetTime
internalTime = 0.0
reft = time.time()
while internalTime < motorTargetTime:
internalTime = time.time() - reft
for i in range(self._joint_number):
p.setJointMotorControl2(bodyIndex=self._robot, jointIndex=self._joint_id[i], controlMode=p.POSITION_CONTROL,
targetPosition=self._joint_currentPos[i] + dydt[i] * internalTime,
positionGain=self._kp, velocityGain=self._kd, force=self._torque, maxVelocity=self._max_velocity, physicsClientId=self._physicsClientId)
Example #4
| Source File: bullet_physics_engine.py From NTP-vat-release with MIT License | 6 votes |
|
def control_joint_pos(body, joint, pos, max_vel=None, max_force=None):
if max_vel is None:
p.setJointMotorControl2(
bodyIndex=body,
jointIndex=joint,
controlMode=p.POSITION_CONTROL,
targetPosition=pos,
force=max_force)
else:
p.setJointMotorControl2(
bodyIndex=body,
jointIndex=joint,
controlMode=p.POSITION_CONTROL,
targetPosition=pos,
targetVelocity=max_vel,
force=max_force)
Example #5
| Source File: world_creation.py From assistive-gym with MIT License | 6 votes |
|
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)
Example #6
| Source File: iiwa_robotiq_3_finger.py From costar_plan with Apache License 2.0 | 5 votes |
|
def arm(self, cmd, mode=pb.POSITION_CONTROL):
'''
Set joint commands for the robot arm.
'''
if len(cmd) > 6:
raise RuntimeError('too many joint positions')
for i, q in enumerate(cmd):
pb.setJointMotorControl2(self.handle, i, mode, q)
Example #7
| Source File: turtlebot.py From costar_plan with Apache License 2.0 | 5 votes |
|
def gripper(self, cmd, mode=pb.POSITION_CONTROL):
'''
Gripper commands need to be mirrored to simulate behavior of the actual
UR5.
'''
pass
Example #8
| Source File: ur5_robotiq.py From costar_plan with Apache License 2.0 | 5 votes |
|
def arm(self, cmd, mode=pb.POSITION_CONTROL):
'''
Set joint commands for the robot arm.
'''
if len(cmd) > self.dof:
raise RuntimeError('too many joint positions')
pb.setJointMotorControlArray(self.handle, self.arm_joint_indices, mode,
cmd,
positionGains=[0.25,0.17,0.11,0.1,0.1,0.1],
velocityGains=[1.5,1.25,1.0,0.5,0.5,0.5],
)#, forces=[100.] * self.dof)
Example #9
| Source File: ur5_robotiq.py From costar_plan with Apache License 2.0 | 5 votes |
|
def gripper(self, cmd, mode=pb.POSITION_CONTROL):
'''
Gripper commands need to be mirrored to simulate behavior of the actual
UR5. Converts one command input to 6 joint positions, used for the
robotiq gripper. This is a rough simulation of the way the robotiq
gripper works in practice, in the absence of a plugin like the one we
use in Gazebo.
Parameters:
-----------
cmd: 1x1 array of floating point position commands in [-0.8, 0]
mode: PyBullet control mode
'''
cmd = cmd[0]
# This is actually only a 1-DOF gripper
if cmd < -0.1:
cmd_array = [-cmd + 0.1, -cmd + 0.1, cmd + 0.15,
-cmd + 0.1, -cmd + 0.1, cmd + 0.15]
else:
cmd_array = [-cmd , -cmd, cmd, -cmd, -cmd, cmd]
forces = [25., 25., 25., 25., 25., 25.]
gains = [0.1, 0.1, 0.15, 0.1, 0.1, 0.15]
#if abs(cmd) < -0.01:
# mode = pb.TORQUE_CONTROL
# forces = [0.] * len(cmd_array)
#else:
#gripper_indices = [left_knuckle, left_inner_knuckle,
# left_fingertip, right_knuckle, right_inner_knuckle,
# right_fingertip]
pb.setJointMotorControlArray(self.handle, self.gripper_indices, mode,
cmd_array,
forces=forces,
positionGains=gains)
Example #10
| Source File: jaco_robotiq.py From costar_plan with Apache License 2.0 | 5 votes |
|
def arm(self, cmd, mode=pb.POSITION_CONTROL):
'''
Set joint commands for the robot arm.
'''
if len(cmd) > 6:
raise RuntimeError('too many joint positions')
for i, q in enumerate(cmd):
pb.setJointMotorControl2(self.handle, i, mode, q)
Example #11
| Source File: jaco_robotiq.py From costar_plan with Apache License 2.0 | 5 votes |
|
def gripper(self, cmd, mode=pb.POSITION_CONTROL):
'''
Gripper commands need to be mirrored to simulate behavior of the actual
UR5.
'''
pass
Example #12
| Source File: robot_bases.py From GtS with MIT License | 5 votes |
|
def set_position(self, position):
"""Set position of joint
Position is defined in real world scale """
if self.jointType == p.JOINT_PRISMATIC:
position = np.array(position) / self.scale
p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,p.POSITION_CONTROL, targetPosition=position)
Example #13
| Source File: robot_bases.py From GtS with MIT License | 5 votes |
|
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)
Example #14
| Source File: minitaur.py From midlevel-reps with MIT License | 5 votes |
|
def _SetDesiredMotorAngleById(self, motor_id, desired_angle):
p.setJointMotorControl2(bodyIndex=self.minitaur,
jointIndex=motor_id,
controlMode=p.POSITION_CONTROL,
targetPosition=desired_angle,
positionGain=self._kp,
velocityGain=self._kd,
force=self.max_force)
Example #15
| Source File: robot_bases.py From midlevel-reps with MIT License | 5 votes |
|
def set_position(self, position):
"""Set position of joint
Position is defined in real world scale """
if self.jointType == p.JOINT_PRISMATIC:
position = np.array(position) / self.scale
p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,p.POSITION_CONTROL, targetPosition=position)
Example #16
| Source File: robot_bases.py From midlevel-reps with MIT License | 5 votes |
|
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)
Example #17
| Source File: gym_mujoco_xml_env.py From bullet-gym with MIT License | 5 votes |
|
def set_position(self, position): p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,p.POSITION_CONTROL, targetPosition=position)
Example #18
| Source File: MJCFCommon.py From bullet-gym with MIT License | 5 votes |
|
def set_position(self, position):
p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,p.POSITION_CONTROL, targetPosition=position)
Example #19
| Source File: motorController.py From bipedal-robot-walking-simulation with MIT License | 5 votes |
|
def setMotorAngle(self, motorTargetAngles):
for i in range(self._joint_number):
self._joint_targetPos[i] = motorTargetAngles[i]
p.setJointMotorControl2(
bodyIndex=self._robot,
jointIndex=self._joint_id[i],
controlMode=p.POSITION_CONTROL,
targetPosition=self._joint_targetPos[i],
positionGain=self._kp,
velocityGain=self._kd,
force=self._torque,
maxVelocity=self._max_velocity,
physicsClientId=self._physicsClientId)
Example #20
| Source File: motorController.py From bipedal-robot-walking-simulation with MIT License | 5 votes |
|
def setMotorsAngleInRealTimestep(self, motorTargetAngles, motorTargetTime, delayTime):
if (motorTargetTime == 0):
self._joint_targetPos = np.array(motorTargetAngles)
for i in range(self._joint_number):
p.setJointMotorControl2(
bodyIndex=self._robot,
jointIndex=self._joint_id[i],
controlMode=p.POSITION_CONTROL,
targetPosition=self._joint_targetPos[i],
positionGain=self._kp,
velocityGain=self._kd,
force=self._torque,
maxVelocity=self._max_velocity,
physicsClientId=self._physicsClientId)
time.sleep(delayTime)
else:
self._joint_currentPos = self._joint_targetPos
self._joint_targetPos = np.array(motorTargetAngles)
for i in range(self._joint_number):
dydt = (self._joint_targetPos - self._joint_currentPos) / motorTargetTime
internalTime = 0.0
reft = time.time()
while internalTime < motorTargetTime:
internalTime = time.time() - reft
for i in range(self._joint_number):
p.setJointMotorControl2(
bodyIndex=self._robot,
jointIndex=self._joint_id[i],
controlMode=p.POSITION_CONTROL,
targetPosition=self._joint_currentPos[i] + dydt[i] * internalTime,
positionGain=self._kp,
velocityGain=self._kd,
force=self._torque,
maxVelocity=self._max_velocity,
physicsClientId=self._physicsClientId)
Example #21
| Source File: motorController.py From bipedal-robot-walking-simulation with MIT License | 5 votes |
|
def setMotorsAngleInFixedTimestep(self, motorTargetAngles, motorTargetTime, delayTime):
if (motorTargetTime == 0):
self._joint_targetPos = np.array(motorTargetAngles)
for i in range(self._joint_number):
p.setJointMotorControl2(
bodyIndex=self._robot,
jointIndex=self._joint_id[i],
controlMode=p.POSITION_CONTROL,
targetPosition=self._joint_targetPos[i],
positionGain=self._kp,
velocityGain=self._kd,
force=self._torque,
maxVelocity=self._max_velocity,
physicsClientId=self._physicsClientId)
p.stepSimulation(physicsClientId=self._physicsClientId)
else:
self._joint_currentPos = self._joint_targetPos
self._joint_targetPos = np.array(motorTargetAngles)
for i in range(self._joint_number):
dydt = (self._joint_targetPos - self._joint_currentPos) / motorTargetTime
internalTime = 0.0
while internalTime < motorTargetTime:
internalTime += self._timeStep
for i in range(self._joint_number):
p.setJointMotorControl2(
bodyIndex=self._robot,
jointIndex=self._joint_id[i],
controlMode=p.POSITION_CONTROL,
targetPosition=self._joint_currentPos[i] + dydt[i] * internalTime,
positionGain=self._kp,
velocityGain=self._kd,
force=self._torque,
maxVelocity=self._max_velocity,
physicsClientId=self._physicsClientId)
p.stepSimulation(physicsClientId=self._physicsClientId)
if delayTime != 0:
for _ in range(int(delayTime / self._timeStep)):
p.stepSimulation(physicsClientId=self._physicsClientId)
Example #22
| Source File: motorController.py From Boston_Dynamics_Atlas_Explained with MIT License | 5 votes |
|
def setMotorAngle(self, motorTargetAngles):
for i in range(self._joint_number):
self._joint_targetPos[i] = motorTargetAngles[i]
p.setJointMotorControl2(bodyIndex=self._robot, jointIndex=self._joint_id[i], controlMode=p.POSITION_CONTROL,
targetPosition=self._joint_targetPos[i],
positionGain=self._kp, velocityGain=self._kd, force=self._torque, maxVelocity=self._max_velocity, physicsClientId=self._physicsClientId)
Example #23
| Source File: motorController.py From Boston_Dynamics_Atlas_Explained with MIT License | 5 votes |
|
def setMotorsAngleInFixedTimestep(self, motorTargetAngles, motorTargetTime, delayTime):
if(motorTargetTime == 0):
self._joint_targetPos = np.array(motorTargetAngles)
for i in range(self._joint_number):
p.setJointMotorControl2(bodyIndex=self._robot, jointIndex=self._joint_id[i], controlMode=p.POSITION_CONTROL,
targetPosition=self._joint_targetPos[i],
positionGain=self._kp, velocityGain=self._kd, force=self._torque, maxVelocity=self._max_velocity, physicsClientId=self._physicsClientId)
p.stepSimulation(physicsClientId=self._physicsClientId)
time.sleep(self._timeStep, physicsClientId=self._physicsClientId)
else:
self._joint_currentPos = self._joint_targetPos
self._joint_targetPos = np.array(motorTargetAngles)
for i in range(self._joint_number):
dydt = (self._joint_targetPos-self._joint_currentPos)/motorTargetTime
internalTime = 0.0
while internalTime < motorTargetTime:
internalTime += self._timeStep
for i in range(self._joint_number):
p.setJointMotorControl2(bodyIndex=self._robot, jointIndex=self._joint_id[i], controlMode=p.POSITION_CONTROL,
targetPosition=self._joint_currentPos[i] + dydt[i] * internalTime,
positionGain=self._kp, velocityGain=self._kd, force=self._torque, maxVelocity=self._max_velocity, physicsClientId=self._physicsClientId)
p.stepSimulation(physicsClientId=self._physicsClientId)
if delayTime != 0:
for _ in range(int(delayTime/self._timeStep)):
p.stepSimulation(physicsClientId=self._physicsClientId)
Example #24
| Source File: icub_env_with_hands.py From pybullet-robot-envs with GNU Lesser General Public License v2.1 | 5 votes |
|
def open_hand(self):
# open fingers
if self._control_arm is 'l':
idx_fingers = [self._joint_name_to_ids[jn] for jn in self.joint_groups['l_hand']]
else:
idx_fingers = [self._joint_name_to_ids[jn] for jn in self.joint_groups['r_hand']]
pos = [0.0] * len(idx_fingers)
p.setJointMotorControlArray(self.robot_id, idx_fingers, p.POSITION_CONTROL,
targetPositions=pos,
positionGains=[0.1] * len(idx_fingers),
velocityGains=[1.0] * len(idx_fingers),
physicsClientId=self._physics_client_id)
Example #25
| Source File: icub_env_with_hands.py From pybullet-robot-envs with GNU Lesser General Public License v2.1 | 5 votes |
|
def grasp(self, pos=None):
# close fingers
if self._control_arm is 'l':
idx_thumb = self._joint_name_to_ids['l_hand::l_tj2']
idx_fingers = [self._joint_name_to_ids[jn] for jn in self.joint_groups['l_hand']]
else:
idx_thumb = self._joint_name_to_ids['r_hand::r_tj2']
idx_fingers = [self._joint_name_to_ids[jn] for jn in self.joint_groups['r_hand']]
# # set also position to other joints to avoid weird movements
# not_idx_fingers = [idx for idx in self._joints_to_control if idx not in idx_fingers]
#
# joint_states = p.getJointStates(self.robot_id, not_idx_fingers)
# joint_poses = [x[0] for x in joint_states]
# p.setJointMotorControlArray(self.robot_id, not_idx_fingers, p.POSITION_CONTROL,
# targetPositions=joint_poses,
# positionGains=[0.1] * len(not_idx_fingers),
# velocityGains=[1.0] * len(not_idx_fingers),
# physicsClientId=self._physics_client_id)
position_control = True
if position_control:
if pos is None:
pos = self._grasp_pos
p.setJointMotorControlArray(self.robot_id, idx_fingers, p.POSITION_CONTROL,
targetPositions=pos,
positionGains=[0.1] * len(idx_fingers),
velocityGains=[1.0] * len(idx_fingers),
forces=[10] * len(idx_fingers),
physicsClientId=self._physics_client_id)
else:
# vel = [0, 1, 1, 1.2, 0, 1, 1, 1.2, 0, 1, 1, 1.2, 0, 1, 1, 1.2, 1.57, 1.5, 1.1, 1.1]
vel = [0, 0.5, 0.6, 0.6, 0, 0.5, 0.6, 0.6, 0, 0.5, 0.6, 0.6, 0, 0.5, 0.6, 0.6, 0, 0.5, 0.6, 0.6]
p.setJointMotorControlArray(self.robot_id, idx_fingers, p.VELOCITY_CONTROL,
targetVelocities=vel,
positionGains=[0.1] * len(idx_fingers),
velocityGains=[1.0] * len(idx_fingers),
physicsClientId=self._physics_client_id)
Example #26
| Source File: panda_env.py From pybullet-robot-envs with GNU Lesser General Public License v2.1 | 5 votes |
|
def reset(self):
# Load robot model
flags = p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES | p.URDF_USE_INERTIA_FROM_FILE
self.robot_id = p.loadURDF(os.path.join(franka_panda.get_data_path(), "panda_model.urdf"),
basePosition=self._base_position, useFixedBase=True, flags=flags,
physicsClientId=self._physics_client_id)
assert self.robot_id is not None, "Failed to load the panda model"
# reset joints to home position
num_joints = p.getNumJoints(self.robot_id, physicsClientId=self._physics_client_id)
idx = 0
for i in range(num_joints):
joint_info = p.getJointInfo(self.robot_id, i, physicsClientId=self._physics_client_id)
joint_name = joint_info[1].decode("UTF-8")
joint_type = joint_info[2]
if joint_type is p.JOINT_REVOLUTE or joint_type is p.JOINT_PRISMATIC:
assert joint_name in self.initial_positions.keys()
self._joint_name_to_ids[joint_name] = i
p.resetJointState(self.robot_id, i, self.initial_positions[joint_name], physicsClientId=self._physics_client_id)
p.setJointMotorControl2(self.robot_id, i, p.POSITION_CONTROL,
targetPosition=self.initial_positions[joint_name],
positionGain=0.2, velocityGain=1.0,
physicsClientId=self._physics_client_id)
idx += 1
self.ll, self.ul, self.jr, self.rs = self.get_joint_ranges()
if self._use_IK:
self._home_hand_pose = [0.2, 0.0, 0.8,
min(m.pi, max(-m.pi, m.pi)),
min(m.pi, max(-m.pi, 0)),
min(m.pi, max(-m.pi, 0))]
self.apply_action(self._home_hand_pose)
p.stepSimulation(physicsClientId=self._physics_client_id)
Example #27
| Source File: turtlebot.py From costar_plan with Apache License 2.0 | 5 votes |
|
def arm(self, cmd, mode=pb.POSITION_CONTROL):
pass
Example #28
| Source File: minitaur.py From soccer-matlab with BSD 2-Clause "Simplified" License | 5 votes |
|
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)
Example #29
| Source File: jacobian.py From soccer-matlab with BSD 2-Clause "Simplified" License | 5 votes |
|
def setJointPosition(robot, position, kp=1.0, kv=0.3):
num_joints = p.getNumJoints(robot)
zero_vec = [0.0] * num_joints
if len(position) == num_joints:
p.setJointMotorControlArray(robot, range(num_joints), p.POSITION_CONTROL,
targetPositions=position, targetVelocities=zero_vec,
positionGains=[kp] * num_joints, velocityGains=[kv] * num_joints)
else:
print("Not setting torque. "
"Expected torque vector of "
"length {}, got {}".format(num_joints, len(torque)))
Example #30
| Source File: saveRestoreStateTest.py From soccer-matlab with BSD 2-Clause "Simplified" License | 5 votes |
|
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)
