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