clik.py

# PYTHON_ARGCOMPLETE_OK
import numpy as np
import time
import pinocchio as pin
import argcomplete, argparse
from functools import partial
from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
from ur_simple_control.clik.clik import getClikArgs, getClikController, controlLoopClik, moveL, compliantMoveL, moveLDualArm
from ur_simple_control.util.define_random_goal import getRandomlyGeneratedGoal


def get_args():
    parser = getMinimalArgParser()
    parser.description = 'Run closed loop inverse kinematics \
    of various kinds. Make sure you know what the goal is before you run!'
    parser = getClikArgs(parser)
    parser.add_argument('--randomly-generate-goal', action=argparse.BooleanOptionalAction,
                         default=False, help="if true, rand generate a goal, if false you type it in via text input")
    argcomplete.autocomplete(parser)
    args = parser.parse_args()
    return args

if __name__ == "__main__": 
    args = get_args()
    robot = RobotManager(args)
    if args.randomly_generate_goal:
        Mgoal = getRandomlyGeneratedGoal(args)
        if args.visualize_manipulator:
            robot.visualizer_manager.sendCommand(
                    {"Mgoal" : Mgoal})
    else:
        Mgoal = robot.defineGoalPointCLI()
    #compliantMoveL(args, robot, Mgoal)
    if robot.robot_name != "yumi":
        moveL(args, robot, Mgoal)
    else:
        goal_transform = pin.SE3.Identity()
        #goal_transform.translation[1] = 0.1
        moveLDualArm(args, robot, Mgoal, goal_transform)
    robot.closeGripper()
    robot.openGripper()
    if not args.pinocchio_only:
        robot.stopRobot()

    if args.visualize_manipulator:
        robot.killManipulatorVisualizer()
    
    if args.save_log:
        robot.log_manager.saveLog()
    #loop_manager.stopHandler(None, None)