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