def obs_cost(self, x, t=0, terminal=False):
x = x[:, :self.nx]
diffx = x - self.desired
Q = self.Q.expand(x.shape[0], *self.Q.shape[1:])
return bquad(Q, diffx)
def obs_cost(self, x, t=0, terminal=False):
x = x[:, :self.nx]
diffx = x - self.desired
Q = torch.diag(torch.Tensor([1,1,0,0])).unsqueeze(0)
Q = Q.expand(x.shape[0], *self.Q.shape[1:])
cost_goal = bquad(Q, diffx)
q = x[:, :self.nq]
ee_pos = self.fwk(q, 1)[:, :3, 3]
obstacle_pos = torch.Tensor([1.0960, 0.0000, 1.1710])
# diff_goal = ee_pos - self.desired_ee_pos
diff_obstacle = ee_pos - obstacle_pos
Q = beye(1, 3, 3)
Q = Q.expand(x.shape[0], *Q.shape[1:])
cost_obstacle = 10 * torch.exp(- 100 * bquad(Q,diff_obstacle))
return cost_goal + cost_obstacle
def obs_cost(self, x, t=0, terminal=False):
q = x[:, :self.nq]
ee = self.fwk(q, 6)[:, :3, 3]
diff_obstacle = ee - self.obstacle
diff_goal = ee - self.goal
Q_goal = self.Q_goal.expand(q.shape[0], *self.Q_goal.shape[1:])
Q_obstacle = self.Q_obstacle.expand(q.shape[0], *self.Q_obstacle.shape[1:])
cost_goal = bquad(Q_goal, diff_goal)
cost_obstacle = 10 * torch.exp(- 200 * bquad(Q_obstacle, diff_obstacle))
x = x[:, :self.nx]
diff_state = x - self.desired
Q_state = self.Q_state.expand(x.shape[0], *self.Q_state.shape[1:])
cost_state = bquad(Q_state, diff_state)
return cost_goal + cost_obstacle
def action_cost(self, u, t=0, terminal=False):
R = self.R.expand(u.shape[0], *self.R.shape[1:])
return bquad(R, u)