Combine Kinematics solver and dynamics solver ?

[Storm3333]

Hi, thanks for the good work on placo. I would like to ask a question which may seem a bit trivial to people who understand Qp optimization and robotics. So forgive me for asking a naive question.

In short, I have used bipedal walktask and walk planner to generate footsteps, and thus getting pose , joint configuration for my robot at each time step using kinematics solver. Now, I would like to add dynamics constraint to this qp problem so that I may get a better motion reference (i.e. pose and joint configuration ) for each time step . I have looked at examples provided placo_example such as sigmaban.py. I have not found any examples including both kinematics and dynamics solver. I'm not sure directly adding dynamics solver and initialize dynamic tasks according to the robot and generated footsteps would achieve what I want. Right now, in my code I define kinematics solver and dynamic solver with respect to the same robot and solve independently in the same dt.

Could you share some hints on this ? It would be very kind of you if you could provide an example. Also, I have read it somewhere that the dynamic solver actually includes the kinematics solver's functionality.

[Gregwar]

Hello,

Kinematics and dynamics solver are solving different problem. Firstly, the decision variables are very different. The kinematics solver solves for $\Delta q$ while the dynamics solver solves for $\ddot q, \tau$ and $f$.

Choosing depends on the type of problem you are addressing. If the constraint you want can be expressed as the first-order approximation, such as CoM velocity or DCM, it can be introduced in a kinematics QP. Else, you might need to switch to second-order and use Dynamics solver for everything.

I would need to know exactly what you are trying to do to help you further.

[Storm3333]

Thanks for replying to me.

I will try to address my question as clearly as possible. The general scenario is I'm trying to record the joints position (i.e. degree for a revolute joint ) for my bipedal robot for each timestep as it is walking a provided trajectory generated by RepetitiveWalkPlaner ( sorry if I memorised the wrong API name ). Since I have a URDF and mass, torque limits and other physics information defined for the robot (sorry if this sounds really weired, this is all new to me), I would like to add dynamics constraints to further optimize the solutions (i.e. joints positions) I get from solving QP task. I was wondering if I should incorprate the dynamic solver, which I don't know how to and it is not shown in placo_examples or should I just use dynamic solver solely ?

Thanks in advance.

[Storm3333]

To be more precis. I can provide the following code :
`self.robot = placo.HumanoidRobot(model_filename,placo.Flags.ignore_collisions)
self.parameters = placo.HumanoidParameters()
if init_params is not None:
self.load_parameters(init_params)
else:
defaults_filename = os.path.join(asset_path, "placo_defaults.json")
self.load_defaults(defaults_filename)

    # Creating the kinematics solver
    self.solver = placo.KinematicsSolver(self.robot)
    self.robot.set_velocity_limits(0.3)
    self.solver.enable_velocity_limits(True)
    self.solver.enable_joint_limits(True)
    self.solver.dt = DT / REFINE
    
    # self.robot.update_kinematics() 

    # Creating the dynamics solver for optimization
    self.dynamics_solver = placo.DynamicsSolver(self.robot)
    self.dynamics_solver.dt = DT / REFINE

    # Creating the walk QP tasks
    self.tasks = placo.WalkTasks()
    if hasattr(self.parameters, "trunk_mode"):
        self.tasks.trunk_mode = self.parameters.trunk_mode
    self.tasks.com_x = 0.0
    self.tasks.initialize_tasks(self.solver, self.robot)
    self.tasks.left_foot_task.orientation().mask.set_axises("xyz", "local")
    self.tasks.right_foot_task.orientation().mask.set_axises("xyz", "local")`

and I would like to add the following dynamic tasks
` def initialize_dynamics_tasks(self):

    """
    # Add CoM task for dynamics
    com_world = self.robot.com_world()
    self.dynamics_com_task = self.dynamics_solver.add_com_task(com_world)
    self.dynamics_com_task.configure("dynamics_com", "soft", 1.0)
    
    #Add trunk orientation task
    trunk_orientation = np.eye(3)
    trunk_orientation = placo.rotation_from_axis("x", np.array([np.cos(self.parameters.walk_trunk_pitch), 0, np.sin(self.parameters.walk_trunk_pitch)]))
    self.dynamics_trunk_task = self.dynamics_solver.add_orientation_task("trunk", trunk_orientation)
    self.dynamics_trunk_task.configure("dynamics_trunk", "soft", 1.0)

    # Add frame tasks for feet
    T_world_left = self.robot.get_T_world_left()
    self.dynamics_left_foot_task = self.dynamics_solver.add_frame_task("left_foot", T_world_left)
    self.dynamics_left_foot_task.configure("dynamics_left_foot", "hard", 1.0, 0.8)
    
    T_world_right = self.robot.get_T_world_right()
    self.dynamics_right_foot_task = self.dynamics_solver.add_frame_task("right_foot", T_world_right)
    self.dynamics_right_foot_task.configure("dynamics_right_foot", "hard", 1.0, 0.8)
    
    # Add planar contacts with friction constraints (friction not implemented yet)
    self.left_contact = self.dynamics_solver.add_planar_contact(self.dynamics_left_foot_task)
    self.left_contact.length = self.parameters.foot_length
    self.left_contact.width = self.parameters.foot_width
    self.left_contact.weight_moments = 1e-3
    #self.left_contact.mu = self.parameters.friction_coeff  # From placo_defaults.json
    
    self.right_contact = self.dynamics_solver.add_planar_contact(self.dynamics_right_foot_task)
    self.right_contact.length = self.parameters.foot_length
    self.right_contact.width = self.parameters.foot_width
    self.right_contact.weight_moments = 1e-3
    #self.right_contact.mu = self.parameters.friction_coeff

    # Add regularization task for posture
    self.dynamics_posture_task = self.dynamics_solver.add_joints_task()
    self.dynamics_posture_task.set_joints({joint: 0.0 for joint in self.robot.joint_names()})
    self.dynamics_posture_task.configure("dynamics_posture", "soft", 1e-6)
    
    # Add torque minimization task
    self.dynamics_torque_task = self.dynamics_solver.add_torque_task()
    self.dynamics_torque_task.configure("dynamics_torque", "soft", 1e-4)

    self.dynamics_solver.enable_joint_limits(True)
    self.dynamics_solver.enable_velocity_limits(True)
    self.dynamics_solver.enable_torque_limits(True)
    print(f"finished dynamic tasks initialization")`

[Gregwar]

If your goal is to ensure dynamics constraints, such as torque limits, you can consider switching to the dynamics solver.

However, this comes with many downsides and pitfalls:

1. It is mostly experimental, in the sense that we didn't use it on actual robots yet
2. Second order dynamics might come with the need to refresh the computation with smaller $\Delta t$ for numerical stability
3. In a dynamics solver, you also solve for external force so that equation of motion is true. This means that you'll have to update the active contacts
4. In second-order, placo relies on PD controller to ensure a dynamics converging to the desired target. This will work much better if you provide a desired velocity, and optionally acceleration. If your original trajectories are differentiable, you should also feed the dynamics solver with those.

Consider watching the following video from Andrea Del Prete about task-space inverse dynamics to get a better understanding: https://www.youtube.com/watch?v=psy4uhi76Ko