def publish_to_youbot(jointState):
pub = rospy.Publisher('arm_1/arm_controller/position_command', JointPositions)
rospy.sleep(0.5)
try:
jp = JointPositions()
jv1 = JointValue()
jv1.joint_uri = jointState.name[0]
jv1.value = jointState.position[0]
jv1.unit = "rad"
jv2 = JointValue()
jv2.joint_uri = jointState.name[1]
jv2.value = jointState.position[1]
jv2.unit = "rad"
jv3 = JointValue()
jv3.joint_uri = jointState.name[2]
jv3.value = jointState.position[2]
jv3.unit = "rad"
jv4 = JointValue()
jv4.joint_uri = jointState.name[3]
jv4.value = jointState.position[3]
jv4.unit = "rad"
jv5 = JointValue()
jv5.joint_uri = jointState.name[4]
jv5.value = jointState.position[4]
jv5.unit = "rad"
p = Poison()
#print p
jp.poisonStamp = p
jp.positions = [jv1, jv2, jv3, jv4, jv5]
pub.publish(jp)
return 'arm moved successfully'
except Exception, e:
print e
return 'arm move failure'
def to_joint_positions(gripper_positions):
robot = os.getenv('ROBOT')
if robot == 'youbot-edufill':
left_gripper_val = gripper_positions[0]
right_gripper_val = gripper_positions[1]
pub = rospy.Publisher("/arm_1/gripper_controller/position_command",
JointPositions)
rospy.sleep(0.5)
try:
jp = JointPositions()
jv1 = JointValue()
jv1.joint_uri = "gripper_finger_joint_l"
jv1.value = left_gripper_val
jv1.unit = "m"
jv2 = JointValue()
jv2.joint_uri = "gripper_finger_joint_r"
jv2.value = right_gripper_val
jv2.unit = "m"
p = Poison()
jp.poisonStamp = p
jp.positions = [jv1, jv2] #list
pub.publish(jp)
rospy.sleep(1.0)
rospy.loginfo('Gripper control command published successfully')
except Exception, e:
rospy.loginfo('%s', e)
def gripperCommand(data):
'''moves the gripper to a predefined configuration.'''
rospy.loginfo(rospy.get_caller_id() + "gripper data: %s", data.data)
#creating a JointValue() List/Array
jvaluegrippers = [0 for i in range(2)]
#creating a JointValue() Object for gripper left
jvalue6 = JointValue()
jvalue6.joint_uri = 'gripper_finger_joint_l'
jvalue6.unit = 'm'
jvalue6.value = data.data[0]
jvaluegrippers[0] = jvalue6
#creating a JointValue() Object for gripper right
jvalue7 = JointValue()
jvalue7.joint_uri = 'gripper_finger_joint_r'
jvalue7.unit = 'm'
jvalue7.value = data.data[1]
jvaluegrippers[1] = jvalue7
#setting JointPosition msg properties
rate = rospy.Rate(20)
msg2 = JointPositions()
msg2.positions = jvaluegrippers
pubGripper.publish(msg2)
def update_gripper_position(self, data):
u"""Открывает/закрывает гриппер."""
self.gripper_position.positions = []
# Open gripper
if data.buttons[6]:
tmp_gripper_position_r = JointValue()
tmp_gripper_position_r.joint_uri = 'gripper_finger_joint_r'
tmp_gripper_position_r.value = 0.011499
tmp_gripper_position_r.unit = 'm'
tmp_gripper_position_r.timeStamp = rospy.Time.now()
self.gripper_position.positions.append(tmp_gripper_position_r)
tmp_gripper_position_l = JointValue()
tmp_gripper_position_l.joint_uri = 'gripper_finger_joint_l'
tmp_gripper_position_l.value = 0.011499
tmp_gripper_position_l.unit = 'm'
tmp_gripper_position_l.timeStamp = rospy.Time.now()
self.gripper_position.positions.append(tmp_gripper_position_l)
# Close gripper
if data.buttons[7]:
tmp_gripper_position_r = JointValue()
tmp_gripper_position_r.joint_uri = 'gripper_finger_joint_r'
tmp_gripper_position_r.value = 0
tmp_gripper_position_r.unit = 'm'
tmp_gripper_position_r.timeStamp = rospy.Time.now()
self.gripper_position.positions.append(tmp_gripper_position_r)
tmp_gripper_position_l = JointValue()
tmp_gripper_position_l.joint_uri = 'gripper_finger_joint_l'
tmp_gripper_position_l.value = 0
tmp_gripper_position_l.unit = 'm'
tmp_gripper_position_l.timeStamp = rospy.Time.now()
self.gripper_position.positions.append(tmp_gripper_position_l)
def make_grip_msg(opening_mm, joint_uri):
left = opening_mm/2000.
right = opening_mm/2000.
# create joint positions message
jp = JointPositions()
# create joint values message for both left and right fingers
jvl = JointValue()
jvr = JointValue()
# Fill in the gripper positions desired
# This is open position (max opening 0.0115 m)
jvl.joint_uri = joint_uri[5]
jvl.unit = 'm'
jvl.value = left
jvr.joint_uri = joint_uri[6]
jvr.unit = 'm'
jvr.value = right
# Append those onto JointPositions
jp.positions.append(copy.deepcopy(jvl))
jp.positions.append(copy.deepcopy(jvr))
return jp
def to_joint_positions(gripper_positions):
robot = os.getenv("ROBOT")
if robot == "youbot-edufill":
left_gripper_val = gripper_positions[0]
right_gripper_val = gripper_positions[1]
pub = rospy.Publisher("/arm_1/gripper_controller/position_command", JointPositions)
rospy.sleep(0.5)
try:
jp = JointPositions()
jv1 = JointValue()
jv1.joint_uri = "gripper_finger_joint_l"
jv1.value = left_gripper_val
jv1.unit = "m"
jv2 = JointValue()
jv2.joint_uri = "gripper_finger_joint_r"
jv2.value = right_gripper_val
jv2.unit = "m"
p = Poison()
jp.poisonStamp = p
jp.positions = [jv1, jv2] # list
pub.publish(jp)
rospy.sleep(1.0)
rospy.loginfo("Gripper control command published successfully")
except Exception, e:
rospy.loginfo("%s", e)
def update_gripper_position(self, data):
u"""Открывает/закрывает гриппер."""
self.gripper_position.positions = []
# Open gripper
if data.buttons[6]:
tmp_gripper_position_r = JointValue()
tmp_gripper_position_r.joint_uri = 'gripper_finger_joint_r'
tmp_gripper_position_r.value = 0.011499
tmp_gripper_position_r.unit = 'm'
tmp_gripper_position_r.timeStamp = rospy.Time.now()
self.gripper_position.positions.append(tmp_gripper_position_r)
tmp_gripper_position_l = JointValue()
tmp_gripper_position_l.joint_uri = 'gripper_finger_joint_l'
tmp_gripper_position_l.value = 0.011499
tmp_gripper_position_l.unit = 'm'
tmp_gripper_position_l.timeStamp = rospy.Time.now()
self.gripper_position.positions.append(tmp_gripper_position_l)
# Close gripper
if data.buttons[7]:
tmp_gripper_position_r = JointValue()
tmp_gripper_position_r.joint_uri = 'gripper_finger_joint_r'
tmp_gripper_position_r.value = 0
tmp_gripper_position_r.unit = 'm'
tmp_gripper_position_r.timeStamp = rospy.Time.now()
self.gripper_position.positions.append(tmp_gripper_position_r)
tmp_gripper_position_l = JointValue()
tmp_gripper_position_l.joint_uri = 'gripper_finger_joint_l'
tmp_gripper_position_l.value = 0
tmp_gripper_position_l.unit = 'm'
tmp_gripper_position_l.timeStamp = rospy.Time.now()
self.gripper_position.positions.append(tmp_gripper_position_l)
def jointStatesReceivedHandler(jointstates): global crntJointPositions global crntJointStates global gripperPositions crntJointPositions = JointPositions() gripperPositions = JointPositions() armStatesReceived = False if jointstates.name[0] == 'arm_joint_1': armStatesReceived = True if armStatesReceived == True: crntJointStates = copy.deepcopy(jointstates) for index in range(0, len(jointstates.position)-2): pos = JointValue() pos.joint_uri = "arm_joint_" + str(index + 1) pos.unit = "rad" pos.value = jointstates.position[index] crntJointPositions.positions.append(pos) pos = JointValue() pos.joint_uri = "gripper_finger_joint_l" pos.unit = "m" pos.value = jointstates.position[5] gripperPositions.positions.append(pos) pos = JointValue() pos.joint_uri = "gripper_finger_joint_r" pos.unit = "m" pos.value = jointstates.position[6] gripperPositions.positions.append(pos)
def make_grip_msg(opening_mm, joint_uri):
left = opening_mm / 2000.
right = opening_mm / 2000.
# create joint positions message
jp = JointPositions()
# create joint values message for both left and right fingers
jvl = JointValue()
jvr = JointValue()
# Fill in the gripper positions desired
# This is open position (max opening 0.0115 m)
jvl.joint_uri = joint_uri[5]
jvl.unit = 'm'
jvl.value = left
jvr.joint_uri = joint_uri[6]
jvr.unit = 'm'
jvr.value = right
# Append those onto JointPositions
jp.positions.append(copy.deepcopy(jvl))
jp.positions.append(copy.deepcopy(jvr))
return jp
def make_brics_msg_gripper(opening_m):
# Turn a desired gripper opening into a brics-friendly message
left = opening_m/2.
right = opening_m/2.
# create joint positions message
jp = JointPositions()
# create joint values message for both left and right fingers
jvl = JointValue()
jvr = JointValue()
# Fill in the gripper positions desired
# This is open position (max opening 0.0115 m)
jvl.joint_uri = 'gripper_finger_joint_l'
jvl.unit = 'm'
jvl.value = left
jvr.joint_uri = 'gripper_finger_joint_r'
jvr.unit = 'm'
jvr.value = right
# Append those onto JointPositions
jp.positions.append(copy.deepcopy(jvl))
jp.positions.append(copy.deepcopy(jvr))
return jp
def talkerGripper(G):
pub = rospy.Publisher('/arm_1/gripper_controller/position_command', JointPositions, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(5) # 10hz
count = 0
while not rospy.is_shutdown():
joint_pos = JointPositions()
joint_number = 1
joint_val_1 = JointValue()
joint_val_2 = JointValue()
joint_val_1.joint_uri = "gripper_finger_joint_l"
joint_val_1.unit = "rad"
joint_val_2.joint_uri = "agripper_finger_joint_r"
joint_val_2.unit = "rad"
joint_val_1 = JointValue()
joint_val_1.joint_uri = "gripper_finger_joint_l"
joint_val_1.unit = "m"
joint_val_1.value = G[0]
joint_val_2 = JointValue()
joint_val_2.joint_uri = "gripper_finger_joint_r"
joint_val_2.unit = "m"
joint_val_2.value = G[1]
poison = Poison()
joint_pos.poisonStamp = poison
joint_pos.positions = [joint_val_1, joint_val_2]
#pub_youbotleaparm.publish(joint_pos)
# rospy.loginfo(joint_pos)
pub.publish(joint_pos)
count+=1
rate.sleep()
def joint_velocities(joint_velocities):
robot = os.getenv('ROBOT')
if robot == 'youbot-edufill':
joint_velocity_1 = joint_velocities[0]
joint_velocity_2 = joint_velocities[1]
joint_velocity_3 = joint_velocities[2]
joint_velocity_4 = joint_velocities[3]
joint_velocity_5 = joint_velocities[4]
pub = rospy.Publisher('arm_1/arm_controller/velocity_command', JointVelocities)
rospy.sleep(0.5)
try:
jv = JointVelocities()
jv1 = JointValue()
jv1.joint_uri = "arm_joint_1"
jv1.value = joint_velocity_1
jv1.unit = "s^-1 rad"
jv2 = JointValue()
jv2.joint_uri = "arm_joint_2"
jv2.value = joint_velocity_2
jv2.unit = "s^-1 rad"
jv3 = JointValue()
jv3.joint_uri = "arm_joint_3"
jv3.value = joint_velocity_3
jv3.unit = "s^-1 rad"
jv4 = JointValue()
jv4.joint_uri = "arm_joint_4"
jv4.value = joint_velocity_4
jv4.unit = "s^-1 rad"
jv5 = JointValue()
jv5.joint_uri = "arm_joint_5"
jv5.value = joint_velocity_5
jv5.unit = "s^-1 rad"
p = Poison()
#print p
jv.poisonStamp = p
jv.velocities = [jv1, jv2, jv3, jv4, jv5]
pub.publish(jv)
return 'arm moved successfully'
except Exception, e:
print e
return 'arm move failure'
def create_gripper_msg(left_gripper, right_gripper):
jp = JointPositions()
jv1 = JointValue()
jv1.joint_uri = "gripper_finger_joint_l"
jv1.value = left_gripper
jv1.unit = "m"
jv2 = JointValue()
jv2.joint_uri = "gripper_finger_joint_r"
jv2.value = right_gripper
jv2.unit = "m"
p = Poison()
jp.poisonStamp = p
jp.positions = [jv1, jv2]
return jp
def create_gripper_msg (left_gripper, right_gripper): jp = JointPositions() jv1 = JointValue() jv1.joint_uri = "gripper_finger_joint_l" jv1.value = left_gripper jv1.unit = "m" jv2 = JointValue() jv2.joint_uri = "gripper_finger_joint_r" jv2.value = right_gripper jv2.unit = "m" p = Poison() jp.poisonStamp = p jp.positions = [jv1, jv2] return jp
def safeHomeCommand(data):
rospy.loginfo(rospy.get_caller_id() + "arm data: %s", data.data)
#creating a JointValue() List/Array
jvaluelist = [0 for i in range(4)]
# #creating a JointValue() Object for joint 1 (base)
# jvalue1 = JointValue()
# jvalue1.joint_uri = 'arm_joint_1'
# jvalue1.unit = 'rad'
# jvalue1.value = data.data[0]
# jvaluelist[0] = jvalue1
#creating a JointValue() Object for joint 2
jvalue2 = JointValue()
jvalue2.joint_uri = 'arm_joint_2'
jvalue2.unit = 'rad'
jvalue2.value = data.data[1]
jvaluelist[0] = jvalue2
#creating a JointValue() Object for joint 3
jvalue3 = JointValue()
jvalue3.joint_uri = 'arm_joint_3'
jvalue3.unit = 'rad'
jvalue3.value = data.data[2]
jvaluelist[1] = jvalue3
#creating a JointValue() Object for joint 4
jvalue4 = JointValue()
jvalue4.joint_uri = 'arm_joint_4'
jvalue4.unit = 'rad'
jvalue4.value = data.data[3]
jvaluelist[2] = jvalue4
#creating a JointValue() Object for joint 5
jvalue5 = JointValue()
jvalue5.joint_uri = 'arm_joint_5'
jvalue5.unit = 'rad'
jvalue5.value = data.data[4]
jvaluelist[3] = jvalue5
#setting JointPosition msg properties
rate = rospy.Rate(20)
msg = JointPositions()
msg.positions = jvaluelist
pubArm.publish(msg)
def Set(self,value):
grp_cmd = JointPositions()
j_cmd = JointValue()
j_cmd.joint_uri = self.joint_name
j_cmd.unit = 'm'
j_cmd.value = value
grp_cmd.positions.append(j_cmd)
print "Sending gripper command: " + str(j_cmd.value)
self._GripperCmdPublisher.publish(grp_cmd)
def create_joint_value_msg_list(self, joint_values, unit):
joint_value_msg_list = []
for i, joint_value in enumerate(joint_values):
joint_value_msg = JointValue()
joint_value_msg.joint_uri = 'arm_joint_' + str(i + 1)
joint_value_msg.unit = unit
joint_value_msg.value = joint_value
joint_value_msg_list.append(joint_value_msg)
return joint_value_msg_list
def make_brics_msg_arm(positions):
# create joint positions message
jp = JointPositions()
for ii in range(5):
jv = JointValue()
jv.joint_uri = 'arm_joint_' + str(ii+1)
jv.unit='rad'
jv.value = positions[ii]
jp.positions.append(copy.deepcopy(jv))
return jp
def _createGripperJointPositions(self, left, right):
jp = JointPositions()
jv1 = JointValue()
jv1.joint_uri = "gripper_finger_joint_l"
jv1.value = left
jv1.unit = "m"
jv2 = JointValue()
jv2.joint_uri = "gripper_finger_joint_r"
jv2.value = right
jv2.unit = "m"
p = Poison()
jp.poisonStamp = p
jp.positions = [jv1, jv2] #list
return jp
def create_null_velocity(unit):
msg = JointVelocities()
time = rospy.Time.now()
for joint_name in joint_names:
j = JointValue()
j.timeStamp = time
j.joint_uri = joint_name
j.unit = unit
msg.velocities.append(j)
return msg
def _createJointPositions(self, joint_config):
jp = JointPositions()
for i in range(5):
jv = JointValue()
jv.joint_uri = self.joint_names[i]
jv.value = joint_config[i]
jv.unit = "rad"
jp.positions.append(jv)
return jp
def moveGripper(self, gPublisher, floatVal):
jp = JointPositions()
myPoison = Poison()
myPoison.originator = 'yb_grip'
myPoison.description = 'whoknows'
myPoison.qos = 0.0
jp.poisonStamp = myPoison
nowTime = rospy.Time.now()
jvl = JointValue()
jvl.timeStamp = nowTime
jvl.joint_uri = 'gripper_finger_joint_l'
jvl.unit = 'm'
jvl.value = floatVal
jp.positions.append(jvl)
jvr = JointValue()
jvr.timeStamp = nowTime
jvr.joint_uri = 'gripper_finger_joint_r'
jvr.unit = 'm'
jvr.value = floatVal
jp.positions.append(jvr)
gPublisher.publish(jp)
def publish_gripper_width(self, width):
desiredPositions = JointPositions()
jointCommands = []
joint = JointValue()
joint.joint_uri = "gripper_finger_joint_l"
joint.unit = "m"
joint.value = width
jointCommands.append(joint)
joint = JointValue()
joint.joint_uri = "gripper_finger_joint_r"
joint.unit = "m"
joint.value = width
jointCommands.append(joint)
desiredPositions.positions = jointCommands
self.gripper_pub.publish(desiredPositions)
def publish_gripper_width(self, width):
desiredPositions = JointPositions()
jointCommands = []
joint = JointValue()
joint.joint_uri = "gripper_finger_joint_l"
joint.unit = "m"
joint.value = width
jointCommands.append(joint)
joint = JointValue()
joint.joint_uri = "gripper_finger_joint_r"
joint.unit = "m"
joint.value = width
jointCommands.append(joint)
desiredPositions.positions = jointCommands
self.gripper_pub.publish(desiredPositions)
def make_arm_msg(arm_js, joint_uri):
A1 = arm_js[0]
A2 = arm_js[1]
A3 = arm_js[2]
A4 = arm_js[3]
A5 = arm_js[4]
# create joint positions message
jp = JointPositions()
# create joint values message for all the joints
jv1 = JointValue()
jv2 = JointValue()
jv3 = JointValue()
jv4 = JointValue()
jv5 = JointValue()
# Fill in the arm positions.
jv1.joint_uri = joint_uri[0]
jv1.unit = 'rad'
jv1.value = A1
jv2.joint_uri = joint_uri[1]
jv2.unit = 'rad'
jv2.value = A2
jv3.joint_uri = joint_uri[2]
jv3.unit = 'rad'
jv3.value = A3
jv4.joint_uri = joint_uri[3]
jv4.unit = 'rad'
jv4.value = A4
jv5.joint_uri = joint_uri[4]
jv5.unit = 'rad'
jv5.value = A5
# Append those onto JointPositions
jp.positions.append(copy.deepcopy(jv1))
jp.positions.append(copy.deepcopy(jv2))
jp.positions.append(copy.deepcopy(jv3))
jp.positions.append(copy.deepcopy(jv4))
jp.positions.append(copy.deepcopy(jv5))
return jp
def make_arm_msg(arm_js, joint_uri):
A1 = arm_js[0]
A2 = arm_js[1]
A3 = arm_js[2]
A4 = arm_js[3]
A5 = arm_js[4]
# create joint positions message
jp = JointPositions()
# create joint values message for all the joints
jv1 = JointValue()
jv2 = JointValue()
jv3 = JointValue()
jv4 = JointValue()
jv5 = JointValue()
# Fill in the arm positions.
jv1.joint_uri = joint_uri[0]
jv1.unit = 'rad'
jv1.value = A1
jv2.joint_uri = joint_uri[1]
jv2.unit = 'rad'
jv2.value = A2
jv3.joint_uri = joint_uri[2]
jv3.unit = 'rad'
jv3.value = A3
jv4.joint_uri = joint_uri[3]
jv4.unit = 'rad'
jv4.value = A4
jv5.joint_uri = joint_uri[4]
jv5.unit = 'rad'
jv5.value = A5
# Append those onto JointPositions
jp.positions.append(copy.deepcopy(jv1))
jp.positions.append(copy.deepcopy(jv2))
jp.positions.append(copy.deepcopy(jv3))
jp.positions.append(copy.deepcopy(jv4))
jp.positions.append(copy.deepcopy(jv5))
return jp
def followPointsInFile(filename, pub=None, grippub=None, startpoint=0, endpoint=None, sleeptime=3):
ptsTool = wptool.WpTool(filename,'r')
readWp = {'wpnum':None, 'wp':None}
while not(readWp['wpnum'] == startpoint):
readWp = ptsTool.ReadWaypoint()
print ("Following waypoints...")
time.sleep(1)
while not(readWp['wpnum'] == endpoint):
jointPos = JointPositions()
gripperPos = JointPositions()
for index in range(0, 5):
pos = JointValue()
pos.joint_uri = "arm_joint_" + str(index + 1)
pos.unit = "rad"
pos.value = readWp['wp'][index]
jointPos.positions.append(pos)
pos = JointValue()
pos.joint_uri = "gripper_finger_joint_l"
pos.unit = 'm'
pos.value = readWp['wp'][5]
gripperPos.positions.append(pos)
pos = JointValue()
pos.joint_uri = "gripper_finger_joint_r"
pos.unit = 'm'
pos.value = readWp['wp'][6]
gripperPos.positions.append(pos)
if (not(pub == None)):
pub.publish(jointPos)
if (not(grippub == None)):
grippub.publish(gripperPos)
time.sleep(sleeptime)
readWp = ptsTool.ReadWaypoint()
ptsTool.Close()
def set_gripper_state(self, open_gripper=True):
"""Open/close gripper.
Arguments:
open_gripper (bool): if True - opens gripper, if False - otherwise
"""
self.gripper_position.positions = []
if open_gripper:
# Open gripper
tmp_gripper_position_r = JointValue()
tmp_gripper_position_r.joint_uri = 'gripper_finger_joint_r'
tmp_gripper_position_r.value = 0.011499
tmp_gripper_position_r.unit = 'm'
tmp_gripper_position_r.timeStamp = rospy.Time.now()
self.gripper_position.positions.append(tmp_gripper_position_r)
tmp_gripper_position_l = JointValue()
tmp_gripper_position_l.joint_uri = 'gripper_finger_joint_l'
tmp_gripper_position_l.value = 0.011499
tmp_gripper_position_l.unit = 'm'
tmp_gripper_position_l.timeStamp = rospy.Time.now()
self.gripper_position.positions.append(tmp_gripper_position_l)
else:
# Close gripper
tmp_gripper_position_r = JointValue()
tmp_gripper_position_r.joint_uri = 'gripper_finger_joint_r'
tmp_gripper_position_r.value = 0
tmp_gripper_position_r.unit = 'm'
tmp_gripper_position_r.timeStamp = rospy.Time.now()
self.gripper_position.positions.append(tmp_gripper_position_r)
tmp_gripper_position_l = JointValue()
tmp_gripper_position_l.joint_uri = 'gripper_finger_joint_l'
tmp_gripper_position_l.value = 0
tmp_gripper_position_l.unit = 'm'
tmp_gripper_position_l.timeStamp = rospy.Time.now()
self.gripper_position.positions.append(tmp_gripper_position_l)
self.gripper_position_publisher.publish(self.gripper_position)
def arm_up_recover(self):
arm_up = create_arm_up()
arm_up[0] = self.configuration[0]
arm_up[4] = self.configuration[4]
joint_positions = JointPositions()
# transform from ROS to BRICS message
for j in range(len(joint_names)):
joint_value = JointValue()
joint_value.joint_uri = joint_names[j]
joint_value.value = limit_joint(joint_names[j], arm_up[j])
joint_value.unit = self.unit
joint_positions.positions.append(joint_value)
self.position_pub.publish(joint_positions)
rospy.sleep(1)
def execute(self, goal):
rospy.logdebug("Executing command goal: " + str(goal))
jnt_pos = JointPositions()
for k in range(len(self.joint_names)):
value = JointValue()
value.joint_uri = self.joint_names[k]
value.unit = "m"
value.value = goal.command.position
jnt_pos.positions.append(value)
self.pub_joint_positions.publish(jnt_pos)
rospy.sleep(2.0)
self.server.set_succeeded()
def arm_up_recover(self):
arm_up = create_arm_up()
arm_up[0] = self.configuration[0]
arm_up[4] = self.configuration[4]
joint_positions = JointPositions()
# transform from ROS to BRICS message
for j in range(len(joint_names)):
joint_value = JointValue()
joint_value.joint_uri = joint_names[j]
joint_value.value = limit_joint(joint_names[j], arm_up[j])
joint_value.unit = self.unit
joint_positions.positions.append(joint_value)
self.position_pub.publish(joint_positions)
rospy.sleep(1)
def get_joint_pos_msg_from_joint_angles(joint_angles):
"""
Converts a list of joint angles to a JointPositions message
:joint_angles: list(float)
:returns: brics_actuator.JointPositions
"""
joint_position_msg = JointPositions()
for i, joint_angle in enumerate(joint_angles):
joint_value_msg = JointValue()
joint_value_msg.joint_uri = 'arm_joint_' + str(i + 1)
joint_value_msg.unit = 'rad'
joint_value_msg.value = joint_angle
joint_position_msg.positions.append(joint_value_msg)
return joint_position_msg
def publish_arm_joint_positions(self, joint_positions):
desiredPositions = JointPositions()
jointCommands = []
for i in range(5):
joint = JointValue()
joint.joint_uri = "arm_joint_" + str(i+1)
joint.unit = "rad"
joint.value = joint_positions[i]
jointCommands.append(joint)
desiredPositions.positions = jointCommands
self.arm_joint_pub.publish(desiredPositions)
def publish_arm_joint_velocities(self, Joint_Velocities):
desiredVelocities = JointVelocities()
jointCommands = []
for i in range(5):
joint = JointValue()
joint.joint_uri = "arm_joint_" + str(i + 1)
joint.unit = "s^-1 rad"
joint.value = Joint_Velocities[i]
jointCommands.append(joint)
desiredVelocities.velocities = jointCommands
self.arm_joint_vel_pub.publish(desiredVelocities)
def publish_arm_joint_positions(self, joint_positions):
desiredPositions = JointPositions()
jointCommands = []
for i in range(5):
joint = JointValue()
joint.joint_uri = "arm_joint_" + str(i + 1)
joint.unit = "rad"
joint.value = joint_positions[i]
jointCommands.append(joint)
desiredPositions.positions = jointCommands
self.arm_joint_pub.publish(desiredPositions)
def set_joints_angles(self, *args): # TODO: remove *args
u"""Set arm joints to defined angles in radians.
Arguments:
*args -- joints angles (j1, j2, j3, j4, j5)
Устанавливает углы поворота степеней подвижности в радианах.
Аргументы:
*args -- уголы соотвествующих степеней (j1, j2, j3, j4, j5)
"""
assert len(args) <= 5
self.joints_positions.positions = []
for i in range(5):
tmp = JointValue()
tmp.timeStamp = rospy.Time.now()
tmp.joint_uri = 'arm_joint_{}'.format(i + 1)
tmp.unit = 'rad'
tmp.value = args[i]
self.joints_positions.positions.append(tmp)
self.joints_positions_publisher.publish(self.joints_positions)
def set_joints_velocities(self, *args): # TODO: remove *args
u"""Set velocity for each joint.
Arguments:
*args -- velocity for each joint (j1, j2, j3, j4, j5)
Устанавливает скорость каждой степени подвижности в радианах/с.
Аргументы:
*args -- скорости соотвествующих степеней (j1, j2, j3, j4, j5)
"""
assert len(args) == 5
self.joints_velocities.velocities = []
for index, value in enumerate(args):
tmp = JointValue()
tmp.timeStamp = rospy.Time.now()
tmp.joint_uri = 'arm_joint_{}'.format(index + 1)
tmp.unit = 's^-1 rad'
tmp.value = value
self.joints_velocities.velocities.append(tmp)
self.joints_velocities_publisher.publish(self.joints_velocities)
arm_joint_publisher = rospy.Publisher(
"/arm_1/arm_controller/position_command",
JointPositions,
queue_size=10)
joint1 = JointValue()
joint2 = JointValue()
joint3 = JointValue()
joint4 = JointValue()
joint5 = JointValue()
poison = Poison()
joint_pos = JointPositions()
delay = rospy.Rate(10)
try:
print(msg)
joint1.joint_uri = "arm_joint_1"
joint1.unit = "rad"
joint1.value = check_positions[0]
joint2.joint_uri = "arm_joint_2"
joint2.unit = "rad"
joint2.value = check_positions[1]
joint3.joint_uri = "arm_joint_3"
joint3.unit = "rad"
joint3.value = check_positions[2]
joint4.joint_uri = "arm_joint_4"
joint4.unit = "rad"
joint4.value = check_positions[3]
joint5.joint_uri = "arm_joint_5"
from brics_actuator.msg import JointPositions, JointValue
from cob_srvs.srv import Trigger
if __name__ == "__main__":
rospy.init_node('gripper_open_node')
gripper_pos_pub = rospy.Publisher('/PG70_controller/command_pos', JointPositions)
gripper_recover_srv = rospy.ServiceProxy('/PG70_controller/recover', Trigger)
# recover
gripper_recover_srv()
rospy.sleep(4.0)
# open the gripper
joint_pos_msg = JointPositions()
joint_val = JointValue()
joint_val.timeStamp = rospy.Time.now()
joint_val.joint_uri = "left_arm_top_finger_joint"
joint_val.unit = "mm"
joint_val.value = 16
joint_pos_msg.positions.append(joint_val)
gripper_pos_pub.publish(joint_pos_msg)
rospy.sleep(4.0)
joint_pos_msg.positions[0].value = 4
gripper_pos_pub.publish(joint_pos_msg)
def execute(self, target="", blocking=True):
component_name = "gripper"
ah = ActionHandle("move_gripper", component_name, target, blocking)
rospy.loginfo("Move <<%s>> to <<%s>>", component_name, target)
# get pose from parameter server
if type(target) is str:
if not rospy.has_param(self.action_server_name_prefix + "/" + component_name + "/" + target):
rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...", self.action_server_name_prefix + "/" + component_name + "/" + target)
ah.set_failed(2)
return ah
param = rospy.get_param(self.action_server_name_prefix + "/" + component_name + "/" + target)
else:
param = target
# check pose
"""
if not type(param) is list: # check outer list
rospy.logerr("no valid parameter for %s: not a list, aborting...", component_name)
print "parameter is:", param
ah.set_failed(3)
return ah
else:
print i,"type1 = ", type(i)
DOF = 2
if not len(param) == DOF: # check dimension
rospy.logerr("no valid parameter for %s: dimension should be %d and is %d, aborting...", component_name, DOF, len(param))
print "parameter is:", param
ah.set_failed(3)
return ah
else:
for i in param:
print i,"type2 = ", type(i)
if not ((type(i) is float) or (type(i) is int)): # check type
print type(i)
rospy.logerr("no valid parameter for %s: not a list of float or int, aborting...", component_name)
print "parameter is:", param
ah.set_failed(3)
return ah
else:
rospy.logdebug("accepted parameter %f for %s", i, component_name)
"""
pose_goal = MoveToJointConfigurationGoal()
DOF = 2
for i in range(DOF):
jv = JointValue()
jv.joint_uri = self.gripper1_joint_names[i]
jv.value = param[i]
jv.unit = "m"
pose_goal.goal.positions.append(jv)
action_server_name = "/arm_1/gripper_controller/MoveToJointConfigurationDirect"
rospy.logdebug("calling %s action server", action_server_name)
client = actionlib.SimpleActionClient(action_server_name, MoveToJointConfigurationAction)
# trying to connect to server
rospy.logdebug("waiting for %s action server to start", action_server_name)
if not client.wait_for_server(rospy.Duration(5)):
# error: server did not respond
rospy.logerr("%s action server not ready within timeout, aborting...", action_server_name)
ah.set_failed(4)
return ah
else:
rospy.logdebug("%s action server ready", action_server_name)
print pose_goal
client.send_goal(pose_goal)
ah.set_client(client)
ah.wait_inside()
# it should block but it does not, so we wait!
rospy.sleep(3.0)
return ah
def execute(self, component_name, target="", blocking=True):
ah = ActionHandle("move_arm_joint_direct", component_name, target, blocking)
rospy.loginfo("Move <<%s>> DIRECT to <<%s>>", component_name, target)
# check pose
if not type(target) is list: # check outer list
rospy.logerr("no valid parameter for %s: not a list, aborting...", component_name)
print "parameter is:", target
ah.set_failed(3)
return ah
else:
#print i,"type1 = ", type(i)
DOF = 5
if not len(target) == DOF: # check dimension
rospy.logerr("no valid parameter for %s: dimension should be %d and is %d, aborting...", component_name, DOF, len(target))
print "parameter is:", target
ah.set_failed(3)
return ah
else:
for i in target:
#print i,"type2 = ", type(i)
if not ((type(i) is float) or (type(i) is int)): # check type
#print type(i)
rospy.logerr("no valid parameter for %s: not a list of float or int, aborting...", component_name)
print "parameter is:", target
ah.set_failed(3)
return ah
else:
rospy.logdebug("accepted parameter %f for %s", i, component_name)
pose_goal = MoveToJointConfigurationGoal()
for i in range(DOF):
jv = JointValue()
jv.joint_uri = self.arm1_joint_names[i]
jv.value = target[i]
jv.unit = "rad"
pose_goal.goal.positions.append(jv)
rospy.logdebug("calling %s action server", self.action_server_name)
# trying to connect to server
rospy.logdebug("waiting for %s action server to start", self.action_server_name)
if not self.client.wait_for_server(rospy.Duration(5)):
# error: server did not respond
rospy.logerr("%s action server not ready within timeout, aborting...", self.action_server_name)
ah.set_failed(4)
return ah
else:
rospy.logdebug("%s action server ready", self.action_server_name)
#print client_goal
self.client.send_goal(pose_goal)
ah.set_client(self.client)
ah.wait_inside()
return ah
pub = rospy.Publisher('/arm_1/gripper_controller/position_command',
JointPositions)
rospy.init_node('simple_gripper_joint_position')
rospy.sleep(0.5)
try:
jp = JointPositions()
# if value == 0: gripper is closed
# if value == 0.0115: griper is open
jv1 = JointValue()
jv1.joint_uri = "gripper_finger_joint_l"
jv1.value = 0.0115
jv1.unit = "m"
jv2 = JointValue()
jv2.joint_uri = "gripper_finger_joint_r"
jv2.value = 0.0115
jv2.unit = "m"
p = Poison()
jp.poisonStamp = p
jp.positions = [jv1, jv2] #list
pub.publish(jp)
rospy.sleep(1.0)
except Exception, e:
if __name__=="__main__":
pub = rospy.Publisher('/arm_1/gripper_controller/position_command', JointPositions)
rospy.init_node('simple_gripper_joint_position')
rospy.sleep(0.5)
try:
jp = JointPositions()
# if value == 0: gripper is closed
# if value == 0.0115: griper is open
jv1 = JointValue()
jv1.joint_uri = "gripper_finger_joint_l"
jv1.value = 0.01
jv1.unit = "m"
jv2 = JointValue()
jv2.joint_uri = "gripper_finger_joint_r"
jv2.value = 0.01
jv2.unit = "m"
p = Poison()
jp.poisonStamp = p
jp.positions = [jv1, jv2] #list
pub.publish(jp)
rospy.sleep(1.0)
def callback(data):
rospy.loginfo(data.data)
#creating a JointValue() List/Array
jvaluelist = [0 for i in range(2)]
#creating a JointValue() Object for joint 2
jvalue2 = JointValue()
jvalue2.joint_uri = 'arm_joint_2'
jvalue2.unit = 'rad'
jvalue2.value = 0.011
jvaluelist[0] = jvalue2
#creating a JointValue() Object for joint 3
jvalue3 = JointValue()
jvalue3.joint_uri = 'arm_joint_3'
jvalue3.unit = 'rad'
jvalue3.value = -0.016
jvaluelist[1] = jvalue3
#setting JointPosition msg properties
rate = rospy.Rate(20)
msg = JointPositions()
msg.positions = jvaluelist
pub_home.publish(msg)
rospy.sleep(5)
#creating a JointValue() List/Array
jvaluelist = [0 for i in range(3)]
#creating a JointValue() Object for joint 1 (base)
jvalue1 = JointValue()
jvalue1.joint_uri = 'arm_joint_1'
jvalue1.unit = 'rad'
jvalue1.value = 0.011
jvaluelist[0] = jvalue1
#creating a JointValue() Object for joint 4
jvalue4 = JointValue()
jvalue4.joint_uri = 'arm_joint_4'
jvalue4.unit = 'rad'
jvalue4.value = 0.0222
jvaluelist[1] = jvalue4
#creating a JointValue() Object for joint 5
jvalue5 = JointValue()
jvalue5.joint_uri = 'arm_joint_5'
jvalue5.unit = 'rad'
jvalue5.value = 0.111
jvaluelist[2] = jvalue5
rospy.sleep(5)
#setting JointPosition msg properties
rate = rospy.Rate(20)
msg = JointPositions()
msg.positions = jvaluelist
pub_home.publish(msg)
#creating a JointValue() List/Array
jvaluegrippers = [0 for i in range(2)]
#creating a JointValue() Object for gripper left
jvalue6 = JointValue()
jvalue6.joint_uri = 'gripper_finger_joint_l'
jvalue6.unit = 'm'
jvalue6.value = 0.001
jvaluegrippers[0] = jvalue6
#creating a JointValue() Object for gripper right
jvalue7 = JointValue()
jvalue7.joint_uri = 'gripper_finger_joint_r'
jvalue7.unit = 'm'
jvalue7.value = 0.001
jvaluegrippers[1] = jvalue7
#setting JointPosition msg properties
rate = rospy.Rate(20)
msg2 = JointPositions()
msg2.positions = jvaluegrippers
pub_gripper.publish(msg2)
def processJoints(self, joint_states):
armStatesReceived = joint_states.name[0] == 'arm_joint_1'
if armStatesReceived and self.joyMsg is not None:
self.crntJointStates = copy.deepcopy(joint_states)
for index in range(0, len(joint_states.position)-2):
pos = JointValue()
pos.joint_uri = "arm_joint_" + str(index + 1)
pos.unit = "rad"
pos.value = joint_states.position[index]
self.crntJointPositions.positions.append(pos)
pos = JointValue()
pos.joint_uri = "gripper_finger_joint_l"
pos.unit = "m"
pos.value = joint_states.position[5]
self.gripperPositions.positions.append(pos)
pos = JointValue()
pos.joint_uri = "gripper_finger_joint_r"
pos.unit = "m"
pos.value = joint_states.position[6]
self.gripperPositions.positions.append(pos)
# desiredPos = copy.deepcopy(self.crntJointPositions)
desiredGripPos = copy.deepcopy(self.gripperPositions)
self.joyMsg.axes = numpy.array(self.joyMsg.axes)
left_axis = self.joyMsg.axes[[0, 1]]
right_axis = self.joyMsg.axes[[2, 5]]
l2_axis = self.joyMsg.axes[[3]] * -1
r2_axis = self.joyMsg.axes[[4]] * -1
square, x, circle, triangle, l1, r1, l2, r2, share, options, l3, r3, ps_button, touch_button = [button == 1 for button in self.joyMsg.buttons]
left_dpad = self.joyMsg.axes[6] == 1
right_dpad = self.joyMsg.axes[6] == -1
up_dpad = self.joyMsg.axes[7] == 1
down_dpad = self.joyMsg.axes[7] == -1
if not self.previous_vel_change:
if square:
self.maxVel += 0.25
if circle:
self.maxVel -= 0.25
if self.maxVel < 0.25:
self.maxVel = 0.25
self.previous_vel_change = square or circle
# object that will carry all desired velocities to each joint
desiredVel = JointVelocities()
for index in range(0, len(joint_states.position)-2):
vel = JointValue()
vel.joint_uri = "arm_joint_" + str(index + 1)
vel.unit = "s^-1 rad" # radians per second
desiredVel.velocities.append(vel)
desiredVel.velocities[0].value -= left_axis[0] * self.maxVel
desiredVel.velocities[1].value -= left_axis[1] * self.maxVel
if not r1:
desiredVel.velocities[2].value -= right_axis[1] * self.maxVel
else:
desiredVel.velocities[3].value -= right_axis[1] * self.maxVel
desiredVel.velocities[4].value -= right_axis[0] * self.maxVel * 2
desiredGripPos.positions[0].value -= 0.0115 * r2_axis[0]
desiredGripPos.positions[1].value = desiredGripPos.positions[0].value
joint_message = ''
vel_message = ''
grip_message = ''
for jointNum in range(5):
joint_message += ('joint ' + str(jointNum) + ': ' + str(self.crntJointPositions.positions[jointNum].value) + ' ')
vel_message += ('veloc ' + str(jointNum) + ': ' + str(desiredVel.velocities[jointNum].value) + ' ')
for gripNum in [0, 1]:
desiredGripPos.positions[gripNum].value = self.checkValues(desiredGripPos.positions[gripNum].value, gripNum + 5)
grip_message += ('grip ' + str(gripNum) + ': ' + str(desiredGripPos.positions[gripNum].value) + ' ')
self.gripperPositions = desiredGripPos
print(joint_message)
print(vel_message)
print(grip_message)
self.gripPub.publish(desiredGripPos)
self.velPub.publish(desiredVel)
def talker(Q):
pub = rospy.Publisher('/arm_1/arm_controller/position_command', JointPositions, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(5) # 10hz
count = 0
while not rospy.is_shutdown():
joint_pos = JointPositions()
joint_number = 1
joint_val_1 = JointValue()
joint_val_2 = JointValue()
joint_val_3 = JointValue()
joint_val_4 = JointValue()
joint_val_5 = JointValue()
joint_val_1.joint_uri = "arm_joint_1"
joint_val_1.unit = "rad"
joint_val_2.joint_uri = "arm_joint_2"
joint_val_2.unit = "rad"
joint_val_3.joint_uri = "arm_joint_3"
joint_val_3.unit = "rad"
joint_val_4.joint_uri = "arm_joint_4"
joint_val_4.unit = "rad"
joint_val_5.joint_uri = "arm_joint_5"
joint_val_5.unit = "rad"
joint_val_1 = JointValue()
joint_val_1.joint_uri = "arm_joint_1"
joint_val_1.unit = "rad"
joint_val_1.value = Q[0] + 2.95 #adding offsets to all angles, we assume 0's
#in candle possition, youbot diver has 0's
#close to folded possition
joint_val_2 = JointValue()
joint_val_2.joint_uri = "arm_joint_2"
joint_val_2.unit = "rad"
joint_val_2.value = Q[1] + 1.05
joint_val_3 = JointValue()
joint_val_3.joint_uri = "arm_joint_3"
joint_val_3.unit = "rad"
joint_val_3.value = Q[2] - 2.44
joint_val_4 = JointValue()
joint_val_4.joint_uri = "arm_joint_4"
joint_val_4.unit = "rad"
joint_val_4.value = Q[3] + 1.73
joint_val_5 = JointValue()
joint_val_5.joint_uri = "arm_joint_5"
joint_val_5.unit = "rad"
joint_val_5.value = Q[4] + 2.95
poison = Poison()
joint_pos.poisonStamp = poison
joint_pos.positions = [joint_val_1, joint_val_2, joint_val_3, joint_val_4, joint_val_5]
#pub_youbotleaparm.publish(joint_pos)
# rospy.loginfo(joint_pos)
pub.publish(joint_pos)
count+=1
rate.sleep()
def update_arm_velocities(self, data):
u"""Задаёт скорости осей манипулятора."""
self.arm_velocities.velocities = []
# 1 joint
tmp_joint_value = JointValue()
tmp_joint_value.joint_uri = 'arm_joint_1'
tmp_joint_value.unit = 's^-1 rad'
tmp_joint_value.value = data.axes[2] * self.arm_speed_multiplier
self.arm_velocities.velocities.append(tmp_joint_value)
# 2 joint
tmp_joint_value = JointValue()
tmp_joint_value.joint_uri = 'arm_joint_2'
tmp_joint_value.unit = 's^-1 rad'
tmp_joint_value.value = data.axes[3] * self.arm_speed_multiplier
self.arm_velocities.velocities.append(tmp_joint_value)
# 3 joint
if notXOR(data.buttons[2], data.buttons[3]):
tmp_joint_value = JointValue()
tmp_joint_value.joint_uri = 'arm_joint_3'
tmp_joint_value.unit = 's^-1 rad'
tmp_joint_value.value = 0
self.arm_velocities.velocities.append(tmp_joint_value)
elif data.buttons[2]:
tmp_joint_value = JointValue()
tmp_joint_value.joint_uri = 'arm_joint_3'
tmp_joint_value.unit = 's^-1 rad'
tmp_joint_value.value = 1 * self.arm_speed_multiplier
self.arm_velocities.velocities.append(tmp_joint_value)
elif data.buttons[3]:
tmp_joint_value = JointValue()
tmp_joint_value.joint_uri = 'arm_joint_3'
tmp_joint_value.unit = 's^-1 rad'
tmp_joint_value.value = -1 * self.arm_speed_multiplier
self.arm_velocities.velocities.append(tmp_joint_value)
# 4 joint
if notXOR(data.buttons[0], data.buttons[1]):
tmp_joint_value = JointValue()
tmp_joint_value.joint_uri = 'arm_joint_4'
tmp_joint_value.unit = 's^-1 rad'
tmp_joint_value.value = 0
self.arm_velocities.velocities.append(tmp_joint_value)
elif data.buttons[0]:
tmp_joint_value = JointValue()
tmp_joint_value.joint_uri = 'arm_joint_4'
tmp_joint_value.unit = 's^-1 rad'
tmp_joint_value.value = 1 * self.arm_speed_multiplier
self.arm_velocities.velocities.append(tmp_joint_value)
elif data.buttons[1]:
tmp_joint_value = JointValue()
tmp_joint_value.joint_uri = 'arm_joint_4'
tmp_joint_value.unit = 's^-1 rad'
tmp_joint_value.value = -1 * self.arm_speed_multiplier
self.arm_velocities.velocities.append(tmp_joint_value)
# 5 joint
if notXOR(data.buttons[4], data.buttons[5]):
tmp_joint_value = JointValue()
tmp_joint_value.joint_uri = 'arm_joint_5'
tmp_joint_value.unit = 's^-1 rad'
tmp_joint_value.value = 0
self.arm_velocities.velocities.append(tmp_joint_value)
elif data.buttons[4]:
tmp_joint_value = JointValue()
tmp_joint_value.joint_uri = 'arm_joint_5'
tmp_joint_value.unit = 's^-1 rad'
tmp_joint_value.value = 1 * self.arm_speed_multiplier
self.arm_velocities.velocities.append(tmp_joint_value)
elif data.buttons[5]:
tmp_joint_value = JointValue()
tmp_joint_value.joint_uri = 'arm_joint_5'
tmp_joint_value.unit = 's^-1 rad'
tmp_joint_value.value = -1 * self.arm_speed_multiplier
self.arm_velocities.velocities.append(tmp_joint_value)
string_msg.data = "e_start"
joint_names = ["arm_joint_1" , "arm_joint_5"]
joint_value_=[4.0, 4.0]
for i in range (0,len(joint_names)):
joint_value_msg = JointValue()
joint_value_msg.joint_uri = joint_names[i]
joint_value_msg.value = joint_value_[i]
joint_value_msg.unit="rad"
joint_position_msg.positions.append(joint_value_msg)
loop_rate = rospy.Rate(10)
while not rospy.is_shutdown():
joint_position_pub.publish(joint_position_msg)
rospy.sleep(0.5)
joint_position_string_pub.publish(string_msg)
loop_rate.sleep()
def main(stdscr, persist):
pub = rospy.Publisher('out', JointPositions, queue_size=1)
rospy.init_node('bras_teleop', anonymous=True)
theta1, theta2, theta3, theta4, theta5 = 0.111, 0.11, -0.11, 0.11, 0.111
jointvalues = [theta1, theta2, theta3, theta4, theta5]
keycode = -1
rate = rospy.Rate(100)
stdscr.addstr("Command\n")
stdscr.addstr("- a/q : theta1 ++/--\n")
stdscr.addstr("- z/s : theta2 ++/--\n")
stdscr.addstr("- e/d : theta3 ++/--\n")
stdscr.addstr("- r/f : theta4 ++/--\n")
stdscr.addstr("- t/g : theta5 ++/--\n")
stdscr.addstr("- ESC : exit\n")
# We will wait for 100 ms for a key to be pressed
if (persist): stdscr.timeout(100)
while (not rospy.is_shutdown()) and (keycode != 27): # 27 is escape
keycode = stdscr.getch() # read pressed key
if keycode == -1: pass # No key has been pressed
elif keycode == ord('a'):
if (jointvalues[0] + 0.05 > 0.0100692
and jointvalues[0] + 0.05 < 5.84014):
jointvalues[0] += 0.05
elif keycode == ord('q'):
if (jointvalues[0] - 0.05 > 0.0100692
and jointvalues[0] - 0.05 < 5.84014):
jointvalues[0] -= 0.05
elif keycode == ord('z'):
if (jointvalues[1] + 0.05 > 0.0100692
and jointvalues[1] + 0.05 < 2.61799):
jointvalues[1] += 0.05
elif keycode == ord('s'):
if (jointvalues[1] - 0.05 > 0.0100692
and jointvalues[1] - 0.05 < 2.61799):
jointvalues[1] -= 0.05
elif keycode == ord('e'):
if (jointvalues[2] + 0.05 > -5.02655
and jointvalues[2] + 0.05 < -0.015708):
jointvalues[2] += 0.05
elif keycode == ord('d'):
if (jointvalues[2] - 0.05 > -5.02655
and jointvalues[2] - 0.05 < -0.015708):
jointvalues[2] -= 0.05
elif keycode == ord('r'):
if (jointvalues[3] + 0.05 > 0.0221239
and jointvalues[3] + 0.05 < 3.4292):
jointvalues[3] += 0.05
elif keycode == ord('f'):
if (jointvalues[3] - 0.05 > 0.0221239
and jointvalues[3] - 0.05 < 3.4292):
jointvalues[3] -= 0.05
elif keycode == ord('t'):
jointvalues[4] += 0.05
elif keycode == ord('g'):
jointvalues[4] -= 0.05
jp = JointPositions()
jv1 = JointValue()
jv2 = JointValue()
jv3 = JointValue()
jv4 = JointValue()
jv5 = JointValue()
jv1.joint_uri = "arm_joint_1"
jv1.unit = "rad"
jv1.value = jointvalues[0]
jv2.joint_uri = "arm_joint_2"
jv2.unit = "rad"
jv2.value = jointvalues[1]
jv3.joint_uri = "arm_joint_3"
jv3.unit = "rad"
jv3.value = jointvalues[2]
jv4.joint_uri = "arm_joint_4"
jv4.unit = "rad"
jv4.value = jointvalues[3]
jv5.joint_uri = "arm_joint_5"
jv5.unit = "rad"
jv5.value = jointvalues[4]
jp.positions.append(copy.deepcopy(jv1))
jp.positions.append(copy.deepcopy(jv2))
jp.positions.append(copy.deepcopy(jv3))
jp.positions.append(copy.deepcopy(jv4))
jp.positions.append(copy.deepcopy(jv5))
pub.publish(jp)
rate.sleep()
if __name__=="__main__":
pub = rospy.Publisher('arm_1/arm_controller/position_command', JointPositions)
rospy.init_node('simple_arm_gripper_position')
rospy.sleep(0.5)
try:
jp = JointPositions()
jv1 = JointValue()
jv1.joint_uri = "arm_joint_1"
jv1.value = 0.011
jv1.unit = "rad"
jv2 = JointValue()
jv2.joint_uri = "arm_joint_2"
jv2.value = 0.011
jv2.unit = "rad"
jv3 = JointValue()
jv3.joint_uri = "arm_joint_3"
jv3.value = -0.016
jv3.unit = "rad"
jv4 = JointValue()
jv4.joint_uri = "arm_joint_4"
jv4.value = 0.023
jv4.unit = "rad"
def main(stdscr, persist):
pub = rospy.Publisher('out',JointPositions, queue_size=1)
rospy.init_node('bras_teleop', anonymous=True)
theta1, theta2, theta3, theta4, theta5 = 0.111,0.11,-0.11,0.11,0.111
jointvalues = [theta1,theta2,theta3,theta4,theta5]
keycode = -1
rate = rospy.Rate(100)
stdscr.addstr("Command\n")
stdscr.addstr("- a/q : theta1 ++/--\n")
stdscr.addstr("- z/s : theta2 ++/--\n")
stdscr.addstr("- e/d : theta3 ++/--\n")
stdscr.addstr("- r/f : theta4 ++/--\n")
stdscr.addstr("- t/g : theta5 ++/--\n")
stdscr.addstr("- ESC : exit\n")
# We will wait for 100 ms for a key to be pressed
if(persist): stdscr.timeout(100)
while (not rospy.is_shutdown()) and (keycode != 27): # 27 is escape
keycode = stdscr.getch() # read pressed key
if keycode == -1 : pass # No key has been pressed
elif keycode == ord('a') :
if(jointvalues[0]+0.05> 0.0100692 and jointvalues[0]+0.05<5.84014 ):jointvalues[0]+=0.05
elif keycode == ord('q') :
if(jointvalues[0]-0.05>0.0100692 and jointvalues[0]-0.05< 5.84014 ):jointvalues[0]-=0.05
elif keycode == ord('z') :
if(jointvalues[1]+0.05> 0.0100692 and jointvalues[1]+0.05< 2.61799 ):jointvalues[1]+=0.05
elif keycode == ord('s') :
if(jointvalues[1]-0.05>0.0100692 and jointvalues[1]-0.05< 2.61799 ):jointvalues[1]-=0.05
elif keycode == ord('e') :
if(jointvalues[2]+0.05>-5.02655 and jointvalues[2]+0.05<-0.015708 ):jointvalues[2]+=0.05
elif keycode == ord('d') :
if(jointvalues[2]-0.05>-5.02655 and jointvalues[2]-0.05<-0.015708 ):jointvalues[2]-=0.05
elif keycode == ord('r') :
if(jointvalues[3]+0.05>0.0221239 and jointvalues[3]+0.05<3.4292 ):jointvalues[3]+=0.05
elif keycode == ord('f') :
if(jointvalues[3]-0.05>0.0221239 and jointvalues[3]-0.05<3.4292 ):jointvalues[3]-=0.05
elif keycode == ord('t') : jointvalues[4]+=0.05
elif keycode == ord('g') : jointvalues[4]-=0.05
jp = JointPositions()
jv1 = JointValue()
jv2 = JointValue()
jv3 = JointValue()
jv4 = JointValue()
jv5 = JointValue()
jv1.joint_uri ="arm_joint_1"
jv1.unit = "rad"
jv1.value =jointvalues[0]
jv2.joint_uri ="arm_joint_2"
jv2.unit = "rad"
jv2.value = jointvalues[1]
jv3.joint_uri = "arm_joint_3"
jv3.unit = "rad"
jv3.value = jointvalues[2]
jv4.joint_uri = "arm_joint_4"
jv4.unit = "rad"
jv4.value = jointvalues[3]
jv5.joint_uri = "arm_joint_5"
jv5.unit = "rad"
jv5.value = jointvalues[4]
jp.positions.append(copy.deepcopy(jv1))
jp.positions.append(copy.deepcopy(jv2))
jp.positions.append(copy.deepcopy(jv3))
jp.positions.append(copy.deepcopy(jv4))
jp.positions.append(copy.deepcopy(jv5))
pub.publish(jp)
rate.sleep()
pose.pose.position.z = z
quat = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
pose.pose.orientation.x = quat[0]
pose.pose.orientation.y = quat[1]
pose.pose.orientation.z = quat[2]
pose.pose.orientation.w = quat[3]
pose.header.frame_id = "/arm_link_0"
pose.header.stamp = rospy.Time.now()
# (conf, success) = iks.call_ik_solver(pose)
(conf, success) = iks.call_constraint_aware_ik_solver(pose)
if (success):
# publish solution directly as joint positions
print(conf)
jp = JointPositions()
for i in range(5):
jv = JointValue()
jv.joint_uri = iks.joint_names[i]
jv.value = conf[i]
jv.unit = "rad"
jp.positions.append(jv)
rospy.sleep(0.5)
print "publishing cmd"
armpub.publish(jp)
else:
print("IK solver didn't find a solution")
if __name__ == "__main__":
pub = rospy.Publisher('arm_1/arm_controller/position_command',
JointPositions)
rospy.init_node('simple_arm_gripper_position')
rospy.sleep(0.5)
try:
jp = JointPositions()
jv1 = JointValue()
jv1.joint_uri = "arm_joint_1"
jv1.value = 0.011
jv1.unit = "rad"
jv2 = JointValue()
jv2.joint_uri = "arm_joint_2"
jv2.value = 0.011
jv2.unit = "rad"
jv3 = JointValue()
jv3.joint_uri = "arm_joint_3"
jv3.value = -0.016
jv3.unit = "rad"
jv4 = JointValue()
jv4.joint_uri = "arm_joint_4"
jv4.value = 0.023
jv4.unit = "rad"
#!/usr/bin/env python
import roslib
roslib.load_manifest('youbot_pick_and_place')
import rospy
import sys
import select, termios, tty, signal
import moveit_commander, moveit_msgs.msg
from brics_actuator.msg import JointPositions, JointValue
import geometry_msgs.msg
gripper_l = JointValue()
gripper_l.joint_uri = 'gripper_finger_joint_l'
gripper_l.unit = 'm'
gripper_r = JointValue()
gripper_r.joint_uri = 'gripper_finger_joint_r'
gripper_r.unit = 'm'
joint_1 = JointValue()
joint_1.joint_uri = 'arm_joint_1'
joint_1.unit = 'rad'
joint_2 = JointValue()
joint_2.joint_uri = 'arm_joint_2'
joint_2.unit = 'rad'
joint_3 = JointValue()
joint_3.joint_uri = 'arm_joint_3'
joint_3.unit = 'rad'
joint_4 = JointValue()
joint_4.joint_uri = 'arm_joint_4'
joint_4.unit = 'rad'
joint_5 = JointValue()
joint_5.joint_uri = 'arm_joint_5'
joint_5.unit = 'rad'