how to deal with `qpos_start` exceeds joint limit. Relaxing joint limit to contain `qpos_start` for planning.

[milong26]

trafficstars

I got these two warning:

[WARNING] qpos_start exceeds joint limit. Relaxing joint limit to contain qpos_start for planning. [WARNING] qpos_goal exceeds joint limit. Relaxing joint limit to contain qpos_goal for planning.

And it looks like the path of robot hand is strange. :sad:

If you need my code: Im working on let franka_sim/bi-franka_panda to grasp a cude.

import numpy as np
import genesis as gs

gs.init(backend=gs.cpu,logging_level='warning')

scene = gs.Scene(
    viewer_options = gs.options.ViewerOptions(
        camera_pos    = (3, -1, 1.5),
        camera_lookat = (0.0, 0.0, 0.5),
        camera_fov    = 30,
        max_FPS       = 60,
    ),
    show_viewer = True,
)
scene.add_entity(gs.morphs.Plane())

table = scene.add_entity(
     gs.morphs.Box(
         size = (0.4, 0.4, 0.4),
         pos  = (0, 0.4, 0.2),
         fixed=True

     )
 )
cube=scene.add_entity(
    gs.morphs.Box(
        size=(0.04,0.04,0.04),
        pos=(0,0.6,0.02),
    )
)

franka = scene.add_entity(
    gs.morphs.MJCF(
        file='xml/franka_sim/bi-franka_panda.xml',
        scale=0.5
    ),
)
scene.build()


motors_dof = np.arange(16)     
fingers_dof = np.arange(16, 18) 

end_effector1 = franka.get_link('panda0_gripper')
# end_effector2= franka.get_link('')

qpos = franka.inverse_kinematics(
    link = end_effector1,
    pos  = np.array([0, 0.6,0.7]),
    quat = np.array([0, 0, 1, 0]),
    respect_joint_limit=False
)
path = franka.plan_path(
    qpos_goal     = qpos,
    num_waypoints = 200, 
)

for waypoint in path:
    franka.control_dofs_position(waypoint)
    scene.step()
for i in range(100):
    scene.step()
for i in range(2000):
    scene.step()