-
Notifications
You must be signed in to change notification settings - Fork 15
/
humanoid_sigmaban.py
115 lines (100 loc) · 3.4 KB
/
humanoid_sigmaban.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
#
# SPDX-License-Identifier: Apache-2.0
# Copyright 2023 Inria
"""SigmaBan humanoid standing on two feet."""
import meshcat_shapes
import numpy as np
import pinocchio as pin
import qpsolvers
from loop_rate_limiters import RateLimiter
import pink
from pink import solve_ik
from pink.tasks import FrameTask, PostureTask
try:
from robot_descriptions.loaders.pinocchio import load_robot_description
except ModuleNotFoundError:
raise ModuleNotFoundError(
"Examples need robot_descriptions, "
"try `pip install robot_descriptions`"
)
if __name__ == "__main__":
robot = load_robot_description(
"sigmaban_description", root_joint=pin.JointModelFreeFlyer()
)
viz = pin.visualize.MeshcatVisualizer(
robot.model, robot.collision_model, robot.visual_model
)
robot.setVisualizer(viz, init=False)
viz.initViewer(open=True)
viz.loadViewerModel()
configuration = pink.Configuration(robot.model, robot.data, robot.q0)
viz.display(configuration.q)
viewer = viz.viewer
left_foot_task = FrameTask(
"left_foot_tip",
position_cost=1.0,
orientation_cost=1.0,
)
torso_task = FrameTask(
"torso",
position_cost=1.0,
orientation_cost=1.0,
)
right_foot_task = FrameTask(
"right_foot_tip",
position_cost=1.0,
orientation_cost=1.0,
)
posture_task = PostureTask(
cost=1e-2, # [cost] / [rad]
)
tasks = [left_foot_task, torso_task, right_foot_task, posture_task]
torso_pose = configuration.get_transform_frame_to_world("torso").copy()
# torso_pose.translation[0] += 0.05
torso_task.set_target(torso_pose)
posture_task.set_target_from_configuration(configuration)
transform_left_foot_tip_target_to_init = pin.SE3(
np.eye(3), np.array([0.0, 0.03, 0.0])
)
transform_right_foot_tip_target_to_init = pin.SE3(
np.eye(3), np.array([0.0, -0.03, 0.0])
)
left_foot_task.set_target(
configuration.get_transform_frame_to_world("left_foot_tip")
* transform_left_foot_tip_target_to_init
)
right_foot_task.set_target(
configuration.get_transform_frame_to_world("right_foot_tip")
* transform_right_foot_tip_target_to_init
)
torso_task.set_target(configuration.get_transform_frame_to_world("torso"))
# Display targets
meshcat_shapes.frame(viewer["left_foot_target"], opacity=0.5)
meshcat_shapes.frame(viewer["right_foot_target"], opacity=0.5)
meshcat_shapes.frame(viewer["torso_target"], opacity=0.5)
viewer["left_foot_target"].set_transform(
left_foot_task.transform_target_to_world.np
)
viewer["right_foot_target"].set_transform(
right_foot_task.transform_target_to_world.np
)
viewer["torso_target"].set_transform(
torso_task.transform_target_to_world.np
)
# Select QP solver
solver = qpsolvers.available_solvers[0]
if "quadprog" in qpsolvers.available_solvers:
solver = "quadprog"
rate = RateLimiter(frequency=200.0)
dt = rate.period
t = 0.0 # [s]
while True:
# Compute velocity and integrate it into next configuration
velocity = solve_ik(configuration, tasks, dt, solver=solver)
configuration.integrate_inplace(velocity, dt)
# Visualize result at fixed FPS
viz.display(configuration.q)
rate.sleep()
t += dt