Genesis-Embodied-AI
Problem in Inverse Kinematics & Motion Planning
I want to try planning with inverse kinematics and I use Piper URDF. I found that if I just use inverse kinematics to calculate the joint position and directly use 'control_dofs_position' to execute these position , it works. But if I use plan_path , it can not work , what's more I found it called in 'plan_path' function : ss = og.SimpleSetup(space) I m considering HOW to solve this problem
Could you share your script and more details?
import genesis as gs
import math
import numpy as np
'''
关节名 joint limit qpos中对应的值
----------------------------------------------------
joint1 [-2.618, 2.618] -> -0.0027
joint2 [ 0 , 3.14 ] -> 1.6617
joint3 [-2.697, 0 ] -> -2.3636
joint4 [-1.832, 1.832] -> -0.1979
joint5 [-1.22 , 1.22 ] -> 1.2200
joint6 [-3.14 , 3.14 ] -> 1.7645
joint7 [ 0 , 0.04 ](prismatic)-> 0.0155
joint8 [-0.04 , 0 ](prismatic) -> -0.0387
'''
gs.init(
seed = None,
precision = '32',
debug = False,
eps = 1e-12,
logging_level = None,
backend = gs.cpu,
theme = 'dark',
logger_verbose_time = True
)
scene = gs.Scene(
sim_options=gs.options.SimOptions(
dt=0.01,
gravity=(0, 0, -10.0),
),
show_viewer=True,
viewer_options=gs.options.ViewerOptions(
camera_pos=(3.5, 0.0, 2.5),
camera_lookat=(0.0, 0.0, 0.5),
camera_fov=40,
),
rigid_options=gs.options.RigidOptions(enable_self_collision = True),
)
plane = scene.add_entity(gs.morphs.Plane())
box = scene.add_entity(gs.morphs.Box(size = (0.04,0.04,0.04),
pos = (1.65,1.0,0.02),
collision = True))
Piper = scene.add_entity(
gs.morphs.URDF(file ='/home/jiziheng/Music/Genesis/Piper_model/urdf/piper_description.urdf',
fixed = True,
collision = True,
pos = (1.0,1.0,0.0)
),
)
cam0 = scene.add_camera()
joint_names = [
'joint1',
'joint2',
'joint3',
'joint4',
'joint5',
'joint6',
'joint7',
'joint8',
]
dofs_idx = [Piper.get_joint(name).dof_idx_local for name in joint_names]
############ Optional: set control gains ############
scene.build()
import IPython
IPython.embed()
Piper.set_dofs_kp(
kp = np.array([4500,4500,4500,4500,4500,4500,100,100]),
dofs_idx_local = dofs_idx
)
Piper.set_dofs_kv(
kv = np.array([450,450,450,450,450,450,10,10]),
dofs_idx_local = dofs_idx
)
Piper.set_dofs_force_range(
lower = np.array([-87,-87,-87,-87,-87,-87,-12,-12]),
upper = np.array([87,87,87,87,87,87,12,12]),
dofs_idx_local = dofs_idx,
)
count = 0
#
end_effector = Piper.get_link("link6")
qpos = Piper.inverse_kinematics(
link = end_effector,
pos = np.array([1.45, 1.0 , 0.02]),
quat = np.array([0,0,1,0])
)
print("--------")
print("Inverse kinematics result:", qpos)
# qpos[-1:] = -0.04
# qpos[-2:-1] = 0.04
while True:
path = Piper.plan_path(
qpos_goal=np.array(qpos),
num_waypoints=20,
planner = 'PRM',
)
# print("-----------")
# print(path)
# print("-----------")
for waypoint in path:
Piper.control_dofs_position(path)
# Piper.control_dofs_position(np.array([-2.9842e-03, 2.5314e+00, -1.7150e+00, 5.1327e-04, 8.4387e-01,
# -1.5748e+00, 3.7011e-02, -2.0058e-02]), dofs_idx)
scene.visualizer.update()
# scene.step()
what's more, I used python 3.9.20 and ompl 1.6.0 , may be version problem? But i am not sure
when I use pycharm, it has the same problem. But when I use in the terminal , it works
when I use pycharm, it has the same problem. But when I use in the terminal , it works
It also not work for me with terminal