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 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 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 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 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 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 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 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 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 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 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 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 _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 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 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 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)
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:
print e
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()
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"
joint5.unit = "rad"
joint5.value = check_positions[4]
joint_pos.poisonStamp = poison
joint_pos.positions = [joint1, joint2, joint3, joint4, joint5]
print("Check Arm-joint")
for i in range(0, 5):
arm_joint_publisher.publish(joint_pos)
delay.sleep()
for i in range(0, 20):
joint1.value = joint_positions[0]
joint2.value = joint_positions[1]
joint3.value = joint_positions[2]
joint4.value = joint_positions[3]
joint5.value = joint_positions[4]
print("Waiting process")
delay.sleep()
def main():
pub = rospy.Publisher('/arm_1/arm_controller/position_command',
JointPositions,
queue_size=10)
rospy.init_node('arm_test')
'''initial positions'''
msg = JointPositions()
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
msg.positions[0].joint_uri = "arm_joint_2"
msg.positions[0].unit = "rad"
msg.positions[0].value = 0.011
msg.positions[0].joint_uri = "arm_joint_3"
msg.positions[0].unit = "rad"
msg.positions[0].value = -0.016
msg.positions[0].joint_uri = "arm_joint_4"
msg.positions[0].unit = "rad"
msg.positions[0].value = 0.0222
msg.positions[0].joint_uri = "arm_joint_5"
msg.positions[0].unit = "rad"
msg.positions[0].value = 0.0111
pub.publish(msg)
# time.sleep(1)
# msg.positions[0].joint_uri = "arm_joint_1"
# msg.positions[0].value = 1.3
# pub.publish(msg)
#
# msg.positions[0].joint_uri = "arm_joint_2"
# msg.positions[0].unit = "rad"
# msg.positions[0].value = 1.3
# pub.publish(msg)
#
# msg.positions[0].joint_uri = "arm_joint_3"
# msg.positions[0].unit = "rad"
# msg.positions[0].value = -1.2
# pub.publish(msg)
#
# time.sleep(2)
#
# msg.positions[0].joint_uri = "arm_joint_4"
# msg.positions[0].unit = "rad"
# msg.positions[0].value = 2.8
# pub.publish(msg)
# gripper_open.main()
#
# time.sleep(2)
#
#
# msg.positions[0].joint_uri = "arm_joint_2"
# msg.positions[0].unit = "rad"
# msg.positions[0].value = 2
# pub.publish(msg)
#
# gripper_close.main()
# time.sleep(2)
#
# msg.positions[0].joint_uri = "arm_joint_2"
# msg.positions[0].unit = "rad"
# msg.positions[0].value = 0.5
# pub.publish(msg)
#
# time.sleep(2)
msg.positions[0].joint_uri = "arm_joint_1"
msg.positions[0].unit = "rad"
msg.positions[0].value = 0.075
pub.publish(msg)
time.sleep(1)
msg.positions[0].joint_uri = "arm_joint_2"
msg.positions[0].unit = "rad"
msg.positions[0].value = 1.3
pub.publish(msg)
time.sleep(1)
msg.positions[0].joint_uri = "arm_joint_4"
msg.positions[0].unit = "rad"
msg.positions[0].value = 2.85
pub.publish(msg)
gripper_open.main()
time.sleep(1)
msg.positions[0].joint_uri = "arm_joint_2"
msg.positions[0].unit = "rad"
msg.positions[0].value = 0.5
pub.publish(msg)
rospy.spin()
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)
except Exception, e:
print e
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)
gripper_pos_pub = rospy.Publisher('/PG70_controller/command_pos', JointPositions)
gripper_vel_pub = rospy.Publisher('/PG70_controller/command_vel', JointVelocities)
rospy.sleep(1.0)
# set joint position
gripper_pos_msg = JointPositions()
gripper_pos = JointValue()
gripper_pos.timeStamp = rospy.Time.now()
gripper_pos.joint_uri = 'left_arm_top_finger_joint'
gripper_pos.unit = 'm'
gripper_pos.value = 0.025
gripper_pos_msg.positions = [gripper_pos]
gripper_pos_pub.publish(gripper_pos_msg)
rospy.sleep(5.0)
# send joint velocities
gripper_vel_msg = JointVelocities()
gripper_vel = JointValue()
gripper_vel.timeStamp = rospy.Time.now()
gripper_vel.joint_uri = 'left_arm_top_finger_joint'
gripper_vel.unit = 'm/s'
gripper_vel_msg.velocities = [gripper_vel]
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)
jv4 = JointValue()
jv4.joint_uri = "arm_joint_4"
jv4.value = 0.023
jv4.unit = "rad"
jv5 = JointValue()
jv5.joint_uri = "arm_joint_5"
jv5.value = 0.12
jv5.unit = "rad"
p = Poison()
jp.poisonStamp = p
jp.positions = [jv1, jv2, jv3, jv4, jv5]
pub.publish(jp)
pub.publish(jp)
except Exception, e:
print e
'''
JOINT LIMITS:
[ WARN] [1340337373.846618553]: Cannot set arm joint 1:
The setpoint angle is out of range. The valid range is between 0.0100692 rad and 5.84014 rad and it is: 0.01 rad
[ WARN] [1340337373.847057559]: Cannot set arm joint 2:
The setpoint angle is out of range. The valid range is between 0.0100692 rad and 2.61799 rad and it is: 0.01 rad
[ WARN] [1340337373.847447567]: Cannot set arm joint 3:
The setpoint angle is out of range. The valid range is between -5.02655 rad and -0.015708 rad and it is: -0.01 rad
def main(self):
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"
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)
rospy.sleep(1)
msg.positions[0].joint_uri = "arm_joint_2"
msg.positions[0].unit = "rad"
msg.positions[0].value = 0.011
pub.publish(msg)
rospy.sleep(1)
msg.positions[0].joint_uri = "arm_joint_3"
msg.positions[0].unit = "rad"
msg.positions[0].value = -0.016
pub.publish(msg)
rospy.sleep(1)
msg.positions[0].joint_uri = "arm_joint_4"
msg.positions[0].unit = "rad"
msg.positions[0].value = 0.0222
pub.publish(msg)
rospy.sleep(1)
msg.positions[0].joint_uri = "arm_joint_5"
msg.positions[0].unit = "rad"
msg.positions[0].value = 0.111
pub.publish(msg)
rospy.sleep(1)
print "time to move"
msg.positions[0].joint_uri = "arm_joint_1"
msg.positions[0].value = 0.011
pub.publish(msg)
rospy.sleep(1)
msg.positions[0].joint_uri = "arm_joint_2"
msg.positions[0].unit = "rad"
msg.positions[0].value = 1.3
pub.publish(msg)
rospy.sleep(1)
msg.positions[0].joint_uri = "arm_joint_3"
msg.positions[0].unit = "rad"
msg.positions[0].value = -1.2
pub.publish(msg)
rospy.sleep(1)
msg.positions[0].joint_uri = "arm_joint_5"
msg.positions[0].unit = "rad"
msg.positions[0].value = 2.8
pub.publish(msg)
rospy.sleep(1)
msg.positions[0].joint_uri = "arm_joint_4"
msg.positions[0].unit = "rad"
msg.positions[0].value = 2.5
pub.publish(msg)
rospy.sleep(1)
msg.positions[0].joint_uri = "arm_joint_3"
msg.positions[0].unit = "rad"
msg.positions[0].value = -1
pub.publish(msg)
gripper_open.main()
jv3.unit = "rad"
jv4 = JointValue()
jv4.joint_uri = "arm_joint_4"
jv4.value = 0.023
jv4.unit = "rad"
jv5 = JointValue()
jv5.joint_uri = "arm_joint_5"
jv5.value = 0.12
jv5.unit = "rad"
p = Poison()
jp.poisonStamp = p
jp.positions = [jv1, jv2, jv3, jv4, jv5]
pub.publish(jp)
pub.publish(jp)
except Exception, e:
print e
'''
JOINT LIMITS:
[ WARN] [1340337373.846618553]: Cannot set arm joint 1:
The setpoint angle is out of range. The valid range is between 0.0100692 rad and 5.84014 rad and it is: 0.01 rad
[ WARN] [1340337373.847057559]: Cannot set arm joint 2:
The setpoint angle is out of range. The valid range is between 0.0100692 rad and 2.61799 rad and it is: 0.01 rad
[ WARN] [1340337373.847447567]: Cannot set arm joint 3:
The setpoint angle is out of range. The valid range is between -5.02655 rad and -0.015708 rad and it is: -0.01 rad
[ WARN] [1340337373.853084080]: Cannot set arm joint 4:
# joint arm_joint_4 valid range is between 0.0221239 and 3.4292
# joint arm_joint_5 valid range is between 0.110619 and 5.64159
# joint_val_1.value = (((hand_normal_[0] /2) + 0.5) * (5.84014 - 0.0100692) ) + 0.0100692
# joint_val_2.value = (((hand_normal_[0] /2) + 0.5) * (2.61799 - 0.0100692) ) + 0.0100692
# joint_val_3.value = (((hand_normal_[0] /2) + 0.5) * (5.02655 - 0.015708) ) - 5.02655
# joint_val_4.value = (((hand_normal_[0] /2) + 0.5) * (3.4292 - 0.0221239) ) + 0.0221239
# joint_val_5.value = (((hand_normal_[0] /2) + 0.5) * (5.64159 - 0.110619) ) + 0.110619
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)
# print "x_vel: %f m/s, y_vel: %f m/s, ang_vel: %f m/s" % (twist.linear.x , twist.linear.y , twist.angular.z )
# print "pitch: %f m/s, yaw: %f m/s, roll: %f m/s" % (hand_pitch_ , hand_yaw_ , hand_roll_)
# r.sleep()
# try:
# sender()
# except rospy.ROSInterruptException:
# pass
# termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
