# Grasp from floor on front
# x = 0.3
# y = 0.0
# z = - 0.1
# roll = math.pi
# pitch = 0.4
# yaw = 0
kinematics_solver = KinematicsSolver("analytical")
# kinematics_solver = KinematicsSolver("geometrical")
print "IK"
if kinematics_solver.check_ik_solver_has_solution(
[x, y, z, roll, pitch, yaw], "/arm_link_0"):
joint_angles = kinematics_solver.get_ik_solution(
[x, y, z, roll, pitch, yaw], "/arm_link_0")
#print joint_angles
move_arm_component.to_joint_positions(joint_angles)
ik_result = True
print ik_result
else:
ik_result = False
print ik_result
# #### 7.move_arm_component.joint_velocities(joint_velocities)
# time = 5.0
# time_taken = 0
# init_time = rospy.get_rostime().secs
# while(init_time <= 0):
# init_time = rospy.get_rostime().secs
# print 'init time recorded is ' + repr(init_time) +' secs'
# #timer = rospy.Timer(rospy.Duration(1), my_callback)
# while(time_taken<time):
# now = rospy.get_rostime().secs
pitch = 0.4
yaw = 0
# Grasp from floor on front
# x = 0.3
# y = 0.0
# z = - 0.1
# roll = math.pi
# pitch = 0.4
# yaw = 0
kinematics_solver = KinematicsSolver("analytical")
# kinematics_solver = KinematicsSolver("geometrical")
print "IK"
if kinematics_solver.check_ik_solver_has_solution([x, y, z, roll, pitch, yaw], "/arm_link_0"):
joint_angles = kinematics_solver.get_ik_solution([x, y, z, roll, pitch, yaw], "/arm_link_0")
#print joint_angles
move_arm_component.to_joint_positions(joint_angles)
ik_result = True
print ik_result
else:
ik_result = False
print ik_result
# #### 7.move_arm_component.joint_velocities(joint_velocities)
# time = 5.0
# time_taken = 0
# init_time = rospy.get_rostime().secs
# while(init_time <= 0):
# init_time = rospy.get_rostime().secs
# print 'init time recorded is ' + repr(init_time) +' secs'
# #timer = rospy.Timer(rospy.Duration(1), my_callback)
# while(time_taken<time):
# now = rospy.get_rostime().secs
import roslib
roslib.load_manifest("edufill_blockly")
import rospy
import move_gripper_component
import move_arm_component
# move_gripper_component example script
#### 1.move_gripper_component.to_pose(command_string)
######### command_string-'OPEN'/'CLOSE'
#### 2.move_gripper_component.to_positions(gripper_value)
######### gripper_value=[left_gripper_value,right_gripper_value]
if __name__ == "__main__":
rospy.init_node('move_gripper')
# initialize the arm to a nice position
move_arm_component.to_joint_positions([0.2, 0.1, 0.1, 0.1, 0.1])
#move gripper by command- OPEN/CLOSE
move_gripper_component.to_pose('OPEN') # component 'to_pose()'
move_gripper_component.to_pose('CLOSE') # component 'to_pose()'
#move gripper by values -[left_gripper_value,right_gripper_value]
left_gripper_value = 0.0000 # range - 0 to 0.0115
right_gripper_value = 0.0
# input argument list
gripper_value = [left_gripper_value, right_gripper_value]
move_gripper_component.to_joint_positions(
gripper_value) # component 'to_joint_positions()'
move_gripper_component.to_pose('CLOSE')
import roslib
roslib.load_manifest("edufill_blockly")
import rospy
import move_gripper_component
import move_arm_component
# move_gripper_component example script
#### 1.move_gripper_component.to_pose(command_string)
######### command_string-'OPEN'/'CLOSE'
#### 2.move_gripper_component.to_positions(gripper_value)
######### gripper_value=[left_gripper_value,right_gripper_value]
if __name__ == "__main__":
rospy.init_node("move_gripper")
# initialize the arm to a nice position
move_arm_component.to_joint_positions([0.2, 0.1, 0.1, 0.1, 0.1])
# move gripper by command- OPEN/CLOSE
move_gripper_component.to_pose("OPEN") # component 'to_pose()'
move_gripper_component.to_pose("CLOSE") # component 'to_pose()'
# move gripper by values -[left_gripper_value,right_gripper_value]
left_gripper_value = 0.0000 # range - 0 to 0.0115
right_gripper_value = 0.0
# input argument list
gripper_value = [left_gripper_value, right_gripper_value]
move_gripper_component.to_joint_positions(gripper_value) # component 'to_joint_positions()'
move_gripper_component.to_pose("CLOSE")
