def AddPlanarBinAndSimpleBox(plant, mass=1.0, mu=1.0, width=0.2, depth=0.05, height=0.3): parser = Parser(plant) bin = parser.AddModelFromFile(FindResource("models/planar_bin.sdf")) plant.WeldFrames( plant.world_frame(), plant.GetFrameByName("bin_base", bin), RigidTransform(RotationMatrix.MakeZRotation(np.pi / 2.0), [0, 0, -0.015])) planar_joint_frame = plant.AddFrame( FixedOffsetFrame( "planar_joint_frame", plant.world_frame(), RigidTransform(RotationMatrix.MakeXRotation(np.pi / 2)))) # TODO(russt): make this a *random* box? # TODO(russt): move random box to a shared py file. box_instance = AddShape(plant, Box(width, depth, height), "box", mass, mu) box_joint = plant.AddJoint( PlanarJoint("box", planar_joint_frame, plant.GetFrameByName("box", box_instance))) box_joint.set_position_limits([-.5, -.1, -np.pi], [.5, .3, np.pi]) box_joint.set_velocity_limits([-2, -2, -2], [2, 2, 2]) box_joint.set_default_translation([0, depth / 2.0]) return box_instance
def addMeshEndEffector(plant, scene_graph, sphere_body): # Initialize sphere body partial_sphere_mesh = Mesh("models/partial_sphere.obj") if plant.geometry_source_is_registered(): plant.RegisterCollisionGeometry( sphere_body, RigidTransform(R=RotationMatrix.MakeXRotation(np.pi / 2)), partial_sphere_mesh, "sphere_body", pydrake.multibody.plant.CoulombFriction(1, 1)) plant.RegisterVisualGeometry( sphere_body, RigidTransform(R=RotationMatrix.MakeXRotation(np.pi / 2)), partial_sphere_mesh, "sphere_body", [.9, .5, .5, 1.0]) # Color geometries = plant.CollectRegisteredGeometries( [sphere_body, plant.GetBodyByName("panda_link8")]) scene_graph.collision_filter_manager().Apply( CollisionFilterDeclaration().ExcludeWithin(geometries))
def PendulumExample(): builder = DiagramBuilder() plant = builder.AddSystem(PendulumPlant()) scene_graph = builder.AddSystem(SceneGraph()) PendulumGeometry.AddToBuilder(builder, plant.get_state_output_port(), scene_graph) MeshcatVisualizerCpp.AddToBuilder( builder, scene_graph, meshcat, MeshcatVisualizerParams(publish_period=np.inf)) builder.ExportInput(plant.get_input_port(), "torque") # Note: The Pendulum-v0 in gym outputs [cos(theta), sin(theta), thetadot] builder.ExportOutput(plant.get_state_output_port(), "state") # Add a camera (I have sugar for this in the manip repo) X_WC = RigidTransform(RotationMatrix.MakeXRotation(-np.pi / 2), [0, -1.5, 0]) rgbd = AddRgbdSensor(builder, scene_graph, X_WC) builder.ExportOutput(rgbd.color_image_output_port(), "camera") diagram = builder.Build() simulator = Simulator(diagram) def reward(system, context): plant_context = plant.GetMyContextFromRoot(context) state = plant_context.get_continuous_state_vector() u = plant.get_input_port().Eval(plant_context)[0] theta = state[0] % 2 * np.pi # Wrap to 2*pi theta_dot = state[1] return (theta - np.pi)**2 + 0.1 * theta_dot**2 + 0.001 * u max_torque = 3 env = DrakeGymEnv(simulator, time_step=0.05, action_space=gym.spaces.Box(low=np.array([-max_torque]), high=np.array([max_torque])), observation_space=gym.spaces.Box( low=np.array([-np.inf, -np.inf]), high=np.array([np.inf, np.inf])), reward=reward, render_rgb_port_id="camera") check_env(env) if show: env.reset() image = env.render(mode='rgb_array') fig, ax = plt.subplots() ax.imshow(image) plt.show()
def AddTriad(source_id, frame_id, scene_graph, length=.25, radius=0.01, opacity=1., X_FT=RigidTransform(), name="frame"): """ Adds illustration geometry representing the coordinate frame, with the x-axis drawn in red, the y-axis in green and the z-axis in blue. The axes point in +x, +y and +z directions, respectively. Args: source_id: The source registered with SceneGraph. frame_id: A geometry::frame_id registered with scene_graph. scene_graph: The SceneGraph with which we will register the geometry. length: the length of each axis in meters. radius: the radius of each axis in meters. opacity: the opacity of the coordinate axes, between 0 and 1. X_FT: a RigidTransform from the triad frame T to the frame_id frame F name: the added geometry will have names name + " x-axis", etc. """ # x-axis X_TG = RigidTransform(RotationMatrix.MakeYRotation(np.pi / 2), [length / 2., 0, 0]) geom = GeometryInstance(X_FT.multiply(X_TG), Cylinder(radius, length), name + " x-axis") geom.set_illustration_properties( MakePhongIllustrationProperties([1, 0, 0, opacity])) scene_graph.RegisterGeometry(source_id, frame_id, geom) # y-axis X_TG = RigidTransform(RotationMatrix.MakeXRotation(np.pi / 2), [0, length / 2., 0]) geom = GeometryInstance(X_FT.multiply(X_TG), Cylinder(radius, length), name + " y-axis") geom.set_illustration_properties( MakePhongIllustrationProperties([0, 1, 0, opacity])) scene_graph.RegisterGeometry(source_id, frame_id, geom) # z-axis X_TG = RigidTransform([0, 0, length / 2.]) geom = GeometryInstance(X_FT.multiply(X_TG), Cylinder(radius, length), name + " z-axis") geom.set_illustration_properties( MakePhongIllustrationProperties([0, 0, 1, opacity])) scene_graph.RegisterGeometry(source_id, frame_id, geom)
def make_gripper_frames(X_G, X_O): """ Takes a partial specification with X_G["initial"] and X_O["initial"] and X_0["goal"], and returns a X_G and times with all of the pick and place frames populated. """ # Define (again) the gripper pose relative to the object when in grasp. p_GgraspO = [0, 0.12, 0] R_GgraspO = RotationMatrix.MakeXRotation(np.pi / 2.0).multiply( RotationMatrix.MakeZRotation(np.pi / 2.0)) X_GgraspO = RigidTransform(R_GgraspO, p_GgraspO) X_OGgrasp = X_GgraspO.inverse() # pregrasp is negative y in the gripper frame (see the figure!). X_GgraspGpregrasp = RigidTransform([0, -0.08, 0]) X_G["pick"] = X_O["initial"].multiply(X_OGgrasp) X_G["prepick"] = X_G["pick"].multiply(X_GgraspGpregrasp) X_G["place"] = X_O["goal"].multiply(X_OGgrasp) X_G["preplace"] = X_G["place"].multiply(X_GgraspGpregrasp) # I'll interpolate to a halfway orientation by converting to axis angle and halving the angle X_GprepickGpreplace = X_G["prepick"].inverse().multiply(X_G["preplace"]) angle_axis = X_GprepickGpreplace.rotation().ToAngleAxis() X_GprepickGclearance = RigidTransform( AngleAxis(angle=angle_axis.angle() / 2.0, axis=angle_axis.axis()), X_GprepickGpreplace.translation() / 2.0 + np.array([0, -0.3, 0])) X_G["clearance"] = X_G["prepick"].multiply(X_GprepickGclearance) # Not lets set timings times = {"initial": 0.0} X_GinitialGprepick = X_G["initial"].inverse().multiply(X_G["prepick"]) times["prepick"] = times["initial"] + 10.0 * \ np.linalg.norm(X_GinitialGprepick.translation()) # Allow some time for the gripper to close. times["pick_start"] = times["prepick"] + 2.0 times["pick_end"] = times["pick_start"] + 2.0 times["postpick"] = times["pick_end"] + 2.0 time_to_from_clearance = 10.0 * \ np.linalg.norm(X_GprepickGclearance.translation()) times["clearance"] = times["postpick"] + time_to_from_clearance times["preplace"] = times["clearance"] + time_to_from_clearance times["place_start"] = times["preplace"] + 2.0 times["place_end"] = times["place_start"] + 2.0 times["postplace"] = times["place_end"] + 2.0 return X_G, times
def test_X_WB(self): """Testing X_WB""" f = self.notebook_locals['compute_X_WB'] # construct a test case theta1, theta2, theta3 = np.pi / 3.0, np.pi / 6.0, np.pi / 4.0 R_WA = RotationMatrix.MakeXRotation(theta1) R_AB = RotationMatrix.MakeZRotation(theta2) R_CB = RotationMatrix.MakeYRotation(theta3) X_WA = RigidTransform(R_WA, [0.1, 0.2, 0.5]) X_AB = RigidTransform(R_AB, [0.3, 0.4, 0.1]) X_CB = RigidTransform(R_CB, [0.5, 0.9, 0.7]) test_X_WB = f(X_WA, X_AB, X_CB) true_X_WB = X_WA.multiply(X_AB) test_result = test_X_WB.multiply(true_X_WB.inverse()) test_result = test_result.GetAsMatrix4() self.assertTrue(np.allclose(test_result, np.eye(4)))
def grasp_poses_example(): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0) parser = Parser(plant, scene_graph) grasp = parser.AddModelFromFile( FindResourceOrThrow( "drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_no_tip.sdf" ), "grasp") brick = parser.AddModelFromFile( FindResourceOrThrow( "drake/examples/manipulation_station/models/061_foam_brick.sdf")) plant.Finalize() meshcat = ConnectMeshcatVisualizer(builder, scene_graph) diagram = builder.Build() context = diagram.CreateDefaultContext() plant_context = plant.GetMyContextFromRoot(context) B_O = plant.GetBodyByName("base_link", brick) X_WO = plant.EvalBodyPoseInWorld(plant_context, B_O) B_Ggrasp = plant.GetBodyByName("body", grasp) p_GgraspO = [0, 0.12, 0] R_GgraspO = RotationMatrix.MakeXRotation(np.pi / 2.0).multiply( RotationMatrix.MakeZRotation(np.pi / 2.0)) X_GgraspO = RigidTransform(R_GgraspO, p_GgraspO) X_OGgrasp = X_GgraspO.inverse() X_WGgrasp = X_WO.multiply(X_OGgrasp) plant.SetFreBodyPose(plant_context, B_Ggrasp, X_WGgrasp) # Open the fingers, too plant.GetJointByName("left_finger_sliding_joint", grasp).set_translation(plant_context, -0.054) plant.GetJointByName("right_finger_sliding_joint", grasp).set_translation(plant_context, 0.054) meshcat.load() diagram.Publish(context)
def addArm(plant, scene_graph=None): """ Creates the panda arm. """ parser = pydrake.multibody.parsing.Parser(plant, scene_graph) arm_instance = parser.AddModelFromFile("models/panda_arm.urdf") panda_offset = 0 if config.num_links == config.NumLinks.TWO: panda_offset = IN_TO_M * 22 elif config.num_links == config.NumLinks.FOUR: panda_offset = IN_TO_M * 40 # Weld panda to world plant.WeldFrames( plant.world_frame(), plant.GetFrameByName("panda_link0", arm_instance), RigidTransform(RotationMatrix().MakeZRotation(np.pi), [0, panda_offset, 0])) sphere_body = plant.AddRigidBody( "sphere_body", arm_instance, pydrake.multibody.tree.SpatialInertia( mass=MASS, p_PScm_E=np.array([0., 0., 0.]), G_SP_E=pydrake.multibody.tree.UnitInertia(1.0, 1.0, 1.0))) if USE_MESH: addMeshEndEffector(plant, scene_graph, sphere_body) else: addPrimitivesEndEffector(plant, scene_graph, sphere_body, arm_instance) X_P_S = RigidTransform( RotationMatrix.MakeZRotation(np.pi / 4).multiply( RotationMatrix.MakeXRotation(-np.pi / 2)), [0, 0, 0.065]) # Roughly aligns axes with world axes plant.WeldFrames(plant.GetFrameByName("panda_link8", arm_instance), plant.GetFrameByName("sphere_body", arm_instance), X_P_S) return arm_instance
def plan_throw( T_world_robotInitial, T_world_gripperObject, p_world_target, # np.array of shape (3,) gripper_to_object_dist, throw_height=0.5, # meters prethrow_height=0.2, prethrow_radius=0.4, throw_angle=np.pi / 4.0, meshcat=None, throw_speed_adjustment_factor=1.0, ): """ only works with the "back portion" of the clutter station until we figure out how to move the bins around motion moves along an arc from a "pre throw" to a "throw" position """ theta = 1.0 * np.arctan2(p_world_target[1], p_world_target[0]) print(f"theta={theta}") T_world_prethrow = RigidTransform( p=np.array([ prethrow_radius * np.cos(theta), prethrow_radius * np.sin(theta), prethrow_height ]), R=RotationMatrix.MakeXRotation(-np.pi / 2).multiply( RotationMatrix.MakeYRotation((theta - np.pi / 2)))) throw_radius = throw_height - prethrow_height T_world_throw = RigidTransform( p=T_world_prethrow.translation() + np.array([ throw_radius * np.cos(theta), throw_radius * np.sin(theta), throw_height - prethrow_height ]), R=RotationMatrix.MakeXRotation(-np.pi / 2).multiply( RotationMatrix.MakeYRotation((theta - np.pi / 2)).multiply( RotationMatrix.MakeXRotation(-np.pi / 2)))) if meshcat: visualize_transform(meshcat, "T_world_prethrow", T_world_prethrow) visualize_transform(meshcat, "T_world_throw", T_world_throw) p_world_object_at_launch = interpolatePosesArcMotion( T_world_prethrow, T_world_throw, t=throw_angle / (np.pi / 2.)).translation() + np.array([0, 0, -gripper_to_object_dist]) pdot_world_launch = interpolatePosesArcMotion_pdot(T_world_prethrow, T_world_throw, t=throw_angle / (np.pi / 2.)) launch_speed_base = np.linalg.norm(pdot_world_launch) launch_speed_required = get_launch_speed_required( theta=throw_angle, x=np.linalg.norm(p_world_target[:2]) - np.linalg.norm(p_world_object_at_launch[:2]), y=p_world_target[2] - p_world_object_at_launch[2]) total_throw_time = launch_speed_base / launch_speed_required / throw_speed_adjustment_factor print(f"p_world_object_at_launch={p_world_object_at_launch}") print(f"target={p_world_target}") print( f"dx={np.linalg.norm(p_world_target[:2]) - np.linalg.norm(p_world_object_at_launch[:2])}" ) print(f"dy={p_world_target[2] - p_world_object_at_launch[2]}") print(f"pdot_world_launch={pdot_world_launch}") print(f"total_throw_time={total_throw_time}") T_world_hackyWayPoint = RigidTransform( p=[-.6, -0.0, 0.6], R=RotationMatrix.MakeXRotation( -np.pi / 2.0 ), #R_WORLD_PRETHROW, #RotationMatrix.MakeXRotation(-np.pi/2.0), ) # event timings (not all are relevant to pose and gripper) # initial pose => prethrow => throw => yays t_goToObj = 1.0 t_holdObj = 0.5 t_goToPreobj = 1.0 t_goToWaypoint = 1.0 t_goToPrethrow = 1.0 t_goToRelease = total_throw_time * throw_angle / (np.pi / 2.) t_goToThrowEnd = total_throw_time * (1 - throw_angle / (np.pi / 2.)) t_throwEndHold = 3.0 ts = np.array([ t_goToObj, t_holdObj, t_goToPreobj, t_goToWaypoint, t_goToPrethrow, t_goToRelease, t_goToThrowEnd, t_throwEndHold ]) cum_pose_ts = np.cumsum(ts) print(cum_pose_ts) # Create pose trajectory t_lst = np.linspace(0, cum_pose_ts[-1], 1000) pose_lst = [] for t in t_lst: if t < cum_pose_ts[1]: pose = interpolatePosesLinear(T_world_robotInitial, T_world_gripperObject, min(t / ts[0], 1.0)) elif t < cum_pose_ts[2]: pose = interpolatePosesLinear(T_world_gripperObject, T_world_robotInitial, (t - cum_pose_ts[1]) / ts[2]) elif t < cum_pose_ts[3]: pose = interpolatePosesLinear(T_world_robotInitial, T_world_hackyWayPoint, (t - cum_pose_ts[2]) / ts[3]) elif t <= cum_pose_ts[4]: pose = interpolatePosesLinear(T_world_hackyWayPoint, T_world_prethrow, (t - cum_pose_ts[3]) / ts[4]) else: pose = interpolatePosesArcMotion( T_world_prethrow, T_world_throw, min((t - cum_pose_ts[4]) / (ts[5] + ts[6]), 1.0)) pose_lst.append(pose) # Create gripper trajectory. gripper_times_lst = np.array([ 0., t_goToObj, t_holdObj, t_goToPreobj, t_goToWaypoint, t_goToPrethrow, t_goToRelease, t_goToThrowEnd, t_throwEndHold ]) gripper_cumulative_times_lst = np.cumsum(gripper_times_lst) GRIPPER_OPEN = 0.5 GRIPPER_CLOSED = 0.0 gripper_knots = np.array([ GRIPPER_OPEN, GRIPPER_OPEN, GRIPPER_CLOSED, GRIPPER_CLOSED, GRIPPER_CLOSED, GRIPPER_CLOSED, GRIPPER_CLOSED, GRIPPER_OPEN, GRIPPER_CLOSED ]).reshape(1, gripper_times_lst.shape[0]) g_traj = PiecewisePolynomial.FirstOrderHold(gripper_cumulative_times_lst, gripper_knots) return t_lst, pose_lst, g_traj
def plan_prethrow_pose( T_world_robotInitial, p_world_target, # np.array of shape (3,) gripper_to_object_dist, throw_height=0.5, # meters prethrow_height=0.2, prethrow_radius=0.4, throw_angle=np.pi / 4.0, meshcat=None, throw_speed_adjustment_factor=1.0, ): """ only works with the "back portion" of the clutter station until we figure out how to move the bins around motion moves along an arc from a "pre throw" to a "throw" position """ theta = 1.0 * np.arctan2(p_world_target[1], p_world_target[0]) print(f"theta={theta}") T_world_prethrow = RigidTransform( p=np.array([ prethrow_radius * np.cos(theta), prethrow_radius * np.sin(theta), prethrow_height ]), R=RotationMatrix.MakeXRotation(-np.pi / 2).multiply( RotationMatrix.MakeYRotation((theta - np.pi / 2)))) throw_radius = throw_height - prethrow_height T_world_throw = RigidTransform( p=T_world_prethrow.translation() + np.array([ throw_radius * np.cos(theta), throw_radius * np.sin(theta), throw_height - prethrow_height ]), R=RotationMatrix.MakeXRotation(-np.pi / 2).multiply( RotationMatrix.MakeYRotation((theta - np.pi / 2)).multiply( RotationMatrix.MakeXRotation(-np.pi / 2)))) if meshcat: visualize_transform(meshcat, "T_world_prethrow", T_world_prethrow) visualize_transform(meshcat, "T_world_throw", T_world_throw) p_world_object_at_launch = interpolatePosesArcMotion( T_world_prethrow, T_world_throw, t=throw_angle / (np.pi / 2.)).translation() + np.array([0, 0, -gripper_to_object_dist]) pdot_world_launch = interpolatePosesArcMotion_pdot(T_world_prethrow, T_world_throw, t=throw_angle / (np.pi / 2.)) launch_speed_base = np.linalg.norm(pdot_world_launch) launch_speed_required = get_launch_speed_required( theta=throw_angle, x=np.linalg.norm(p_world_target[:2]) - np.linalg.norm(p_world_object_at_launch[:2]), y=p_world_target[2] - p_world_object_at_launch[2]) total_throw_time = launch_speed_base / launch_speed_required / throw_speed_adjustment_factor # print(f"p_world_object_at_launch={p_world_object_at_launch}") # print(f"target={p_world_target}") # print(f"dx={np.linalg.norm(p_world_target[:2]) - np.linalg.norm(p_world_object_at_launch[:2])}") # print(f"dy={p_world_target[2] - p_world_object_at_launch[2]}") # print(f"pdot_world_launch={pdot_world_launch}") # print(f"total_throw_time={total_throw_time}") return T_world_prethrow, T_world_throw
def run_benchmark( zmq_url=None, P_WORLD_TARGET=np.array([-1, 1, 0]), MAX_APPROACH_ANGLE=-45 / 180.0 * np.pi, OBJECT_TO_TOSS="ball", GRIPPER_TO_OBJECT_COM_DIST=0.11, LAUNCH_ANGLE_THRESH=3 / 180.0 * np.pi, # 3 seems to work well? verbose=False, **kwargs, ): # Initialize global plants ONCE for IK calculations # This speed up exectuation greatly. GLOBAL_PLANT, GLOBAL_CONTEXT = get_basic_manip_station() """ https://schunk.com/fileadmin/pim/docs/IM0026091.PDF - gripper frame to tip is about .115 m for reference GRIPPER_TO_OBJECT_FRAME_DIST = 0.12 # meters, this is how much "above" the balls origin we must send the gripper body frame in order to successfully grasp the object OBJECT_FRAME_TO_COM_DIST = 0.05 / 2 """ T_world_target = RigidTransform(RotationMatrix(), P_WORLD_TARGET) T_world_objectInitial = RigidTransform( #p=[-.1, -.69, 1.04998503e-01], # sphere p=[-0.1, -0.69, 0.09], # foam_brick R=RotationMatrix.MakeZRotation(np.pi / 2.0)) T_world_gripperObject = RigidTransform( p=T_world_objectInitial.translation() + np.array([0, 0, 0.025 + GRIPPER_TO_OBJECT_COM_DIST]), R=RotationMatrix.MakeXRotation(-np.pi / 2.0)) T_world_objectCOM = RigidTransform( T_world_objectInitial.rotation(), T_world_objectInitial.translation() + np.array([0, 0, 0.025])) # Set starting and ending joint angles for throw throw_heading = np.arctan2(P_WORLD_TARGET[1], P_WORLD_TARGET[0]) ja1 = throw_heading - np.pi # TODO: edit these to work better for large angles. PRETHROW_JA = np.array([ja1, 0, 0, 1.9, 0, -1.9, 0, 0, 0]) THROWEND_JA = np.array([ja1, 0, 0, 0.4, 0, -0.4, 0, 0, 0]) T_world_prethrowPose = jointangles_to_pose( plant=GLOBAL_PLANT, context=GLOBAL_CONTEXT, jointangles=PRETHROW_JA[:7], ) T_world_robotInitial, meshcat = setup_manipulation_station( T_world_objectInitial, zmq_url, T_world_target, manipuland=OBJECT_TO_TOSS) #object frame viz if meshcat: visualize_transform(meshcat, "T_world_obj0", T_world_objectInitial) visualize_transform(meshcat, "T_world_objectCOM", T_world_objectCOM) visualize_transform(meshcat, "T_world_gripperObject", T_world_gripperObject) T_world_target = RigidTransform(p=P_WORLD_TARGET, R=RotationMatrix()) visualize_transform(meshcat, "target", T_world_target) def throw_objective(inp, g=9.81, alpha=1, return_other=None): throw_motion_time, release_frac = inp release_ja = PRETHROW_JA + release_frac * (THROWEND_JA - PRETHROW_JA) T_world_releasePose = jointangles_to_pose(plant=GLOBAL_PLANT, context=GLOBAL_CONTEXT, jointangles=release_ja[:7]) p_release = (T_world_releasePose.translation() + T_world_releasePose.rotation().multiply( [0, GRIPPER_TO_OBJECT_COM_DIST, 0])) J_release = spatial_velocity_jacobian_at_jointangles( plant=GLOBAL_PLANT, context=GLOBAL_CONTEXT, jointangles=release_ja[:7], gripper_to_object_dist=GRIPPER_TO_OBJECT_COM_DIST # <==== important )[3:6] v_release = J_release @ ( (THROWEND_JA - PRETHROW_JA) / throw_motion_time)[:7] if v_release[:2] @ (P_WORLD_TARGET - p_release)[:2] <= 0: return 1000 x = np.linalg.norm((P_WORLD_TARGET - p_release)[:2]) y = (P_WORLD_TARGET - p_release)[2] vx = np.linalg.norm(v_release[:2]) vy = v_release[2] tta = x / vx y_hat = vy * tta - 0.5 * g * tta**2 phi_hat = np.arctan((vy - g * tta) / vx) objective = ((y_hat - y)**2 + np.maximum(phi_hat - MAX_APPROACH_ANGLE, 0)**2 #+ (phi_hat - MAX_APPROACH_ANGLE) ** 2 ) if objective < 1e-6: # if we hit target at correct angle # then try to move as slow as possible objective -= throw_motion_time**2 if return_other == "launch": return np.arctan2(vy, vx) elif return_other == "land": return phi_hat elif return_other == "tta": return tta else: return objective res = scipy.optimize.differential_evolution(throw_objective, bounds=[(1e-3, 3), (0.1, 0.9)], seed=43) throw_motion_time, release_frac = res.x assert throw_motion_time > 0 assert 0 < release_frac < 1 plan_launch_angle = throw_objective(res.x, return_other="launch") plan_land_angle = throw_objective(res.x, return_other="land") tta = throw_objective(res.x, return_other="tta") if verbose: print(f"Throw motion will take: {throw_motion_time:.4f} seconds") print(f"Releasing at {release_frac:.4f} along the motion") print(f"Launch angle (degrees): {plan_launch_angle / np.pi * 180.0}") print(f"Plan land angle (degrees): {plan_land_angle / np.pi * 180.0}") print(f"time to arrival: {tta}") ''' timings ''' t_goToObj = 1.0 t_holdObj = 2.0 t_goToPreobj = 1.0 t_goToWaypoint = 2.0 t_goToPrethrow = 4.0 # must be greater than 1.0 for a 1 second hold to stabilize t_goToThrowEnd = throw_motion_time # plan pickup t_lst, q_knots, total_time = plan_pickup(T_world_robotInitial, T_world_gripperObject, t_goToObj=t_goToObj, t_holdObj=t_holdObj, t_goToPreobj=t_goToPreobj) # clear the bins via a waypoint T_world_hackyWayPoint = RigidTransform( p=[-.6, -0.0, 0.6], R=RotationMatrix.MakeXRotation( -np.pi / 2.0 ), #R_WORLD_PRETHROW, #RotationMatrix.MakeXRotation(-np.pi/2.0), ) t_lst, q_knots = add_go_to_pose_via_jointinterpolation( T_world_robotInitial, T_world_hackyWayPoint, t_start=total_time, t_lst=t_lst, q_knots=q_knots, time_interval_s=t_goToWaypoint) # go to prethrow t_lst, q_knots = add_go_to_ja_via_jointinterpolation( pose_to_jointangles(T_world_hackyWayPoint), PRETHROW_JA, t_start=total_time + t_goToWaypoint, t_lst=t_lst, q_knots=q_knots, time_interval_s=t_goToPrethrow, hold_time_s=1.0, ) # go to throw t_lst, q_knots = add_go_to_ja_via_jointinterpolation( PRETHROW_JA, THROWEND_JA, t_start=total_time + t_goToWaypoint + t_goToPrethrow, t_lst=t_lst, q_knots=q_knots, time_interval_s=t_goToThrowEnd, num_samples=30, include_end=True) # turn trajectory into joint space q_knots = np.array(q_knots) q_traj = PiecewisePolynomial.CubicShapePreserving(t_lst, q_knots[:, 0:7].T) ''' Gripper reference trajectory (not used, we use a closed loop controller instead) ''' # make gripper trajectory assert t_holdObj > 1.5 gripper_times_lst = np.array([ 0., t_goToObj, 1.0, # pickup object 0.25, # pickup object t_holdObj - 1.25, # pickup object t_goToPreobj, t_goToWaypoint, t_goToPrethrow, release_frac * t_goToThrowEnd, 1e-9, (1 - release_frac) * t_goToThrowEnd, ]) gripper_cumulative_times_lst = np.cumsum(gripper_times_lst) GRIPPER_OPEN = 0.5 GRIPPER_CLOSED = 0.0 gripper_knots = np.array([ GRIPPER_OPEN, GRIPPER_OPEN, GRIPPER_OPEN, # pickup object GRIPPER_CLOSED, # pickup object GRIPPER_CLOSED, # pickup object GRIPPER_CLOSED, GRIPPER_CLOSED, GRIPPER_CLOSED, GRIPPER_CLOSED, GRIPPER_OPEN, GRIPPER_OPEN, ]).reshape(1, gripper_times_lst.shape[0]) g_traj = PiecewisePolynomial.FirstOrderHold(gripper_cumulative_times_lst, gripper_knots) get_gripper_controller_3 = lambda station_plant: GripperControllerUsingIiwaStateV3( plant=station_plant, gripper_to_object_dist=GRIPPER_TO_OBJECT_COM_DIST, T_world_objectPickup=T_world_gripperObject, T_world_prethrow=T_world_prethrowPose, planned_launch_angle=plan_launch_angle, launch_angle_thresh=LAUNCH_ANGLE_THRESH, dbg_state_prints=verbose) # do the thing simulator, _, meshcat, loggers = BuildAndSimulateTrajectory( q_traj=q_traj, g_traj=g_traj, get_gripper_controller=get_gripper_controller_3, T_world_objectInitial= T_world_objectInitial, # where to init the object in the world T_world_targetBin= T_world_target, # where the ball should hit - aka where the bin will catch it zmq_url=zmq_url, time_step= 1e-3, # target (-6, 6, -1). 1e-3 => overshoot barely, 1e-4 => undershoot barely, look around 7.92-7.94 s in sim include_target_bin=False, manipuland=OBJECT_TO_TOSS) fly_time = max(tta + 1, 2) if verbose: print( f"Throw motion should happen from 9.5 seconds to {10 + throw_motion_time} seconds" ) print(f"Running for {q_traj.end_time() + fly_time} seconds") if meshcat: meshcat.start_recording() simulator.AdvanceTo(q_traj.end_time() + fly_time) if meshcat: meshcat.stop_recording() meshcat.publish_recording() all_log_times = loggers["state"].sample_times() chosen_idxs = (9 < all_log_times) & (all_log_times < 100) log_times = loggers["state"].sample_times()[chosen_idxs] log_states = loggers["state"].data()[:, chosen_idxs] p_objects = np.zeros((len(log_times), 3)) p_objects[:, 0] = log_states[4] p_objects[:, 1] = log_states[5] p_objects[:, 2] = log_states[6] deltas = p_objects - P_WORLD_TARGET land_idx = np.where(deltas[:, 2] > 0)[0][-1] p_land = p_objects[land_idx] is_overthrow = np.linalg.norm(p_land[:2]) > np.linalg.norm( P_WORLD_TARGET[:2]) delta_land = deltas[land_idx] land_pos_error = np.linalg.norm(delta_land) * (1 if is_overthrow else -1) aim_angle_error = (np.arctan2(p_land[1], p_land[0]) - np.arctan2(P_WORLD_TARGET[1], P_WORLD_TARGET[0])) v_land = ((p_objects[land_idx] - p_objects[land_idx - 1]) / (log_times[land_idx] - log_times[land_idx - 1])) sim_land_angle = np.arctan(v_land[2] / np.linalg.norm(v_land[:2])) land_angle_error = sim_land_angle - plan_land_angle ret_dict = dict( land_pos_error=land_pos_error, land_angle_error=land_angle_error, aim_angle_error=aim_angle_error, time_to_arrival=tta, land_time=log_times[land_idx], land_x=p_land[0], land_y=p_land[1], land_z=p_land[2], sim_land_angle=sim_land_angle, plan_land_angle=plan_land_angle, plan_launch_angle=plan_launch_angle, release_frac=release_frac, throw_motion_time=throw_motion_time, ) return ret_dict
times["pick_end"] = times["pick_start"] + 2.0 times["postpick"] = times["pick_end"] + 2.0 time_to_from_clearance = 10.0 * \ np.linalg.norm(X_GprepickGclearance.translation()) times["clearance"] = times["postpick"] + time_to_from_clearance times["preplace"] = times["clearance"] + time_to_from_clearance times["place_start"] = times["preplace"] + 2.0 times["place_end"] = times["place_start"] + 2.0 times["postplace"] = times["place_end"] + 2.0 return X_G, times X_G = { "initial": RigidTransform(RotationMatrix.MakeXRotation(-np.pi / 2.0), [0, -0.25, 0.25]) } X_O = { "initial": RigidTransform(RotationMatrix.MakeZRotation(np.pi / 2.0), [-.2, -.75, 0.025]), "goal": RigidTransform(RotationMatrix.MakeZRotation(np.pi), [.75, 0, 0.025]) } X_G, times = make_gripper_frames(X_G, X_O) print( f"Sanity check: The entire maneuver will take {times['postplace']} seconds to execute." )