drake - Reference state trajectory system publishes zeros at t0, publishes the first ref state at t1. How to fix this? - Stack O

admin管理员组

文章数量:1415653

Hope you are well. I have a closed-loop simulation with a controller, which tracks the reference trajectory. I noticed a very large tau_cmd at t0. Then by logging the feedback error I noticed the desired states are all set to zeros at t0 instead of the first state of the ref traj, which was the reason of a large torque. Then at t1, the first state in the ref traj was published. I did explicitly set the initial state in the s_ref_traj_sys_context to the first state in the reference trajectory after connecting all ports and build the diagram:

diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()

s_ref_traj_sys_context = diagram.GetMutableSubsystemContext(s_ref_sys, diagram_context)
s_ref_traj_sys_context.get_mutable_discrete_state_vector().SetFromVector(state_traj_ref[0, :])

Did I miss something? Is there a recommended way of feeding a ref traj for tracking? Cheers

Editted: here is the reference trajectory class defined using LeafSystem

class JointStateReferenceTrajectory(LeafSystem):
    def __init__(self, 
                 t_offset, 
                 query_rate,
                 s_traj,
                 times,
                 hs,
                 a_init,
                 print_to_console=False):
        super().__init__()
        self._print_to_console = print_to_console
        self.N, self.nx = s_traj.shape
        self._s_traj_raw = s_traj
        self._state_port_index = self.DeclareDiscreteState(self.nx)
        self.times = times
        self.hs = hs
        self.acceleration_init = a_init # 7x1

        # construct the piecewise trajectory here
        self._refTraj = PiecewisePolynomial.FirstOrderHold(self.times, self._s_traj_raw.T)

        # period_sec: The period (in seconds) at which the event should recur. 
        # offset_sec: The time offset (in seconds) before the first occurrence of the event. 
        # update: callback function
        self.DeclarePeriodicUnrestrictedUpdateEvent(period_sec=query_rate, offset_sec=t_offset, update=self.Update)

        # declare output port 
        self.DeclareStateOutputPort("ref joint state", self._state_port_index)
        self.DeclareVectorOutputPort("ref joint acceleration", int(self.nx/2), self.CalcReferenceAcceleration)
        # self.DeclareVectorOutputPort("joint states", self.nx, self.CalcOutput) # this is to use with a callback function

        # declare publishing port
        self.DeclareForcedPublishEvent(self.PrintToConsole)
    
    def Update(self, context, discrete_state):
        t = context.get_time()
        traj_i = self._refTraj.value(t)
        # discrete_state.get_mutable_discrete_state().SetAtIndex(0, traj_i)
        discrete_state.get_mutable_discrete_state().set_value(traj_i)
        
        if self._print_to_console:
            self.PrintToConsole(context)

本文标签：