def test_forcetorquesensor(self):
import pybullet as p
p.connect(p.DIRECT)
hinge = p.loadURDF("../simulator/data/hinge.urdf")
print("mass of linkA = 1kg, linkB = 1kg, total mass = 2kg")
hingeJointIndex = 0
# by default, joint motors are enabled, maintaining zero velocity
p.setJointMotorControl2(hinge, hingeJointIndex, p.VELOCITY_CONTROL, 0,
0, 0)
p.setGravity(0, 0, -10)
p.stepSimulation()
print("joint state without sensor")
print(p.getJointState(0, 0))
p.enableJointForceTorqueSensor(hinge, hingeJointIndex)
p.stepSimulation()
print("joint state with force/torque sensor, gravity [0,0,-10]")
print(p.getJointState(0, 0))
p.setGravity(0, 0, 0)
p.stepSimulation()
print("joint state with force/torque sensor, no gravity")
print(p.getJointState(0, 0))
p.disconnect()
def __init__(self, bodyID, joints, hand, eeName, ik, torque_limits, startq=None):
'''Establish all the robot specific variables and set up key
data structures. Eventually it might be nice to read the specific variables
from a combination of the urdf and a yaml file'''
self.bodyID = bodyID
self.__robot = pb_robot.body.Body(self.bodyID)
self.joints = joints
self.jointsID = [j.jointID for j in self.joints]
self.eeFrame = self.__robot.link_from_name(eeName)
self.hand = hand
self.torque_limits = torque_limits
# Eventually add a more fleshed out planning suite
self.birrt = pb_robot.planners.BiRRTPlanner()
self.snap = pb_robot.planners.SnapPlanner()
# Add force torque sensor at wrist
self.ft_joint = self.__robot.joint_from_name('panda_hand_joint')
p.enableJointForceTorqueSensor(self.__robot.id, self.ft_joint.jointID, enableSensor=1)
#self.control = PandaControls(self)
# We manually maintain the kinematic tree of grasped objects by
# keeping track of a dictionary of the objects and their relations
# to the arm (normally the grasp matrix)
self.grabbedRelations = dict()
self.grabbedObjects = dict()
# Use IK fast for inverse kinematics
self.ik_info = ik
# Set the robot to the default home position
if startq is not None:
self.startq = startq #[0, -numpy.pi/4.0, 0, -0.75*numpy.pi, 0, numpy.pi/2.0, numpy.pi/4.0]
self.SetJointValues(self.startq)
self.collisionfn_cache = {}
def __init__(self, gripperUid, linkIndex):
self.gripperUid = gripperUid
self.sensorIndex = linkIndex
self.loopCounter = 0
# enable force torque sensors
p.enableJointForceTorqueSensor(self.gripperUid, self.sensorIndex)
# initialize line to visualize FT
self.ftLineId = p.addUserDebugLine(
[0, 0, 0], [0, 0.1, 0], [0, 0, 0],
parentObjectUniqueId=self.gripperUid,
parentLinkIndex=self.sensorIndex)
# initialize line to visualize contact center of pressure
self.cpLineId = p.addUserDebugLine(
[0, 0, 0], [0, 0.01, 0], [1, 0, 0],
parentObjectUniqueId=self.gripperUid,
parentLinkIndex=self.sensorIndex)
self.bias = np.zeros(6)
self.contactState = False
self.contactStateCounter = 0
self.sensor_thickness = 0.00929 # sensor thickness in m
def __init__(self, dt=1e-3):
self.dt = dt
self.omega_xyz = None
self.omega = None
self.v = None
self.record_rt = False # record video in real time
self.record_stepped = False # record video in sim steps # TODO: Deprecated?
# print(p.getJointInfo(bot, 3))
# Record Video in real time
if self.record_rt is True:
p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "file1.mp4")
if self.record_stepped is True:
output = "video.avi"
img = pyautogui.screenshot()
img = cv2.cvtColor(np.array(img), cv2.COLOR_RGB2BGR)
# get info from img
height, width, channels = img.shape
# Define the codec and create VideoWriter object
fourcc = cv2.VideoWriter_fourcc(*'mp4v')
self.out = cv2.VideoWriter(output, fourcc, 20.0, (width, height))
p.setTimeStep(self.dt)
p.setRealTimeSimulation(useRealTime)
# Disable the default velocity/position motor:
for i in range(p.getNumJoints(bot)):
p.setJointMotorControl2(bot, i, p.VELOCITY_CONTROL, force=0.5)
# force=1 allows us to easily mimic joint friction rather than disabling
p.enableJointForceTorqueSensor(bot, i,
1) # enable joint torque sensing
def set_ft_sensor_at(self, joint_id, enable=True):
if joint_id in self._ft_joints and not enable:
self._ft_joints.remove(joint_id)
elif joint_id not in self._ft_joints and enable:
self._ft_joints.append(joint_id)
pb.enableJointForceTorqueSensor(self._id, joint_id, enable, self._uid)
def init_joints(self):
"""Initialising the starting joint values"""
for joint, index in self.jointIndex.items():
p.setJointMotorControl2(self.nao,
index[0],
p.POSITION_CONTROL,
targetPosition=0,
force=1000)
p.enableJointForceTorqueSensor(self.nao, index[0], enableSensor=1)
shoulderPitch = np.pi / 2.
shoulderRoll = 0.1
# this makes the arms hang down and not forward
p.setJointMotorControl2(self.nao,
56,
p.POSITION_CONTROL,
targetPosition=shoulderPitch,
force=1000)
p.setJointMotorControl2(self.nao,
39,
p.POSITION_CONTROL,
targetPosition=shoulderPitch,
force=1000)
p.setJointMotorControl2(self.nao,
57,
p.POSITION_CONTROL,
targetPosition=-shoulderRoll,
force=1000)
p.setJointMotorControl2(self.nao,
40,
p.POSITION_CONTROL,
targetPosition=shoulderRoll,
force=1000)
def __init__(self, parent, config):
super().__init__(parent, config)
# (optional) filter for child frames
if 'frame_filter' in config:
self.frame_ids = parent.find_frames_ids(config.get('frame_filter'))
else:
self.frame_ids = list(range(p.getNumJoints(parent.uid)))
self.uid = parent.uid
for frame_id in self.frame_ids:
p.enableJointForceTorqueSensor(self.uid,
frame_id,
enableSensor=True)
num_joints = len(self.frame_ids)
self.observation_space = spaces.Dict({
'force':
spaces.Box(-10, 10, shape=(
num_joints,
3,
), dtype='float32'),
'torque':
spaces.Box(-10, 10, shape=(
num_joints,
3,
), dtype='float32'),
})
def __init__(self,
initial_pos=[0, 0, 0],
inital_orn=bullet.getQuaternionFromAxisAngle([0, 0, 0]),
time_step=0.01):
super().__init__(self.model_path,
initial_pos,
inital_orn,
time_step=time_step)
self.arm_link_num = 8
self.gripper_link_num = 10
self.arm_dof_num = 6
self.dof_num = self.arm_dof_num + self.gripper_dof_num
self.gripper_dof_num = 6
self.arm_idx_range = range(0, self.arm_dof_num)
self.griper_idx_range = range(self.arm_dof_num,
self.arm_link_num + self.gripper_dof_num)
self.joint_indices = range(self.arm_dof_num + self.gripper_dof_num)
self.init_action_space()
self.init_observation_space()
bullet.enableJointForceTorqueSensor(self.id,
range(self.dof_num),
enableSensor=True)
def addJointMeasurement(self, joint, variables=None):
""" Measure a specific joint with specific variables ( Position, Velocity, Force) in generalized coordinates.
"""
# Take only valid links
if joint in self.joints.keys():
joint_id = self.joints[joint]["jointIndex"]
else:
return
variable_keys = ["position", "velocity", "force", "motor_torque"]
variables = sorted(
[variable for variable in variables if variable in variable_keys]
)
if not variables:
return
if not hasattr(self, "measurements"):
self.measurements = {"joints": {}, "links": {}}
if "forces" in variables:
pybullet.enableJointForceTorqueSensor(
bodyUniqueId=self.id,
jointIndex=joint_id,
enableSensor=True,
physicsClientId=self.client_id,
)
self.measurements["joints"].update({joint: variables})
def loadURDF_all(self):
self.UR5UrdfPath = "./urdf/ur5_robotiq85.urdf"
# add search path for loadURDFs
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, -9.8)
# p.setTimeStep(0.1)
# Load URDF
# define world
self.planeID = p.loadURDF("plane.urdf")
tableStartPos = [0.7, 0.0, 0.8]
tableStartOrientation = p.getQuaternionFromEuler(
[0, 0, 90.0 * Deg2Rad])
self.tableID = p.loadURDF("./urdf/objects/table.urdf",
tableStartPos,
tableStartOrientation,
useFixedBase=True,
flags=p.URDF_USE_INERTIA_FROM_FILE)
# define environment
self.blockPos = [
0.6 + 0.3 * (random.random() - 0.5), 0.3 * (random.random() - 0.5),
0.85
]
#self.blockPos = [0.65, 0.025, 0.87]
self.blockOri = p.getQuaternionFromEuler([0, 0, 0])
self.boxId = p.loadURDF("./urdf/objects/block.urdf",
self.blockPos,
self.blockOri,
flags=p.URDF_USE_INERTIA_FROM_FILE,
useFixedBase=True)
robotStartPos = [0.0, 0.0, 0.0]
robotStartOrn = p.getQuaternionFromEuler([0, 0, 0])
print("----------------------------------------")
print("Loading robot from {}".format(self.UR5UrdfPath))
self.robotID = p.loadURDF(self.UR5UrdfPath,
robotStartPos,
robotStartOrn,
useFixedBase=True,
flags=p.URDF_USE_INERTIA_FROM_FILE)
# flags=p.URDF_USE_INERTIA_FROM_FILE)
self.controlJoints = [
"shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
"wrist_1_joint", "wrist_2_joint", "wrist_3_joint",
"robotiq_85_left_knuckle_joint", "robotiq_85_right_knuckle_joint",
"robotiq_85_left_inner_knuckle_joint",
"robotiq_85_right_inner_knuckle_joint",
"robotiq_85_left_finger_tip_joint",
"robotiq_85_right_finger_tip_joint", "ee_fixed_joint",
"world_arm_joint"
]
self.joints = []
for i in range(p.getNumJoints(self.robotID)):
p.enableJointForceTorqueSensor(self.robotID, i)
info = p.getJointInfo(self.robotID, i)
self.joints.append(info[1].decode("utf-8"))
print(info)
def set_joint_force_torque_sensor(self, on_off: bool):
"""
Enable/Disable joint force torque sensor
"""
for joint_index in self.free_joint_indices:
p.enableJointForceTorqueSensor(
self.id, joint_index, on_off, **self._client_kwargs
)
def _setup_gripper(self):
# add kuka arm
# self.armId = p.loadSDF("kuka_iiwa/kuka_with_gripper2.sdf")[0]
# self.armId = p.loadURDF("franka_panda/panda.urdf", useFixedBase=True)
self.armId = p.loadURDF("kuka_iiwa/model.urdf", [0, 0, 0],
useFixedBase=True)
p.resetBasePositionAndOrientation(self.armId, [0, 0, 0], [0, 0, 0, 1])
# TODO modify dynamics to induce traps
# for j in range(p.getNumJoints(self.armId)):
# p.changeDynamics(self.armId, j, linearDamping=0, angularDamping=0)
# orientation of the end effector
self.endEffectorOrientation = p.getQuaternionFromEuler(
[0, math.pi / 2, 0])
self.endEffectorIndex = kukaEndEffectorIndex
self.numJoints = p.getNumJoints(self.armId)
# get the joint ids
# TODO try out arm with attached grippers
# self.armInds = [i for i in range(pandaNumDofs)]
self.armInds = [i for i in range(self.numJoints)]
# create a constraint to keep the fingers centered
# c = p.createConstraint(self.armId,
# 9,
# self.armId,
# 10,
# jointType=p.JOINT_GEAR,
# jointAxis=[1, 0, 0],
# parentFramePosition=[0, 0, 0],
# childFramePosition=[0, 0, 0])
# p.changeConstraint(c, gearRatio=-1, erp=0.1, maxForce=50)
# joint damping coefficents
# self.jd = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
# self.jd = [
# 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001,
# 0.00001, 0.00001, 0.00001, 0.00001
# ]
p.enableJointForceTorqueSensor(self.armId, self.endEffectorIndex)
self._calculate_init_joints()
for i in self.armInds:
p.resetJointState(self.armId, i, self.initJoints[i])
# self._open_gripper()
# self._close_gripper()
# make arm translucent
visual_data = p.getVisualShapeData(self.armId)
for link in visual_data:
link_id = link[1]
if link_id == -1:
continue
rgba = list(link[7])
rgba[3] = 0.4
p.changeVisualShape(self.armId, link_id, rgbaColor=rgba)
def enable_joint_sensor(self, joint_uid):
"""Enable joint force torque sensor.
Args:
joint_uid: A tuple of the body Unique ID and the joint index.
"""
body_uid, joint_ind = joint_uid
pybullet.enableJointForceTorqueSensor(bodyUniqueId=body_uid,
jointIndex=joint_ind,
enableSensor=1,
physicsClientId=self.uid)
def load(self, base_pose):
#p.resetSimulation()
orn_q = p.getQuaternionFromEuler(base_pose[3:6])
self.robot_id = p.loadURDF("urdf/kuka_iiwa_6/modified_model.urdf",
basePosition=base_pose[:3],
baseOrientation=orn_q,
useFixedBase=True)
print('robot joint num:', p.getNumJoints(self.robot_id))
p.enableJointForceTorqueSensor(self.robot_id, self.force_joint_index)
self.reset_joint([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
p.stepSimulation()
def init_simulation():
global quadruped, motor_id_list
physicsClient = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
motor_id_list = [4, 5, 6, 12, 13, 14, 0, 1, 2, 8, 9, 10]
all_motor_id_list = [4, 5, 6, 12, 13, 14, 0, 1, 2, 8, 9, 10, 3, 7, 11, 15]
p.setGravity(0, 0, -9.8)
# p.setPhysicsEngineParameter(fixedTimeStep=1.0 / 10., numSolverIterations=550, numSubSteps=4)
p.resetDebugVisualizerCamera(0.2, 45, -30, [1, -1, 1])
planeId = p.loadURDF("plane.urdf")
p.changeDynamics(bodyUniqueId=planeId,
linkIndex=-1,
contactStiffness=500000,
contactDamping=5000)
init_position = [0, 0, 0.5]
quadruped = p.loadURDF("mini_cheetah/mini_cheetah.urdf",
init_position,
useFixedBase=False)
num_joints = p.getNumJoints(quadruped)
compensate = [-1, 1, 1, 1, 1, 1, -1, -1, -1, 1, -1, -1, 1, 1, 1, 1]
# init_pos = [-1, -0.8, 1.75, 1, -0.8, 1.75, 1, 0.8, -1.75, -1, 0.8, -1.75, 0, 0, 0, 0]
# init_pos = [0, -0.8, 1.75, 0, -0.8, 1.75, 0, 0.8, -1.75, -0, 0.8, -1.75, 0, 0, 0, 0]
init_pos = [
-0.2, -1.1, 2.8, 0.2, -1.1, 2.8, 0.2, 1.1, -2.8, -0.2, 1.1, -2.8, 0, 0,
0, 0
]
init_force = [
200, 200, 200, 200, 200, 200, 200, 200, 200, 200, 200, 200, 200, 200,
200, 200
]
# init_force = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
init_new_pos = []
init_new_force = []
for j in range(16):
init_new_pos.append(init_pos[j] * compensate[j])
init_new_force.append(init_force[j] * compensate[j])
# p.setJointMotorControlArray(quadruped,
# jointIndices=all_motor_id_list,
# controlMode=p.POSITION_CONTROL,
# targetPositions=init_new_pos,
# forces=init_new_force)
for j in range(16):
print(num_joints)
p.changeVisualShape(quadruped, j, rgbaColor=[1, 1, 1, 1])
force = 500
pos = 0
# p.setJointMotorControl2(quadruped, j, p.VELOCITY_CONTROL, force=force)
p.setJointMotorControl2(quadruped,
all_motor_id_list[j],
p.POSITION_CONTROL,
init_new_pos[j],
force=force)
p.enableJointForceTorqueSensor(quadruped, j, 1)
def __init__(self, body_id):
self.body_id = body_id
self.joints = {}
numJoints = pb.getNumJoints(body_id)
names = self.jointNames
for i in range(numJoints):
info = pb.getJointInfo(body_id, i)
jointName = info[1].decode("utf-8")
if jointName not in names:
continue
self.joints[jointName] = info
jointID = info[0]
pb.resetJointState(self.body_id, jointID, 0)
pb.setJointMotorControl2(body_id,
jointID,
pb.VELOCITY_CONTROL,
targetVelocity=0,
force=100)
pb.enableJointForceTorqueSensor(self.body_id, jointID, True)
self.jointIDs = [
self.joints[name][0] for name in Robotiq3FGripper.jointNames
]
self._targets = [0, 0, 0]
joint1Stops = np.array(range(256)) / 255.
joint2Stops = np.zeros(256)
joint2Stops[128:] = np.array(range(128)) / 128.
joint3Stops = np.zeros(256)
joint3Stops[:192] = np.array(range(0, -192, -1)) / 192.
joint3Stops[192:] = np.array(range(-192, -128)) / 192.
self.jointStops = np.zeros((256, 9))
self.jointStops[:, 0] = joint1Stops
self.jointStops[:, 3] = joint1Stops
self.jointStops[:, 6] = joint1Stops
self.jointStops[:, 1] = joint2Stops
self.jointStops[:, 4] = joint2Stops
self.jointStops[:, 7] = joint2Stops
self.jointStops[:, 2] = joint3Stops
self.jointStops[:, 5] = joint3Stops
self.jointStops[:, 8] = joint3Stops
# grasp modes: normal, pinch, wide, scissors, narrow
# urdf joint limit are [0.16, -0.25], but 0.16 causes a self collision in pinch mode,
# which messes up the FT sensor reading, so we use 0.13 instead
self.modeStopsB = [0, 0.13, -0.25, -0.25, 0.13]
self.modeStopsC = [0, -0.13, 0.25, 0.25, -0.13]
self.last_joint_positions = np.zeros(len(self.jointNames))
self.last_joint_speeds = np.zeros(len(self.jointNames))
self._mode_limit = 255
def __init__(self, Uid, joint_num):
self.Uid = Uid
self.joint_num = joint_num
p.enableJointForceTorqueSensor(self.Uid, self.joint_num, 1)
self.joint_force_fx = []
self.joint_force_fy = []
self.joint_force_fz = []
self.joint_force_Mx = []
self.joint_force_My = []
self.joint_force_Mz = []
self.joint_motor_torque = []
self.joint_vel = []
def __init__(self,
initial_pos=[0, 0, 0],
initial_orn=bullet.getQuaternionFromEuler([0, 0, 0])):
self.config = RobotUR5RobotiqC2Config()
RobotEnv.__init__(self, self.config, initial_pos, initial_orn)
for idx in range(self.config.joints_num):
bullet.enableJointForceTorqueSensor(self.id,
idx,
enableSensor=True)
self.curr_action = np.zeros(self.config.arm_active_joints_num)
self.prev_action = np.zeros(self.config.arm_active_joints_num)
self.ext_action_space()
self.ext_state_space()
def reset(self):
p.resetSimulation()
planeId = p.loadURDF("plane.urdf")
print(self.joint_list)
self.robot_id = p.loadURDF(self.assets + "/ur_description/ur5.urdf",
self.robot_start_pos,
self.robot_start_orientation,
flags=p.URDF_USE_SELF_COLLISION)
for i in range(1, 7):
p.enableJointForceTorqueSensor(self.robot_id, i)
p.stepSimulation()
reward = self.get_reward
def enable_joint_sensor(self, joint_name, enable=True):
"""Activates or deactivates the force-torque sensor for a given joint.
:type joint_name: str
:type enable: bool
"""
pb.enableJointForceTorqueSensor(self.__bulletId,
self.joints[joint_name].jointIndex,
enable,
physicsClientId=self.__client_id)
if enable:
self.joint_sensors.add(joint_name)
else:
self.joint_sensors.remove(joint_name)
def __init__(self,
visualize=True,
bullet=None,
shift=0,
start=None,
goal=None):
self.pw = PipeWorld(visualize=visualize, bullet=bullet)
self.pipe_attach = None
self.target_grip = 0.015
self.default_width = 0.010
self.total_timeout = 3
self.steps_taken = 0
self.dt_pose = 0.1
if self.pw.handonly:
self.ee_link = -1
self.finger_joints = (0, 1)
p.enableJointForceTorqueSensor(self.pw.robot, 0)
p.enableJointForceTorqueSensor(self.pw.robot, 1)
else:
self.ee_link = 8 # 8#joint_from_name(self.pw.robot, "panda_hand_joint")
self.joints = [1, 2, 3, 4, 5, 6, 7]
self.finger_joints = (
9, 10
) # [joint_from_name(self.pw.robot,"panda_finger_joint"+str(side)) for side in [1,2]]
p.enableJointForceTorqueSensor(self.pw.robot, 9)
p.enableJointForceTorqueSensor(self.pw.robot, 10)
next(
joint_controller(self.pw.robot, self.joints,
[0., 0., -1.5708, 0., 1.8675, 0., 0]))
self._shift = shift
self.do_setup()
def reset(self):
# start position is along the x axis, in negative direction
start_positions = [0, -math.pi/2, math.pi/2, -math.pi/2, -math.pi/2, 0]
for i in range(6):
pb.resetJointState(self.body_id, self.base_joint_id + i, start_positions[i])
pb.setJointMotorControl2(self.body_id,
self.base_joint_id + i,
controlMode=pb.VELOCITY_CONTROL,
targetVelocity = 0,
force = URArm.SIM_MAX_JOINT_FORCE)
pb.enableJointForceTorqueSensor(self.body_id, self.ft_sensor_id, True)
self.ft_bias = np.zeros(6)
pb.stepSimulation()
self.ft_bias[:] = self.ur_get_tcp_sensor_force()
def add_gripper(self, gripper_fn):
self.gid = p.loadURDF(gripper_fn)
self.drawFrame([0, 0, 0])
p.resetJointState(self.gid, 2, 1)
p.enableJointForceTorqueSensor(self.gid, 2)
p.enableJointForceTorqueSensor(self.gid, 5)
p.enableJointForceTorqueSensor(self.gid, 6)
p.enableJointForceTorqueSensor(self.gid, 7)
def __init__(self):
ShowBase.__init__(self)
self.setBackgroundColor(0.0, 0.0, 0.0)
self.disableMouse()
self.camLens.setNearFar(1.0, 100.0)
self.camLens.setFov(75.0)
self.root = self.render.attachNewNode("Root")
self.root.setPos(0.0, 0.0, 0.0)
self.taskMgr.add(self.physicsTask, "physicsTask")
self.taskMgr.add(self.egoUpdateTask, "egoUpdateTask")
self.modelCube = loader.loadModel("cube.egg")
self.cube = self.modelCube.copyTo(self.root)
self.cube.setPos((0,0,0))
physicsClient = p.connect(p.DIRECT)#or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
p.setGravity(0,0,-10)
planeId = p.loadURDF("plane.urdf")
cubeStartPos = [0,0,1]
cubeStartOrientation = p.getQuaternionFromEuler([0,0,0])
p.setAdditionalSearchPath(os.path.dirname(os.path.abspath(__file__))) #optionally
self.boxId = p.loadURDF("car.urdf",cubeStartPos, cubeStartOrientation)
p.enableJointForceTorqueSensor(self.boxId, 0)
p.enableJointForceTorqueSensor(self.boxId, 1)
p.changeDynamics(self.boxId, -1, lateralFriction=0.1)
p.changeDynamics(self.boxId, 0, lateralFriction=0.1)
p.changeDynamics(self.boxId, 1, lateralFriction=0.1)
self.control_force = 0
self.semi_force = 1
inputState.watchWithModifiers("throttle", "w")
inputState.watchWithModifiers("brake", "s")
inputState.watchWithModifiers("exit", "escape")
inputState.watchWithModifiers("oobe", "o")
self.prev_control_force = 0
self.control_alpha = 0.9999
self.integrator = 0
def reset(self):
objects = p.loadSDF(
os.path.join(self.urdfRootPath,
"kuka_iiwa/kuka_with_gripper2.sdf"))
self.kukaUid = objects[0]
p.resetBasePositionAndOrientation(self.kukaUid, self.base_pose[0],
self.base_pose[1])
# 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.jointPositions = [
0.0, 0.0, 0.0, 0.5 * math.pi, 0, -math.pi * 0.5 * 0.66, 0, 0,
-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)
# add FT sensor
p.enableJointForceTorqueSensor(self.kukaUid, self.kukaEndEffectorIndex,
True)
self.endEffectorPos = [0.0, 0.0, 0.4]
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:
self.motorNames.append(str(jointInfo[1]))
self.motorIndices.append(i)
class simulation:
physicsClient = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, -9.8)
planeId = p.loadURDF("plane.urdf")
#shapeID = p.createCollisionShape(shapeType=p.GEOM_SPHERE, radius=0.05)
handID = p.loadURDF(
"Hand-Assem-By-Part-URDF.SLDASM/urdf/Hand-Assem-By-Part-URDF.SLDASM.urdf",
basePosition=[0, 0, 0],
useFixedBase=True,
flags=p.URDF_USE_SELF_COLLISION)
#p.createMultiBody(baseMass=1, baseCollisionShapeIndex=shapeID, basePosition=[0.5, 0.5, 0.5])
shapeID = p.loadURDF(
"Hand-Assem-By-Part-URDF.SLDASM/urdf/Example Sphere.urdf")
for i in range(p.getNumJoints(handID)):
p.enableJointForceTorqueSensor(handID, i)
resizeObj = resize.Resize(range(3, 20, 2))
while True:
p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING)
resizeObj.iterate()
shapeID = p.loadURDF(
"Hand-Assem-By-Part-URDF.SLDASM/urdf/Example Sphere.urdf")
jointValues.getJointValues(handID)
p.stepSimulation()
time.sleep(1. / 240.)
p.disconnect()
def __init__(self, parent, config, link_name=None, force_enabled=False):
super(ForceTorqueSensor, self).__init__(parent, config)
self.uid = parent.uid
self.frame_id = parent.get_frame_id(
config.get('frame')) if 'frame' in config else -1
p.enableJointForceTorqueSensor(self.uid,
self.frame_id,
enableSensor=True)
self.observation_space = spaces.Dict({
'force':
spaces.Box(-10, 10, shape=(3, ), dtype='float32'),
'torque':
spaces.Box(-10, 10, shape=(3, ), dtype='float32'),
})
def __init__(self, client):
self.client = client
f_name = os.path.join(os.path.dirname(__file__), 'car.urdf')
self.id = p.loadURDF(fileName=f_name,
basePosition=[0, 0, 0.1],
physicsClientId=client)
p.enableJointForceTorqueSensor(self.id, 0, True)
# Joint indices as found by p.getJointInfo()
self.steering_joints = [0, 2]
self.drive_joints = [1, 3, 4, 5]
# Joint speed
self.joint_speed = 0
# Drag constants
self.c_rolling = 0.2
self.c_drag = 0.01
# Throttle constant increases "speed" of the car
self.c_throttle = 20
def __init__(self, config, initial_pos, initial_orn):
model_path = config.model_path
if model_path.startswith('/'):
fullpath = model_path
else:
fullpath = os.path.join(os.path.dirname(__file__), 'models',
model_path)
if not os.path.exists(fullpath):
raise IOError('File {} does not found'.format(fullpath))
self.id = bullet.loadURDF(fullpath)
# self.id = bullet.loadURDF(fullpath, initial_pos, initial_orn, flags=bullet.URDF_USE_INERTIA_FROM_FILE)
for idx in range(config.joints_num):
bullet.enableJointForceTorqueSensor(self.id,
idx,
enableSensor=True)
self.action_space = self.init_action_space()
self.state_space = self.init_state_space()
def __setup_pybullet_simulation(self):
"""
Set the physical parameters of the world in which the simulation
will run, and import the models to be simulated
"""
pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
pybullet.setGravity(0, 0, -9.81)
pybullet.setTimeStep(self.time_step_s)
pybullet.loadURDF("plane_transparent.urdf", [0, 0, 0])
self.__load_robot_urdf()
self.__set_pybullet_params()
self.__load_stage()
self.__disable_pybullet_velocity_control()
# enable force sensor on tips
for joint_index in self.pybullet_tip_link_indices:
pybullet.enableJointForceTorqueSensor(self.finger_id,
joint_index,
enableSensor=True)
import pybullet as p
p.connect(p.DIRECT)
hinge = p.loadURDF("hinge.urdf")
print("mass of linkA = 1kg, linkB = 1kg, total mass = 2kg")
hingeJointIndex = 0
#by default, joint motors are enabled, maintaining zero velocity
p.setJointMotorControl2(hinge, hingeJointIndex, p.VELOCITY_CONTROL, 0, 0, 0)
p.setGravity(0, 0, -10)
p.stepSimulation()
print("joint state without sensor")
print(p.getJointState(0, 0))
p.enableJointForceTorqueSensor(hinge, hingeJointIndex)
p.stepSimulation()
print("joint state with force/torque sensor, gravity [0,0,-10]")
print(p.getJointState(0, 0))
p.setGravity(0, 0, 0)
p.stepSimulation()
print("joint state with force/torque sensor, no gravity")
print(p.getJointState(0, 0))
p.disconnect()
