Problem in Inverse Kinematics & Motion Planning

[kelo020304]

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

[ZhitaoWang111]

I encountered the same problem.

[zhouxian]

Could you share your script and more details?

[kelo020304]

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()

[kelo020304]

what's more, I used python 3.9.20 and ompl 1.6.0 , may be version problem? But i am not sure

[zswang666]

Could you also send the error message?

[kelo020304]

Could you also send the error message?

[lmyhit]

when I use pycharm, it has the same problem. But when I use in the terminal , it works

[kelo020304]

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