Repository: Genesis-Embodied-AI/Genesis
File: genesis/ext/pyrender/interaction/mouse_spring.py (Python)

Code before:
        pose: Pose = Pose.from_geom(self.held_geom)
        self.held_point_in_local = pose.inverse_transform_point(control_point)
        self.prev_control_point = control_point

    def detach(self):
        self.held_geom = None

    def apply_force(self, control_point: Vec3, delta_time: float):
        _ensure_torch_imported()
        
        # works ok:
        # delta: Vec3 = control_point - self.prev_control_point
        # pos = Vec3.from_tensor(self.held_geom.entity.get_pos())
        # pos = pos + delta
        # self.held_geom.entity.set_pos(pos.as_tensor())
        self.prev_control_point = control_point

        # do simple force on COM only:
        link: RigidLink = self.held_geom.link
        link_pos: Vec3 = Vec3.from_tensor(link.get_pos())
        lin_vel: Vec3 = Vec3.from_tensor(link.get_vel())
        ang_vel: Vec3 = Vec3.from_tensor(link.get_ang())

        pos_err_v: Vec3 = control_point - link_pos
        vel_err_v: Vec3 = Vec3.zero() - lin_vel
        inv_mass: float = float(1.0 / link.get_mass() if 0.0 < link.get_mass() else 0.0)

        inv_dt: float = 1.0 / delta_time
        tau: float = 1.0 / 2
        damp: float = 1.0 * 2

        total_impulse: Vec3 = Vec3.zero()

        for i in range(3):
            dir: Vec3 = Vec3.zero()
            dir.v[i] = 1.0
            pos_err: float = dir.dot(pos_err_v)
            vel_err: float = dir.dot(vel_err_v)
            err: float = tau * pos_err * inv_dt + damp * vel_err
            vm: float = inv_mass
            imp: float = err * vm

            lin_vel += imp * dir * inv_mass
            total_impulse.v[i] = imp

        total_force = total_impulse * inv_dt
        force_tensor: torch.Tensor = total_force.as_tensor().unsqueeze(0)
        link.solver.apply_links_external_force(force_tensor, (link.idx,), ref='link_com', local=False)

        # print("vel", lin_vel, "total_impulse", total_impulse)
        # print("exp", lin_vel + total_force * inv_mass * link.solver.dt, "at dt", link.solver.dt, "inv dt", inv_dt)

Review comment (suggestion):
I don't know why, but `0.0 < link.get_mass()` is confusing to me. I would use `link.get_mass() > 0.0`

Code after:
    def attach(self, picked_entity: RigidEntity, control_point: Vec3):
        # for now, we just pick the first geometry
        self.held_geom = picked_entity.geoms[0]
        pose: Pose = Pose.from_geom(self.held_geom)
        self.held_point_in_local = pose.inverse_transform_point(control_point)
        self.prev_control_point = control_point

    def detach(self):
        self.held_geom = None

    def apply_force(self, control_point: Vec3, delta_time: float):
        _ensure_torch_imported()
        
        # works ok:
        # delta: Vec3 = control_point - self.prev_control_point
        # pos = Vec3.from_tensor(self.held_geom.entity.get_pos())
        # pos = pos + delta
        # self.held_geom.entity.set_pos(pos.as_tensor())
        self.prev_control_point = control_point

        # do simple force on COM only:
        link: RigidLink = self.held_geom.link
        link_pos: Vec3 = Vec3.from_tensor(link.get_pos())
        lin_vel: Vec3 = Vec3.from_tensor(link.get_vel())
        ang_vel: Vec3 = Vec3.from_tensor(link.get_ang())

        pos_err_v: Vec3 = control_point - link_pos
        vel_err_v: Vec3 = Vec3.zero() - lin_vel
        inv_mass: float = float(1.0 / link.get_mass() if link.get_mass() > 0.0 else 0.0)

        inv_dt: float = 1.0 / delta_time
        # these are temporary values, till we fix an issue with apply_links_external_force.
        # after fixing it, use tau = damp = 1.0:
        tau: float = MOUSE_SPRING_POSITION_CORRECTION_FACTOR
        damp: float = MOUSE_SPRING_VELOCITY_CORRECTION_FACTOR

        total_impulse: Vec3 = Vec3.zero()

        for i in range(3):
            dir: Vec3 = Vec3.zero()
            dir.v[i] = 1.0
            pos_err: float = dir.dot(pos_err_v)
            vel_err: float = dir.dot(vel_err_v)
            error: float = tau * pos_err * inv_dt + damp * vel_err
            virtual_mass: float = 1.0 / (inv_mass + 1e-24)
            impulse: float = error * virtual_mass

            lin_vel += impulse * dir * inv_mass
            total_impulse.v[i] = impulse

        # Apply the new force