def get_quaternion_waypoints(point, start_quat, end_quat, step_size=np.pi/16): angle = quat_angle_between(start_quat, end_quat) for t in np.arange(0, angle, step_size): fraction = t/angle quat = p.getQuaternionSlerp(start_quat, end_quat, interpolationFraction=fraction) #quat = quaternion_slerp(start_quat, end_quat, fraction=fraction) yield (point, quat) yield (point, end_quat)
def main(): val = vs() # Set val initial pose # high cond number # init_pos = [[0.2]] * len(val.left_arm_joint_indices) init_pos2 = [] for joint in val.left_arm_joints: init_pos2.append([val.home[joint]]) print(init_pos2) init_pos = [[0.3]] * len(val.left_arm_joint_indices) p.resetJointStatesMultiDof(val.robot, jointIndices=val.left_arm_joint_indices, targetValues=init_pos2) time.sleep(0.5) # Optional cartesian velocity controller test val.cart_vel_linear_test() val.cart_vel_angular_test() # Get Jacobian testing script jac_trn, jac_rot = get_jacobian(val.robot, val.left_tool) jac = get_arm_jacobian(val.robot, "left", val.left_tool) # Get pose testing script pos, rot = get_pose(val.robot, val.left_tool) print("axis angle result: {}".format(rot)) # Define and draw goal pose cur_pos, cur_rot = get_pose(val.robot, val.left_tool) goal_pos = tuple( np.asarray(np.array(cur_pos) + np.array([0.05, 0.05, -0.1]))) goal_rot = p.getQuaternionSlerp(cur_rot, (0, 0, 0, 1), 0.4) draw_pose(cur_pos, cur_rot) draw_pose(goal_pos, goal_rot) # camera test camera_test() val.pbvs("left", goal_pos, goal_rot, kv=1.0, kw=0.8, eps_pos=0.005, eps_rot=0.05, plot_pose=True, perturb_jacobian=False, perturb_Jac_joint_mu=0.2, perturb_Jac_joint_sigma=0.2, perturb_orientation=False, mu_R=0.3, sigma_R=0.3, plot_result=True) p.disconnect()
def interpolate_poses(pose1, pose2, pos_step_size=0.01, ori_step_size=np.pi/16): pos1, quat1 = pose1 pos2, quat2 = pose2 num_steps = int(math.ceil(max(get_distance(pos1, pos2)/pos_step_size, quat_angle_between(quat1, quat2)/ori_step_size))) for i in range(num_steps): fraction = float(i) / num_steps pos = (1-fraction)*np.array(pos1) + fraction*np.array(pos2) quat = p.getQuaternionSlerp(quat1, quat2, interpolationFraction=fraction) #quat = quaternion_slerp(quat1, quat2, fraction=fraction) yield (pos, quat) yield pose2
def interpolate_poses_by_num_steps(pose1, pose2, num_steps=5): pos1, quat1 = pose1 pos2, quat2 = pose2 for i in range(num_steps): fraction = float(i) / num_steps pos = (1 - fraction) * np.array(pos1) + fraction * np.array(pos2) quat = p.getQuaternionSlerp(quat1, quat2, interpolationFraction=fraction) #quat = quaternion_slerp(quat1, quat2, fraction=fraction) yield (pos, quat) yield pose2
#getQuaternionSlerp frameData = motion_dict['Frames'][frame] frameDataNext = motion_dict['Frames'][frameNext] #print("duration=",frameData[0]) #print(pos=[frameData]) basePos1Start = [frameData[1],frameData[2],frameData[3]] basePos1End = [frameDataNext[1],frameDataNext[2],frameDataNext[3]] basePos1 = [basePos1Start[0]+frameFraction*(basePos1End[0]-basePos1Start[0]), basePos1Start[1]+frameFraction*(basePos1End[1]-basePos1Start[1]), basePos1Start[2]+frameFraction*(basePos1End[2]-basePos1Start[2])] baseOrn1Start = [frameData[5],frameData[6], frameData[7],frameData[4]] baseOrn1Next = [frameDataNext[5],frameDataNext[6], frameDataNext[7],frameDataNext[4]] baseOrn1 = p.getQuaternionSlerp(baseOrn1Start,baseOrn1Next,frameFraction) #pre-rotate to make z-up y2zPos=[0,0,0.0] y2zOrn = p.getQuaternionFromEuler([1.57,0,0]) basePos,baseOrn = p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1) p.resetBasePositionAndOrientation(humanoid, basePos,baseOrn) # once=False chestRotStart = [frameData[9],frameData[10],frameData[11],frameData[8]] chestRotEnd = [frameDataNext[9],frameDataNext[10],frameDataNext[11],frameDataNext[8]] chestRot = p.getQuaternionSlerp(chestRotStart,chestRotEnd,frameFraction) neckRotStart = [frameData[13],frameData[14],frameData[15],frameData[12]] neckRotEnd= [frameDataNext[13],frameDataNext[14],frameDataNext[15],frameDataNext[12]] neckRot = p.getQuaternionSlerp(neckRotStart,neckRotEnd,frameFraction)
while (p.isConnected()): target_frame = p.readUserDebugParameter(frame_id) cur_frame = int(np.minimum(target_frame, nframe - 1)) next_frame = int(np.minimum(cur_frame + 1, nframe - 1)) frac = target_frame - cur_frame cur_basePos = np.array(sample_data['pos']['base'][cur_frame]) next_basePos = np.array(sample_data['pos']['base'][next_frame]) target_basePos = cur_basePos + frac * (next_basePos - cur_basePos) for i in range(len(joint_list)): cur_orn = sample_data['orn'][joint_list[i]][cur_frame] next_orn = sample_data['orn'][joint_list[i]][next_frame] if i in JOINT_SPHERICAL: target_orn[i] = p.getQuaternionSlerp(cur_orn, next_orn, frac) elif i in JOINT_REVOLUTE: target_orn[i] = cur_orn + frac * (next_orn - cur_orn) print("{}".format(target_basePos)) p.resetBasePositionAndOrientation(humanoid_id, target_basePos, target_orn[0]) for i in spherical_ind: p.resetJointStateMultiDof(humanoid_id, i, targetValue=target_orn[i]) for i in revolute_ind: th = target_orn[i] p.resetJointState(humanoid_id, i, targetValue=th) p.stepSimulation()
def main(): val = vs() # Set val initial pose init_pos = [] for joint in val.left_arm_joints: init_pos.append([val.home[joint]]) p.resetJointStatesMultiDof(val.robot, jointIndices=val.left_arm_joint_indices, targetValues=init_pos) time.sleep(0.5) # joint velocity controller test - linear velocity print("joint velocity controller test - linear velocity") val.cart_vel_linear_test(t=1.5, v=0.05) # joint velocity controller test - angular velocity print("joint velocity controller test - angular velocity") val.cart_vel_angular_test(t=1.5, w=0.25) # Define and draw goal pose cur_pos, cur_rot = get_pose(val.robot, val.left_tool) goal_pos_global = tuple(np.asarray(np.array(cur_pos) + np.array([0.05, 0.05, -0.1]))) goal_rot_global = p.getQuaternionSlerp(cur_rot, (0, 0, 0, 1), 0.4) # weird thing happens if it is 0.7 draw_pose(cur_pos, cur_rot) draw_pose(goal_pos_global, goal_rot_global) # transform goal_pose to camera frame goal_pos, goal_rot = SE32_trans_rot(val.Trc.inverse() * trans_rot2SE3(goal_pos_global, goal_rot_global)) # Position Based Visual Servoing with no perturbation print("Position Based Visual Servoing with no perturbation") val.pbvs("left", goal_pos, goal_rot, kv=1.0, kw=0.8, eps_pos=0.005, eps_rot=0.05, plot_pose=True, perturb_jacobian=False, perturb_orientation=False, plot_result=True) # reset to home pose p.resetJointStatesMultiDof(val.robot, jointIndices=val.left_arm_joint_indices, targetValues=init_pos) draw_pose(cur_pos, cur_rot) draw_pose(goal_pos_global, goal_rot_global) # Position Based Visual Servoing with perturbation in Jacobian print("Position Based Visual Servoing with perturbation in Jacobian") val.pbvs("left", goal_pos, goal_rot, kv=1.0, kw=0.8, eps_pos=0.005, eps_rot=0.05, plot_pose=True, perturb_jacobian=True, perturb_Jac_joint_mu=0.2, perturb_Jac_joint_sigma=0.2, perturb_orientation=False, mu_R=0.3, sigma_R=0.3, plot_result=True) # reset to home pose p.resetJointStatesMultiDof(val.robot, jointIndices=val.left_arm_joint_indices, targetValues=init_pos) draw_pose(cur_pos, cur_rot) draw_pose(goal_pos_global, goal_rot_global) # execute Particle-Filter based PBVS, using default PBVS parameters which can be changed val.pbvs_pf("left", goal_pos, goal_rot) p.disconnect()
#getQuaternionSlerp frameData = motion_dict['Frames'][frame] frameDataNext = motion_dict['Frames'][frameNext] #print("duration=",frameData[0]) #print(pos=[frameData]) basePos1Start = [frameData[1],frameData[2],frameData[3]] basePos1End = [frameDataNext[1],frameDataNext[2],frameDataNext[3]] basePos1 = [basePos1Start[0]+frameFraction*(basePos1End[0]-basePos1Start[0]), basePos1Start[1]+frameFraction*(basePos1End[1]-basePos1Start[1]), basePos1Start[2]+frameFraction*(basePos1End[2]-basePos1Start[2])] baseOrn1Start = [frameData[5],frameData[6], frameData[7],frameData[4]] baseOrn1Next = [frameDataNext[5],frameDataNext[6], frameDataNext[7],frameDataNext[4]] baseOrn1 = p.getQuaternionSlerp(baseOrn1Start,baseOrn1Next,frameFraction) #pre-rotate to make z-up if (useZUp): y2zPos=[0,0,0.0] y2zOrn = p.getQuaternionFromEuler([1.57,0,0]) basePos,baseOrn = p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1) p.resetBasePositionAndOrientation(humanoid, basePos,baseOrn) y2zPos=[0,2,0.0] y2zOrn = p.getQuaternionFromEuler([1.57,0,0]) basePos,baseOrn = p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1) p.resetBasePositionAndOrientation(humanoid2, basePos,baseOrn) chestRotStart = [frameData[9],frameData[10],frameData[11],frameData[8]] chestRotEnd = [frameDataNext[9],frameDataNext[10],frameDataNext[11],frameDataNext[8]]
def slerp(q0, q1, t): return pb.getQuaternionSlerp(q0, q1, t)