def open(self, req):
next_state = Command()
next_state.position = [0, 0, 0] + [self.current_state.goal_pos[3]]
next_state.velocity = [0.1, 0.1, 0.1, 0]
self.pub.publish(next_state)
#might need a sleep and a spin_once\
self.lock.acquire()
final_position = self.current_state
self.lock.release()
return OpenResponse(final_position)
def close(self, req):
self.lock.acquire()
initial_position = self.current_state.goal_pos
self.lock.release()
next_state = Command()
next_state.position = 3 * [req.joint_position] + [initial_position[3]]
next_state.velocity = [0.1, 0.1, 0.1, 0.0]
self.pub.publish(next_state)
#might need a sleep and a spin_once\
self.lock.acquire()
final_position = self.current_state
self.lock.release()
return CloseResponse(final_position)
def preshape(self, req):
self.lock.acquire()
initial_position = self.current_state.goal_pos
self.lock.release()
next_state = Command()
next_state.position = list(
initial_position[0:3]) + [req.preshape_position]
next_state.velocity = [0.0, 0.0, 0.0, 0.1]
self.pub.publish(next_state)
#might need a sleep and a spin_once
self.lock.acquire()
final_position = self.current_state
self.lock.release()
return PreshapeResponse(final_position)
def close_until_force(self, req):
self.lock.acquire()
initial_position = self.current_state.goal_pos
self.lock.release()
while (all([abs(x) < 0.20 for x in self.current_state.load])):
next_state = Command()
next_state.position = [x + .1 for x in initial_position[0:3]
] + [initial_position[3]]
initial_position = next_state.position
next_state.velocity = [0.1, 0.1, 0.1, 0]
self.pub.publish(next_state)
rospy.sleep(0.1)
self.lock.acquire()
final_position = self.current_state
self.lock.release()
return IncrementalCloseResponse(final_position)
import rospy
from reflex_sf_msgs.msg import Command
rospy.init_node('reflex_sf_dof_tour')
pub = rospy.Publisher('/reflex_hand2/command', Command, queue_size=10)
FINGER_CLOSED =2
FINGER_PINCH = 1
PRESHAPE_CYLINDER = 0
PRESHAPE_SPHERICAL = 1.5
PRESHAPE_PINCH = 2.5
# finger 1 (right-hand index) close
rospy.sleep(2)
command=Command()
command.position=[2,2,2,0]
command.velocity=[0,0,0,0]
command.force=[0.1,0.1,0.1,0.1]
pub.publish(command)
rospy.sleep(2)
command=Command()
command.position=[0.5,0.5,0.5,0]
command.velocity=[0,0,0,0]
command.force=[0.1,0.1,0.1,0.1]
pub.publish(command)
rospy.sleep(2)