in experiments/rolling/RollingEnv.py [0:0]
def simulate(self, goal, K):
"""
Simulate rolling ball to goal location with PD control, based on tactile signal
goal: goal ball location in tactile space, range 0-1, e.g. np.array([0.5, 0.5])
K: vel = K.dot(goal-state), e.g. np.array([[1,2],[3,4]])
"""
# Main simulation loop
simulation_time = 31 # Duration simulation
self.goal = goal
self.time_render = []
self.time_vis = []
self.reset()
xyz = [0, 0, 0.055]
costs = 0.0
vel = [0, 0]
self.logs["goal"] = goal
for i in range(simulation_time):
pb.changeConstraint(self.cid, xyz, maxForce=5)
# position, orientation = self.digits.get_pose(self.objId, -1)
self.step()
state = self.pose_estimation(self.color[1], self.depth[1])
vel = self.controller_Kx(state, goal, vel, K)
self.logs["states"].append(state)
xyz[:2] += vel
for _ in range(self.skipFrame):
xyz[:2] += vel
self.step(render=False)
c = self.cost_function(state, goal, vel, xyz=xyz)
costs += c
c = self.cost_function(state, goal, vel, xyz=xyz)
costs += c
return costs