def getExtendedObservation(self):
self._observation = self._kuka.getObservation()
gripperState = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaGripperIndex)
gripperPos = gripperState[0]
gripperOrn = gripperState[1]
blockPos,blockOrn = p.getBasePositionAndOrientation(self.blockUid)
invGripperPos,invGripperOrn = p.invertTransform(gripperPos,gripperOrn)
gripperMat = p.getMatrixFromQuaternion(gripperOrn)
dir0 = [gripperMat[0],gripperMat[3],gripperMat[6]]
dir1 = [gripperMat[1],gripperMat[4],gripperMat[7]]
dir2 = [gripperMat[2],gripperMat[5],gripperMat[8]]
gripperEul = p.getEulerFromQuaternion(gripperOrn)
#print("gripperEul")
#print(gripperEul)
blockPosInGripper,blockOrnInGripper = p.multiplyTransforms(invGripperPos,invGripperOrn,blockPos,blockOrn)
projectedBlockPos2D =[blockPosInGripper[0],blockPosInGripper[1]]
blockEulerInGripper = p.getEulerFromQuaternion(blockOrnInGripper)
#print("projectedBlockPos2D")
#print(projectedBlockPos2D)
#print("blockEulerInGripper")
#print(blockEulerInGripper)
#we return the relative x,y position and euler angle of block in gripper space
blockInGripperPosXYEulZ =[blockPosInGripper[0],blockPosInGripper[1],blockEulerInGripper[2]]
#p.addUserDebugLine(gripperPos,[gripperPos[0]+dir0[0],gripperPos[1]+dir0[1],gripperPos[2]+dir0[2]],[1,0,0],lifeTime=1)
#p.addUserDebugLine(gripperPos,[gripperPos[0]+dir1[0],gripperPos[1]+dir1[1],gripperPos[2]+dir1[2]],[0,1,0],lifeTime=1)
#p.addUserDebugLine(gripperPos,[gripperPos[0]+dir2[0],gripperPos[1]+dir2[1],gripperPos[2]+dir2[2]],[0,0,1],lifeTime=1)
self._observation.extend(list(blockInGripperPosXYEulZ))
return self._observation
def _termination(self):
#print (self._kuka.endEffectorPos[2])
state = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex)
actualEndEffectorPos = state[0]
#print("self._envStepCounter")
#print(self._envStepCounter)
if (self.terminated or self._envStepCounter>1000):
self._observation = self.getExtendedObservation()
return True
if (actualEndEffectorPos[2] <= 0.10):
self.terminated = 1
#print("closing gripper, attempting grasp")
#start grasp and terminate
fingerAngle = 0.3
for i in range (1000):
graspAction = [0,0,0.001,0,fingerAngle]
self._kuka.applyAction(graspAction)
p.stepSimulation()
fingerAngle = fingerAngle-(0.3/100.)
if (fingerAngle<0):
fingerAngle=0
self._observation = self.getExtendedObservation()
return True
return False
def _termination(self):
#print (self._kuka.endEffectorPos[2])
state = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex)
actualEndEffectorPos = state[0]
#print("self._envStepCounter")
#print(self._envStepCounter)
if (self.terminated or self._envStepCounter>self._maxSteps):
self._observation = self.getExtendedObservation()
return True
maxDist = 0.005
closestPoints = p.getClosestPoints(self._kuka.trayUid, self._kuka.kukaUid,maxDist)
if (len(closestPoints)):#(actualEndEffectorPos[2] <= -0.43):
self.terminated = 1
#print("terminating, closing gripper, attempting grasp")
#start grasp and terminate
fingerAngle = 0.3
for i in range (100):
graspAction = [0,0,0.0001,0,fingerAngle]
self._kuka.applyAction(graspAction)
p.stepSimulation()
fingerAngle = fingerAngle-(0.3/100.)
if (fingerAngle<0):
fingerAngle=0
for i in range (1000):
graspAction = [0,0,0.001,0,fingerAngle]
self._kuka.applyAction(graspAction)
p.stepSimulation()
blockPos,blockOrn=p.getBasePositionAndOrientation(self.blockUid)
if (blockPos[2] > 0.23):
#print("BLOCKPOS!")
#print(blockPos[2])
break
state = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex)
actualEndEffectorPos = state[0]
if (actualEndEffectorPos[2]>0.5):
break
self._observation = self.getExtendedObservation()
return True
return False
def _step_continuous(self, action):
"""Applies a continuous velocity-control action.
Args:
action: 5-vector parameterizing XYZ offset, vertical angle offset
(radians), and grasp angle (radians).
Returns:
observation: Next observation.
reward: Float of the per-step reward as a result of taking the action.
done: Bool of whether or not the episode has ended.
debug: Dictionary of extra information provided by environment.
"""
# Perform commanded action.
self._env_step += 1
self._kuka.applyAction(action)
for _ in range(self._actionRepeat):
p.stepSimulation()
if self._renders:
time.sleep(self._timeStep)
if self._termination():
break
# If we are close to the bin, attempt grasp.
state = p.getLinkState(self._kuka.kukaUid,
self._kuka.kukaEndEffectorIndex)
end_effector_pos = state[0]
if end_effector_pos[2] <= 0.1:
finger_angle = 0.3
for _ in range(500):
grasp_action = [0, 0, 0, 0, finger_angle]
self._kuka.applyAction(grasp_action)
p.stepSimulation()
#if self._renders:
# time.sleep(self._timeStep)
finger_angle -= 0.3/100.
if finger_angle < 0:
finger_angle = 0
for _ in range(500):
grasp_action = [0, 0, 0.001, 0, finger_angle]
self._kuka.applyAction(grasp_action)
p.stepSimulation()
if self._renders:
time.sleep(self._timeStep)
finger_angle -= 0.3/100.
if finger_angle < 0:
finger_angle = 0
self._attempted_grasp = True
observation = self._get_observation()
done = self._termination()
reward = self._reward()
debug = {
'grasp_success': self._graspSuccess
}
return observation, reward, done, debug
def getObservation(self):
observation = []
state = p.getLinkState(self.kukaUid,self.kukaGripperIndex)
pos = state[0]
orn = state[1]
euler = p.getEulerFromQuaternion(orn)
observation.extend(list(pos))
observation.extend(list(euler))
return observation
def getExtendedObservation(self):
self._observation = self._kuka.getObservation()
eeState = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex)
endEffectorPos = eeState[0]
endEffectorOrn = eeState[1]
blockPos,blockOrn = p.getBasePositionAndOrientation(self.blockUid)
invEEPos,invEEOrn = p.invertTransform(endEffectorPos,endEffectorOrn)
blockPosInEE,blockOrnInEE = p.multiplyTransforms(invEEPos,invEEOrn,blockPos,blockOrn)
blockEulerInEE = p.getEulerFromQuaternion(blockOrnInEE)
self._observation.extend(list(blockPosInEE))
self._observation.extend(list(blockEulerInEE))
return self._observation
def accurateCalculateInverseKinematics( kukaId, endEffectorId, targetPos, threshold, maxIter):
closeEnough = False
iter = 0
dist2 = 1e30
while (not closeEnough and iter<maxIter):
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,targetPos)
for i in range (numJoints):
p.resetJointState(kukaId,i,jointPoses[i])
ls = p.getLinkState(kukaId,kukaEndEffectorIndex)
newPos = ls[4]
diff = [targetPos[0]-newPos[0],targetPos[1]-newPos[1],targetPos[2]-newPos[2]]
dist2 = (diff[0]*diff[0] + diff[1]*diff[1] + diff[2]*diff[2])
closeEnough = (dist2 < threshold)
iter=iter+1
#print ("Num iter: "+str(iter) + "threshold: "+str(dist2))
return jointPoses
def testJacobian(self):
import pybullet as p
clid = p.connect(p.SHARED_MEMORY)
if (clid < 0):
p.connect(p.DIRECT)
time_step = 0.001
gravity_constant = -9.81
urdfs = [
"TwoJointRobot_w_fixedJoints.urdf", "TwoJointRobot_w_fixedJoints.urdf",
"kuka_iiwa/model.urdf", "kuka_lwr/kuka.urdf"
]
for urdf in urdfs:
p.resetSimulation()
p.setTimeStep(time_step)
p.setGravity(0.0, 0.0, gravity_constant)
robotId = p.loadURDF(urdf, useFixedBase=True)
p.resetBasePositionAndOrientation(robotId, [0, 0, 0], [0, 0, 0, 1])
numJoints = p.getNumJoints(robotId)
endEffectorIndex = numJoints - 1
# Set a joint target for the position control and step the sim.
self.setJointPosition(robotId, [0.1 * (i % 3) for i in range(numJoints)])
p.stepSimulation()
# Get the joint and link state directly from Bullet.
mpos, mvel, mtorq = self.getMotorJointStates(robotId)
result = p.getLinkState(robotId,
endEffectorIndex,
computeLinkVelocity=1,
computeForwardKinematics=1)
link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = result
# Get the Jacobians for the CoM of the end-effector link.
# Note that in this example com_rot = identity, and we would need to use com_rot.T * com_trn.
# The localPosition is always defined in terms of the link frame coordinates.
zero_vec = [0.0] * len(mpos)
jac_t, jac_r = p.calculateJacobian(robotId, endEffectorIndex, com_trn, mpos, zero_vec,
zero_vec)
assert (allclose(dot(jac_t, mvel), link_vt))
assert (allclose(dot(jac_r, mvel), link_vr))
p.disconnect()
def _get_simple_observation(self):
"""Observations for simplified observation space.
Returns:
Numpy array containing location and orientation of nearest block and
location of end-effector.
"""
state = pybullet.getLinkState(self._kuka.kukaUid,
self._kuka.kukaEndEffectorIndex,
physicsClientId=self.cid)
end_effector_pos = np.array(state[0])
end_effector_ori = np.array(state[1])
distances = []
pos_and_ori = []
for uid in self._block_uids:
pos, ori = pybullet.getBasePositionAndOrientation(
uid, physicsClientId=self.cid)
pos, ori = np.array(pos), np.array(ori)
pos_and_ori.append((pos, ori))
distances.append(np.linalg.norm(end_effector_pos - pos))
pos, ori = pos_and_ori[np.argmin(distances)]
return np.concatenate((pos, ori, end_effector_pos, end_effector_ori))
def _update_observation(self):
"""
Update the observation from BulletPhysics
Observation contains:
* 3x servomotor {cos(position), sin(position), speed}
* position - target (x, y, z)
* target (x, y, z)
"""
# Each servomotor position, speed and torque
all_states = p.getJointStates(self.robot_id, self.joint_list)
for i, (pos, vel, _, tor) in enumerate(all_states):
self.torques[i] = tor / self.servo_max_torque
self.observation[3*i:3*i+3] = [
np.cos(pos),
np.sin(pos),
np.clip(vel / self.servo_max_speed, -1., 1.),
]
# Endcap position and orientation
endcap_id = 5
position, _, _, _, _, _ = p.getLinkState(self.robot_id, endcap_id)
self.observation[-6:-3] = position - self.target_position
def apply_A_kuka(motorCommands):
kuka_ee_link_Index = self.get_kuka_ee_linkIndex()
motorCommands = motorCommands[
16:] # the first three command will be xyz and the second three command will be rpy
kuka_ee_index = self.get_kuka_ee_linkIndex()
kuka_ee_state = p.getLinkState(
self.kuka_handId, kuka_ee_link_Index
) #mamad:returns endeffector info position orientation
pos_state = kuka_ee_state[0] #mamad: linkWorldPosition
orn_state = kuka_ee_state[1] #mamad: linkWorldOrientation
pos_command = motorCommands[0:3]
orn_command = motorCommands[3:]
new_pos = pos_command
new_orn = new_pos
#getting joint values using inverse kinematic
jointPoses = p.calculateInverseKinematics(self.kuka_handId,
kuka_ee_index, new_pos,
new_orn)
kuka_activeJ = self.get_kuka_Active_joints()
#Applying new_joint_pos to kuka
counter = 0
return jointPoses
def _calculate_ik(self, targetPos, targetQuat, threshold=1e-5, maxIter=1000, nJoints=6):
closeEnough = False
iter_count = 0
dist2 = None
best_ret, best_dist = None, float('inf')
while (not closeEnough and iter_count < maxIter):
jointPoses = list(p.calculateInverseKinematics(self._armID, 5, targetPos, targetQuat, JOINT_MIN, JOINT_MAX))
for i in range(nJoints):
jointPoses[i] = max(min(jointPoses[i], JOINT_MAX[i]), JOINT_MIN[i])
p.resetJointState(self._armID, i, jointPoses[i])
ls = p.getLinkState(self._armID, 5, computeForwardKinematics=1)
newPos, newQuat = ls[4], ls[5]
dist2 = sum([(targetPos[i] - newPos[i]) ** 2 for i in range(3)])
closeEnough = dist2 < threshold
iter_count += 1
if dist2 < best_dist:
best_ret, best_dist = (jointPoses, newPos, newQuat), dist2
return best_ret
def get_link_state(self, link=None):
"""Returns the state of the named link.
If None is passed as link, the object's pose is returned as LinkState
:type link: str, NoneType
:rtype: LinkState
"""
if link is not None and link not in self.link_index_map:
raise Exception('Link "{}" is not defined'.format(link))
zero_vector = Vector3(0, 0, 0)
if link is None or link == self.base_link:
frame = self.pose()
return LinkState(frame, frame, frame, zero_vector, zero_vector)
else:
ls = pb.getLinkState(self.__bulletId,
self.link_index_map[link],
0,
physicsClientId=self.__client_id)
return LinkState(Frame(Vector3(*ls[0]), Quaternion(*ls[1])),
Frame(Vector3(*ls[2]), Quaternion(*ls[3])),
Frame(Vector3(*ls[4]), Quaternion(*ls[5])),
zero_vector, zero_vector)
def retract(self, record=False):
"""Retract step."""
cur_joint = np.array(self._panda.getJointStates()[0])
cur_joint[-2:] = 0
self.step(cur_joint.tolist()) # grasp
pos, orn = p.getLinkState(self._panda.pandaUid,
self._panda.pandaEndEffectorIndex)[:2]
observations = []
for i in range(10):
pos = (pos[0], pos[1], pos[2] + 0.03)
jointPoses = np.array(
p.calculateInverseKinematics(self._panda.pandaUid,
self._panda.pandaEndEffectorIndex,
pos))
jointPoses[-2:] = 0.0
self.step(jointPoses.tolist())
observation = self._get_observation()
if record:
observations.append(observation)
return (self._reward(), observations) if record else self._reward()
def getExtendedObservation(self):
self._observation = self._kuka.getObservation()
gripperState = p.getLinkState(self._kuka.kukaUid,
self._kuka.kukaGripperIndex)
gripperPos = gripperState[0]
gripperOrn = gripperState[1]
blockPos, blockOrn = p.getBasePositionAndOrientation(self.blockUid)
invGripperPos, invGripperOrn = p.invertTransform(
gripperPos, gripperOrn)
gripperMat = p.getMatrixFromQuaternion(gripperOrn)
dir0 = [gripperMat[0], gripperMat[3], gripperMat[6]]
dir1 = [gripperMat[1], gripperMat[4], gripperMat[7]]
dir2 = [gripperMat[2], gripperMat[5], gripperMat[8]]
gripperEul = p.getEulerFromQuaternion(gripperOrn)
# print("gripperEul")
# print(gripperEul)
blockPosInGripper, blockOrnInGripper = p.multiplyTransforms(
invGripperPos, invGripperOrn, blockPos, blockOrn)
projectedBlockPos2D = [blockPosInGripper[0], blockPosInGripper[1]]
blockEulerInGripper = p.getEulerFromQuaternion(blockOrnInGripper)
# print("projectedBlockPos2D")
# print(projectedBlockPos2D)
# print("blockEulerInGripper")
# print(blockEulerInGripper)
# we return the relative x,y position and euler angle of block in gripper space
blockInGripperPosXYEulZ = [
blockPosInGripper[0], blockPosInGripper[1], blockEulerInGripper[2]
]
# p.addUserDebugLine(gripperPos,[gripperPos[0]+dir0[0],gripperPos[1]+dir0[1],gripperPos[2]+dir0[2]],[1,0,0],lifeTime=1)
# p.addUserDebugLine(gripperPos,[gripperPos[0]+dir1[0],gripperPos[1]+dir1[1],gripperPos[2]+dir1[2]],[0,1,0],lifeTime=1)
# p.addUserDebugLine(gripperPos,[gripperPos[0]+dir2[0],gripperPos[1]+dir2[1],gripperPos[2]+dir2[2]],[0,0,1],lifeTime=1)
self._observation.extend(list(blockInGripperPosXYEulZ))
return self._observation
def ik_human(self,
body,
target_joint,
target_pos,
target_orient,
max_iterations=50,
max_ik_human=10,
distance_thres=0.03,
quaternion_thres=0.1,
half_range=False):
best_ik_joints = None
best_ik_distance = 0
best_num = 0
num = 0
for iter in range(max_ik_human):
num += 1
target_joint_positions = self.ik(body,
target_joint,
target_pos,
target_orient,
ik_indices=list(range(7)),
max_iterations=max_iterations,
half_range=half_range)
for i in range(len(target_joint_positions)):
p.resetJointState(body, i, target_joint_positions[i])
new_pos, new_orient = p.getLinkState(
body, target_joint, computeForwardKinematics=True)[:2]
dist = np.linalg.norm(np.array(target_pos) - np.array(new_pos))
dist_q = np.linalg.norm(
np.array(target_orient) - np.array(new_orient))
if dist < distance_thres and dist_q < quaternion_thres:
return target_joint_positions
if best_ik_joints is None or dist < best_ik_distance:
best_ik_joints = target_joint_positions
best_ik_distance = dist
best_num = num
return best_ik_joints
def applyAction(self, motorCommand):
jointPoses = motorCommand
if not self.joint_control:
state = p.getLinkState(self.kukaUid, self.kukaEndEffectorIndex)
pos = np.append(motorCommand, 0.3)
if (pos[0] > 1):
pos[0] = 1
if (pos[0] < 0):
pos[0] = 0
if (pos[1] < -.45):
pos[1] = -.45
if (pos[1] > 0.45):
pos[1] = 0.45
orn = p.getQuaternionFromEuler([0, -math.pi, 0]) # -math.pi,yaw])
jointPoses = p.calculateInverseKinematics(
self.kukaUid,
self.kukaEndEffectorIndex,
pos,
orn,
jointDamping=self.jd)
for i in range(len(jointPoses)):
# print(i)
p.setJointMotorControl2(
bodyUniqueId=self.kukaUid,
jointIndex=i,
controlMode=p.POSITION_CONTROL,
targetPosition=jointPoses[i],
force=self.maxForce,
maxVelocity=self.maxVelocity * 3,
# positionGain=0.3,
# velocityGain=0.1
)
def step(self, action, custom_reward=None):
p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING)
p.setJointMotorControlArray(self.dancerUid, [self.joint2Index[joint] for joint in self.joint_names], p.POSITION_CONTROL, action)
p.stepSimulation()
# get states
jointStates = {}
for joint in self.joint_names:
jointStates[joint] = p.getJointState(self.dancerUid, self.joint2Index[joint])
linkStates = {}
for link in self.link_names:
linkStates[link] = p.getLinkState(self.dancerUid, self.link2Index[link])
# recover color
for index, color in self.linkColor.items():
p.changeVisualShape(self.dancerUid, index, rgbaColor=color)
# check collision
collision = False
for link in self.link_names:
if len(p.getContactPoints(bodyA=self.dancerUid, linkIndexA=self.link2Index[link])) > 0:
collision = True
for contact in p.getContactPoints(bodyA=self.dancerUid, bodyB=self.dancerUid, linkIndexA=self.link2Index[link]):
print("Collision Occurred in Link {} & Link {}!!!".format(contact[3], contact[4]))
p.changeVisualShape(self.dancerUid, contact[3], rgbaColor=[1,0,0,1])
p.changeVisualShape(self.dancerUid, contact[4], rgbaColor=[1,0,0,1])
self.step_counter += 1
if custom_reward is None:
# default reward
reward = 0
done = False
else:
# custom reward
reward, done = custom_reward(jointStates=jointStates, linkStates=linkStates, collision=collision, step_counter=self.step_counter)
info = {'collision': collision}
observation = [jointStates[joint][0] for joint in self.joint_names]
return observation, reward, done, info
def step(self, action):
dv = 0.005
dx = action[0] * dv
dy = action[1] * dv
dz = action[2] * dv
self.current_pos = p.getLinkState(self.kuka_id, self.num_joints - 1)[4]
# logging.debug("self.current_pos={}\n".format(self.current_pos))
self.new_robot_pos = [
self.current_pos[0] + dx, self.current_pos[1] + dy,
self.current_pos[2] + dz
]
#logging.debug("self.new_robot_pos={}\n".format(self.new_robot_pos))
self.robot_joint_positions = p.calculateInverseKinematics(
bodyUniqueId=self.kuka_id,
endEffectorLinkIndex=self.num_joints - 1,
targetPosition=[
self.new_robot_pos[0], self.new_robot_pos[1],
self.new_robot_pos[2]
],
targetOrientation=self.orientation,
jointDamping=self.joint_damping,
)
for i in range(self.num_joints):
p.resetJointState(
bodyUniqueId=self.kuka_id,
jointIndex=i,
targetValue=self.robot_joint_positions[i],
)
p.stepSimulation()
#在代码开始部分，如果定义了is_good_view，那么机械臂的动作会变慢，方便观察
if self.is_good_view:
time.sleep(0.05)
self.step_counter += 1
return self._reward()
def getCameraImage(self):
"""Return camera image from CAMERA_JOINT_INDEX.
"""
# compute eye and target position for viewMatrix
pos, orn, _, _, _, _ = p.getLinkState(self.robotId,
self.CAMERA_JOINT_INDEX)
cameraPos = np.array(pos)
cameraMat = np.array(p.getMatrixFromQuaternion(orn)).reshape(3, 3).T
eyePos = cameraPos + self.CAMERA_EYE_SCALE * cameraMat[
self.CAMERA_EYE_INDEX]
targetPos = cameraPos + self.CAMERA_TARGET_SCALE * cameraMat[
self.CAMERA_EYE_INDEX]
up = self.CAMERA_UP_SCALE * cameraMat[self.CAMERA_UP_INDEX]
# p.addUserDebugLine(eyePos, targetPos, lineColorRGB=[1, 0, 0], lifeTime=0.1) # red line for camera vector
# p.addUserDebugLine(eyePos, eyePos + up * 0.5, lineColorRGB=[0, 0, 1], lifeTime=0.1) # blue line for up vector
viewMatrix = p.computeViewMatrix(eyePos, targetPos, up)
image = p.getCameraImage(self.CAMERA_PIXEL_WIDTH,
self.CAMERA_PIXEL_HEIGHT,
viewMatrix,
self.projectionMatrix,
shadow=1,
lightDirection=[1, 1, 1],
renderer=p.ER_BULLET_HARDWARE_OPENGL)
return image
def update_position(instance):
if isinstance(instance, Instance):
pos, orn = p.getBasePositionAndOrientation(instance.pybullet_uuid)
instance.set_position(pos)
instance.set_rotation([orn[-1], orn[0], orn[1], orn[2]])
elif isinstance(instance, InstanceGroup):
poses_rot = []
poses_trans = []
for link_id in instance.link_ids:
if link_id == -1:
pos, orn = p.getBasePositionAndOrientation(
instance.pybullet_uuid)
else:
_, _, _, _, pos, orn = p.getLinkState(
instance.pybullet_uuid, link_id)
poses_rot.append(
np.ascontiguousarray(
quat2rotmat([orn[-1], orn[0], orn[1], orn[2]])))
poses_trans.append(np.ascontiguousarray(xyz2mat(pos)))
#print(instance.pybullet_uuid, link_id, pos, orn)
instance.poses_rot = poses_rot
instance.poses_trans = poses_trans
def get_imu_raw(self, verbose=False):
"""
Simulates the IMU at the IMU link location.
TODO: Add noise model, make the refresh rate vary (currently in sync with the PyBullet time steps)
:param verbose: Optional - Set to True to print the linear acceleration and angular velocity
:return: concatenated 3-axes values for linear acceleration and angular velocity
"""
[quart_link, lin_vel, ang_vel] = pb.getLinkState(self.body, linkIndex=Links.IMU,
computeLinkVelocity=1)[5:8]
# [lin_vel, ang_vel] = p.getLinkState(bodyUniqueId=self.soccerbotUid, linkIndex=Links.HEAD_1, computeLinkVelocity=1)[6:8]
# print(p.getLinkStates(bodyUniqueId=self.soccerbotUid, linkIndices=range(0,20,1), computeLinkVelocity=1))
# p.getBaseVelocity(self.soccerbotUid)
lin_vel = np.array(lin_vel, dtype=np.float32)
self.gravity = [0, 0, -9.81]
lin_acc = (lin_vel - self.prev_lin_vel) / self.time_step_sim
lin_acc -= self.gravity
rot_mat = np.array(pb.getMatrixFromQuaternion(quart_link), dtype=np.float32).reshape((3, 3))
lin_acc = np.matmul(rot_mat, lin_acc)
ang_vel = np.array(ang_vel, dtype=np.float32)
self.prev_lin_vel = lin_vel
if verbose:
print(f'lin_acc = {lin_acc}', end="\t\t")
print(f'ang_vel = {ang_vel}')
return np.concatenate((lin_acc, ang_vel))
def _get_obs(self):
state = []
base_link_info = p.getLinkState(self.walker_id, 3, computeLinkVelocity=1, computeForwardKinematics=1)
base_pos = base_link_info[0]
base_orn = p.getEulerFromQuaternion(base_link_info[1])
state.append(base_pos[2]) # Z
state.append(base_orn[1]) # Pitch
for s in p.getJointStates(self.walker_id, self.motor_joints):
state.append(s[0])
base_linvel = base_link_info[6]
base_angvel = base_link_info[7]
state.append(np.clip(base_linvel[1], -10, 10)) # Y
state.append(np.clip(base_linvel[2], -10, 10)) # Z
state.append(np.clip(base_angvel[1], -10, 10)) # Pitch
for s in p.getJointStates(self.walker_id, self.motor_joints):
state.append(np.clip(s[1], -10, 10))
return np.array(state)
def timer_callback(_obj, _event):
global apply_force, fpss
cnt = next(counter)
showm.render()
if cnt % 1 == 0:
fps = showm.frame_rate
fpss = np.append(fpss, fps)
tb.message = "Avg. FPS: " + str(np.round(np.mean(fpss), 0)) + \
"\nSim Steps: " + str(cnt)
# Updating the position and orientation of each individual brick.
for idx, brick in enumerate(bricks):
sync_brick(idx, brick)
pos, _ = p.getBasePositionAndOrientation(rope)
if apply_force:
p.applyExternalForce(rope,
-1,
forceObj=[-500, 0, 0],
posObj=pos,
flags=p.WORLD_FRAME)
apply_force = False
pos = p.getLinkState(rope, p.getNumJoints(rope) - 1)[4]
ball_actor.SetPosition(*pos)
sync_chain(rope_actor, rope)
utils.update_actor(brick_actor)
utils.update_actor(rope_actor)
# Simulate a step.
p.stepSimulation()
if cnt == 130:
showm.exit()
def step(self, ctrl):
p.setJointMotorControl2(self.cartpole,
0,
p.TORQUE_CONTROL,
force=ctrl * 100)
p.stepSimulation()
self.step_ctr += 1
pendulum_height = p.getLinkState(self.cartpole, 1)[0][2]
# x, x_dot, theta, theta_dot
obs = self.get_obs()
x, x_dot, theta_1, theta_dot_1 = obs
height_rew = pendulum_height
ctrl_pen = np.square(ctrl[0]) * 0.01
r = height_rew - np.square(x) * 0.2 - ctrl_pen
done = self.step_ctr > self.max_steps
#p.removeAllUserDebugItems()
#p.addUserDebugText("Pendulum height: % 3.3f, -abs(x) % 3.3f, the %3.3f, done: %s" % (pendulum_height, - abs(x) * 0.2, theta_1, done), [-1, 0, 2])
return np.array(obs), r, done, {}
def apply_A_kuka(motorCommands):
kuka_ee_link_Index = self.get_kuka_ee_linkIndex()
motorCommands = motorCommands[
16:] # the first three command will be xyz and the second three command will be rpy
kuka_ee_index = self.get_kuka_ee_linkIndex()
kuka_ee_state = p.getLinkState(
self.kuka_handId, kuka_ee_link_Index
) #mamad:returns endeffector info position orientation
pos_state = kuka_ee_state[0] #mamad: linkWorldPosition
orn_state = kuka_ee_state[1] #mamad: linkWorldOrientation
pos_command = motorCommands[0:3]
orn_command = motorCommands[3:]
new_pos = pos_command
new_orn = p.getQuaternionFromEuler(orn_command)
#getting joint values using inverse kinematic
jointPoses = p.calculateInverseKinematics(self.kuka_handId,
kuka_ee_index, new_pos,
new_orn)
jointPoses = (-0.36350324105018117, 0.08434877972795868,
1.3253607910308352, -1.3023695529206833,
1.1981002550715345, -2.4402425653234032,
-0.3733762283635729, 0.011011669082923765,
0.01099649407428263, 0.010663992122582346,
0.01090501619723987, -0.405810978009454,
0.7964372388949594, 2.128544355877948e-06,
8.43294195425921e-08, 4.4055107276974525e-07,
0.4617504554232501, 1.3463347345016152e-07,
2.0510552526533784e-06, -1.1743303952431874,
-0.21315269585784075, -0.6981321138974356,
5.54231275569411e-06)
kuka_activeJ = self.get_kuka_Active_joints()
#Applying new_joint_pos to kuka
counter = 0
return list(jointPoses[0:8])
def get_state_robot_effector(self):
return p.getLinkState(self.robot_id, self.end_eff_idx)[0]
p.setTimeStep(time_step)
p.setGravity(0.0, 0.0, gravity_constant)
p.loadURDF("plane.urdf", [0, 0, -0.3])
kukaId = p.loadURDF("kuka_iiwa/model.urdf", [0, 0, 0])
p.resetBasePositionAndOrientation(kukaId, [0, 0, 0], [0, 0, 0, 1])
kukaEndEffectorIndex = 6
numJoints = p.getNumJoints(kukaId)
if (numJoints != 7):
exit()
# Set a joint target for the position control and step the sim.
setJointPosition(kukaId, [0.1] * p.getNumJoints(kukaId))
p.stepSimulation()
# Get the joint and link state directly from Bullet.
pos, vel, torq = getJointStates(kukaId)
result = p.getLinkState(kukaId,
kukaEndEffectorIndex,
computeLinkVelocity=1,
computeForwardKinematics=1)
link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = result
# Get the Jacobians for the CoM of the end-effector link.
# Note that in this example com_rot = identity, and we would need to use com_rot.T * com_trn.
# The localPosition is always defined in terms of the link frame coordinates.
zero_vec = [0.0] * numJoints
jac_t, jac_r = p.calculateJacobian(kukaId, kukaEndEffectorIndex, com_trn, pos,
zero_vec, zero_vec)
print("Link linear velocity of CoM from getLinkState:")
print(link_vt)
print("Link linear velocity of CoM from linearJacobian * q_dot:")
print(multiplyJacobian(jac_t, vel))
print("Link angular velocity of CoM from getLinkState:")
print(link_vr)
lineId2 = p.addUserDebugLine([0,0,0],[0,0,1],[1,0,0])
lineId3= p.addUserDebugLine([0,0,0],[0,0,1],[1,0,0])
print("lineId=",lineId)
camInfo = p.getDebugVisualizerCamera()
lastTime = time.time()
lastControlTime = time.time()
lastLidarTime = time.time()
frame=0
while (True):
nowTime = time.time()
#render Camera at 10Hertz
if (nowTime-lastTime>.1):
ls = p.getLinkState(car,zed_camera_joint, computeForwardKinematics=True)
camPos = ls[0]
camOrn = ls[1]
camMat = p.getMatrixFromQuaternion(camOrn)
upVector = [0,0,1]
forwardVec = [camMat[0],camMat[3],camMat[6]]
#sideVec = [camMat[1],camMat[4],camMat[7]]
camUpVec = [camMat[2],camMat[5],camMat[8]]
camTarget = [camPos[0]+forwardVec[0]*10,camPos[1]+forwardVec[1]*10,camPos[2]+forwardVec[2]*10]
camUpTarget = [camPos[0]+camUpVec[0],camPos[1]+camUpVec[1],camPos[2]+camUpVec[2]]
viewMat = p.computeViewMatrix(camPos, camTarget, camUpVec)
projMat = camInfo[3]
#p.getCameraImage(320,200,viewMatrix=viewMat,projectionMatrix=projMat, flags=p.ER_NO_SEGMENTATION_MASK, renderer=p.ER_BULLET_HARDWARE_OPENGL)
p.getCameraImage(320,200,viewMatrix=viewMat,projectionMatrix=projMat, renderer=p.ER_BULLET_HARDWARE_OPENGL)
lastTime=nowTime
def import_articulated_object(self, obj, class_id=None):
"""
Import an articulated object into the simulator
:param obj: Object to load
:param class_id: Class id for rendering semantic segmentation
:return: pybulet id
"""
if class_id is None:
class_id = self.next_class_id
self.next_class_id += 1
ids = obj.load()
visual_objects = []
link_ids = []
poses_rot = []
poses_trans = []
for shape in p.getVisualShapeData(ids):
id, link_id, type, dimensions, filename, rel_pos, rel_orn, color = shape[:
8]
if type == p.GEOM_MESH:
filename = filename.decode('utf-8')
if filename not in self.visual_objects.keys():
self.renderer.load_object(filename,
transform_orn=rel_orn,
transform_pos=rel_pos,
input_kd=color[:3],
scale=np.array(dimensions))
self.visual_objects[filename] = len(
self.renderer.visual_objects) - 1
visual_objects.append(self.visual_objects[filename])
link_ids.append(link_id)
elif type == p.GEOM_SPHERE:
filename = os.path.join(gibson2.assets_path,
'models/mjcf_primitives/sphere8.obj')
self.renderer.load_object(filename,
transform_orn=rel_orn,
transform_pos=rel_pos,
input_kd=color[:3],
scale=[
dimensions[0] / 0.5,
dimensions[0] / 0.5,
dimensions[0] / 0.5
])
visual_objects.append(len(self.renderer.visual_objects) - 1)
link_ids.append(link_id)
elif type == p.GEOM_CAPSULE or type == p.GEOM_CYLINDER:
filename = os.path.join(gibson2.assets_path,
'models/mjcf_primitives/cube.obj')
self.renderer.load_object(filename,
transform_orn=rel_orn,
transform_pos=rel_pos,
input_kd=color[:3],
scale=[
dimensions[1] / 0.5,
dimensions[1] / 0.5,
dimensions[0]
])
visual_objects.append(len(self.renderer.visual_objects) - 1)
link_ids.append(link_id)
elif type == p.GEOM_BOX:
filename = os.path.join(gibson2.assets_path,
'models/mjcf_primitives/cube.obj')
self.renderer.load_object(filename,
transform_orn=rel_orn,
transform_pos=rel_pos,
input_kd=color[:3],
scale=np.array(dimensions))
visual_objects.append(len(self.renderer.visual_objects) - 1)
link_ids.append(link_id)
if link_id == -1:
pos, orn = p.getBasePositionAndOrientation(id)
else:
_, _, _, _, pos, orn = p.getLinkState(id, link_id)
poses_rot.append(np.ascontiguousarray(quat2rotmat(xyzw2wxyz(orn))))
poses_trans.append(np.ascontiguousarray(xyz2mat(pos)))
self.renderer.add_instance_group(object_ids=visual_objects,
link_ids=link_ids,
pybullet_uuid=ids,
class_id=class_id,
poses_rot=poses_rot,
poses_trans=poses_trans,
dynamic=True,
robot=None)
return ids
def init_grasp(self):
try:
p.removeBody(self.box_id)
except:
pass
pos_traj = np.load(os.path.join(self.env_root, 'init', 'pos.npy'))
orn_traj = np.load(os.path.join(self.env_root, 'init', 'orn.npy'))
self.fix_orn = np.load(os.path.join(self.env_root, 'init', 'orn.npy'))
for j in range(7):
self.p.resetJointState(self.robotId, j, self.data_q[0][j],
self.data_dq[0][j])
self.robot.gripperControl(0)
for init_t in range(100):
box = p.getAABB(self.obj_id, -1)
center = [(x + y) * 0.5 for x, y in zip(box[0], box[1])]
center[0] -= 0.05
center[1] -= 0.05
center[2] += 0.03
# center = (box[0]+box[1])*0.5
points = np.array([pos_traj[0], center])
start_id = 0
init_traj = point2traj(points)
start_id = self.move(init_traj, orn_traj, start_id)
# grasping
grasp_stage_num = 10
for grasp_t in range(grasp_stage_num):
gripperPos = grasp_t / float(grasp_stage_num) * 180.0
self.robot.gripperControl(gripperPos)
start_id += 1
pos = p.getLinkState(self.robotId, 7)[0]
up_traj = point2traj([pos, [pos[0], pos[1] + 0.05, pos[2] + 0.3]])
start_id = self.move(up_traj, orn_traj, start_id)
if self.opt.rand_start == 'rand':
# move in z-axis direction
pos = p.getLinkState(self.robotId, 7)[0]
up_traj = point2traj([
pos, [pos[0], pos[1], pos[2] + (random.random() - 0.5) * 0.1]
])
start_id = self.move(up_traj, orn_traj, start_id)
# move in y-axis direction
pos = p.getLinkState(self.robotId, 7)[0]
up_traj = point2traj([
pos,
[pos[0], pos[1] + (random.random() - 0.5) * 0.2 + 0.2, pos[2]]
])
start_id = self.move(up_traj, orn_traj, start_id)
# move in x-axis direction
pos = p.getLinkState(self.robotId, 7)[0]
up_traj = point2traj([
pos, [pos[0] + (random.random() - 0.5) * 0.2, pos[1], pos[2]]
])
start_id = self.move(up_traj, orn_traj, start_id)
elif self.opt.rand_start == 'two':
prob = random.random()
if prob < 0.5:
pos = p.getLinkState(self.robotId, 7)[0]
up_traj = point2traj([pos, [pos[0], pos[1] + 0.2, pos[2]]])
start_id = self.move(up_traj, orn_traj, start_id)
if self.opt.rand_box == 'rand':
self.box_file = os.path.join(self.env_root,
"urdf/openbox/openbox.urdf")
self.box_position = [
0.42 + (random.random() - 0.5) * 0.2,
0.00 + (random.random() - 0.5) * 0.4, 0.34
]
self.box_scaling = 0.00037
self.box_orientation = p.getQuaternionFromEuler(
[0, 0, math.pi / 2])
self.box_id = p.loadURDF(fileName=self.box_file,
basePosition=self.box_position,
baseOrientation=self.box_orientation,
globalScaling=self.box_scaling
) #, physicsClientId=self.physical_id)
else:
self.box_file = os.path.join(self.env_root,
"urdf/openbox/openbox.urdf")
self.box_position = [0.42, 0.00, 0.34]
self.box_scaling = 0.00037
self.box_orientation = p.getQuaternionFromEuler(
[0, 0, math.pi / 2])
self.box_id = p.loadURDF(fileName=self.box_file,
basePosition=self.box_position,
baseOrientation=self.box_orientation,
globalScaling=self.box_scaling
) #, physicsClientId=self.physical_id)
texture_path = os.path.join(self.env_root, 'texture/sun_textures')
texture_file = os.path.join(
texture_path,
random.sample(os.listdir(texture_path), 1)[0])
textid = p.loadTexture(texture_file)
# p.changeVisualShape (self.box_id, -1, rgbaColor=[1, 1, 1, 0.9])
# p.changeVisualShape (self.box_id, -1, textureUniqueId=textid)
p.changeVisualShape(self.box_id, -1, rgbaColor=[1, 0, 0, 1])
self.start_pos = p.getLinkState(self.robotId, 7)[0]
box = p.getAABB(self.box_id, -1)
box_center = [(x + y) * 0.5 for x, y in zip(box[0], box[1])]
obj = p.getAABB(self.obj_id, -1)
obj_center = [(x + y) * 0.5 for x, y in zip(obj[0], obj[1])]
self.last_aabb_dist = sum([
(x - y)**2 for x, y in zip(box_center, obj_center)
])**0.5
def load_urdf(self,
id,
filename,
start_pose,
remove_friction=False,
static=False,
label="thing",
description=""):
""" """
try:
use_fixed_base = 1 if static is True else 0
base_link_sim_id = p.loadURDF(
filename,
start_pose.position().to_array(),
start_pose.quaternion(),
useFixedBase=use_fixed_base,
flags=p.URDF_ENABLE_SLEEPING
or p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES)
self.entity_id_map[id] = base_link_sim_id
# Create a joint map to ease exploration
self.reverse_entity_id_map[base_link_sim_id] = id
self.joint_id_map[base_link_sim_id] = {}
self.reverse_joint_id_map[base_link_sim_id] = {}
for i in range(0, p.getNumJoints(base_link_sim_id)):
info = p.getJointInfo(base_link_sim_id, i)
self.joint_id_map[base_link_sim_id][info[1]] = info[0]
self.reverse_joint_id_map[base_link_sim_id][info[0]] = info[1]
# If file successfully loaded
if base_link_sim_id < 0:
raise RuntimeError("Invalid URDF")
scene_node = SceneNode(pose=start_pose, is_static=True)
scene_node.id = id
scene_node.label = label
scene_node.description = description
sim_id = self.entity_id_map[id]
visual_shapes = p.getVisualShapeData(sim_id)
for shape in visual_shapes:
link_id = shape[1]
type = shape[2]
dimensions = shape[3]
mesh_file_path = shape[4]
position = shape[5]
orientation = shape[6]
rgba_color = shape[7]
if link_id != -1:
link_state = p.getLinkState(sim_id, link_id)
t_link = link_state[0]
q_link = link_state[1]
t_inertial = link_state[2]
q_inertial = link_state[3]
tf_world_link = np.dot(translation_matrix(t_link),
quaternion_matrix(q_link))
tf_inertial_link = np.dot(translation_matrix(t_inertial),
quaternion_matrix(q_inertial))
world_transform = np.dot(tf_world_link,
np.linalg.inv(tf_inertial_link))
else:
t_link, q_link = p.getBasePositionAndOrientation(sim_id)
world_transform = np.dot(translation_matrix(t_link),
quaternion_matrix(q_link))
if type == p.GEOM_SPHERE:
primitive_shape = Sphere(dimensions[0] * 2.0)
elif type == p.GEOM_BOX:
primitive_shape = Box(dimensions[0], dimensions[1],
dimensions[2])
elif type == p.GEOM_CYLINDER:
primitive_shape = Cylinder(dimensions[1] * 2.0,
dimensions[0])
elif type == p.GEOM_PLANE:
primitive_shape = Box(dimensions[0], dimensions[1], 0.0001)
elif type == p.GEOM_MESH:
primitive_shape = Mesh("file://" + mesh_file_path,
scale_x=dimensions[0],
scale_y=dimensions[1],
scale_z=dimensions[2])
else:
raise NotImplementedError(
"Shape capsule not supported at the moment")
if link_id != -1:
shape_transform = np.dot(translation_matrix(position),
quaternion_matrix(orientation))
shape_transform = np.dot(world_transform, shape_transform)
shape_transform = np.linalg.inv(
np.dot(np.linalg.inv(shape_transform),
scene_node.pose.transform()))
position = translation_from_matrix(shape_transform)
orientation = quaternion_from_matrix(shape_transform)
primitive_shape.pose.pos.x = position[0]
primitive_shape.pose.pos.y = position[1]
primitive_shape.pose.pos.z = position[2]
primitive_shape.pose.from_quaternion(
orientation[0], orientation[1], orientation[2],
orientation[3])
else:
shape_transform = np.dot(translation_matrix(position),
quaternion_matrix(orientation))
shape_transform = np.dot(world_transform, shape_transform)
position = translation_from_matrix(shape_transform)
orientation = quaternion_from_matrix(shape_transform)
scene_node.pose.pos.x = position[0]
scene_node.pose.pos.y = position[1]
scene_node.pose.pos.z = position[2]
scene_node.pose.from_quaternion(orientation[0],
orientation[1],
orientation[2],
orientation[3])
if len(rgba_color) > 0:
primitive_shape.color[0] = rgba_color[0]
primitive_shape.color[1] = rgba_color[1]
primitive_shape.color[2] = rgba_color[2]
primitive_shape.color[3] = rgba_color[3]
scene_node.shapes.append(primitive_shape)
self.entity_map[id] = scene_node
return True, scene_node
rospy.loginfo(
"[simulation] '{}' File successfully loaded".format(filename))
except Exception as e:
rospy.logwarn("[simulation] Error loading URDF '{}': {}".format(
filename, e))
return False, None
#kukaId = p.loadURDF("kuka_iiwa/model.urdf",[0,0,0])
#kukaId = p.loadURDF("kuka_lwr/kuka.urdf",[0,0,0])
#kukaId = p.loadURDF("humanoid/nao.urdf",[0,0,0])
p.resetBasePositionAndOrientation(kukaId,[0,0,0],[0,0,0,1])
numJoints = p.getNumJoints(kukaId)
kukaEndEffectorIndex = numJoints - 1
# Set a joint target for the position control and step the sim.
setJointPosition(kukaId, [0.1] * numJoints)
p.stepSimulation()
# Get the joint and link state directly from Bullet.
pos, vel, torq = getJointStates(kukaId)
mpos, mvel, mtorq = getMotorJointStates(kukaId)
result = p.getLinkState(kukaId, kukaEndEffectorIndex, computeLinkVelocity=1, computeForwardKinematics=1)
link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = result
# Get the Jacobians for the CoM of the end-effector link.
# Note that in this example com_rot = identity, and we would need to use com_rot.T * com_trn.
# The localPosition is always defined in terms of the link frame coordinates.
zero_vec = [0.0] * len(mpos)
jac_t, jac_r = p.calculateJacobian(kukaId, kukaEndEffectorIndex, com_trn, mpos, zero_vec, zero_vec)
print ("Link linear velocity of CoM from getLinkState:")
print (link_vt)
print ("Link linear velocity of CoM from linearJacobian * q_dot:")
print (multiplyJacobian(kukaId, jac_t, vel))
print ("Link angular velocity of CoM from getLinkState:")
print (link_vr)
print ("Link angular velocity of CoM from angularJacobian * q_dot:")
def applyMMMTranslationToURDFBody(urdf_body_id, tx, ty, tz):
# get the translation to the pelvis frame
#tpcom, rpcom, tplcom, rplcom, translation_to_pelvis_frame, rpf, v, omega = p.getLinkState(
# urdf_body_id, 1, True, True)
# get the translation to the left leg frame
tllcom, rllcom, tlllcom, rlllcom, translation_to_left_leg_frame, rllf, vll, omegall = p.getLinkState(
urdf_body_id, 3, True, True)
# get the translation to the right leg frame
trlcom, rrlcom, trllcom, rrllcom, translation_to_right_leg_frame, rrlf, vrl, omegarl = p.getLinkState(
urdf_body_id, 2, True, True)
t_base_com, r_base_com = p.getBasePositionAndOrientation(urdf_body_id)
t_phb_center_to_base_com = [0, 0, 0]
for i in range(3):
t_phb_center_to_base_com[i] = t_base_com[i] - 0.5 * (
translation_to_right_leg_frame[i] +
translation_to_left_leg_frame[i])
t_base_com = [
tx + t_phb_center_to_base_com[0], ty + t_phb_center_to_base_com[1],
tz + t_phb_center_to_base_com[2]
]
p.resetBasePositionAndOrientation(urdf_body_id, t_base_com, r_base_com)
def _get_current_end_effector_position(self):
real_position = np.array(list(p.getLinkState(self.arm, 5, computeForwardKinematics=1)[4]))
#real_position[2] = -real_position[2] #SIM z coordinates are reversed
#adjusted_position = real_position + SIM_START_POSITION
return real_position
def applyAction(self, motorCommands):
#print ("self.numJoints")
#print (self.numJoints)
if (self.useInverseKinematics):
dx = motorCommands[0]
dy = motorCommands[1]
dz = motorCommands[2]
da = motorCommands[3]
fingerAngle = motorCommands[4]
state = p.getLinkState(self.kukaUid,self.kukaEndEffectorIndex)
actualEndEffectorPos = state[0]
#print("pos[2] (getLinkState(kukaEndEffectorIndex)")
#print(actualEndEffectorPos[2])
self.endEffectorPos[0] = self.endEffectorPos[0]+dx
if (self.endEffectorPos[0]>0.65):
self.endEffectorPos[0]=0.65
if (self.endEffectorPos[0]<0.50):
self.endEffectorPos[0]=0.50
self.endEffectorPos[1] = self.endEffectorPos[1]+dy
if (self.endEffectorPos[1]<-0.17):
self.endEffectorPos[1]=-0.17
if (self.endEffectorPos[1]>0.22):
self.endEffectorPos[1]=0.22
#print ("self.endEffectorPos[2]")
#print (self.endEffectorPos[2])
#print("actualEndEffectorPos[2]")
#print(actualEndEffectorPos[2])
#if (dz<0 or actualEndEffectorPos[2]<0.5):
self.endEffectorPos[2] = self.endEffectorPos[2]+dz
self.endEffectorAngle = self.endEffectorAngle + da
pos = self.endEffectorPos
orn = p.getQuaternionFromEuler([0,-math.pi,0]) # -math.pi,yaw])
if (self.useNullSpace==1):
if (self.useOrientation==1):
jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos,orn,self.ll,self.ul,self.jr,self.rp)
else:
jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos,lowerLimits=self.ll, upperLimits=self.ul, jointRanges=self.jr, restPoses=self.rp)
else:
if (self.useOrientation==1):
jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos,orn,jointDamping=self.jd)
else:
jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos)
#print("jointPoses")
#print(jointPoses)
#print("self.kukaEndEffectorIndex")
#print(self.kukaEndEffectorIndex)
if (self.useSimulation):
for i in range (self.kukaEndEffectorIndex+1):
#print(i)
p.setJointMotorControl2(bodyUniqueId=self.kukaUid,jointIndex=i,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[i],targetVelocity=0,force=self.maxForce,maxVelocity=self.maxVelocity, positionGain=0.3,velocityGain=1)
else:
#reset the joint state (ignoring all dynamics, not recommended to use during simulation)
for i in range (self.numJoints):
p.resetJointState(self.kukaUid,i,jointPoses[i])
#fingers
p.setJointMotorControl2(self.kukaUid,7,p.POSITION_CONTROL,targetPosition=self.endEffectorAngle,force=self.maxForce)
p.setJointMotorControl2(self.kukaUid,8,p.POSITION_CONTROL,targetPosition=-fingerAngle,force=self.fingerAForce)
p.setJointMotorControl2(self.kukaUid,11,p.POSITION_CONTROL,targetPosition=fingerAngle,force=self.fingerBForce)
p.setJointMotorControl2(self.kukaUid,10,p.POSITION_CONTROL,targetPosition=0,force=self.fingerTipForce)
p.setJointMotorControl2(self.kukaUid,13,p.POSITION_CONTROL,targetPosition=0,force=self.fingerTipForce)
else:
for action in range (len(motorCommands)):
motor = self.motorIndices[action]
p.setJointMotorControl2(self.kukaUid,motor,p.POSITION_CONTROL,targetPosition=motorCommands[action],force=self.maxForce)
controllers = [e[0] for e in p.getVREvents()]
for j in range(p.getNumJoints(kuka_gripper)):
print(p.getJointInfo(kuka_gripper, j))
while True:
events = p.getVREvents()
for e in (events):
# Only use one controller
###########################################
# This is important: make sure there's only one VR Controller!
if e[0] == controllers[0]:
break
sq_len = euc_dist(p.getLinkState(kuka, 6)[0], e[POSITION])
# A simplistic version of gripper control
#@TO-DO: Add slider for the gripper
#for i in range(p.getNumJoints(kuka_gripper)):
i = 4
p.setJointMotorControl2(kuka_gripper,
i,
p.POSITION_CONTROL,
targetPosition=e[ANALOG] * 0.05,
force=10)
i = 6
p.setJointMotorControl2(kuka_gripper,
i,
p.POSITION_CONTROL,
if (useRealTimeSimulation):
dt = datetime.now()
t = (dt.second / 60.) * 2. * math.pi
else:
t = t + 0.01
time.sleep(0.01)
for i in range(1):
pos = [2. * math.cos(t), 2. * math.cos(t), 0. + 2. * math.sin(t)]
jointPoses = p.calculateInverseKinematics(sawyerId,
sawyerEndEffectorIndex,
pos,
jointDamping=jd,
solver=ikSolver,
maxNumIterations=100)
#reset the joint state (ignoring all dynamics, not recommended to use during simulation)
for i in range(numJoints):
jointInfo = p.getJointInfo(sawyerId, i)
qIndex = jointInfo[3]
if qIndex > -1:
p.resetJointState(sawyerId, i, jointPoses[qIndex - 7])
ls = p.getLinkState(sawyerId, sawyerEndEffectorIndex)
if (hasPrevPose):
p.addUserDebugLine(prevPose, pos, [0, 0, 0.3], 1, trailDuration)
p.addUserDebugLine(prevPose1, ls[4], [1, 0, 0], 1, trailDuration)
prevPose = pos
prevPose1 = ls[4]
hasPrevPose = 1
def speed(self): if self.bodyPartIndex == -1: (vx, vy, vz), _ = p.getBaseVelocity(self.bodies[self.bodyIndex]) else: (x,y,z), (a,b,c,d), _,_,_,_, (vx, vy, vz), (vr,vp,vy) = p.getLinkState(self.bodies[self.bodyIndex], self.bodyPartIndex, computeLinkVelocity=1) return np.array([vx, vy, vz])
maxForce = 6
kp, kd, ki = 0.005, 0.005, 0.000
init = time()
target_x = 0
target_pitch =0
prev_error = 0
prev_error_pitch = 0
encoder_pos = [0,0]
encoder_vel = [0,0]
while(1):
posi = p.getBasePositionAndOrientation(bot)[0]
orie = p.getBasePositionAndOrientation(bot)[1]
euler = p.getEulerFromQuaternion(orie)
pitch = euler[1]
dt = time()-init
p1 = p.getLinkState(bot,9)[0]
p2 = p.getLinkState(bot,10)[0]
p3 = p.getLinkState(bot,11)[0]
p1 = np.array(p1)
p2 = np.array(p2)
p3 = np.array(p3)
direc = p1- (p2+p3)/2
sum = direc + posi
sum = magnitude(sum)
direc = magnitude(direc)
posi = magnitude(posi)
error = sum - direc -posi
feedback = kp*error + kd*(error - prev_error)/dt
feedback/=10
error_pitch = (pitch-target_pitch)
feedback1 = kp*error_pitch + kd*(error_pitch - prev_error_pitch)/dt + (ki*dt*error/abs(error+1e-6) if abs(error)<0.01 else 0)
def state_fields_of_pose_of(self, body_id, link_id=-1): # a method you will most probably need a lot to get pose and orientation if link_id == -1: (x, y, z), (a, b, c, d) = p.getBasePositionAndOrientation(body_id) else: (x, y, z), (a, b, c, d), _, _, _, _ = p.getLinkState(body_id, link_id) return np.array([x, y, z, a, b, c, d])
p.setJointMotorControlArray(teoId,
jointIndices,
controlMode=mode,
forces=maxForces,
targetVelocities = maxVelocities,
targetPositions = targetPos
)
timestep = 1/240
p.setTimeStep(timestep)
pos_l = np.asarray(p.getLinkState(teoId,name2id[b'l_sole_joint'])[0])
pos_r = np.asarray(p.getLinkState(teoId,name2id[b'r_sole_joint'])[0])
#
# Step Parameters
#
# All units in S.I.
step_duration = args.step_duration
step_height = 0.1
step_length = 0.05
hip_step_width = (l13-l16)
com_height_offset = -0.005
com_forward_ofset = -0.01
n_steps = args.n_steps
def main(unused_argv):
vrb = ViveRobotBridge()
rospy.init_node('raven_vive_teleop')
temp_flag = 0
pre_position = [0,0,0]
pre_pose = [1, 0, 0, 0]
temp = [1, 0, 0, 0]
# same loop rate as gym environment
rate = rospy.Rate(60.0)
env = Environment(
assets_root,
disp=True,
hz=60)
task = tasks.names[task_name]()
task.mode = mode
agent = task.oracle(env)
env.set_task(task)
obs = env.reset()
info = None
ee_pose = ((0.46562498807907104, -0.375, 0.3599780201911926), (0.0, 0.0, 0.0, 1.0))
ee_position = [0.0, 0.0, 0.0]
br = tf.TransformBroadcaster()
rci = RobotControlInterface(env)
while not rospy.is_shutdown():
#p.stepSimulation()
#p.getCameraImage(480, 320)
if vrb.trigger_pressed_event == 1:
# get current pose of the end-effector
ee_position_ref = list(p.getLinkState(env.ur5, env.ee_tip)[4])
#ee_orientation = p.getLinkState(self.ur5, self.ee_tip)[5]
vrb.trigger_pressed_event = 0
print("I--trigger pressed!\n")
if vrb.trigger_released_event == 1:
vrb.trigger_released_event = 0
print("O--trigger released!\n")
if vrb.trigger_current_status == 1:
vive_rotation = vrb.vive_controller_rotation
ee_rotation = kdl.Rotation.Quaternion(vive_rotation[0],vive_rotation[1],vive_rotation[2],vive_rotation[3])
r1 = kdl.Rotation.RotZ(-math.pi/2)
ee_rotation = r1 * ee_rotation
ee_rotation.DoRotX(math.pi/2)
ee_rotation.DoRotZ(math.pi/2)
ee_rotation.DoRotY(math.pi)
ee_orientation = ee_rotation.GetQuaternion()
# br.sendTransform(
# (0.0, 0.0, 1.0),
# ee_rotation.GetQuaternion(),
# rospy.Time.now(),
# "transformed_controller_frame",
# "world"
# )
#ee_orientation = (0,0,0,1)
ee_position[0] = ee_position_ref[0] + vrb.vive_controller_translation[1]
ee_position[1] = ee_position_ref[1] - vrb.vive_controller_translation[0]
# z axis limit
z_control = ee_position_ref[2] + vrb.vive_controller_translation[2]
if z_control < 0.02:
z_control = 0.02
ee_position[2] = z_control
ee_pose = (tuple(ee_position), tuple(ee_orientation))
#env.movep(ee_pose)
#rci.publish_pose_message(ee_pose)
joint_position = rci.solve_ik(ee_pose)
rci.movej_fast(joint_position)
rci.publish_joint_states_msg()
#targj = env.solve_ik(ee_pose)
#movej(env,targj, speed=0.01)
#rci.movej(env,rci.joint_states)
# joint_state_msg = JointState()
# joint_state_msg.header.stamp = rospy.Time().now()
# joint_state_msg.name = ['shoulder_pan_joint',
# 'shoulder_lift_joint',
# 'elbow_joint',
# 'wrist_1_joint',
# 'wrist_2_joint',
# 'wrist_3_joint']
# joint_state_msg.position = targj
# joint_state_pub.publish(joint_state_msg)
# add a marker to indicate the projection of the ee on the workspace
marker_head_point = [ee_position[0], ee_position[1], 0.05]
marker_tail_point = [ee_position[0], ee_position[1], 0.06]
p.addUserDebugLine( marker_head_point,
marker_tail_point,
lineColorRGB=[1, 0, 0],
lifeTime=0.2,
lineWidth=3)
if env.ee.check_grasp() == True:
print("grasp succeed!")
env.reset()
ee_position_ref = list(p.getLinkState(env.ur5, env.ee_tip)[4])
if vrb.grasp == 1:
env.ee.activate()
else:
env.ee.release()
p.stepSimulation()
rate.sleep()
# record the postions and orentations for inverse kinematics
i=0
while 1:
i+=1
p.stepSimulation()
time.sleep(0.03)
thumb(1.5, 1.57)
if(i==10):
indexF(1.4, 2.47)
midF(1.4, 1.47)
ringF(1.4, 1.47)
pinkyF(1.5, 1.57)
if(i==50):
#index:51, mid:42, ring: 33, pinky:24, thumb 62
print("Thumb Position: ", p.getLinkState(sawyerId, 62)[0]) # get position of thumb tip
print("Thumb Orientation: ", p.getLinkState(sawyerId, 62)[1]) # get orientation of thumb tip
print("Index Position: ", p.getLinkState(sawyerId, 51)[0])
print("Index Orientation: ", p.getLinkState(sawyerId, 51)[1])
print("Mid: Position: ", p.getLinkState(sawyerId, 42)[0])
print("Mid: Orientation: ", p.getLinkState(sawyerId, 42)[1])
print("Ring Position: ", p.getLinkState(sawyerId, 33)[0])
print("Ring Orientation: ", p.getLinkState(sawyerId, 33)[1])
print("Pinky Position: ", p.getLinkState(sawyerId, 24)[0])
print("Pinky Orientation: ", p.getLinkState(sawyerId, 24)[1])
print("Palm Position: ", p.getLinkState(sawyerId, 20)[0])
print("Palm Orientation: ", p.getLinkState(sawyerId, 20)[1])
break
# pick up obejct
i=0
maxNumIterations=100,
residualThreshold=.01)
else:
jointPoses = p.calculateInverseKinematics(kukaId,
kukaEndEffectorIndex,
pos,
solver=ikSolver)
if (useSimulation):
for i in range(numJoints):
p.setJointMotorControl2(bodyIndex=kukaId,
jointIndex=i,
controlMode=p.POSITION_CONTROL,
targetPosition=jointPoses[i],
targetVelocity=0,
force=500,
positionGain=0.03,
velocityGain=1)
else:
#reset the joint state (ignoring all dynamics, not recommended to use during simulation)
for i in range(numJoints):
p.resetJointState(kukaId, i, jointPoses[i])
ls = p.getLinkState(kukaId, kukaEndEffectorIndex)
if (hasPrevPose):
p.addUserDebugLine(prevPose, pos, [0, 0, 0.3], 1, trailDuration)
p.addUserDebugLine(prevPose1, ls[4], [1, 0, 0], 1, trailDuration)
prevPose = pos
prevPose1 = ls[4]
hasPrevPose = 1
