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 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(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.2 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)
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)
def pose_command_sender():
print "pose command sender running \n"
pub_pose = rospy.Publisher('/reflex_hand1/command', Command, queue_size=10)
rate = rospy.Rate(10)
sleep_time = 0.01
position_threshold = [ 1.57 ] * 4
pose_position = [0.0] * 4
pose_cmd = Command()
pose_cmd.position = pose_position
rospy.sleep(sleep_time)
pub_pose.publish(pose_cmd)
while not rospy.is_shutdown():
print "increamental change ... "
for i in range(4):
if pose_position[i] < position_threshold[i]:
pose_position[i] += 0.1
pose_cmd.position = pose_position
pub_pose.publish(pose_cmd)
rate.sleep()
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)
print "Where HAND_NAME is either hand1 or hand2"
exit(0)
hand = sys.argv[1]
rospy.init_node('reflex_sf_dof_tour')
pub = rospy.Publisher('/reflex_%s/command'%(hand,), Command, queue_size=10)
FINGER_CLOSED = 4.6
FINGER_PINCH = 3.5
PRESHAPE_CYLINDER = 0
PRESHAPE_SPHERICAL = 1.5
PRESHAPE_PINCH = 2.5
opencmd = Command()
opencmd.position = [0,0,0,0]
opencmd.velocity = [1,1,1,1]
opencmd.force = [1,1,1,1]
cmd = Command()
cmd.position = [0,0,0,0]
# preshape
for i in range(100):
if rospy.is_shutdown():
exit(0)
print "Opening..."
pub.publish(opencmd)
rospy.sleep(2)
cmd.position = [FINGER_CLOSED,FINGER_CLOSED,FINGER_CLOSED,0]
