def calculate_place_joint_values(poses: List[Pose], pre_place_jvs: List[JointValues], move_group: MoveGroup, world_view_client: WorldViewClient) \ -> List[JointValues]: """ Calculates the place joint values Since we want the robot to move from pre place to place pose, we use the corresponding pre place joint values as seed for the place joint values. Parameters ---------- poses : List[Pose] A list of poses for which the joint values should be calculated pre_place_jvs : List[JointValues] The seeds for every pose move_group : MoveGroup world_view_client: WorldViewClient Returns ------ List[JointValues] A list of joint values for every pose """ end_effector = move_group.get_end_effector() place_jvs = [] for i in range(len(pre_place_jvs)): # The i-th pre place joint values are used for the i-th position as seed joint_values = end_effector.inverse_kinematics(poses[i], collision_check=True, seed=pre_place_jvs[i]) place_jvs.append(joint_values) return place_jvs
def main(): ioloop = asyncio.get_event_loop() register_asyncio_shutdown_handler(ioloop) # create instance of movegroup to provide motion services move_group = MoveGroup() try: print('---------- wsg gripper -------------') # create instance of wsg gripper by name properties = WeissWsgGripperProperties('wsg50') wsg_gripper = WeissWsgGripper(properties, move_group.motion_service) print('homeing') ioloop.run_until_complete(wsg_gripper.homing()) print('move gripper') ioloop.run_until_complete(wsg_gripper.move(0.1, 0.05, 0.05, True)) print('perform grasp') result = ioloop.run_until_complete(wsg_gripper.grasp(0.02, 0.1, 0.05)) print('---------- common gripper -------------') # create instance of common gripper by name and driver rosnode name properties1 = CommonGripperProperties('wsg50', 'xamla/wsg_driver') gripper = CommonGripper(properties1, move_group.motion_service) print('move') result1 = ioloop.run_until_complete(gripper.move(0.1, 0.005)) finally: ioloop.close()
def main(): # create move group instance move_group = MoveGroup() # get default endeffector of the movegroup end_effector = move_group.get_end_effector() # create cartesian path and equivalent joint path t1 = [0.502522, 0.2580, 0.3670] q1 = Quaternion(w=0.304389, x=0.5272, y=0.68704, z=0.39666) t2 = [0.23795, 0.46845, 0.44505] q2 = Quaternion(w=0.212097, x=0.470916, y=0.720915, z=0.462096) t3 = [0.6089578, 0.3406782, 0.208865] q3 = Quaternion(w=0.231852, x=0.33222, y=0.746109, z=0.528387) pose_1 = Pose(t1, q1) pose_2 = Pose(t2, q2) pose_3 = Pose(t3, q3) cartesian_path = CartesianPath([pose_1, pose_2, pose_3]) plan = end_effector.move_cartesian(cartesian_path, collision_check=True).plan() stepped_client = plan.execute_supervised() robot_chat = RobotChatClient() robot_chat_stepped_motion = RobotChatSteppedMotion(robot_chat, stepped_client, move_group.name) loop = asyncio.get_event_loop() register_asyncio_shutdown_handler(loop) try: loop.run_until_complete( robot_chat_stepped_motion.handle_stepwise_motions()) finally: loop.close()
def calculate_pre_place_joint_values(pre_place_poses: List[Pose], jv_home: JointValues, move_group: MoveGroup, world_view_client: WorldViewClient, world_view_folder: str) \ -> List[JointValues]: """ Calculates the pre place joint values Since we want the robot arm to go back and forth from the home configuration to the poses on the grid, we use the home joint value as a const seed for every pose to minimize the distance for the joints to make. Calling inverse_kinematics_many with "const_seed = True" lets us exclusively using the jv_home joint values as seed. "const_seed = False" would use the previously calculated joint values as seed for the next on when traversing pre_place_poses. This could be useful when we went from pose to pose without visiting the jv_home configuration in between. Parameters ---------- pre_place_poses : List[Pose] A list of poses for which the joint values should be calculated jv_home : JointValues The seed to be used move_group : MoveGroup world_view_client: WorldViewClient Returns ------ List[JointValues] A list of joint values for every pose """ end_effector = move_group.get_end_effector() try: pre_place_jvs = example_02_3_create_joint_values_from_poses.main( pre_place_poses, jv_home, end_effector) except Exception as e: print("The inverse kinematics operation could not be applied on all the positions.") world_view_client.remove_element("generated", world_view_folder) raise e # export every calculated joint values to world view to illustrate the result for i in range(len(pre_place_jvs)): world_view_client.add_joint_values("joint_values_{}".format(str(i).zfill(2)), "/{}/generated/pre_place_joint_values".format(world_view_folder), pre_place_jvs[i]) return pre_place_jvs
def find_shifted_joint_values(joint_values: JointValues, diff_pose: Pose, move_group: MoveGroup) -> JointValues: """ Find joint values for the shifted pose of the joint_values end effector, if possible. This function takes a translation vector and joint values and tries to find corresponding joint values shifted by "diff_pose" in Cartesian Space This is done by getting the current pose of the end effector joint values, applying the translation and rotation of "diff_pose", and then applying inverse kinematics to generate the joint values anew at the proposed poses. Parameters ---------- joint_values: JointValues The joint values to be shifted diff_pose: Pose The pose which defines translation an rotation move_group: MoveGroup Returns ---------- JointValues The shifted jointValues Exceptions ---------- ServiceException Raised when the inverse kinematics operation was not successfull """ # Get the pose of the end effetcor end_effector = move_group.get_end_effector() old_pose = end_effector.compute_pose(joint_values) # translate and rotate newPose = old_pose.translate(diff_pose.translation).rotate(diff_pose.quaternion) # calculate joint values at new pose, which might throw an exception if not # possible shifted_joint_values = end_effector.inverse_kinematics(newPose, seed=joint_values, collision_check = True) return shifted_joint_values
def main(poses: List[Pose], pre_place_jvs: List[JointValues], home: JointValues, move_group : MoveGroup) : """ This function does a pick and place motion to every pose Parameters ---------- poses : List[Pose] Defines the place poses to be addressed pre_place_jvs : List[JointValues] Defines the joint values of the pre place positions home : JointValues The joint values, to which the robot returns after place move_group : MoveGroup """ loop = asyncio.get_event_loop() # Register the loop handler to be shutdown appropriately register_asyncio_shutdown_handler(loop) async def place(jv_pre_place: JointValues, place: Pose) -> None: """ This asynchronous function lets the robot move from home to every place position and back in a "pick and place" manner The movement from pre place JointValues to place Pose and back is done by moving the end effector in a linear fashion in cartesian space. The movement is planned an executed on the fly. Parameters ---------- jv_pre_place : JointValues The pre place configuration jv_place : JointValues The place configuration """ end_effector = move_group.get_end_effector() # Creates a joint path over the joint values to the target pose joint_path = JointPath(home.joint_set, [home, jv_pre_place ]) # Move over the joints to target pose await move_group.move_joints_collision_free(joint_path) pre_place_pose = end_effector.compute_pose(jv_pre_place) await end_effector.move_poses_linear(CartesianPath([pre_place_pose, place]), velocity_scaling = 0.05) # do something, e.g. place an object await end_effector.move_poses_linear(CartesianPath([place, pre_place_pose]), velocity_scaling = 0.05) # Creates a joint path over the joint values to the home pose joint_path_back = JointPath(home.joint_set, [jv_pre_place, home ]) # Move back over the joint path await move_group.move_joints_collision_free(joint_path_back) try: # Move to home position loop.run_until_complete(move_group.move_joints_collision_free(home)) # For every pose we want to address, do a pick and place for i in range(len(pre_place_jvs)): print("Placing element {}".format(i+1)) loop.run_until_complete(place(pre_place_jvs[i], poses[i])) finally: loop.close()
def main(): world_view_client = WorldViewClient() input_var = input("Please type in the torch filename (with path) of the sensorhead stereo camera calibration (e.g. \"calibration/torso_cameras/stereo_cams_<serial1>_<serial2>.t7\" ): ") cam_calib_fn = str(input_var) stereocalib_sensorhead_cams = torchfile.load(cam_calib_fn) end_of_right_serial = cam_calib_fn.rfind(".") start_of_right_serial = cam_calib_fn.rfind("_") right_serial = cam_calib_fn[start_of_right_serial+1:end_of_right_serial] end_of_left_serial = start_of_right_serial start_of_left_serial = cam_calib_fn.rfind("_", 0, end_of_left_serial) left_serial = cam_calib_fn[start_of_left_serial+1:end_of_left_serial] print("right_serial:") print(right_serial) print("left_serial:") print(left_serial) print("Please type in the exposure time of the cameras [in microseconds],") input_var = input("or press \'enter\' to use 120000ms:") if input_var == "" : exposure_time = 120000 else : exposure_time = int(input_var) print("exposure_time:") print(exposure_time) print(type(exposure_time)) move_group_names = MotionService.query_available_move_groups() move_group_name = move_group_names[2].name # left_arm_torso flange_link_name = move_group_names[2].end_effector_link_names[0] # arm_left_link_tool0 print("For which arm will the tooltip be calibrated? Please type l or r, then press \'Enter\'.") print("l: left arm") print("r: right arm") which = sys.stdin.read(1) if which == "r" : move_group_name = move_group_names[3].name # right_arm_torso flange_link_name = move_group_names[3].end_effector_link_names[0] # arm_right_link_tool0 print("move_group_name:") print(move_group_name) move_group = MoveGroup(move_group_name) print("flange_link_name") print(flange_link_name) torso_link_name = "torso_link_b1" print("torso_link_name:") print(torso_link_name) last_slash = cam_calib_fn.rfind("/") torso_to_cam_fn = cam_calib_fn[:last_slash] + "/LeftCam_torso_joint_b1.t7" print("Please type in the name of the torchfile (with path) containing the transformation between torso joint and torso cameras,") input_var = input("or press \'enter\' to use " + torso_to_cam_fn) if input_var != "" : torso_to_cam_fn = str(input_var) print("torso_to_cam_fn:") print(torso_to_cam_fn) torso_to_cam_t7 = torchfile.load(torso_to_cam_fn) torso_to_cam = Pose.from_transformation_matrix(matrix=torso_to_cam_t7, frame_id=torso_link_name, normalize_rotation=False) pattern_localizer = patternlocalisation.PatternLocalisation() pattern_localizer.circleFinderParams.minArea = 50 pattern_localizer.circleFinderParams.maxArea = 1000 pattern_localizer.setPatternData(8, 11, 0.003) pattern_localizer.setStereoCalibration(stereocalib_sensorhead_cams) #pattern_localizer.setFindCirclesGridFlag(cv.CALIB_CB_ASYMMETRIC_GRID) # Don't use "cv.CALIB_CB_CLUSTERING", because it's much more sensitive to background clutter. pattern_localizer.setFindCirclesGridFlag(cv.CALIB_CB_ASYMMETRIC_GRID + cv.CALIB_CB_CLUSTERING) world_view_folder = '/Calibration' storage_folder = '/tmp/calibration/storage_tooltipcalib' offline = False tooltip_calib = TooltipCalibration(pattern_localizer, stereocalib_sensorhead_cams, left_serial, right_serial, exposure_time, world_view_client, world_view_folder, move_group, torso_to_cam, storage_folder, torso_link_name, flange_link_name, offline) tooltip_calib.runTooltipCalib()
def get_move_group() -> MoveGroup: return MoveGroup(get_move_group_name())
def get_full_body_move_group() -> MoveGroup: return MoveGroup("/sda10f")
def get_left_move_group() -> MoveGroup: return MoveGroup("/sda10f/sda10f_r1_controller")
def get_right_move_group() -> MoveGroup: return MoveGroup("/sda10f/sda10f_r2_controller")
def plan_null_space_torso_move( left_move_group: MoveGroup, right_move_group: MoveGroup, full_body_start: MoveGroup, torso_joint_name: str, goal_torso_angle: float) -> List[JointValues] : """ Calculates a list of JointValues, describing the null space torso move Parameters ---------- left_move_group : MoveGroup The MoveGroup of the left arm right_move_group : MoveGroup The MoveGroup of the right arm full_body_start : MoveGroup The The MoveGroup of the whole robot torso_joint_name : str The name of the torso joint goal_torso_angle : float The target torso angle the torso should reach Returns ---------- List[JointValues] The JointValues of the movement """ # Get the JointValues of the current left and right arm configuration left_start = left_move_group.get_current_joint_positions() right_start = right_move_group.get_current_joint_positions() # Get the JointValues of the whole body full_body = full_body_start.get_current_joint_positions() # Get the end effectors of the left and right arm, respectively left_end_effector = left_move_group.get_end_effector() right_end_effector = right_move_group.get_end_effector() # Calculate poses of the end effectors left_start_pose = left_end_effector.compute_pose(left_start) right_start_pose = right_end_effector.compute_pose(right_start) # Get the current value of the torso joint found, start_torso_angle = full_body.try_get_value(torso_joint_name) assert(found) # Define the amount of the JointValues configurations the movement should consist of N = max(10, int(abs(goal_torso_angle - start_torso_angle)/math.radians(1))) threshold = math.pi/4 waypoints = [] for i in range(N): last_full_body = copy.deepcopy(full_body) # Calculate the current angle between start and goal alpha = (i - 1) / (N - 1) angle = (1-alpha) * start_torso_angle + alpha * goal_torso_angle print("Calculate index {} of {} with angle {}".format(i + 1, N, angle)) # Now we update the JointValues of the robot by adjusting the angle of # the torso joint only full_body = full_body.set_values(JointSet(torso_joint_name), [angle]) # Calculate the inverse kinematics for the left arm and give the # altered "full_body" object (with updated angle) as seed. # Since the move group for the left arm does not contain the joint # of the torso, it can only update the joint of the arm and assumes # the torso joint given by the seed to be fixed left_tmp_jv = left_end_effector.inverse_kinematics( pose=left_start_pose, collision_check=True, seed=full_body) # Update the JointValues of the result of IK of the left arm full_body = full_body.set_values(left_tmp_jv.joint_set, left_tmp_jv.values) # Do the above for the right arm right_tmp_jv = right_end_effector.inverse_kinematics( pose=right_start_pose, collision_check=True, seed=full_body) full_body = full_body.set_values(right_tmp_jv.joint_set, right_tmp_jv.values) # Assert there were no IK jumps # assert(2 > full_body.ik_jump_threshold detectIKJump(full_body, last_full_body, threshold)) #, 'jump IK detected') waypoints.append(full_body) return waypoints
def main(): # create move group instance move_group = MoveGroup() # get default endeffector of the movegroup end_effector = move_group.get_end_effector() # create cartesian path and equivalent joint path t1 = [0.502522, 0.2580, 0.3670] q1 = Quaternion(w=0.304389, x=0.5272, y=0.68704, z=0.39666) t2 = [0.23795, 0.46845, 0.44505] q2 = Quaternion(w=0.212097, x=0.470916, y=0.720915, z=0.462096) t3 = [0.6089578, 0.3406782, 0.208865] q3 = Quaternion(w=0.231852, x=0.33222, y=0.746109, z=0.528387) pose_1 = Pose(t1, q1) pose_2 = Pose(t2, q2) pose_3 = Pose(t3, q3) cartesian_path = CartesianPath([pose_1, pose_2, pose_3]) joint_path = end_effector.inverse_kinematics_many(cartesian_path, False).path loop = asyncio.get_event_loop() register_asyncio_shutdown_handler(loop) async def example_moves(): print('test MoveGroup class') print('---------------- move joints -------------------') await move_group.move_joints(joint_path) print('---------------- move joints collision free -------------------') await move_group.move_joints_collision_free(joint_path) print('---------------- move joints supervised -------------------') stepped_motion_client = move_group.move_joints_supervised( joint_path, 0.5) await run_supervised(stepped_motion_client) print('----------------move joints collision free supervised -------------------') stepped_motion_client = move_group.move_joints_collision_free_supervised( joint_path, 0.5) await run_supervised(stepped_motion_client) print('test EndEffector class') print('---------------- move poses -------------------') await end_effector.move_poses(cartesian_path) print('---------------- move poses collision free -------------------') await end_effector.move_poses_collision_free(cartesian_path) print('---------------- move poses supervised -------------------') stepped_motion_client = end_effector.move_poses_supervised( cartesian_path, None, 0.5) await run_supervised(stepped_motion_client) print('---------------- move poses collision free supervised -------------------') stepped_motion_client = end_effector.move_poses_collision_free_supervised( cartesian_path, None, 0.5) await run_supervised(stepped_motion_client) try: loop.run_until_complete(example_moves()) finally: loop.close()