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 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 jsPoscontrol(self, qd):
#Initialize Position Command Msgs
qposMsg = JointPositions()
qposMsg.positions = [
JointValue(),
JointValue(),
JointValue(),
JointValue(),
JointValue()
]
#Fill Position Msg
qposMsg.positions[0].joint_uri = 'arm_joint_1'
qposMsg.positions[0].unit = 'rad'
qposMsg.positions[0].value = qd[0] + self.q_offset[0]
qposMsg.positions[1].joint_uri = 'arm_joint_2'
qposMsg.positions[1].unit = 'rad'
qposMsg.positions[1].value = qd[1] + self.q_offset[1]
qposMsg.positions[2].joint_uri = 'arm_joint_3'
qposMsg.positions[2].unit = 'rad'
qposMsg.positions[2].value = qd[2] + self.q_offset[2]
qposMsg.positions[3].joint_uri = 'arm_joint_4'
qposMsg.positions[3].unit = 'rad'
qposMsg.positions[3].value = qd[3] + self.q_offset[3]
qposMsg.positions[4].joint_uri = 'arm_joint_5'
qposMsg.positions[4].unit = 'rad'
qposMsg.positions[4].value = qd[4] + self.q_offset[4]
#Publish Position Msg
self.pub_qpos.publish(qposMsg)
def jsPoscontrol(self,qd):
#Initialize Position Command Msgs
qposMsg = JointPositions()
qposMsg.positions = [JointValue(), JointValue(), JointValue(), JointValue(), JointValue()]
#Fill Position Msg
qposMsg.positions[0].joint_uri = 'arm_joint_1'
qposMsg.positions[0].unit = 'rad'
qposMsg.positions[0].value = qd[0] + self.q_offset[0]
qposMsg.positions[1].joint_uri = 'arm_joint_2'
qposMsg.positions[1].unit = 'rad'
qposMsg.positions[1].value = qd[1] + self.q_offset[1]
qposMsg.positions[2].joint_uri = 'arm_joint_3'
qposMsg.positions[2].unit = 'rad'
qposMsg.positions[2].value = qd[2] + self.q_offset[2]
qposMsg.positions[3].joint_uri = 'arm_joint_4'
qposMsg.positions[3].unit = 'rad'
qposMsg.positions[3].value = qd[3] + self.q_offset[3]
qposMsg.positions[4].joint_uri = 'arm_joint_5'
qposMsg.positions[4].unit = 'rad'
qposMsg.positions[4].value = qd[4] + self.q_offset[4]
#Publish Position Msg
self.pub_qpos.publish(qposMsg)
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 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 main():
pubPos = rospy.Publisher('/arm_1/arm_controller/position_command',
JointPositions,
queue_size=10)
rospy.init_node('armcommand', anonymous=True)
rate = rospy.Rate(1) # 10hz
jp = JointPositions()
joints = []
for i in range(0, 5):
joints.append(JointValue())
joints[i].joint_uri = "arm_joint_" + str(i + 1)
joints[i].unit = "rad"
while not rospy.is_shutdown():
joints[0].value = 3
joints[1].value = 1
joints[2].value = -1
joints[3].value = 1.85
joints[4].value = 3
jp.positions = joints
pubPos.publish(jp)
rate.sleep()
def main():
pubPos = rospy.Publisher('/arm_1/arm_controller/position_command',
JointPositions,
queue_size=10)
pubVel = rospy.Publisher('/arm_1/arm_controller/velocity_command',
JointVelocities,
queue_size=10)
rospy.init_node('armcommand', anonymous=True)
rate = rospy.Rate(1) # 10hz
jp = JointPositions()
joints = []
for i in range(0, 5):
joints.append(JointValue())
joints[i].joint_uri = "arm_joint_" + str(i + 1)
joints[i].unit = "rad"
jv = JointVelocities()
vels = []
for i in range(0, 5):
vels.append(JointValue())
vels[i].joint_uri = "arm_joint_" + str(i + 1)
vels[i].unit = "s^-1 rad"
joints[0].value = 3
joints[1].value = 1
joints[2].value = -1
joints[3].value = 1.85
joints[4].value = 3
j = 1
c = 1
inverse = False
while not rospy.is_shutdown():
if not inverse:
j = j + 1
if inverse:
j = j - 1
if j > 50:
inverse = True
c = -c
joints[0].value += 0
joints[1].value += c * 0.008 * j
joints[2].value += -c * 0.008 * j
joints[3].value += 0
joints[4].value += 0
jp.positions = joints
pubPos.publish(jp)
rate.sleep()
def main():
rospy.init_node('armcommand', anonymous=True)
pubPos = rospy.Publisher('/arm_1/arm_controller/position_command', JointPositions, queue_size=10)
pubVel = rospy.Publisher('/arm_1/arm_controller/velocity_command', JointVelocities, queue_size=10)
rospy.sleep(2) #wait for connection
rate = rospy.Rate(70) # 10hz
#initialise positions
jp = JointPositions()
joints = []
for i in range(0, 5):
joints.append(JointValue())
joints[i].joint_uri = "arm_joint_" + str(i+1)
joints[i].unit = "rad"
#initialise velocities
jv = JointVelocities()
vels = []
for i in range(0, 5):
vels.append(JointValue())
vels[i].joint_uri = "arm_joint_" + str(i+1)
vels[i].unit = "s^-1 rad"
#go to home position
for i in range(0,5):
joints[i].value = 0.05
joints[2].value = -joints[2].value
jp.positions = joints;
pubPos.publish(jp)
rospy.sleep(5)
#start main movement
j = 1
while not rospy.is_shutdown():
for i in range(0,5):
joints[i].value = 0.008*j
for i in range(0,5):
vels[i].value = 0.0001*j + abs(random.random() * 0.05)
joints[2].value = -joints[2].value
vels[2].value = -vels[2].value
jp.positions = joints;
jv.velocities = vels;
#pubPos.publish(jp)
pubVel.publish(jv)
j = j+1
rate.sleep()
def poseGrap1(self):
if self.state == 'home':
joint_1.value = 2.9544
joint_2.value = 2.617
joint_3.value = -1.0732
joint_4.value = 0.32213
joint_5.value = 3
pos = JointPositions()
pos.positions = (joint_1, joint_2, joint_3, joint_4, joint_5)
self.armPub.publish(pos)
self.state = 'grap1'
def poseStart(self):
if self.state == 'home':
joint_1.value = 0.01006921
joint_2.value = 0.01006921
joint_3.value = -0.0157081
joint_4.value = 0.0221391
joint_5.value = 0.11062
pos = JointPositions()
pos.positions = (joint_1, joint_2, joint_3, joint_4, joint_5)
self.armPub.publish(pos)
self.state = 'start'
def poseGrap2(self):
if self.state == 'home':
joint_1.value = 2.9
joint_2.value = 1.5
joint_3.value = -2.0
joint_4.value = 2.5
joint_5.value = 3
pos = JointPositions()
pos.positions = (joint_1, joint_2, joint_3, joint_4, joint_5)
self.armPub.publish(pos)
self.state = 'grap2'
def poseHome(self):
if self.state == 'start' or self.state == 'grap1' or self.state == 'grap2':
joint_1.value = 2.9
joint_2.value = 1.1
joint_3.value = -2.5
joint_4.value = 0.023
joint_5.value = 3
pos = JointPositions()
pos.positions = (joint_1, joint_2, joint_3, joint_4, joint_5)
self.armPub.publish(pos)
self.state = 'home'
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 main(self):
# rospy.init_node('putitback')
arm_pub = rospy.Publisher('/arm_1/arm_controller/position_command',
JointPositions,
queue_size=10)
# arm_ik_control.go_to_xyz(0.1, -0.2, 0.3, arm_pub)
initial_position.main()
gripper_open.main()
msg = JointPositions()
rospy.sleep(5)
print "moving joint 1" #move joint 3 and joint 2
msg.positions = [JointValue()]
msg.positions[0].timeStamp = rospy.Time.now()
msg.positions[0].unit = "rad"
msg.positions[0].joint_uri = "arm_joint_1"
msg.positions[0].value = 0.04
arm_pub.publish(msg)
rospy.sleep(2)
msg.positions[0].joint_uri = "arm_joint_5"
msg.positions[0].value = 2.8
arm_pub.publish(msg)
rospy.sleep(2)
msg.positions[0].joint_uri = "arm_joint_1"
msg.positions[0].value = 0.01005
arm_pub.publish(msg)
rospy.sleep(3)
print "moving joint 2"
msg.positions[0].joint_uri = "arm_joint_4"
msg.positions[0].value = 1.7
arm_pub.publish(msg)
rospy.sleep(2)
msg.positions[0].joint_uri = "arm_joint_3"
msg.positions[0].value = -0.6
arm_pub.publish(msg)
rospy.sleep(1)
msg.positions[0].joint_uri = "arm_joint_2"
msg.positions[0].value = 1.4
arm_pub.publish(msg)
gripper_close.main()
rospy.sleep(2)
initial_position.main()
arm_ik_control.go_to_xyz_rev(-0.069, -0.447, 0.12, arm_pub)
def main():
pub = rospy.Publisher('/arm_1/arm_controller/position_command',
JointPositions,
queue_size=10)
# rospy.init_node('arm_test')
'''initial positions'''
msg = JointPositions()
print "going to initial position"
print "moving joint 1"
msg.positions = [JointValue()]
msg.positions[0].timeStamp = rospy.Time.now()
msg.positions[0].joint_uri = "arm_joint_1"
msg.positions[0].unit = "rad"
msg.positions[0].value = 0.011
pub.publish(msg)
time.sleep(1)
print "moving joint 2"
msg.positions[0].joint_uri = "arm_joint_2"
msg.positions[0].unit = "rad"
msg.positions[0].value = 0.011
pub.publish(msg)
time.sleep(1)
print "moving joint 3"
msg.positions[0].joint_uri = "arm_joint_3"
msg.positions[0].unit = "rad"
msg.positions[0].value = -0.016
pub.publish(msg)
time.sleep(1)
print "moving joint 4"
msg.positions[0].joint_uri = "arm_joint_4"
msg.positions[0].unit = "rad"
msg.positions[0].value = 0.0222
# msg.positions[0].value = 1
pub.publish(msg)
time.sleep(1)
print "moving joint 5"
msg.positions[0].joint_uri = "arm_joint_5"
msg.positions[0].unit = "rad"
msg.positions[0].value = 0.111
pub.publish(msg)
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 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 gen_joint_msg(joint_angles):
msg_arr = [0 for i in range(5)]
for i in range(5):
msg_arr[i] = JointPositions()
msg_arr[i].positions = [JointValue()]
msg_arr[i].positions[0].timeStamp = rospy.Time.now()
msg_arr[i].positions[0].joint_uri = "arm_joint_"+str(i+1)
msg_arr[i].positions[0].unit = "rad"
# not sure about this, truncate the angles in the ik solution when they are out of range. Usually the difference is small
if (i == 3 and joint_angles[i] > 3.429):
msg_arr[i].positions[0].value = 3.429
elif (i == 2 and joint_angles[i] > -0.0157):
msg_arr[i].positions[0].value = -0.01571
else:
msg_arr[i].positions[0].value = joint_angles[i]
return msg_arr
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 move_gripper_to(self, pose_name):
if pose_name not in self.gripper_positions:
return False
joint_values = self.gripper_positions[pose_name]
print(pose_name, joint_values)
joint_uris = ['gripper_finger_joint_l', 'gripper_finger_joint_r']
joint_pos_msg = JointPositions()
joint_value_msg_list = []
for i in range(2):
joint_value = JointValue(joint_uri=joint_uris[i],
unit='m',
value=joint_values[i])
joint_value_msg_list.append(joint_value)
joint_pos_msg.positions = joint_value_msg_list
self.gripper_pos_pub.publish(joint_pos_msg)
rospy.sleep(3)
return True
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 poseCallback(simple_msg):
hard_msg = JointPositions()
for i in range(5):
hard_msg.positions.append(JointValue())
hard_msg.positions[i].joint_uri = 'arm_joint_' + str(i + 1)
hard_msg.positions[i].unit = 'rad'
hard_msg.positions[i].value = simple_msg.data[i]
pose_pub.publish(hard_msg)
def go_to(self, position):
jp_msg = JointPositions()
for i in range(N):
jp_msg.positions.append(JointValue())
jp_msg.positions[i].joint_uri = 'arm_joint_' + str(i + 1)
jp_msg.positions[i].unit = 'rad'
jp_msg.positions[i].value = position[i]
self.jp_pub.publish(jp_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 _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 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 createArmPositionCommand(newArmPosition, t):
msg = JointPositions()
for i, val in enumerate(newArmPosition, 1):
msg.positions.append(
JointValue(timeStamp=t,
value=val * pi / 180,
unit="rad",
joint_uri="arm_joint_%s" % (i, )))
return msg
def createGripperPositionCommand(newPos, t):
msg = JointPositions()
joint = JointValue(timeStamp=t,
value=newPos,
unit="m",
joint_uri="gripper_finger_joint_l")
msg.positions.append(joint)
joint.joint_uri = "gripper_finger_joint_r"
msg.positions.append(joint)
return msg
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 move_arm_to(self, pose_name):
if pose_name not in self.joint_angles:
return False
joint_values = self.joint_angles[pose_name]
print(pose_name, joint_values)
try:
self.arm.set_joint_value_target(joint_values)
except Exception as e:
rospy.logerr('unable to set target position: %s' % (str(e)))
return False
status = self.arm.go(wait=True)
if not status:
rospy.logwarn(
"Failed moving arm with moveit, trying position command")
joint_pos_msg = JointPositions()
joint_value_msg_list = self.create_joint_value_msg_list(
joint_values, 'rad')
joint_pos_msg.positions = joint_value_msg_list
self.joint_pos_pub.publish(joint_pos_msg)
rospy.sleep(3)
return True
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 goToPose(self, q):
jp_msg = JointPositions()
# for i in range(1):
# jp_msg.positions.append(JointValue())
# jp_msg.positions[i].joint_uri = 'arm_joint_' + str(i+1)
# jp_msg.positions[i].unit = 'rad'
jp_msg.positions.append(JointValue())
jp_msg.positions[0].joint_uri = 'arm_joint_5'
jp_msg.positions[0].unit = 'rad'
jp_msg.positions[0].value = q
self.jp_pub.publish(jp_msg)
def __init__(self):
u"""Конструктор класса."""
self.base_velocity = Twist()
self.gripper_position = JointPositions()
self.arm_velocities = JointVelocities()
self.base_speed_multiplier = 0.2
self.arm_speed_multiplier = 0.5
self.base_velocity_publisher = rospy.Publisher('/cmd_vel', Twist)
self.gripper_position_publisher = rospy.Publisher(
'/arm_1/gripper_controller/position_command', JointPositions)
self.arm_velocities_publisher = rospy.Publisher(
'/arm_1/arm_controller/velocity_command', JointVelocities)
rospy.Subscriber("/joy", Joy, self.update_data_from_joy)
def make_movements_callback(req):
global file_with_data
jp_pub = rospy.Publisher("arm_1/arm_controller/position_command",
JointPositions,
queue_size=10,
latch=True)
jp_msg = JointPositions()
for i in range(5):
jp_msg.positions.append(JointValue())
jp_msg.positions[i].joint_uri = 'arm_joint_' + str(i + 1)
jp_msg.positions[i].unit = 'rad'
file_with_poses = open(fwp_name, "r")
string_of_poses = file_with_poses.readlines()
file_with_poses.close()
js_sub = rospy.Subscriber("joint_states", JointState, js_callback)
r = rospy.Rate(1.0)
for pose in string_of_poses:
angles = [float(angle) for angle in pose.split()]
for i in range(5):
jp_msg.positions[i].value = angles[i]
jp_pub.publish(jp_msg)
achieved = False
while not achieved:
achieved = True
r.sleep()
lock.acquire()
for i in range(5):
if abs(angles[i] - cur_angles[i]) > 0.2:
achieved = False
break
lock.release()
js_sub.unregister()
# for returning to startup (embryo) position
for i in range(5):
jp_msg.positions[i].value = 0.025
jp_msg.positions[2].value = -0.02
jp_msg.positions[4].value = 0.12
jp_pub.publish(jp_msg)
lock.acquire()
file_with_data.close()
lock.release()
return EmptyResponse()
def main():
pub = rospy.Publisher('/arm_1/gripper_controller/position_command',
JointPositions,
queue_size=10)
#rospy.init_node('gripper_test')
msg = JointPositions()
msg.positions = [JointValue()]
msg.positions[0].timeStamp = rospy.Time.now()
msg.positions[0].joint_uri = "gripper_finger_joint_l"
msg.positions[0].unit = "m"
msg.positions[0].value = 0.0
pub.publish(msg)
msg.positions[0].joint_uri = "gripper_finger_joint_r"
msg.positions[0].unit = "m"
msg.positions[0].value = 0.0
pub.publish(msg)
time.sleep(1)
msg.positions[0].joint_uri = "gripper_finger_joint_l"
msg.positions[0].unit = "m"
msg.positions[0].value = 0.0114
pub.publish(msg)
time.sleep(1)
pub.publish(msg)
msg.positions[0].joint_uri = "gripper_finger_joint_r"
msg.positions[0].unit = "m"
msg.positions[0].value = 0.0114
pub.publish(msg)
def file_exit():
print "shutting down"
rospy.on_shutdown(file_exit)
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)
# msg.direction.z = hand_direction_[2]
# msg.normal.x = hand_normal_[0]
# msg.normal.y = hand_normal_[1]
# msg.normal.z = hand_normal_[2]
# msg.palmpos.x = hand_palm_pos_[0]
# msg.palmpos.y = hand_palm_pos_[1]
# msg.palmpos.z = hand_palm_pos_[2]
# msg.ypr.x = hand_yaw_
# msg.ypr.y = hand_pitch_
# msg.ypr.z = hand_roll_
# We don't publish native data types, see ROS best practices
# pub.publish(hand_direction=hand_direction_,hand_normal = hand_normal_, hand_palm_pos = hand_palm_pos_, hand_pitch = hand_pitch_, hand_roll = hand_roll_, hand_yaw = hand_yaw_)
#pub_ros.publish(msg)
# Save some CPU time, circa 100Hz publishing.
joint_pos = JointPositions()
joint_number = 0
key = getKey()
if key in moveBindings.keys():
joint_number = moveBindings[key]
print joint_number
default_joint_one = 0.0100692
default_joint_two = 0.0100692
default_joint_three = -5.02655
default_joint_four = 0.0221239
default_joint_five = 0.110619
joint_val_1 = JointValue()
import roslib; roslib.load_manifest('youbot_examples')
import rospy
import brics_actuator.msg
from brics_actuator.msg import JointPositions, JointValue, Poison
import sys, select, termios, tty, signal
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"
import brics_actuator.msg
from brics_actuator.msg import JointPositions, JointValue, Poison
import sys, select, termios, tty, signal
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"
def closeGripper(self):
gripper_l.value = 0.0025
gripper_r.value = 0.0025
pos = JointPositions()
pos.positions = (gripper_l, gripper_r)
self.gripperPub.publish(pos)
def openGripper(self):
gripper_l.value = 0.0115
gripper_r.value = 0.0115
pos = JointPositions()
pos.positions = (gripper_l, gripper_r)
self.gripperPub.publish(pos)
