ZhongyuWang-cart/IK_qp
1
0
Fork 0
forked from ZhengLiu-cart/IK_qp
Code Pull Requests Activity

update the mjc function names

Browse Source
This commit is contained in:
LiuzhengSJ
2026-06-05 10:23:02 +01:00
parent ddbbb1746e
commit aefc7bacd5
2 changed files with 31 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
kine_ctrl/main.py
View File

@ -109,7 +109,7 @@ def demo_position_control():
solve_sum = 0
for i in range(2000):
for i in range(10):
print(f'\n-------------- in i = {i} ----------------')
joint_rand = np.random.uniform(ub/180*pi, lb/180*pi)
print(f'the predefined joints are {joint_rand}')
@ -146,6 +146,10 @@ def demo_position_control():
print(f'-- success, in the qp ik, fk_qp_p2 = {fk_qp_p2}, d_p_ik = {d_p_ik}')
if d_p_ik < 0.01:
result[0][1] += 1
robot_mjk.send_command(joint_solution)
robot_mjk.wait_until_reached()
robot_mjk.print_state()
else:
fk_qp2 = robot_kine_qp.forward_kinematics(joint_solution, tool=tool_name)
fk_qp_p2 = np.concatenate([fk_qp2['position'], fk_qp2['rpy']], axis=0)