def check_collision(self, other_id=None, collision_distance=0.0):
others_id = [other_id]
if other_id is None:
others_id = [
p.getBodyUniqueId(i) for i in range(p.getNumBodies())
if p.getBodyUniqueId(i) != self.id
]
for other_id in others_id:
if len(
p.getClosestPoints(bodyA=self.id,
bodyB=other_id,
distance=collision_distance)) != 0:
return True
return False
def run(self) -> Dict[core.PhysicalObject, Dict[str, list]]:
steps_per_frame = self.scene.step_rate // self.scene.frame_rate
max_step = (self.scene.frame_end + 1) * steps_per_frame
obj_idxs = [pb.getBodyUniqueId(i) for i in range(pb.getNumBodies())]
animation = {
obj_id: {
"position": [],
"quaternion": [],
"velocity": [],
"angular_velocity": []
}
for obj_id in obj_idxs
}
for current_step in range(max_step):
if current_step % steps_per_frame == 0:
for obj_idx in obj_idxs:
position, quaternion = self.get_position_and_rotation(
obj_idx)
velocity, angular_velocity = self.get_velocities(obj_idx)
animation[obj_idx]["position"].append(position)
animation[obj_idx]["quaternion"].append(quaternion)
animation[obj_idx]["velocity"].append(velocity)
animation[obj_idx]["angular_velocity"].append(
angular_velocity)
pb.stepSimulation()
return {
self.objects_to_pybullet.inverse[obj_idx]: anim
for obj_idx, anim in animation.items()
}
def createMaiskolben(x, y, z, m_list, m_hit):
for i in range(10):
h = i * 0.4 + z
pixel_1 = p.loadURDF("pixel.urdf", [0.52263 + x, 0 + y, 3 + h],
useMaximalCoordinates=False)
pixel_2 = p.loadURDF("pixel.urdf", [-0.52263 + x, 0 + y, 3 + h],
useMaximalCoordinates=False)
pixel_3 = p.loadURDF("pixel.urdf", [0 + x, 0.52263 + y, 3 + h],
useMaximalCoordinates=False)
pixel_4 = p.loadURDF("pixel.urdf", [0 + x, -0.52263 + y, 3 + h],
useMaximalCoordinates=False)
pixel_5 = p.loadURDF("pixel.urdf", [0.36995 + x, 0.36995 + y, 3 + h],
useMaximalCoordinates=False)
pixel_6 = p.loadURDF("pixel.urdf", [-0.36995 + x, -0.36995 + y, 3 + h],
useMaximalCoordinates=False)
pixel_7 = p.loadURDF("pixel.urdf", [0.36995 + x, -0.36995 + y, 3 + h],
useMaximalCoordinates=False)
pixel_8 = p.loadURDF("pixel.urdf", [-0.36995 + x, 0.36995 + y, 3 + h],
useMaximalCoordinates=False)
m_list.append(pixel_1)
m_list.append(pixel_2)
m_list.append(pixel_3)
m_list.append(pixel_4)
m_list.append(pixel_5)
m_list.append(pixel_6)
m_list.append(pixel_7)
m_list.append(pixel_8)
j = 0
for pixel in m_list:
body = p.getBodyUniqueId(pixel)
while len(m_hit) < body:
m_hit.append(j)
m_hit.append(j)
def update_closest_points(self):
others_id = [
p.getBodyUniqueId(i) for i in range(p.getNumBodies())
if p.getBodyUniqueId(i) != self.body_id
]
self.closest_points_to_others = [
sorted(list(
p.getClosestPoints(bodyA=self.body_id,
bodyB=other_id,
distance=self.max_distance_from_others)),
key=lambda contact_points: contact_points[8])
if other_id != 0 else [] for other_id in others_id
]
self.closest_points_to_self = [
p.getClosestPoints(bodyA=self.body_id,
bodyB=self.body_id,
distance=0,
linkIndexA=link1,
linkIndexB=link2)
for link1, link2 in self.link_pairs
]
def check_overlap(self, obj: core.PhysicalObject) -> bool:
obj_idx = self.objects_to_pybullet[obj]
body_ids = [pb.getBodyUniqueId(i) for i in range(pb.getNumBodies())]
for body_id in body_ids:
if body_id == obj_idx:
continue
overlap_points = pb.getClosestPoints(obj_idx, body_id, distance=0)
if overlap_points:
# TODO: we can easily get a suggested correction here
# i = np.argmin([o[8] for o in overlap_points], axis=0) # find the most overlapping point
# push = np.array(overlap_points[i][7]) * (overlap_points[i][8] + margin)
return True
return False
def test(args):
count = 0
env = gym.make(args.env)
env.env.configure(args)
print("args.render=", args.render)
if (args.render == 1):
env.render(mode="human")
env.reset()
w, h, vmat,projmat,camup,camfwd,hor,ver,yaw,pitch,dist,target= p.getDebugVisualizerCamera()
dist = 0.4
yaw = 0
p.resetDebugVisualizerCamera(dist,yaw, pitch,target)
for obindex in range (p.getNumBodies()):
obuid = p.getBodyUniqueId(obindex)
p.changeDynamics(obuid, -1, linearDamping=0, angularDamping=0)
for l in range (p.getNumJoints(obuid)):
p.changeDynamics(obuid, l, linearDamping=0, angularDamping=0, jointDamping=0)
#if (l==0):
# p.setJointMotorControl2(obuid,l,p.POSITION_CONTROL,targetPosition=0)
if (l==2):
jp,jv,_,_ = p.getJointState(obuid,l)
p.resetJointState(obuid,l, jp,0.01 )
if (args.resetbenchmark):
while (1):
env.reset()
print("p.getNumBodies()=", p.getNumBodies())
print("count=", count)
count += 1
print("action space:")
sample = env.action_space.sample()
action = sample * 0.0
action = [0,0]#sample * 0.0
print("action=")
print(action)
for i in range(args.steps):
obs, rewards, done, _ = env.step(action)
if (args.rgb):
print(env.render(mode="rgb_array"))
print("obs=")
print(obs)
print("rewards")
print(rewards)
print("done")
print(done)
def run(self, duration: float = 1.0) -> Dict[Object3D, Dict[str, list]]:
max_step = math.floor(self.step_rate * duration)
steps_per_frame = self.step_rate // self.frame_rate
obj_idxs = [pb.getBodyUniqueId(i) for i in range(pb.getNumBodies())]
animation = {obj_id: {"position": [], "orient_quat": []}
for obj_id in obj_idxs}
for current_step in range(max_step):
if current_step % steps_per_frame == 0:
for obj_idx in obj_idxs:
pos, quat = pb.getBasePositionAndOrientation(obj_idx)
animation[obj_idx]["position"].append(pos)
animation[obj_idx]["orient_quat"].append(quat) # roll, pitch, yaw
pb.stepSimulation()
return {self.objects_by_idx[obj_idx]: anim for obj_idx, anim in animation.items()}
def high_contrast_bodies(alpha: float = 0.5):
"""Makes all bodies transparent and gives each body an individual color.
Args:
alpha (float): Regulates the strength of transparency.
"""
num_bodies = p.getNumBodies()
hsv = [(x * 1.0 / num_bodies, 0.5, 0.5) for x in range(num_bodies)]
rgb = list(map(lambda x: colorsys.hsv_to_rgb(*x), hsv))
for i in range(num_bodies):
body_unique_id = p.getBodyUniqueId(i)
for link_index in range(-1, p.getNumJoints(body_unique_id)):
p.changeVisualShape(
body_unique_id,
link_index,
rgbaColor=[rgb[i][0], rgb[i][1], rgb[i][2], alpha])
def get_sensor_image(self):
"""
Returns the raw and clipped sensor image. The clipped image
is generated based on the thickness of the tactile layer.
Returns:
(np.array, np.array, np.array, np.array) : RGB image, clipped RGB image, clipped depth image, seg image.
"""
# update sensor position and parameters
self._update_pose()
self._update_sensor()
rgb_img, depth_img, seg_img = self._camera.get_pybullet_image()
# update the contact information
self._contacts = Contact(self._sensor_id)
# clip the images to the depth of the tactile sensor
mask = np.where(depth_img >= self.max_buffer_depth)
depth_img[mask] = self.max_buffer_depth
# basically all the RGB image should be the color of the tactile surface
clipped_rgb_img = np.copy(rgb_img)
clipped_rgb_img[:, :, :] = self.background_color
# clip the segmentation image
clipped_seg_img = np.copy(seg_img)
clipped_seg_img[mask] = -1
if self._use_force:
# store the raw clipped images in the image buffer with the Z position of the object
obj_id = p.getBodyUniqueId(p.getNumBodies() - 1)
position, _ = p.getBasePositionAndOrientation(obj_id)
self._image_buf.store(clipped_rgb_img, depth_img, clipped_seg_img,
position[-1], self._time)
# compute the penetration based on the equilibrium of contact information and the spring models
img_equilibrium = self.compute_equilibrium()
return rgb_img, img_equilibrium['rgb_img'], img_equilibrium[
'depth_img'], seg_img, img_equilibrium['seg_img']
else:
return rgb_img, clipped_rgb_img, depth_img, seg_img, clipped_seg_img
def run_sim():
physicsClient = p.connect(p.GUI) #or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
p.setGravity(0, 0, -10)
planeId = p.loadURDF("assets/plane.urdf")
startPos = [0, 0, 1]
startOrientation = p.getQuaternionFromEuler([0, 0, 0])
robot = p.loadURDF("assets/my-robot/robot.urdf", startPos,
startOrientation)
# print("------")
wheel_rad = 0.0762
# print(p.getBodyUniqueId(robot))
bodyUniqueID = p.getBodyUniqueId(robot)
# for i in range(0, p.getNumJoints(robot)):
# print(p.getJointInfo(robot, i))
# p.setJointMotorControlArray(bodyUniqueID, [2, 3, 1, 0], p.VELOCITY_CONTROL, targetVelocities = [100, 100, 100, 100])
interface = OneWayWPILibWSInterfaceApp()
# leftVel = 0
# rightVel = 0
timeStep = 0.015
p.setTimeStep(timeStep)
while True:
startTime = time.time()
leftVel, rightVel = interface.get_velocity()
leftVel = leftVel / wheel_rad
rightVel = rightVel / wheel_rad
# print(leftVel, rightVel)
p.setJointMotorControlArray(
bodyUniqueID, [2, 3, 1, 0],
p.VELOCITY_CONTROL,
targetVelocities=[leftVel, leftVel, rightVel, rightVel])
p.stepSimulation()
print(time.time() - startTime)
time.sleep(timeStep)
# print(p.getJointStates(bodyUniqueID, [2, 3, 1, 0]))
# for state in p.getJointStates(bodyUniqueID, [2, 3, 1, 0]):
# print(state[1])
cubePos, cubeOrn = p.getBasePositionAndOrientation(robot)
print(cubePos, cubeOrn)
p.disconnect()
def getSceneAABB():
aabbMins = []
aabbMaxs = []
for i in range(p.getNumBodies()):
uid = p.getBodyUniqueId(i)
aabb = p.getAABB(uid)
aabbMins.append(np.array(aabb[0]))
aabbMaxs.append(np.array(aabb[1]))
if len(aabbMins):
sceneAABBMin = aabbMins[0]
sceneAABBMax = aabbMaxs[0]
for aabb in aabbMins:
sceneAABBMin = np.minimum(sceneAABBMin, aabb)
for aabb in aabbMaxs:
sceneAABBMax = np.maximum(sceneAABBMax, aabb)
print("sceneAABBMin=", sceneAABBMin)
print("sceneAABBMax=", sceneAABBMax)
def get_body_ids():
return sorted([p.getBodyUniqueId(i) for i in range(p.getNumBodies())])
def get_bodies():
return [p.getBodyUniqueId(i) for i in range(p.getNumBodies())]
def __init__(
self,
xml_path='/home/shjliu/workspace/EPR/bullet3-new/test-bullet/roomdoor',
num_solver_iterations=10,
frame_skip=4,
time_step=1 / 240.,
debug=0,
render=0,
actuator_lower_limit=(),
actuator_upper_limit=(),
robot_init_pos=(0, 0, 200),
door_init_pos=(3, 0, 0),
goal_pos=(3, 0, 0),
door_controller_k=10**5,
door_controller_d1=10**4,
door_controller_d2=10**2,
door_controller_th_handle=-30 * np.pi / 180,
door_controller_th_door=5 * np.pi / 180,
handle_controller_k=10**0.3,
handle_controller_d=10**-1):
# Set goal
self.goal_pos = goal_pos
self.robot_init_pos = robot_init_pos
self.door_init_pos = door_init_pos
# Camera parameters
self._cam_dist = 5
self._cam_yaw = 60
self._cam_pitch = -66
self._render_width = 320
self._render_height = 240
# Open physics client, load files and set parameters
self.physicsClient_ID = -1
self.xml_path = xml_path
self.num_solver_iterations = num_solver_iterations
self.frame_skip = frame_skip
self.time_step = time_step
self._set_physics_client(render)
# Create relevant dictionaries
body_name_2_joints = {}
for body_id in range(pybullet.getNumBodies()):
# Obtain name of specific body and decode it
(bin_body_name, _) = pybullet.getBodyInfo(body_id)
body_name = bin_body_name.decode("utf8")
if body_name == "base_link":
body_name = "door"
# Get the amount of joints, their names and put them into a dictionary
num_joints = pybullet.getNumJoints(body_id)
joint_name_2_joint_id = {}
if num_joints:
for joint_id in range(num_joints):
(_, bin_joint_name, joint_type, q_index, u_index, flags,
joint_damping, joint_friction, joint_lower_limit,
joint_upper_limit, joint_max_force, joint_max_velocity,
link_name, joint_axis, parent_frame_pos, parent_frame_orn,
parent_index) = pybullet.getJointInfo(body_id, joint_id)
joint_name = bin_joint_name.decode("utf8")
if joint_type == 4:
num_joints -= 1
else:
joint_name_2_joint_id.update({
joint_name: {
"joint_id": joint_id,
"joint_type": joint_type,
"joint_damping": joint_damping,
"joint_lower_limit": joint_lower_limit,
"joint_upper_limit": joint_upper_limit
}
})
for i in joint_name_2_joint_id:
print("joint info=", i, joint_name_2_joint_id[i])
body_name_2_joints.update({
body_name: {
"body_id": body_id,
"unique_body_id": pybullet.getBodyUniqueId(body_id),
"body_name": body_name,
"num_joints": num_joints,
"joint_dict": joint_name_2_joint_id
}
})
self.body_name_2_joints = body_name_2_joints
print("body info=", self.body_name_2_joints)
# Important link ID's
self.door_link_IDs = {
'handle_inside': 7,
'handle_outside': 9,
'door_COM': 5
}
self.robot_link_IDs = {
'torso': 0,
'right_hand': 28,
'left_hand': 34,
}
# Indicate names of controllable joints: ASSUMES ROBOT ROOT BODY IS NAMED 'robot' AND DOOR ROOT BODY 'door'
self.robot_joints = [
joint_name
for joint_name in body_name_2_joints['robot']['joint_dict']
]
self.robot_joints_index = [
body_name_2_joints['robot']['joint_dict'][joint_name]['joint_id']
for joint_name in body_name_2_joints['robot']['joint_dict']
]
self.door_joints = [
joint_name
for joint_name in body_name_2_joints['door']['joint_dict']
]
self.door_joints_index = [
body_name_2_joints['door']['joint_dict'][joint_name]['joint_id']
for joint_name in body_name_2_joints['door']['joint_dict']
]
# joint_low = [body_name_2_joints['robot']['joint_dict'][joint_name]['joint_lower_limit'] for joint_name in body_name_2_joints['robot']['joint_dict']]
# joint_high = [body_name_2_joints['robot']['joint_dict'][joint_name]['joint_upper_limit'] for joint_name in body_name_2_joints['robot']['joint_dict']]
# Initialize door controllers
self.door_controller_k = door_controller_k
self.door_controller_d1 = door_controller_d1
self.door_controller_d2 = door_controller_d2
self.door_controller_th_handle = door_controller_th_handle
self.door_controller_th_door = door_controller_th_door
self.handle_controller_k = handle_controller_k
self.handle_controller_d = handle_controller_d
# Initialize cost function weight placeholders. Will be set in hidden method
self.w_alive, self.w_hand_on_handle, self.w_at_target, self.w_move_door, self.w_move_handle, self.w_time, self.w_control = (
0, 0, 0, 0, 0, 0, 0)
self.set_reward_weights()
# Initialize action and observation space
if not actuator_lower_limit:
actuator_lower_limit = -np.pi * np.ones_like(
self.robot_joints_index)
actuator_upper_limit = np.pi * np.ones_like(
self.robot_joints_index)
else:
assert (len(self.robot_joints_index) ==
len(actuator_lower_limit) & len(self.robot_joints_index) ==
len(actuator_upper_limit))
self.action_space = gym.spaces.Box(low=actuator_lower_limit,
high=actuator_upper_limit,
dtype=np.float32)
self.action = self.action_space.sample()
obs, _, _, _ = self.step(self.action, debug=0)
low = np.full(obs.shape, -float('inf'))
high = np.full(obs.shape, float('inf'))
self.observation_space = gym.spaces.Box(low=low,
high=high,
dtype=obs.dtype)
self.obs_dict = {}
self.reward_dict = {}
# Debug
if debug:
self._debug_link_ids = []
self._debug_lines_ids = []
self._debug_links_text_init()
self._debug_links_text_remove()
self._debug_links_lines_init()
self._debug_links_lines_remove()
def main():
p.setRealTimeSimulation(1)
p.setGravity(0, 0, 0) # we don't use gravity yet
# debug colors only for show
colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1], [0, 0, 0, 1] ]
currentColor = 0
try:
body = p.getBodyUniqueId(mantis_robot)
EndEffectorIndex = p.getNumJoints(body) -1
print("EndEffectorIndex %i" % EndEffectorIndex)
tx = 0
ty = 5
tz = 3
tj = 0
tp = 0
tr = math.pi / 2
orn = p.getQuaternionFromEuler([tj,tp,tr])
jointT = p.calculateInverseKinematics(body, EndEffectorIndex, [tx, ty,tz], orn, jointDamping=jd)
p.setJointMotorControlArray(body,[1,2,3,4,5,6] , controlMode=p.POSITION_CONTROL , targetPositions=jointT )
print("x:%f y:%f z:%f j:%f p:%f r:%f " % (tx,ty,tz,tj,tp,tr))
print(jointT)
p.resetBasePositionAndOrientation(target, [tx, ty, tz ], orn)
loop = 0
hasPrevPose = 0
trailDuration = 15
targetSelected = False
distance = 5
yaw = 90
motorDir = [1,-1,1,1,1,1]
while (p.isConnected()):
time.sleep(1. / 240.) # set to 240 fps
p.stepSimulation()
# get target positon
targetPos, targetOrn = p.getBasePositionAndOrientation(target)
# do Inverse Kinematics
# jointT = p.calculateInverseKinematics(body, EndEffectorIndex, targetPos, targetOrn)
jointT = p.calculateInverseKinematics(body, EndEffectorIndex, targetPos, orn)
# set Robot Joints
p.setJointMotorControlArray(body,[1,2,3,4,5,6] , controlMode=p.POSITION_CONTROL , targetPositions=jointT )
# reduce the output speed to 1/10 of the simulation speed
# ~24 fps
loop +=1
if loop > 10:
loop = 0
# sends data to the robot over ethernet/UDP
# just a simple string with the motor number and the angle positon in 12 bit range (0-4096)
data_string = ""
for i in range ( 1, EndEffectorIndex):
jt = p.getJointState(body, i )
data_string += "motor%i:%i\r\n" % ( i, scale(motorDir[i-1] * jt[0]))
transfer_socket.sendto(bytes(data_string, "utf-8"), (transfer_ip, transfer_port))
# generates the trail for debug only
# optional
ls = p.getLinkState(body, EndEffectorIndex)
targetPos, targetOrn = p.getBasePositionAndOrientation(target)
if (hasPrevPose):
p.addUserDebugLine(prevPose, targetPos, [0, 0, 0.3], 1, trailDuration)
p.addUserDebugLine(prevPose1, ls[4], [1, 0, 0], 1, trailDuration)
prevPose = targetPos
prevPose1 = ls[4]
hasPrevPose = 1
# get mouse events
# changes color of the object clickt on and prints some info
mouseEvents = p.getMouseEvents()
for e in mouseEvents:
if ((e[0] == 2) and (e[3] == 0) and (e[4] & p.KEY_WAS_RELEASED) and (targetSelected == True) ):
# after mouse lost reset posion to prevent the target from floting around
targetSelected = False
targetPos, targetOrn = p.getBasePositionAndOrientation(target)
p.resetBasePositionAndOrientation(target, targetPos, targetOrn)
if ((e[0] == 2) and (e[3] == 0) and (e[4] & p.KEY_WAS_TRIGGERED)):
mouseX = e[1]
mouseY = e[2]
rayFrom, rayTo = getRayFromTo(mouseX, mouseY)
rayInfo = p.rayTest(rayFrom, rayTo)
for l in range(len(rayInfo)):
hit = rayInfo[l]
objectUid = hit[0]
jointUid = hit[1]
if( objectUid == target):
targetSelected = True
if (objectUid >= 1):
# this is for debug click on an object to get id
# oject will change color this has no effect
print("obj %i joint %i" % (objectUid , jointUid))
p.changeVisualShape(objectUid, jointUid, rgbaColor=colors[currentColor])
currentColor += 1
if (currentColor >= len(colors)):
currentColor = 0
except KeyboardInterrupt:
print("KeyboardInterrupt has been caught.")
finally:
endProgramm()
useMaximalCoordinates=False)
pixel_8 = p.loadURDF("pixel.urdf", [-0.36995, 0.36995, 3 + h],
useMaximalCoordinates=False)
maiskolben_1.append(pixel_1)
maiskolben_1.append(pixel_2)
maiskolben_1.append(pixel_3)
maiskolben_1.append(pixel_4)
maiskolben_1.append(pixel_5)
maiskolben_1.append(pixel_6)
maiskolben_1.append(pixel_7)
maiskolben_1.append(pixel_8)
j = 0
for pixel in maiskolben_1:
body = p.getBodyUniqueId(pixel)
while len(maiskolben_1_hit) < body:
maiskolben_1_hit.append(j)
maiskolben_1_hit.append(j)
collisionFilterGroup = 0
collisionFilterMask = 0
p.setCollisionFilterGroupMask(col, -1, collisionFilterGroup,
collisionFilterMask)
p.setCollisionFilterGroupMask(col2, -1, collisionFilterGroup,
collisionFilterMask)
collisionFilterGroup = 0
collisionFilterMask = 0
def bodies(self):
for i in range(p.getNumBodies()):
bid = p.getBodyUniqueId(i)
if bid != self.planeId and bid != self.gid and bid != self.bin:
yield bid
exclude[1,5] = 1
exclude[1,8] = 0
for j in range(len(bodies)):
for i in range(p.getNumJoints(bodies[j])):
p.setJointMotorControl2(bodies[j],i,p.VELOCITY_CONTROL, force=0)
_,name,_,_,_,_,_,_,minLimit,maxLimit,_,_,linkName = p.getJointInfo(bodies[j],i)
print "Joint named ", name, "indices:",j, "," , i
print "Link named" , linkName
print "\n"
print bodies
for x in range(0,p.getNumBodies(0)):
print "Body: (",x,")", p.getBodyInfo(p.getBodyUniqueId(x))
t = 0
while True:
#print math.sin(t*speed)/2*(maxLimit-minLimit)+minLimit
for j in range(len(bodies)):
for i in range(p.getNumJoints(bodies[j])):
_,name,_,_,_,_,_,_,minLimit,maxLimit,_,_,_ = p.getJointInfo(bodies[j],i)
if exclude[j,i] == 0:
p.setJointMotorControl2(bodies[j],i,p.POSITION_CONTROL,math.sin(t*speed)/2*(maxLimit-minLimit),p_gain)
#print p.getJointState(bodies[j],i)
p.stepSimulation()
t = t + fixedTimeStep
#for j in range(len(bodies)):
def get_body_names():
return [
p.getBodyInfo(p.getBodyUniqueId(i))[1] for i in range(p.getNumBodies())
]
def main():
p.setRealTimeSimulation(1)
p.setGravity(0, 0, 0) # we don't use gravity yet
# debug colors only for show
colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1],
[0, 0, 0, 1]]
currentColor = 0
try:
print("open SpaceMouse")
print(spacenavigator.list_devices())
success = spacenavigator.open()
body = p.getBodyUniqueId(mantis_robot)
EndEffectorIndex = p.getNumJoints(body) - 1
print("")
print("EndEffectorIndex %i" % EndEffectorIndex)
# i = 0
# for i in range(0, p.getNumJoints(body)):
# n = p.getJointInfo(body,i)
# alpha robot orientierung
# orn = p.getQuaternionFromEuler([math.pi/2,0,math.pi/2])
# p.resetBasePositionAndOrientation(body, [0, 0, 2 ], orn)
tx = 0
ty = 5
tz = 3
tj = 0
tp = 0
tr = math.pi / 2
orn = p.getQuaternionFromEuler([tj, tp, tr])
jointT = p.calculateInverseKinematics(body,
EndEffectorIndex, [tx, ty, tz],
orn,
jointDamping=jd)
p.setJointMotorControlArray(body, [1, 2, 3, 4, 5, 6],
controlMode=p.POSITION_CONTROL,
targetPositions=jointT)
print("x:%f y:%f z:%f j:%f p:%f r:%f " % (tx, ty, tz, tj, tp, tr))
print(jointT)
p.resetBasePositionAndOrientation(target, [tx, ty, tz], orn)
loop = 0
hasPrevPose = 0
trailDuration = 15
distance = 5
yaw = 90
motorDir = [1, -1, 1, 1, 1, 1]
while (p.isConnected()):
time.sleep(1. / 240.) # set to 40 fps
p.stepSimulation()
# targetVelocity = p.readUserDebugParameter(targetVelocitySlider)
if success:
state = spacenavigator.read()
# print(state)
tx += state[1] * 0.01
ty += state[2] * 0.01
tz += state[3] * 0.01
# tj += state[4]*0.005
tp += state[5] * 0.005
tr += state[6] * -0.005
# print("x:%f y:%f z:%f j:%f p:%f r:%f " % (state[1],state[2],state[3],state[4],state[5],state[6]))
# orn = p.getQuaternionFromEuler([tj,tp,tr])
orn = p.getQuaternionFromEuler([tj, tp, tr])
jointT = p.calculateInverseKinematics(body, EndEffectorIndex,
[tx, ty, tz], orn)
p.setJointMotorControlArray(body, [1, 2, 3, 4, 5, 6],
controlMode=p.POSITION_CONTROL,
targetPositions=jointT)
# print("x:%f y:%f z:%f j:%f p:%f r:%f " % (tx,ty,tz,tj,tp,tr))
p.resetBasePositionAndOrientation(target, [tx, ty, tz], orn)
targetPos, targetOrn = p.getBasePositionAndOrientation(target)
# p.resetDebugVisualizerCamera(distance, yaw, 20, targetPos)
loop += 1
if loop > 10:
loop = 0
data_string = ""
for i in range(1, EndEffectorIndex):
jt = p.getJointState(body, i)
data_string += "motor%i:%i\r\n" % (
i, scale(motorDir[i - 1] * jt[0]))
transfer_socket.sendto(bytes(data_string, "utf-8"),
(transfer_ip, transfer_port))
ls = p.getLinkState(body, EndEffectorIndex)
targetPos, targetOrn = p.getBasePositionAndOrientation(target)
if (hasPrevPose):
p.addUserDebugLine(prevPose, targetPos, [0, 0, 0.3], 1,
trailDuration)
p.addUserDebugLine(prevPose1, ls[4], [1, 0, 0], 1,
trailDuration)
prevPose = targetPos
prevPose1 = ls[4]
hasPrevPose = 1
# get mouse events
mouseEvents = p.getMouseEvents()
for e in mouseEvents:
if ((e[0] == 2) and (e[3] == 0)
and (e[4] & p.KEY_WAS_TRIGGERED)):
mouseX = e[1]
mouseY = e[2]
rayFrom, rayTo = getRayFromTo(mouseX, mouseY)
rayInfo = p.rayTest(rayFrom, rayTo)
for l in range(len(rayInfo)):
hit = rayInfo[l]
objectUid = hit[0]
jointUid = hit[1]
if (objectUid >= 1):
# this is for debug click on an object to get id
# oject will change color this has no effect
# changing color real time seems to slow
print("obj %i joint %i" % (objectUid, jointUid))
# n = p.getJointInfo(objectUid,jointUid)
n = p.getJointState(objectUid, jointUid)
print(n)
p.changeVisualShape(objectUid,
jointUid,
rgbaColor=colors[currentColor])
currentColor += 1
if (currentColor >= len(colors)):
currentColor = 0
except KeyboardInterrupt:
print("KeyboardInterrupt has been caught.")
finally:
endProgramm()
def get_body_from_pb_id(i):
return p.getBodyUniqueId(i, physicsClientId=CLIENT)
def get_bodies():
return [
p.getBodyUniqueId(i, physicsClientId=CLIENT)
for i in range(p.getNumBodies(physicsClientId=CLIENT))
]
