diff --git a/python/examples/graz/clik_client.py b/python/examples/graz/clik_client.py index 95828d7ba422bd764f91f138848e22682c040b87..59d71a8b43bc95140f53181f5492a88d31583a24 100644 --- a/python/examples/graz/clik_client.py +++ b/python/examples/graz/clik_client.py @@ -20,6 +20,11 @@ def get_args(): parser.add_argument("--host", type=str, help="host ip address", default="127.0.0.1") parser.add_argument("--port-wrench", type=int, help="host's port", default=6666) parser.add_argument("--port-command", type=int, help="host's port", default=7777) + # put your own default here + # it's translation + orientation as a quarternion + parser.add_argument( + "--init-pose", type=float, nargs=7, default=[0.3, 0.3, 0.3, 0.0, 0.0, 0.0, 1.0] + ) args = parser.parse_args() return args @@ -82,10 +87,11 @@ def controlLoopClikExternalGoal( if __name__ == "__main__": args = get_args() robot = RobotManager(args) - - # get expected behaviour here (library can't know what the end is - you have to do this here) - if not args.pinocchio_only: - robot.stopRobot() + translation = np.array(args.init_pose[:3]) + orientation = pin.Quaternion(np.array(args.init_pose[3:])) + init_goal = pin.SE3(orientation, translation) + moveL(args, robot, init_goal) + print("arrived at initial position") # VERY important that the first q we'll pass as desired is the current q, meaning the robot won't move # this is set with init_value @@ -113,6 +119,10 @@ if __name__ == "__main__": loop_manager = ControlLoopManager(robot, control_loop, args, {}, log_item) loop_manager.run() + # get expected behaviour here (library can't know what the end is - you have to do this here) + if not args.pinocchio_only: + robot.stopRobot() + if args.save_log: robot.log_manager.plotAllControlLoops()