def test_frame_velocity(self): frame_velocity = FrameVelocity() self.assertTrue(isinstance(frame_velocity, BasicVector)) self.assertTrue(isinstance(copy.copy(frame_velocity), FrameVelocity)) self.assertTrue(isinstance(frame_velocity.Clone(), FrameVelocity)) self.assertEqual(frame_velocity.size(), FrameVelocity.kSize) # - Accessors. self.assertTrue( isinstance(frame_velocity.get_velocity(), SpatialVelocity)) # - Value. self.assertTrue( np.allclose(frame_velocity.get_velocity().rotational(), 3 * [0.])) self.assertTrue( np.allclose(frame_velocity.get_velocity().translational(), 3 * [0.])) # - Mutators. w = [0.1, 0.3, 0.5] v = [0., 1., 2.] frame_velocity.set_velocity(SpatialVelocity(w=w, v=v)) self.assertTrue( np.allclose(frame_velocity.get_velocity().rotational(), w)) self.assertTrue( np.allclose(frame_velocity.get_velocity().translational(), v)) # - Ensure ordering is ((wx, wy, wz), (vx, vy, vz)) velocity_expected = np.hstack((w, v)) velocity_actual = frame_velocity.get_value() self.assertTrue(np.allclose(velocity_actual, velocity_expected)) # - Fully-parameterized constructor. frame_velocity1 = FrameVelocity(SpatialVelocity(w=w, v=v)) self.assertTrue( np.allclose(frame_velocity1.get_velocity().rotational(), w)) self.assertTrue( np.allclose(frame_velocity1.get_velocity().translational(), v))
def test_road_odometry(self): RoadOdometry() rg = make_two_lane_road() lane_0 = rg.junction(0).segment(0).lane(0) lane_1 = rg.junction(0).segment(0).lane(1) lane_pos = LanePosition(s=1., r=2., h=3.) road_pos = RoadPosition(lane=lane_0, pos=lane_pos) w = [5., 7., 9.] v = [11., 13., 15.] frame_velocity = FrameVelocity(SpatialVelocity(w=w, v=v)) road_odom = RoadOdometry( road_position=road_pos, frame_velocity=frame_velocity) self.assertEqual(road_odom.lane.id().string(), lane_0.id().string()) self.assertTrue(np.allclose(road_odom.pos.srh(), lane_pos.srh())) self.assertTrue(np.allclose( frame_velocity.get_velocity().translational(), road_odom.vel.get_velocity().translational())) lane_pos_new = LanePosition(s=10., r=20., h=30.) v_new = [42., 43., 44.] frame_velocity_new = FrameVelocity(SpatialVelocity(w=w, v=v_new)) road_odom.lane = lane_1 road_odom.pos = lane_pos_new road_odom.vel = frame_velocity_new self.assertEqual(road_odom.lane.id().string(), lane_1.id().string()) self.assertTrue(np.allclose(road_odom.pos.srh(), lane_pos_new.srh())) self.assertTrue(np.allclose( frame_velocity_new.get_velocity().translational(), road_odom.vel.get_velocity().translational()))
def test_spatial_velocity(self): velocity = SpatialVelocity() # - Accessors. self.assertTrue(isinstance(velocity.rotational(), np.ndarray)) self.assertTrue(isinstance(velocity.translational(), np.ndarray)) self.assertEqual(velocity.rotational().shape, (3, )) self.assertEqual(velocity.translational().shape, (3, )) # - Fully-parameterized constructor. w = [0.1, 0.3, 0.5] v = [0., 1., 2.] velocity1 = SpatialVelocity(w=w, v=v) self.assertTrue(np.allclose(velocity1.rotational(), w)) self.assertTrue(np.allclose(velocity1.translational(), v))
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_spatial_velocity(self): velocity = SpatialVelocity() # - Accessors. self.assertTrue(isinstance(velocity.rotational(), np.ndarray)) self.assertTrue(isinstance(velocity.translational(), np.ndarray)) self.assertEqual(velocity.rotational().shape, (3,)) self.assertEqual(velocity.translational().shape, (3,)) # - Fully-parameterized constructor. w = [0.1, 0.3, 0.5] v = [0., 1., 2.] velocity1 = SpatialVelocity(w=w, v=v) self.assertTrue(np.allclose(velocity1.rotational(), w)) self.assertTrue(np.allclose(velocity1.translational(), v))
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_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 test_closest_pose(self): ClosestPose() rg = make_two_lane_road() lane_0 = rg.junction(0).segment(0).lane(0) lane_1 = rg.junction(0).segment(0).lane(1) lane_pos = LanePosition(s=1., r=2., h=3.) road_pos = RoadPosition(lane=lane_0, pos=lane_pos) w = [5., 7., 9.] v = [11., 13., 15.] frame_velocity = FrameVelocity(SpatialVelocity(w=w, v=v)) road_odom = RoadOdometry( road_position=road_pos, frame_velocity=frame_velocity) closest_pose = ClosestPose(odom=road_odom, dist=12.7) self.assertEqual(closest_pose.odometry.lane.id().string(), lane_0.id().string()) self.assertEqual(closest_pose.distance, 12.7) closest_pose.odometry.lane = lane_1 closest_pose.distance = 5.9 self.assertEqual(closest_pose.odometry.lane.id().string(), lane_1.id().string()) self.assertEqual(closest_pose.distance, 5.9)
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())
def test_multibody_tree_kinematics(self): file_name = FindResourceOrThrow( "drake/examples/double_pendulum/models/double_pendulum.sdf") plant = MultibodyPlant() Parser(plant).AddModelFromFile(file_name) plant.Finalize() context = plant.CreateDefaultContext() world_frame = plant.world_frame() base = plant.GetBodyByName("base") base_frame = plant.GetFrameByName("base") X_WL = plant.CalcRelativeTransform(context, frame_A=world_frame, frame_B=base_frame) self.assertIsInstance(X_WL, Isometry3) p_AQi = plant.CalcPointsPositions(context=context, frame_B=base_frame, p_BQi=np.array([[0, 1, 2], [10, 11, 12]]).T, frame_A=world_frame).T self.assertTupleEqual(p_AQi.shape, (2, 3)) Jv_WL = plant.CalcFrameGeometricJacobianExpressedInWorld( context=context, frame_B=base_frame, p_BoFo_B=[0, 0, 0]) self.assertTupleEqual(Jv_WL.shape, (6, plant.num_velocities())) nq = plant.num_positions() nv = plant.num_velocities() wrt_list = [ (JacobianWrtVariable.kQDot, nq), (JacobianWrtVariable.kV, nv), ] for wrt, nw in wrt_list: Jw_ABp_E = plant.CalcJacobianSpatialVelocity(context=context, with_respect_to=wrt, frame_B=base_frame, p_BP=np.zeros(3), frame_A=world_frame, frame_E=world_frame) self.assert_sane(Jw_ABp_E) self.assertEqual(Jw_ABp_E.shape, (6, nw)) # Compute body pose. X_WBase = plant.EvalBodyPoseInWorld(context, base) self.assertIsInstance(X_WBase, Isometry3) # Set pose for the base. X_WB_desired = Isometry3.Identity() X_WB = plant.CalcRelativeTransform(context, world_frame, base_frame) plant.SetFreeBodyPose(context=context, body=base, X_WB=X_WB_desired) self.assertTrue(np.allclose(X_WB.matrix(), X_WB_desired.matrix())) # Set a spatial velocity for the base. v_WB = SpatialVelocity(w=[1, 2, 3], v=[4, 5, 6]) plant.SetFreeBodySpatialVelocity(context=context, body=base, V_WB=v_WB) v_base = plant.EvalBodySpatialVelocityInWorld(context, base) self.assertTrue(np.allclose(v_base.rotational(), v_WB.rotational())) self.assertTrue( np.allclose(v_base.translational(), v_WB.translational())) # Compute accelerations. vdot = np.zeros(nv) A_WB_array = plant.CalcSpatialAccelerationsFromVdot(context=context, known_vdot=vdot) self.assertEqual(len(A_WB_array), plant.num_bodies())
def test_multibody_tree_kinematics(self): file_name = FindResourceOrThrow( "drake/examples/double_pendulum/models/double_pendulum.sdf") plant = MultibodyPlant() Parser(plant).AddModelFromFile(file_name) plant.Finalize() context = plant.CreateDefaultContext() world_frame = plant.world_frame() base = plant.GetBodyByName("base") base_frame = plant.GetFrameByName("base") X_WL = plant.CalcRelativeTransform( context, frame_A=world_frame, frame_B=base_frame) self.assertIsInstance(X_WL, Isometry3) p_AQi = plant.CalcPointsPositions( context=context, frame_B=base_frame, p_BQi=np.array([[0, 1, 2], [10, 11, 12]]).T, frame_A=world_frame).T self.assertTupleEqual(p_AQi.shape, (2, 3)) Jv_WL = plant.CalcFrameGeometricJacobianExpressedInWorld( context=context, frame_B=base_frame, p_BoFo_B=[0, 0, 0]) self.assertTupleEqual(Jv_WL.shape, (6, plant.num_velocities())) nq = plant.num_positions() nv = plant.num_velocities() wrt_list = [ (JacobianWrtVariable.kQDot, nq), (JacobianWrtVariable.kV, nv), ] for wrt, nw in wrt_list: Jw_ABp_E = plant.CalcJacobianSpatialVelocity( context=context, with_respect_to=wrt, frame_B=base_frame, p_BP=np.zeros(3), frame_A=world_frame, frame_E=world_frame) self.assert_sane(Jw_ABp_E) self.assertEqual(Jw_ABp_E.shape, (6, nw)) # Compute body pose. X_WBase = plant.EvalBodyPoseInWorld(context, base) self.assertIsInstance(X_WBase, Isometry3) # Set pose for the base. X_WB_desired = Isometry3.Identity() X_WB = plant.CalcRelativeTransform(context, world_frame, base_frame) plant.SetFreeBodyPose( context=context, body=base, X_WB=X_WB_desired) self.assertTrue(np.allclose(X_WB.matrix(), X_WB_desired.matrix())) # Set a spatial velocity for the base. v_WB = SpatialVelocity(w=[1, 2, 3], v=[4, 5, 6]) plant.SetFreeBodySpatialVelocity( context=context, body=base, V_WB=v_WB) v_base = plant.EvalBodySpatialVelocityInWorld(context, base) self.assertTrue(np.allclose(v_base.rotational(), v_WB.rotational())) self.assertTrue(np.allclose(v_base.translational(), v_WB.translational())) # Compute accelerations. vdot = np.zeros(nv) A_WB_array = plant.CalcSpatialAccelerationsFromVdot( context=context, known_vdot=vdot) self.assertEqual(len(A_WB_array), plant.num_bodies())
def test_multibody_tree_kinematics(self): file_name = FindResourceOrThrow( "drake/examples/double_pendulum/models/double_pendulum.sdf") plant = MultibodyPlant() AddModelFromSdfFile(file_name, plant) plant.Finalize() context = plant.CreateDefaultContext() tree = plant.tree() world_frame = plant.world_frame() base = plant.GetBodyByName("base") base_frame = plant.GetFrameByName("base") X_WL = tree.CalcRelativeTransform( context, frame_A=world_frame, frame_B=base_frame) self.assertIsInstance(X_WL, Isometry3) p_AQi = tree.CalcPointsPositions( context=context, frame_B=base_frame, p_BQi=np.array([[0, 1, 2], [10, 11, 12]]).T, frame_A=world_frame).T self.assertTupleEqual(p_AQi.shape, (2, 3)) Jv_WL = tree.CalcFrameGeometricJacobianExpressedInWorld( context=context, frame_B=base_frame, p_BoFo_B=[0, 0, 0]) self.assertTupleEqual(Jv_WL.shape, (6, plant.num_velocities())) # Compute body pose. X_WBase = tree.EvalBodyPoseInWorld(context, base) self.assertIsInstance(X_WBase, Isometry3) # All body poses. X_WB_list = tree.CalcAllBodyPosesInWorld(context) self.assertEqual(len(X_WB_list), 4) for X_WB in X_WB_list: self.assertIsInstance(X_WB, Isometry3) # Compute body velocities. v_WB_list = tree.CalcAllBodySpatialVelocitiesInWorld(context) self.assertEqual(len(v_WB_list), 4) for v_WB in v_WB_list: self.assertIsInstance(v_WB, SpatialVelocity) # - Sanity check. v_WBase_flat = np.hstack(( v_WB_list[0].rotational(), v_WB_list[0].translational())) self.assertTrue(np.allclose(v_WBase_flat, np.zeros(6))) # Set pose for the base. X_WB_desired = Isometry3.Identity() X_WB = tree.CalcRelativeTransform(context, world_frame, base_frame) tree.SetFreeBodyPoseOrThrow( body=base, X_WB=X_WB_desired, context=context) self.assertTrue(np.allclose(X_WB.matrix(), X_WB_desired.matrix())) # Set a spatial velocity for the base. v_WB = SpatialVelocity(w=[1, 2, 3], v=[4, 5, 6]) tree.SetFreeBodySpatialVelocityOrThrow( body=base, V_WB=v_WB, context=context) v_base = tree.EvalBodySpatialVelocityInWorld(context, base) self.assertTrue(np.allclose(v_base.rotational(), v_WB.rotational())) self.assertTrue(np.allclose(v_base.translational(), v_WB.translational()))
def test_multibody_tree_kinematics(self): file_name = FindResourceOrThrow( "drake/examples/double_pendulum/models/double_pendulum.sdf") plant = MultibodyPlant() AddModelFromSdfFile(file_name, plant) plant.Finalize() context = plant.CreateDefaultContext() tree = plant.tree() world_frame = plant.world_frame() base = plant.GetBodyByName("base") base_frame = plant.GetFrameByName("base") X_WL = tree.CalcRelativeTransform(context, frame_A=world_frame, frame_B=base_frame) self.assertIsInstance(X_WL, Isometry3) p_AQi = tree.CalcPointsPositions(context=context, frame_B=base_frame, p_BQi=np.array([[0, 1, 2], [10, 11, 12]]).T, frame_A=world_frame).T self.assertTupleEqual(p_AQi.shape, (2, 3)) Jv_WL = tree.CalcFrameGeometricJacobianExpressedInWorld( context=context, frame_B=base_frame, p_BoFo_B=[0, 0, 0]) self.assertTupleEqual(Jv_WL.shape, (6, plant.num_velocities())) # Compute body pose. X_WBase = tree.EvalBodyPoseInWorld(context, base) self.assertIsInstance(X_WBase, Isometry3) # All body poses. X_WB_list = tree.CalcAllBodyPosesInWorld(context) self.assertEqual(len(X_WB_list), 4) for X_WB in X_WB_list: self.assertIsInstance(X_WB, Isometry3) # Compute body velocities. v_WB_list = tree.CalcAllBodySpatialVelocitiesInWorld(context) self.assertEqual(len(v_WB_list), 4) for v_WB in v_WB_list: self.assertIsInstance(v_WB, SpatialVelocity) # - Sanity check. v_WBase_flat = np.hstack( (v_WB_list[0].rotational(), v_WB_list[0].translational())) self.assertTrue(np.allclose(v_WBase_flat, np.zeros(6))) # Set pose for the base. X_WB_desired = Isometry3.Identity() X_WB = tree.CalcRelativeTransform(context, world_frame, base_frame) tree.SetFreeBodyPoseOrThrow(body=base, X_WB=X_WB_desired, context=context) self.assertTrue(np.allclose(X_WB.matrix(), X_WB_desired.matrix())) # Set a spatial velocity for the base. v_WB = SpatialVelocity(w=[1, 2, 3], v=[4, 5, 6]) tree.SetFreeBodySpatialVelocityOrThrow(body=base, V_WB=v_WB, context=context) v_base = tree.EvalBodySpatialVelocityInWorld(context, base) self.assertTrue(np.allclose(v_base.rotational(), v_WB.rotational())) self.assertTrue( np.allclose(v_base.translational(), v_WB.translational()))
def reset(self): if self.reset_sim: print("Resetting") self.build(real_time_rate=self.real_time_rate, is_visualizing=self.is_visualizing) while True: p_WQ_new = np.random.uniform(low=[0.05, -0.1, 0.5], high=[0.5, 0.1, 0.5]) # p_WQ_new = np.array([0.2, 0, 0.5]) passed, q_home_full = GetConfiguration(p_WQ_new) if passed: break q_home_kuka = GetKukaQKnots(q_home_full)[0] # set initial hinge angles of the cupboard. # setting hinge angle to exactly 0 or 90 degrees will result in intermittent contact # with small contact forces between the door and the cupboard body. self.left_hinge_joint.set_angle( context=self.manip_station_sim.station.GetMutableSubsystemContext( self.manip_station_sim.plant, self.context), angle=-np.pi / 2 + 0.001) self.right_hinge_joint.set_angle( context=self.manip_station_sim.station.GetMutableSubsystemContext( self.manip_station_sim.plant, self.context), angle=np.pi / 2 - 0.001) # set initial pose of the object self.manip_station_sim.SetObjectTranslation(self.goal_position) if self.manip_station_sim.object_base_link_name is not None: self.manip_station_sim.tree.SetFreeBodyPoseOrThrow( self.manip_station_sim.plant.GetBodyByName( self.manip_station_sim.object_base_link_name, self.manip_station_sim.object), self.manip_station_sim.X_WObject, self.manip_station_sim.station.GetMutableSubsystemContext( self.manip_station_sim.plant, self.context)) if self.manip_station_sim.object_base_link_name is not None: self.manip_station_sim.tree.SetFreeBodySpatialVelocityOrThrow( self.manip_station_sim.plant.GetBodyByName( self.manip_station_sim.object_base_link_name, self.manip_station_sim.object), SpatialVelocity(np.zeros(3), np.zeros(3)), self.manip_station_sim.station.GetMutableSubsystemContext( self.manip_station_sim.plant, self.context)) # set initial state of the robot self.manip_station_sim.station.SetIiwaPosition(q_home_kuka, self.context) self.manip_station_sim.station.SetIiwaVelocity(np.zeros(7), self.context) self.manip_station_sim.station.SetWsgPosition(0.02, self.context) self.manip_station_sim.station.SetWsgVelocity(0, self.context) self.simulator.Initialize() self._episode_steps = 0 return self._getObservation()