diff --git a/cfg/active_grasp.yaml b/cfg/active_grasp.yaml index e2d24e1..3d1742e 100644 --- a/cfg/active_grasp.yaml +++ b/cfg/active_grasp.yaml @@ -1,6 +1,7 @@ bt_sim: gui: False cam_noise: False + gripper_force: 10 scene: mustard1.yaml grasp_controller: diff --git a/scripts/bt_sim_node.py b/scripts/bt_sim_node.py index a7a62ec..48cd295 100755 --- a/scripts/bt_sim_node.py +++ b/scripts/bt_sim_node.py @@ -242,6 +242,7 @@ class GraspActionPlugin(Plugin): super().__init__(rate) self.gripper = gripper self.dt = 1.0 / self.rate + self.force = rospy.get_param("~gripper_force") self.init_action_server() def init_action_server(self): @@ -253,7 +254,7 @@ class GraspActionPlugin(Plugin): def action_goal_cb(self): self.elapsed_time = 0.0 goal = self.action_server.accept_new_goal() - self.gripper.set_desired_speed(-0.1) + self.gripper.set_desired_speed(-0.1, force=self.force) def update(self): if self.action_server.is_active():