epilectrik
/
teleop
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

[optim] optim g1 ik

main
silencht 2 years ago
parent
da2e481823
commit
716bad44ac
1 changed files with 64 additions and 55 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 119
      teleop/robot_control/robot_arm_ik.py

119
teleop/robot_control/robot_arm_ik.py
View File

@ -78,45 +78,45 @@ class Arm_IK:
self.init_data = np.zeros(self.reduced_robot.model.nq)
# Initialize the Meshcat visualizer
self.vis = MeshcatVisualizer(self.reduced_robot.model, self.reduced_robot.collision_model, self.reduced_robot.visual_model)
self.vis.initViewer(open=True)
self.vis.loadViewerModel("pinocchio")
self.vis.displayFrames(True, frame_ids=[49, 81, 101, 102], axis_length = 0.15, axis_width = 5)
self.vis.display(pin.neutral(self.reduced_robot.model))
# for i in range(self.reduced_robot.model.nframes):
# frame = self.reduced_robot.model.frames[i]
# frame_id = self.reduced_robot.model.getFrameId(frame.name)
# print(f"Frame ID: {frame_id}, Name: {frame.name}")
# Enable the display of end effector target frames with short axis lengths and greater width.
frame_viz_names = ['L_ee_target', 'R_ee_target']
FRAME_AXIS_POSITIONS = (
np.array([[0, 0, 0], [1, 0, 0],
[0, 0, 0], [0, 1, 0],
[0, 0, 0], [0, 0, 1]]).astype(np.float32).T
)
FRAME_AXIS_COLORS = (
np.array([[1, 0, 0], [1, 0.6, 0],
[0, 1, 0], [0.6, 1, 0],
[0, 0, 1], [0, 0.6, 1]]).astype(np.float32).T
)
axis_length = 0.1
axis_width = 10
for frame_viz_name in frame_viz_names:
self.vis.viewer[frame_viz_name].set_object(
mg.LineSegments(
mg.PointsGeometry(
position=axis_length * FRAME_AXIS_POSITIONS,
color=FRAME_AXIS_COLORS,
),
mg.LineBasicMaterial(
linewidth=axis_width,
vertexColors=True,
),
)
)
# # Initialize the Meshcat visualizer for visualization
# self.vis = MeshcatVisualizer(self.reduced_robot.model, self.reduced_robot.collision_model, self.reduced_robot.visual_model)
# self.vis.initViewer(open=True)
# self.vis.loadViewerModel("pinocchio")
# self.vis.displayFrames(True, frame_ids=[49, 81, 101, 102], axis_length = 0.15, axis_width = 5)
# self.vis.display(pin.neutral(self.reduced_robot.model))
# # for i in range(self.reduced_robot.model.nframes):
# # frame = self.reduced_robot.model.frames[i]
# # frame_id = self.reduced_robot.model.getFrameId(frame.name)
# # print(f"Frame ID: {frame_id}, Name: {frame.name}")
# # Enable the display of end effector target frames with short axis lengths and greater width.
# frame_viz_names = ['L_ee_target', 'R_ee_target']
# FRAME_AXIS_POSITIONS = (
# np.array([[0, 0, 0], [1, 0, 0],
# [0, 0, 0], [0, 1, 0],
# [0, 0, 0], [0, 0, 1]]).astype(np.float32).T
# )
# FRAME_AXIS_COLORS = (
# np.array([[1, 0, 0], [1, 0.6, 0],
# [0, 1, 0], [0.6, 1, 0],
# [0, 0, 1], [0, 0.6, 1]]).astype(np.float32).T
# )
# axis_length = 0.1
# axis_width = 10
# for frame_viz_name in frame_viz_names:
# self.vis.viewer[frame_viz_name].set_object(
# mg.LineSegments(
# mg.PointsGeometry(
# position=axis_length * FRAME_AXIS_POSITIONS,
# color=FRAME_AXIS_COLORS,
# ),
# mg.LineBasicMaterial(
# linewidth=axis_width,
# vertexColors=True,
# ),
# )
# )
# Creating Casadi models and data for symbolic computing
self.cmodel = cpin.Model(self.reduced_robot.model)
@ -149,12 +149,12 @@ class Arm_IK:
# Defining the optimization problem
self.opti = casadi.Opti()
self.var_q = self.opti.variable(self.reduced_robot.model.nq)
# self.param_q_ik_last = self.opti.parameter(self.reduced_robot.model.nq)
self.var_q_last = self.opti.parameter(self.reduced_robot.model.nq) # for smooth
self.param_tf_l = self.opti.parameter(4, 4)
self.param_tf_r = self.opti.parameter(4, 4)
self.totalcost = casadi.sumsqr(self.error(self.var_q, self.param_tf_l, self.param_tf_r))
self.regularization = casadi.sumsqr(self.var_q)
# self.smooth_cost = casadi.sumsqr(self.var_q - self.param_q_ik_last)
self.smooth_cost = casadi.sumsqr(self.var_q - self.var_q_last)
# Setting optimization constraints and goals
self.opti.subject_to(self.opti.bounded(
@ -162,20 +162,20 @@ class Arm_IK:
self.var_q,
self.reduced_robot.model.upperPositionLimit)
)
self.opti.minimize(20 * self.totalcost + 0.01 * self.regularization)
# self.opti.minimize(20 * self.totalcost + 0.001*self.regularization + 0.1*self.smooth_cost)
# self.opti.minimize(20 * self.totalcost + 0.01 * self.regularization)
self.opti.minimize(20 * self.totalcost + 0.01* self.regularization + 0.1 * self.smooth_cost) # for smooth
opts = {
'ipopt':{
'print_level':0,
'max_iter':100,
'tol':1e-5
'max_iter':50,
'tol':1e-4
},
'print_time':True
'print_time':False
}
self.opti.solver("ipopt", opts)
def adjust_pose(self, human_left_pose, human_right_pose, human_arm_length=0.60, robot_arm_length=1.20):
def adjust_pose(self, human_left_pose, human_right_pose, human_arm_length=0.60, robot_arm_length=0.75):
scale_factor = robot_arm_length / human_arm_length
robot_left_pose = human_left_pose.copy()
robot_right_pose = human_right_pose.copy()
@ -188,20 +188,21 @@ class Arm_IK:
self.init_data = motorstate
self.opti.set_initial(self.var_q, self.init_data)
self.vis.viewer['L_ee_target'].set_transform(left_pose) # for visualization
self.vis.viewer['R_ee_target'].set_transform(right_pose) # for visualization
# left_pose, right_pose = self.adjust_pose(left_pose, right_pose)
# self.vis.viewer['L_ee_target'].set_transform(left_pose) # for visualization
# self.vis.viewer['R_ee_target'].set_transform(right_pose) # for visualization
self.opti.set_value(self.param_tf_l, left_pose)
self.opti.set_value(self.param_tf_r, right_pose)
self.opti.set_value(self.var_q_last, self.init_data) # for smooth
try:
# sol = self.opti.solve()
sol = self.opti.solve_limited()
sol_q = self.opti.value(self.var_q)
self.vis.display(sol_q) # for visualization
# self.vis.display(sol_q) # for visualization
self.init_data = sol_q
if motorV is not None:
@ -233,12 +234,20 @@ if __name__ == "__main__":
np.array([0.23, -0.2, 0.2]),
)
rotation_speed = 0.005 # Rotation speed in radians per iteration
user_input = input("Please enter the start signal (enter 's' to start the subsequent program):")
if user_input.lower() == 's':
for i in range(100):
L_tf_target.translation += np.array([0.002, 0.002, 0.002])
R_tf_target.translation += np.array([0.002, -0.002, 0.002])
for i in range(150):
angle = rotation_speed * i
L_quat = pin.Quaternion(np.cos(angle / 2), 0, np.sin(angle / 2), 0) # y axis
R_quat = pin.Quaternion(np.cos(angle / 2), 0, 0, np.sin(angle / 2)) # z axis
L_tf_target.translation += np.array([0.001, 0.001, 0.001])
R_tf_target.translation += np.array([0.001, -0.001, 0.001])
L_tf_target.rotation = L_quat.toRotationMatrix()
R_tf_target.rotation = R_quat.toRotationMatrix()
arm_ik.ik_fun(L_tf_target.homogeneous, R_tf_target.homogeneous)
time.sleep(0.02)
time.sleep(0.05)
Write Preview
Loading…
Cancel
Save