Abnormal movement of the robotic arm when I run GO-1 in Agobot g1 #144
Description
Thank you for your outstanding work，I run the code on Agibot G1 in genie_sim and I feed
payload = { "top": img_h, "right": img_r, "left": img_l, "instruction": "pick the box", "state": np.expand_dims(state, axis=0), "ctrl_freqs": np.array([30]), }this to GO-1model, the state is left_arm[0,7], left_ee[7,8], right_arm[8,15], right_ee[15,16].But I get the action is
action_queue: [4.031 0.441 0.012 5.094 -0.475 -0.473 0.398 0.043 -0.215 0.008 0.439 -0.055 -0.400 4.938 -0.171 0.020] action_queue shape: (30, 16) max min action: 5.3125 -0.51953125 action_queue: [3.922 0.543 0.019 5.094 -0.488 -0.455 0.379 0.085 -0.285 0.061 0.447 0.010 -0.355 4.875 -0.126 0.021] action_queue shape: (30, 16) max min action: 5.21875 -0.51171875 action_queue: [3.984 0.471 0.010 5.125 -0.500 -0.453 0.377 0.054 -0.243 0.010 0.436 0.016 -0.354 4.906 -0.115 0.015],the dataset_stats.json is "action": { "mean": [ -1.0137895, 0.6469389, 0.61943513, -0.9803505, 0.5940099, 0.9803505, 0.00141076, 0.0, 1.0361551, -0.6618055, -0.79369825, 0.8947412, -0.5716255, -0.9803505, -0.03847568, 0.0 ], "std": [ 0.54978794, 0.3524604, 0.53088915, 0.36732885, 0.47180042, 0.4573713, 0.6099905, 1.0, 0.55983984, 0.3511852, 0.54829943, 0.39228913, 0.5156797, 0.44490147, 0.629012, 1.0 ],I dont finetue the model.This is infer code
` rclpy.init()
sim_ros_node = SimROSNode()
spin_thread = threading.Thread(target=rclpy.spin, args=(sim_ros_node,))
spin_thread.start()
init_frame = True
bridge = CvBridge()
count = 0
SIM_INIT_TIME = 10
action_queue = None
instruction = None
test_unit = False
if not test_unit:
print("start infer client")
while rclpy.ok():
print("finish ros init")
if action_queue:
print("start action queue")
is_end = True if len(action_queue) == 1 else False
sim_ros_node.publish_joint_command(action_queue.popleft(), is_end)
# print("action_queue", len(action_queue))
else:
print("start else")
img_h_raw = sim_ros_node.get_img_head()
img_l_raw = sim_ros_node.get_img_left_wrist()
img_r_raw = sim_ros_node.get_img_right_wrist()
act_raw = sim_ros_node.get_joint_state()
infer_start = sim_ros_node.is_infer_start()
# print("img_h",img_h_raw )
print("recevie img")
if (init_frame or infer_start) and (
img_h_raw
and img_l_raw
and img_r_raw
and act_raw
and img_h_raw.header.stamp
== img_l_raw.header.stamp
== img_r_raw.header.stamp
):
print("finish receiving img ")
sim_time = get_sim_time(sim_ros_node)
if sim_time > SIM_INIT_TIME:
init_frame = False
count = count + 1
img_h = bridge.compressed_imgmsg_to_cv2(
img_h_raw, desired_encoding="rgb8"
)
img_l = bridge.compressed_imgmsg_to_cv2(
img_l_raw, desired_encoding="rgb8"
)
img_r = bridge.compressed_imgmsg_to_cv2(
img_r_raw, desired_encoding="rgb8"
)
state = np.array(act_raw.position)
payload = {
"top": img_h,
"right": img_r,
"left": img_l,
"instruction": "pick the box",
"state": np.expand_dims(state, axis=0),
"ctrl_freqs": np.array([30]),
}
print("befor send action from go1")
print("len of state", state.shape)
send_payload = policy.serialize_payload(payload)
actions = policy.predict_action(send_payload)
action_queue = deque(actions)
print("send action from go1", action_queue)
# print("action_queue len:", len(action_queue))
sim_ros_node.loop_rate.sleep()`,Where could there be a problem。