From 034c9dc54e8a5f0ea2edb13bb698ac06c48f475b Mon Sep 17 00:00:00 2001 From: m-guberina <gubi.guberina@gmail.com> Date: Tue, 4 Feb 2025 19:20:04 +0100 Subject: [PATCH] added go to initial pose in clik_client.py --- python/examples/graz/clik_client.py | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/python/examples/graz/clik_client.py b/python/examples/graz/clik_client.py index 95828d7..59d71a8 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() -- GitLab