def test_pose_bundle(self): num_poses = 7 bundle = PoseBundle(num_poses) # - Accessors. self.assertEqual(bundle.get_num_poses(), num_poses) self.assertTrue(isinstance(bundle.get_pose(0), Isometry3)) self.assertTrue(isinstance(bundle.get_velocity(0), FrameVelocity)) # - Mutators. kIndex = 5 p = [0, 1, 2] q = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9])) bundle.set_pose(kIndex, Isometry3(q, p)) w = [0.1, 0.3, 0.5] v = [0., 1., 2.] frame_velocity = FrameVelocity(SpatialVelocity(w=w, v=v)) bundle.set_velocity(kIndex, frame_velocity) self.assertTrue( (bundle.get_pose(kIndex).matrix() == Isometry3(q, p).matrix()).all()) vel_actual = bundle.get_velocity(kIndex).get_velocity() self.assertTrue(np.allclose(vel_actual.rotational(), w)) self.assertTrue(np.allclose(vel_actual.translational(), v)) name = "Alice" bundle.set_name(kIndex, name) self.assertEqual(bundle.get_name(kIndex), name) instance_id = 42 # Supply a random instance id. bundle.set_model_instance_id(kIndex, instance_id) self.assertEqual(bundle.get_model_instance_id(kIndex), instance_id)
def test_pose_selector(self): kScanDistance = 4. rg = make_two_lane_road() lane = rg.junction(0).segment(0).lane(0) pose0 = PoseVector() pose0.set_translation([1., 0., 0.]) pose1 = PoseVector() # N.B. Set pose1 3 meters ahead of pose0. pose1.set_translation([4., 0., 0.]) bundle = PoseBundle(num_poses=2) bundle.set_pose(0, Isometry3(Quaternion(), pose0.get_translation())) bundle.set_pose(1, Isometry3(Quaternion(), pose1.get_translation())) closest_pose = PoseSelector.FindSingleClosestPose( lane=lane, ego_pose=pose0, traffic_poses=bundle, scan_distance=kScanDistance, side=AheadOrBehind.kAhead, path_or_branches=ScanStrategy.kPath) self.assertEqual(closest_pose.odometry.lane.id().string(), lane.id().string()) self.assertTrue(closest_pose.distance == 3.) closest_pair = PoseSelector.FindClosestPair( lane=lane, ego_pose=pose0, traffic_poses=bundle, scan_distance=kScanDistance, path_or_branches=ScanStrategy.kPath) self.assertEqual( closest_pair[AheadOrBehind.kAhead].odometry.lane.id().string(), lane.id().string()) self.assertEqual(closest_pair[AheadOrBehind.kAhead].distance, 3.) self.assertEqual( closest_pair[AheadOrBehind.kBehind].odometry.lane.id().string(), lane.id().string()) self.assertEqual(closest_pair[AheadOrBehind.kBehind].distance, float('inf')) lane_pos = LanePosition(s=1., r=0., h=0.) road_pos = RoadPosition(lane=lane, pos=lane_pos) w = [1., 2., 3.] v = [-4., -5., -6.] frame_velocity = FrameVelocity(SpatialVelocity(w=w, v=v)) road_odom = RoadOdometry(road_position=road_pos, frame_velocity=frame_velocity) sigma_v = PoseSelector.GetSigmaVelocity(road_odom) self.assertEqual(sigma_v, v[0])
def test_pose_vector(self): value = PoseVector() self.assertTrue(isinstance(value, BasicVector)) self.assertTrue(isinstance(copy.copy(value), PoseVector)) self.assertTrue(isinstance(value.Clone(), PoseVector)) self.assertEqual(value.size(), PoseVector.kSize) # - Accessors. self.assertTrue(isinstance(value.get_isometry(), Isometry3)) self.assertTrue(isinstance(value.get_rotation(), Quaternion)) self.assertTrue(isinstance(value.get_translation(), np.ndarray)) # - Value. self.assertTrue( np.allclose(value.get_isometry().matrix(), np.eye(4, 4))) # - Mutators. p = [0, 1, 2] q = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9])) X_expected = Isometry3(q, p) value.set_translation(p) value.set_rotation(q) self.assertTrue( np.allclose(value.get_isometry().matrix(), X_expected.matrix())) # - Ensure ordering is ((px, py, pz), (qw, qx, qy, qz)) vector_expected = np.hstack((p, q.wxyz())) vector_actual = value.get_value() self.assertTrue(np.allclose(vector_actual, vector_expected)) # - Fully-parameterized constructor. value1 = PoseVector(rotation=q, translation=p) self.assertTrue( np.allclose(value1.get_isometry().matrix(), X_expected.matrix()))
def CalcKinematics(self, X_L7E, l7_frame, world_frame, tree_iiwa, context_iiwa, handle_angle_ref): # calculate Geometric jacobian (6 by 7 matrix) of point Q in EE frame. p_L7Q = X_L7E.multiply(p_EQ) Jv_WL7q = tree_iiwa.CalcFrameGeometricJacobianExpressedInWorld( context=context_iiwa, frame_B=l7_frame, p_BoFo_B=p_L7Q) # Translation # Hr: handle reference frame X_WHr = Isometry3() X_WHr.set_rotation( RollPitchYaw(0, 0, -handle_angle_ref).ToRotationMatrix().matrix()) X_WHr.set_translation(p_WC_left_hinge + [ -r_handle * np.sin(handle_angle_ref), -r_handle * np.cos(handle_angle_ref), 0 ]) X_HrW = X_WHr.inverse() X_WL7 = tree_iiwa.CalcRelativeTransform(context_iiwa, frame_A=world_frame, frame_B=l7_frame) p_WQ = X_WL7.multiply(p_L7Q) p_HrQ = X_HrW.multiply(p_WQ) return Jv_WL7q, p_HrQ
def test_idm_controller(self): rg = make_two_lane_road() idm = IdmController( road=rg, path_or_branches=ScanStrategy.kPath, road_position_strategy=RoadPositionStrategy.kExhaustiveSearch, period_sec=0.) context = idm.CreateDefaultContext() output = idm.AllocateOutput() # Fix the inputs. pose_vector1 = PoseVector() pose_vector1.set_translation([1., 2., 3.]) ego_pose_index = idm.ego_pose_input().get_index() context.FixInputPort(ego_pose_index, pose_vector1) w = [0., 0., 0.] v = [1., 0., 0.] frame_velocity1 = FrameVelocity(SpatialVelocity(w=w, v=v)) ego_velocity_index = idm.ego_velocity_input().get_index() context.FixInputPort(ego_velocity_index, frame_velocity1) pose_vector2 = PoseVector() pose_vector2.set_translation([6., 0., 0.]) bundle = PoseBundle(num_poses=1) bundle.set_pose( 0, Isometry3(Quaternion(), pose_vector2.get_translation())) traffic_index = idm.traffic_input().get_index() context.FixInputPort(traffic_index, framework.AbstractValue.Make(bundle)) # Verify the inputs. pose_vector_eval = idm.EvalVectorInput(context, 0) self.assertTrue( np.allclose(pose_vector1.get_translation(), pose_vector_eval.get_translation())) frame_velocity_eval = idm.EvalVectorInput(context, 1) self.assertTrue( np.allclose(frame_velocity1.get_velocity().translational(), frame_velocity_eval.get_velocity().translational())) self.assertTrue( np.allclose(frame_velocity1.get_velocity().rotational(), frame_velocity_eval.get_velocity().rotational())) # Verify the outputs. idm.CalcOutput(context, output) accel_command_index = idm.acceleration_output().get_index() accel = output.get_vector_data(accel_command_index) self.assertEqual(len(accel.get_value()), 1) self.assertTrue(accel.get_value() < 0.)
def CalcKinematics(self, X_L7E, l7_frame, world_frame, tree_iiwa, context_iiwa, t_plan): ''' :param X_L7E: transformation from frame E (end effector) to frame L7. :param l7_frame: A BodyFrame object of frame L7. :param world_frame: A BodyFrame object of the world frame. :param tree_iiwa: A MultibodyTree object of the robot. :param context_iiwa: A Context object that describes the current state of the robot. :param t_plan: time passed since the beginning of this Plan, expressed in seconds. :return: Jv_WL7q: geometric jacbian of point Q in frame L7. p_HrQ: position of point Q relative to frame Hr. ''' # calculate Geometric jacobian (6 by 7 matrix) of point Q in frame L7. p_L7Q = X_L7E.multiply(p_EQ) Jv_WL7q = tree_iiwa.CalcFrameGeometricJacobianExpressedInWorld( context=context_iiwa, frame_B=l7_frame, p_BoFo_B=p_L7Q) # Translation # Hr: handle reference frame # p_HrQ: position of point Q relative to frame Hr. # X_WHr: transformation from Hr to world frame W. X_WHr = Isometry3() # Your code here #------------------------------------------------------------------------# angle = self.traj.value(t_plan) X_WHr.set_rotation( np.array([[np.cos(angle), np.sin(angle), 0.], [-np.sin(angle), np.cos(angle), 0.], [0., 0., 1.]])) X_WHr.set_translation( p_WC_handle + [-r_handle * np.sin(angle), -r_handle * np.cos(angle), 0.]) #------------------------------------------------------------------------# X_HrW = X_WHr.inverse() X_WL7 = tree_iiwa.CalcRelativeTransform(context_iiwa, frame_A=world_frame, frame_B=l7_frame) p_WQ = X_WL7.multiply(p_L7Q) p_HrQ = X_HrW.multiply(p_WQ) return Jv_WL7q, p_HrQ
def read_poses_from_file(filename): pose_dict = {} row_num = 0 cur_matrix = np.eye(4) with open(filename, "r") as f: for line in f: line = line.rstrip() if not line.lstrip(" ").startswith("["): object_name = line else: row = np.matrix(line) cur_matrix[row_num, :] = row row_num += 1 if row_num == 4: pose_dict[object_name] = Isometry3(cur_matrix) translation = pose_dict[object_name].translation() translation[2] = -dz_table_top_robot_base pose_dict[object_name].set_translation(translation) pose_dict[object_name].set_rotation(np.eye(3)) cur_matrix = np.eye(4) row_num %= 4 return pose_dict
def CalcKinematics(self, X_L7E, l7_frame, world_frame, tree_iiwa, context_iiwa, handle_angle_ref): # calculate Geometric jacobian (6 by 7 matrix) of point Q in frame L7. p_L7Q = X_L7E.multiply(p_EQ) Jv_WL7q = tree_iiwa.CalcFrameGeometricJacobianExpressedInWorld( context=context_iiwa, frame_B=l7_frame, p_BoFo_B=p_L7Q) # Translation # Hr: handle reference frame # p_HrQ position of point Q relative to frame Hr. # X_WHr: transformation from Hr to world frame W. X_WHr = Isometry3() X_HrW = X_WHr.inverse() X_WL7 = tree_iiwa.CalcRelativeTransform(context_iiwa, frame_A=world_frame, frame_B=l7_frame) p_WQ = X_WL7.multiply(p_L7Q) p_HrQ = X_HrW.multiply(p_WQ) return Jv_WL7q, p_HrQ
def test_pose_aggregator(self): aggregator = PoseAggregator() # - Set-up. instance_id1 = 5 # Supply a random instance id. port1 = aggregator.AddSingleInput("pose_only", instance_id1) self.assertEqual(port1.get_data_type(), PortDataType.kVectorValued) self.assertEqual(port1.size(), PoseVector.kSize) instance_id2 = 42 # Supply another random, but unique, id. ports2 = aggregator.AddSinglePoseAndVelocityInput( "pose_and_velocity", instance_id2) self.assertEqual(ports2.pose_input_port.get_data_type(), PortDataType.kVectorValued) self.assertEqual(ports2.pose_input_port.size(), PoseVector.kSize) self.assertEqual(ports2.velocity_input_port.get_data_type(), PortDataType.kVectorValued) self.assertEqual(ports2.velocity_input_port.size(), FrameVelocity.kSize) num_poses = 1 port3 = aggregator.AddBundleInput("pose_bundle", num_poses) self.assertEqual(port3.get_data_type(), PortDataType.kAbstractValued) # - CalcOutput. context = aggregator.CreateDefaultContext() output = aggregator.AllocateOutput() p1 = [0, 1, 2] pose1 = PoseVector() pose1.set_translation(p1) p2 = [5, 7, 9] pose2 = PoseVector() pose2.set_translation(p2) w = [0.3, 0.4, 0.5] v = [0.5, 0.6, 0.7] velocity = FrameVelocity() velocity.set_velocity(SpatialVelocity(w=w, v=v)) p3 = [50, 70, 90] q3 = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9])) bundle = PoseBundle(num_poses) bundle.set_pose(0, Isometry3(q3, p3)) bundle_value = AbstractValue.Make(bundle) context.FixInputPort(0, pose1) context.FixInputPort(1, pose2) context.FixInputPort(2, velocity) context.FixInputPort(3, bundle_value) aggregator.CalcOutput(context, output) value = output.get_data(0).get_value() self.assertEqual(value.get_num_poses(), 3) isom1_actual = Isometry3() isom1_actual.set_translation(p1) self.assertTrue( (value.get_pose(0).matrix() == isom1_actual.matrix()).all()) isom2_actual = Isometry3() isom2_actual.set_translation(p2) self.assertTrue( (value.get_pose(1).matrix() == isom2_actual.matrix()).all()) vel_actual = value.get_velocity(1).get_velocity() self.assertTrue(np.allclose(vel_actual.rotational(), w)) self.assertTrue(np.allclose(vel_actual.translational(), v)) self.assertTrue( (value.get_pose(2).matrix() == Isometry3(q3, p3).matrix()).all())