def __init__(self):
self.kinematics = kinematics.Kinematics('arm')
self.move_arm = move_arm.MoveArm('arm')
# distance from object center to pre-grasp
self.pre_grasp_distance = rospy.get_param('~pre_grasp_distance')
# distance from object to grasp
self.grasp_distance = rospy.get_param('~grasp_distance')
# height of the post-grasp over the object center
self.post_grasp_height = rospy.get_param('~post_grasp_height')
# at which angle of approach to start searching for a solution
self.orbit_start_angle = 30.0 * math.pi / 180.0
rospy.loginfo('Pre-grasp distance: %f', self.pre_grasp_distance)
rospy.loginfo('Grasp distance: %f', self.grasp_distance)
rospy.loginfo('Post-grasp height: %f', self.post_grasp_height)
self.sss = simple_script_server.simple_script_server()
# action clients
#rospy.loginfo('Waiting for 'grasp_posture_control' action')
#self.hand_action = actionlib.SimpleActionClient('grasp_posture_control', object_manipulation_msgs.msg.GraspHandPostureExecutionAction)
#self.hand_action.wait_for_server()
#rospy.loginfo('Found action 'grasp_posture_control'')
# service servers
self.pickup_server = rospy.Service('pickup',
mdr_behavior_msgs.srv.Pickup,
self.pickup)
rospy.loginfo('"pickup" service advertised')
def __init__(self, default_pitch=(math.pi / 2.0)):
rospy.loginfo("Initializing arm control.")
rospy.loginfo("Waiting for [%s] server..." % (self.CART_SERVER))
self.move_arm_cart_server = SimpleActionClient(self.CART_SERVER, MoveArmAction)
self.move_arm_cart_server.wait_for_server()
self.script_server = simple_script_server()
self.pitch = default_pitch
def main():
rospy.init_node(NODE)
print "==> %s started " % NODE
# init
print "--> initializing sss"
sss = simple_script_server()
print "--> setup care-o-bot for capture"
# get position from parameter server
position_path = rospy.get_param('position_path', None)
if position_path is None:
print "[ERROR]: no path for positions set"
return
with open(position_path, 'r') as f:
positions = yaml.load(f)
system_path = rospy.get_param('system_path', None)
if system_path is None:
print "[ERROR]: no path for system configuration set"
return
with open(system_path, 'r') as f:
system = yaml.load(f)
print "==> capturing samples"
start = rospy.Time.now()
t = DhHardwareTest()
t.capture_loop(positions, system, sss)
print "finished after %s seconds" % (rospy.Time.now() - start).to_sec()
def __init__(self):
'''
Configures the calibration node
Reads configuration from parameter server or uses default values
'''
rospy.init_node(NODE)
print "==> started " + NODE
# get parameter from parameter server or set defaults
self.pattern_size = rospy.get_param('~pattern_size', "9x6")
self.square_size = rospy.get_param('~square_size', 0.03)
self.alpha = rospy.get_param('~alpha', 0.0)
self.verbose = rospy.get_param('~verbose', True)
self.save_result = rospy.get_param('~save_result', False)
# split pattern_size string into tuple, e.g '9x6' -> tuple(9,6)
self.pattern_size = tuple((int(self.pattern_size.split("x")[0]),
int(self.pattern_size.split("x")[1])))
self.camera_info = CameraInfo()
self.camera_info_received = False
self.latest_image = Image()
self.bridge = CvBridge()
# set up Checkerboard and CheckerboardDetector
self.board = Checkerboard(self.pattern_size, self.square_size)
self.detector = CheckerboardDetector(self.board)
self.sss = simple_script_server()
self.ts = TorsoState()
def __init__(self):
'''
Configures the calibration node
Reads configuration from parameter server or uses default values
'''
rospy.init_node(NODE)
print "==> started " + NODE
# get parameter from parameter server or set defaults
self.pattern_size = rospy.get_param('~pattern_size', "9x6")
self.square_size = rospy.get_param('~square_size', 0.03)
self.alpha = rospy.get_param('~alpha', 0.0)
self.verbose = rospy.get_param('~verbose', True)
self.save_result = rospy.get_param('~save_result', False)
# split pattern_size string into tuple, e.g '9x6' -> tuple(9,6)
self.pattern_size = tuple((int(self.pattern_size.split(
"x")[0]), int(self.pattern_size.split("x")[1])))
self.camera_info = CameraInfo()
self.camera_info_received = False
self.latest_image = Image()
self.bridge = CvBridge()
# set up Checkerboard and CheckerboardDetector
self.board = Checkerboard(self.pattern_size, self.square_size)
self.detector = CheckerboardDetector(self.board)
self.sss = simple_script_server()
self.ts = TorsoState()
def init():
global confirm
sss = simple_script_server()
say("I am ready. I will initialize myself.")
say("Please confirm.")
confirm = False
while confirm==False: rospy.sleep(0.1)
# initialize components
handle_head = sss.init("head")
handle_torso = sss.init("torso")
if handle_torso.get_error_code() != 0:
return say("failed to initialize torso")
handle_tray = sss.init("tray")
handle_arm = sss.init("arm")
if handle_arm.get_error_code() != 0:
return say("failed to initialize arm")
handle_sdh = sss.init("sdh")
handle_base = sss.init("base")
if handle_base.get_error_code() != 0:
return say("failed to initialize base")
# recover components
handle_head = sss.recover("head")
if handle_head.get_error_code() != 0:
return say("failed to recover head")
say("All components are ready. Please drive me around until I am localized.")
return True
def __init__(self, ui, parent = None):
QThread.__init__(self, parent)
self.ui = ui
self.mode="linear"
self.add[str].connect(self.onAdd)
self.rem[str].connect(self.onRem)
self.sub1 = rospy.Subscriber("/chromosom/addComponent", String, self.add_callback)
self.sub2 = rospy.Subscriber("/chromosom/remComponent", String, self.rem_callback)
self.pub_add = rospy.Publisher('/chromosom/addComponent', String)
self.pub_rem = rospy.Publisher('/chromosom/remComponent', String)
ui.btnOpen.clicked.connect(self.open_gripper)
ui.btnClose.clicked.connect(self.close_gripper)
ui.btnOn.clicked.connect(self.power_on)
ui.btnOff.clicked.connect(self.power_off)
ui.btnToolChange.clicked.connect(self.tool_change)
ui.rLin.clicked.connect(self.lin)
ui.rDWA.clicked.connect(self.DWA)
ui.btnA.clicked.connect(self.A)
ui.btnB.clicked.connect(self.B)
self.sss = simple_script_server()
def __init__(self):
self._ros = rosHelper.ROS()
self._ros.configureROS(packageName="cob_script_server")
import simple_script_server
self._ros.initROS()
self._ss = simple_script_server.simple_script_server()
def __main__():
rospy.init_node('cob_robot_calibration_generate_cal')
listener = tf.TransformListener()
rospy.sleep(1)
# sss = simple_script_server()
# sss.move("head", "back")
sss = simple_script_server()
sss.move("head", "back")
minimal_system = rospy.get_param('minimal_system', None)
sensors_path = rospy.get_param('sensors', None)
output_system = rospy.get_param('output_system', None)
free_system = rospy.get_param('free_system', None)
with open(sensors_path, "r") as f:
sensors = yaml.load(f)
pdb.set_trace()
transformations = get_chains(sensors)# only interested in the sensor chains entry not the rest
print sensors
pdb.set_trace()
transformation_dict = generate_transformation_dict(
transformations, listener)
print transformation_dict
with open(minimal_system, "r") as f:
system = yaml.load(f)
pdb.set_trace()
if 'transforms' in system:
t = dict(transformation_dict, **system['transforms'])
else:
t = transformation_dict
system['checkerboards'] = rospy.get_param('~checkerboards', None)
if system['checkerboards'] is None:
print '[ERROR]: Parameter checkerboards not found. Make sure it is set and try again'
return
system['transforms'] = t
dh_chains = generate_chain_parameter(sensors)
system['dh_chains'] = dh_chains
with open(output_system, 'w') as f:
f.write('####### This file is autogenerated. Do not edit #######\n')
f.write(yaml.dump(system))
free = system.copy()
free = settozero(free)
'''
print yaml.dump(system)
print "Calibration transformation suggestions are %s"%to_calib
print yaml.dump(free)
'''
with open(free_system, 'w') as f:
f.write(
'####### Use this file as template for free_0.yaml - free_2.yaml. #######\n')
f.write(yaml.dump(free))
def main():
rospy.init_node(NODE)
print "==> %s started " % NODE
rospy.sleep(4)
# service client
checkerboard_checker_name = "/image_capture/visibility_check"
visible = rospy.ServiceProxy(checkerboard_checker_name, Visible)
rospy.wait_for_service(checkerboard_checker_name, 6)
print "--> service client for for checking for chessboards initialized"
kinematics_capture_service_name = "/collect_data/capture"
capture_kinematics = rospy.ServiceProxy(kinematics_capture_service_name,
Capture)
rospy.wait_for_service(kinematics_capture_service_name, 6)
print "--> service client for capture robot_states initialized"
image_capture_service_name = "/image_capture/capture"
capture_image = rospy.ServiceProxy(image_capture_service_name, Capture)
rospy.wait_for_service(image_capture_service_name, 6)
print "--> service client for capture images initialized"
# init
print "--> initializing sss"
sss = simple_script_server()
'''
sss.init("base")
sss.init("torso")
sss.init("head")
sss.recover("base")
sss.recover("torso")
sss.recover("head")
print "-> set joint_stiffness to 2000"
stiffnessM = SetJointStiffnessRequest()
#print stiffnessM.joint_stiffness
stiffnessM.joint_stiffness = [1500]*7
jointstiffness_srv = rospy.ServiceProxy("/arm_controller/set_joint_stiffness",SetJointStiffness)
rospy.wait_for_service("/arm_controller/set_joint_stiffness",2)
print stiffnessM.joint_stiffness
print jointstiffness_srv(stiffnessM)
'''
print "--> setup care-o-bot for capture"
sss.move("head", "back")
# get position from parameter server
position_path = rospy.get_param('~position_path', None)
if position_path is None:
print "[ERROR]: no path for positions set"
return
with open(position_path, 'r') as f:
positions = yaml.load(f)
print "==> capturing samples"
start = rospy.Time.now()
capture_loop(positions, sss, visible, capture_kinematics, capture_image)
sss.move("arm", "calibration")
print "finished after %s seconds" % (rospy.Time.now() - start).to_sec()
def init_pos():
sss = simple_script_server()
sss.move("arm_right", "home")
sss.move("arm_right", [[
-0.039928546971938594, 0.7617065276699337, 0.01562043859727158,
-1.7979678396373568, 0.013899422367821046, 1.0327319085987252,
0.021045294231647915
]])
def __init__(self,servers):
rospy.init_node('grasp_deliver')
self.sss = simple_script_server.simple_script_server()
self.su = script_utils.script_utils()
self.util = ScriptUtils(servers)
self.sss.Init("tray")
self.sss.Init("torso")
self.sss.Init("arm")
self.sss.Init("sdh")
def init_pos():
sss = simple_script_server()
# sss.move("arm_right", [[-0.039928546971938594, 0.7617065276699337, 0.01562043859727158, -1.7979678396373568, 0.013899422367821046, 1.0327319085987252, 0.021045294231647915]])
sss.move("arm_left", [
[
2.274847163975995, -0.08568661652370757, -0.8273207226405264,
-1.9404110013940103, 1.306315205401929, 1.5627416614617742,
-1.1199725164070955
],
])
def main():
rospy.init_node(NODE)
print "==> started " + NODE
sss = simple_script_server()
# movements
print "--> moving arm to 'pregrasp'"
sss.move("arm", "pregrasp")
print "--> moving arm to 'calibration'"
sss.move("arm", "calibration")
def __init__(self):
'''
Configures the calibration node
Reads configuration from parameter server or uses default values
'''
# get parameter from parameter server or set defaults
self.pattern_size = rospy.get_param('~pattern_size', "9x6")
self.square_size = rospy.get_param('~square_size', 0.03)
self.alpha = rospy.get_param('~alpha', 0.0)
self.verbose = rospy.get_param('~verbose', True)
self.save_result = rospy.get_param('~save_result', False)
# split pattern_size string into tuple, e.g '9x6' -> tuple(9,6)
self.pattern_size = tuple((int(self.pattern_size.split("x")[0]),
int(self.pattern_size.split("x")[1])))
self.camera_info = CameraInfo()
self.camera_info_received = False
self.latest_image = Image()
self.bridge = CvBridge()
# set up Checkerboard and CheckerboardDetector
self.board = Checkerboard(self.pattern_size, self.square_size)
self.detector = CheckerboardDetector(self.board)
self.sss = simple_script_server()
topic_name = '/stereo/left/'
# subscribe to /cam3d/rgb/camera_info for camera_matrix and distortion coefficients
rospy.Subscriber(topic_name + 'camera_info', CameraInfo,
self.__camera_info_callback__)
# subscribe to /cam3d/rgb/image_raw for image data
rospy.Subscriber(topic_name + 'image_color', Image,
self.__image_raw_callback__)
# wait until camera informations are recieved.
start_time = rospy.Time.now()
while not (self.camera_info_received or rospy.is_shutdown()):
rospy.sleep(0.05)
if start_time + rospy.Duration(2.0) < rospy.Time.now():
# print warning every 2 seconds if the message is stil missing
print "--> still waiting for /cam3d/rgb/camera_info"
start_time = rospy.Time.now()
# convert camera matrix an distortion coefficients and store them
camera_matrix = self.camera_info.K
cm = np.asarray(camera_matrix)
self.cm = np.reshape(cm, (3, 3))
dist_coeffs = self.camera_info.D
self.dc = np.asarray(dist_coeffs)
self.frame = self.camera_info.header.frame_id
def __init__(self):
'''
Configures the calibration node
Reads configuration from parameter server or uses default values
'''
# get parameter from parameter server or set defaults
self.pattern_size = rospy.get_param('~pattern_size', "9x6")
self.square_size = rospy.get_param('~square_size', 0.03)
self.alpha = rospy.get_param('~alpha', 0.0)
self.verbose = rospy.get_param('~verbose', True)
self.save_result = rospy.get_param('~save_result', False)
# split pattern_size string into tuple, e.g '9x6' -> tuple(9,6)
self.pattern_size = tuple((int(self.pattern_size.split(
"x")[0]), int(self.pattern_size.split("x")[1])))
self.camera_info = CameraInfo()
self.camera_info_received = False
self.latest_image = Image()
self.bridge = CvBridge()
# set up Checkerboard and CheckerboardDetector
self.board = Checkerboard(self.pattern_size, self.square_size)
self.detector = CheckerboardDetector(self.board)
self.sss = simple_script_server()
topic_name = '/stereo/left/'
# subscribe to /cam3d/rgb/camera_info for camera_matrix and distortion coefficients
rospy.Subscriber(topic_name + 'camera_info', CameraInfo,
self.__camera_info_callback__)
# subscribe to /cam3d/rgb/image_raw for image data
rospy.Subscriber(
topic_name + 'image_color', Image, self.__image_raw_callback__)
# wait until camera informations are recieved.
start_time = rospy.Time.now()
while not (self.camera_info_received or rospy.is_shutdown()):
rospy.sleep(0.05)
if start_time + rospy.Duration(2.0) < rospy.Time.now():
# print warning every 2 seconds if the message is stil missing
print "--> still waiting for /cam3d/rgb/camera_info"
start_time = rospy.Time.now()
# convert camera matrix an distortion coefficients and store them
camera_matrix = self.camera_info.K
cm = np.asarray(camera_matrix)
self.cm = np.reshape(cm, (3, 3))
dist_coeffs = self.camera_info.D
self.dc = np.asarray(dist_coeffs)
self.frame = self.camera_info.header.frame_id
def __init__(self, timeout=120.0, arm_name='arm',
arm_recover_srv_name='/arm_controller/lwr_node/recover'):
smach.State.__init__(self, input_keys=['init_robot_goal'],
output_keys=['initialised_components'],
outcomes=['succeeded', 'failed'])
self.timeout = timeout
self.sss = simple_script_server.simple_script_server()
self.arm = moveit_commander.MoveGroupCommander(arm_name)
self.arm_recover_srv_name = arm_recover_srv_name
self.arm_recover_client = rospy.ServiceProxy(self.arm_recover_srv_name, Empty)
def init_pos():
# Trick to move base back to odom_combined
switch_controller = rospy.ServiceProxy('/base/controller_manager/switch_controller', SwitchController)
print(switch_controller(None, ['odometry_controller', ], 1)) # switch off
time.sleep(1.0)
print(switch_controller(['odometry_controller', ], None, 1)) # switch on
time.sleep(1.0)
sss = simple_script_server()
sss.move("arm_left", "side") # for better pics
sss.move("arm_right", [[0.6849513492283021, 0.9811503738420306, -0.05053975117653131, -1.4680375957626568, -0.0824580145043452, 0.4964406318714998, -0.5817382519875354]])
def setupRobot( robotConfig, req ):
rospy.loginfo( 'Preparing robot for navigation scenario ...' )
scriptServer = simple_script_server()
#config = yaml.load( file( robotConfig, 'r' ))
#if config:
# for key, value in config.items():
# rospy.loginfo( 'Moving %s to %s' % ( key, value ))
# scriptServer.move( key, value, blocking=True )
for key, value in robotConfig.items():
rospy.loginfo( 'Moving %s to %s' % ( key, value ))
scriptServer.move( key, value, blocking=True )
return True, ''
def execute(self):
if self.joint_goal is None:
error_code = self.plan(update_ps=False)
if not error_code.success:
raise error_code
#ToDo: in final version: use result from planning step instead of calling move_arm
sss = simple_script_server.simple_script_server()
if type(self.target) is str:
target = self.target
else:
target = [list(self.joint_goal.position)]
return MotionHandleSSS(sss, ('arm', target))
def main():
rospy.init_node(NODE)
print "==> %s started " % NODE
## service client
#checkerboard_checker_name = "/image_capture/visibility_check"
#visible = rospy.ServiceProxy(checkerboard_checker_name, Visible)
#rospy.wait_for_service(checkerboard_checker_name, 2)
#print "--> service client for for checking for chessboards initialized"
#kinematics_capture_service_name = "/collect_data/capture"
#capture_kinematics = rospy.ServiceProxy(
#kinematics_capture_service_name, Capture)
#rospy.wait_for_service(kinematics_capture_service_name, 2)
#print "--> service client for capture robot_states initialized"
#image_capture_service_name = "/image_capture/capture"
#capture_image = rospy.ServiceProxy(image_capture_service_name, Capture)
#rospy.wait_for_service(image_capture_service_name, 2)
#print "--> service client for capture images initialize
# init
print "--> initializing sss"
sss = simple_script_server()
sss.init("base")
sss.init("torso")
sss.init("head")
sss.recover("base")
sss.recover("torso")
sss.recover("head")
sss.move("arm_left", "home") #move to home position
sss.move("arm_right", "home") #move to home position
sss.move("head", "back") #move to calibration position
rospy.wait_for_service("/move_group/plan_execution/set_parameters",
timeout=30.0)
moveit_commander.roscpp_initialize(sys.argv) # init of moveit
robot = moveit_commander.RobotCommander() # init of moveit wrt robot
arm_left_group = moveit_commander.MoveGroupCommander(
"arm_left") # define groups
arm_right_group = moveit_commander.MoveGroupCommander("arm_right")
arm_left_group.set_pose_reference_frame('torso_3_link') #*** Set referece
arm_right_group.set_pose_reference_frame('torso_3_link') #*** Set referece
arm_list = [
arm_left_group, arm_right_group
] # list of groups for which we want to test the trajectories used for calibration
#scene = moveit_commander.PlanningSceneInterface() # init if scene if collisiton is to be avoided and for the addition of the cb pattern later
print "--> setup care-o-bot for capture"
start = rospy.Time.now()
for i in xrange(len(arm_list)):
trajectory_test_loop(arm_list[i])
sss.move(arm_list[i].get_name(), "home")
print "finished after %s seconds" % (rospy.Time.now() - start).to_sec()
sss.move("arm_left", "home") #move to home position
sss.move("arm_right", "home") #move to home position
def Start(self):
self.Parse()
global ah_counter
ah_counter = 0
self.sss = simple_script_server.simple_script_server()
self.graph = self.sss.action_handle.get_graph()
rospy.loginfo("Starting <<%s>> script...", self.basename)
self.Initialize()
self.Run()
# wait until last threaded action finishes
rospy.loginfo("Wait for script to finish...")
while ah_counter != 0:
rospy.sleep(1)
rospy.loginfo("...script finished.")
def Start(self):
self.Parse()
global ah_counter
ah_counter = 0
self.sss = simple_script_server.simple_script_server()
self.graph = self.sss.action_handle.get_graph()
rospy.loginfo("Starting <<%s>> script...",self.basename)
self.Initialize()
self.Run()
# wait until last threaded action finishes
rospy.loginfo("Wait for script to finish...")
while ah_counter != 0:
rospy.sleep(1)
rospy.loginfo("...script finished.")
def __main__():
rospy.init_node('cob_robot_calibration_generate_cal')
listener = tf.TransformListener()
rospy.sleep(1)
sss = simple_script_server()
sss.move("head", "back")
minimal_system = rospy.get_param('minimal_system', None)
sensors = rospy.get_param('sensors', None)
output_system = rospy.get_param('output_system', None)
free_system = rospy.get_param('free_system', None)
z = yaml.load(open(sensors, 'r'))
transformations = get_chains(z)
print z
transformation_dict = generate_transformation_dict(
transformations, listener)
print transformation_dict
system = yaml.load(open(minimal_system, 'r'))
if 'transforms' in system:
t = dict(transformation_dict, **system['transforms'])
else:
t = transformation_dict
system['checkerboards'] = rospy.get_param('~checkerboards', None)
if system['checkerboards'] is None:
print '[ERROR]: Parameter checkerboards not found. Make sure it is set and try again'
return
system['transforms'] = t
free = system.copy()
free = settozero(free)
'''
print yaml.dump(system)
print "Calibration transformation suggestions are %s"%to_calib
print yaml.dump(free)
'''
with open(output_system, 'w') as f:
f.write('####### This file is autogenerated. Do not edit #######\n')
f.write(yaml.dump(system))
with open(free_system, 'w') as f:
f.write('####### Use this file as template for free_0.yaml - free_2.yaml. #######\n')
f.write(yaml.dump(free))
def main():
rospy.init_node(NODE)
print "==> %s started " % NODE
# service client
image_capture_service_name = "/collect_data/capture"
capture = rospy.ServiceProxy(image_capture_service_name, Capture)
rospy.wait_for_service(image_capture_service_name, 1)
print "--> service client for capture images initialized"
# init
print "--> initializing sss"
sss = simple_script_server()
sss.init("base")
sss.init("torso")
sss.init("head")
sss.recover("base")
sss.recover("torso")
sss.recover("head")
print "--> setup care-o-bot for capture"
sss.move("head", "back")
# get position from parameter server
positions_back = rospy.get_param("/script_server/arm/all_robot_back")
positions_center = rospy.get_param("/script_server/arm/all_robot_center")
positions_right = rospy.get_param("/script_server/arm/all_robot_right")
positions_left = rospy.get_param("/script_server/arm/all_robot_left")
torso_positions = ["calib_front_left2", "calib_front_right2", "calib_right2", "calib_left2", "calib_back_left2", "calib_back_right2", "home"]
print "==> capturing images BACK"
sss.move("torso", "back")
capture_loop_arm(positions_back, sss, capture)
print "==> capturing images LEFT"
sss.move("torso", "left")
capture_loop_arm(positions_left, sss, capture)
print "==> capturing images RIGHT"
sss.move("torso", "right")
capture_loop_arm(positions_right, sss, capture)
print "==> capturing images CENTER"
sss.move("torso", "home")
capture_loop_arm(positions_center, sss, capture)
print "==> capturing TORSO samples"
sss.move("arm", "calibration")
capture_loop_torso(torso_positions, sss, capture)
def main():
rospy.init_node(NODE)
print "==> %s started " % NODE
rospy.sleep(4)
# service client
checkerboard_checker_name = "/image_capture/visibility_check"
visible = rospy.ServiceProxy(checkerboard_checker_name, Visible)
rospy.wait_for_service(checkerboard_checker_name, 2)
print "--> service client for for checking for chessboards initialized"
kinematics_capture_service_name = "/collect_data/capture"
capture_kinematics = rospy.ServiceProxy(
kinematics_capture_service_name, Capture)
rospy.wait_for_service(kinematics_capture_service_name, 2)
print "--> service client for capture robot_states initialized"
image_capture_service_name = "/image_capture/capture"
capture_image = rospy.ServiceProxy(image_capture_service_name, Capture)
rospy.wait_for_service(image_capture_service_name, 2)
print "--> service client for capture images initialized"
# init
print "--> initializing sss"
sss = simple_script_server()
sss.init("base")
sss.init("torso")
sss.init("head")
sss.recover("base")
sss.recover("torso")
sss.recover("head")
print "--> setup care-o-bot for capture"
sss.move("head", "back")
# get position from parameter server
position_path = rospy.get_param('~position_path', None)
if position_path is None:
print "[ERROR]: no path for positions set"
return
with open(position_path, 'r') as f:
positions = yaml.load(f)
print "==> capturing samples"
start = rospy.Time.now()
capture_loop(positions, sss, visible, capture_kinematics, capture_image)
sss.move("arm", "calibration")
print "finished after %s seconds" % (rospy.Time.now() - start).to_sec()
def Parse(self):
rospy.loginfo("Start parsing...")
global graph
global function_counter
function_counter = 0
# run script in simulation mode
self.sss = simple_script_server.simple_script_server(parse=True)
self.Initialize()
self.Run()
# save graph on parameter server for further processing
# self.graph = graph
rospy.set_param("/script_server/graph", self.graph.string())
self.graph_pub.publish(graph.string())
rospy.loginfo("...parsing finished")
function_counter = 0
return graph.string()
def Parse(self):
rospy.loginfo("Start parsing...")
global graph
global function_counter
function_counter = 0
# run script in simulation mode
self.sss = simple_script_server.simple_script_server(parse=True)
self.Initialize()
self.Run()
# save graph on parameter server for further processing
# self.graph = graph
rospy.set_param("/script_server/graph", self.graph.string())
self.graph_pub.publish(graph.string())
rospy.loginfo("...parsing finished")
function_counter = 0
return graph.string()
def main():
rospy.init_node(NODE)
print "==> %s started " % NODE
## service client
#checkerboard_checker_name = "/image_capture/visibility_check"
#visible = rospy.ServiceProxy(checkerboard_checker_name, Visible)
#rospy.wait_for_service(checkerboard_checker_name, 2)
#print "--> service client for for checking for chessboards initialized"
#kinematics_capture_service_name = "/collect_data/capture"
#capture_kinematics = rospy.ServiceProxy(
#kinematics_capture_service_name, Capture)
#rospy.wait_for_service(kinematics_capture_service_name, 2)
#print "--> service client for capture robot_states initialized"
#image_capture_service_name = "/image_capture/capture"
#capture_image = rospy.ServiceProxy(image_capture_service_name, Capture)
#rospy.wait_for_service(image_capture_service_name, 2)
#print "--> service client for capture images initialized"
# init
print "--> initializing sss"
sss = simple_script_server()
sss.init("base")
sss.init("torso")
sss.init("head")
sss.recover("base")
sss.recover("torso")
sss.recover("head")
print "--> setup care-o-bot for capture"
sss.move("head", "back")
# get position from parameter server
position_path = rospy.get_param('position_path', None)
if position_path is None:
print "[ERROR]: no path for positions set"
return
with open(position_path, 'r') as f:
positions = yaml.load(f)
print "==> capturing samples"
start = rospy.Time.now()
capture_loop(positions, sss)
print "finished after %s seconds" % (rospy.Time.now() - start).to_sec()
def execute(self, userdata):
# determine target position
if self.pose != '':
pose = self.pose
elif type(userdata.pose) is str:
pose = userdata.pose
elif type(userdata.pose) is list:
pose = []
pose.append(userdata.pose[0])
pose.append(userdata.pose[1])
pose.append(userdata.pose[2])
else: # this should never happen
userdata.message = []
userdata.message.append(5)
userdata.message.append('Invalid userdata "pose"')
userdata.message.append(userdata.pose)
return 'failed'
# try reaching pose
cob_script_server = simple_script_server.simple_script_server()
handle_base = cob_script_server.move('base', pose, False, self.mode)
move_second = False
timeout = 0
while True:
move_base_state = handle_base.get_state()
if (move_base_state == actionlib_msgs.msg.GoalStatus.SUCCEEDED
) and (not move_second):
# do a second movement to place the robot more exactly
handle_base = cob_script_server.move('base', pose, False,
self.mode)
move_second = True
elif (move_base_state == actionlib_msgs.msg.GoalStatus.SUCCEEDED
) and (move_second):
userdata.message = []
userdata.message.append(3)
userdata.message.append('Pose was succesfully reached')
return 'succeeded'
elif move_base_state == actionlib_msgs.msg.GoalStatus.REJECTED or move_base_state == actionlib_msgs.msg.GoalStatus.PREEMPTED or move_base_state == actionlib_msgs.msg.GoalStatus.ABORTED or move_base_state == actionlib_msgs.msg.GoalStatus.LOST:
self.speak_pub.publish('I can not reach my target position.')
rospy.logerr('base movement failed with state: %d',
move_base_state)
return 'failed'
rospy.sleep(0.1)
def __init__(self):
smach.State.__init__(self,
outcomes=['succeeded', 'failed'],
input_keys=['place_goal'],
output_keys=['place_feedback', 'place_result'])
self.kinematics = kinematics.Kinematics('arm')
self.move_arm = move_arm.MoveArm('arm')
# distance from object center to pre-grasp
self.pre_grasp_distance = rospy.get_param('~pre_grasp_distance')
# distance from object to grasp
self.grasp_distance = rospy.get_param('~grasp_distance')
# height of the post-grasp over the object center
self.post_grasp_height = rospy.get_param('~post_grasp_height')
# at which angle of approach to start searching for a solution
self.orbit_start_angle = 30.0 * math.pi / 180.0
rospy.loginfo('Pre-grasp distance: %f', self.pre_grasp_distance)
rospy.loginfo('Grasp distance: %f', self.grasp_distance)
rospy.loginfo('Post-grasp height: %f', self.post_grasp_height)
self.sss = simple_script_server.simple_script_server()
def main():
rospy.init_node(NODE)
print "==> %s started " % NODE
# service client
image_capture_service_name = "/image_capture/capture"
capture = rospy.ServiceProxy(image_capture_service_name, Capture)
rospy.wait_for_service(image_capture_service_name, 1)
print "--> service client for capture images initialized"
# init
print "--> initializing sss"
sss = simple_script_server()
sss.init("base")
sss.init("torso")
sss.init("head")
sss.recover("base")
sss.recover("torso")
sss.recover("head")
print "--> setup care-o-bot for capture"
sss.move("head", "back")
sss.move("torso", "home")
print "==> capturing images"
positions = rospy.get_param("/script_server/arm/all_intrinsic")
for pos in positions:
print "--> moving arm to position '%s'" % pos
sss.move("arm", pos)
sss.sleep(1.5)
print "--> capturing '%s' sample" % pos
res = capture()
if not res:
print "--> ERROR during capture, skipping sample..."
print "==> capturing finished, moving are to 'calibration' position"
sss.move("arm", "calibration")
def init():
global confirm
sss = simple_script_server()
say("I am ready. I will initialize myself.")
say("Please confirm.")
confirm = False
while confirm == False:
rospy.sleep(0.1)
# initialize components
handle_head = sss.init("head")
handle_torso = sss.init("torso")
if handle_torso.get_error_code() != 0:
return say("failed to initialize torso")
handle_tray = sss.init("tray")
handle_arm = sss.init("arm")
if handle_arm.get_error_code() != 0:
return say("failed to initialize arm")
handle_sdh = sss.init("sdh")
handle_base = sss.init("base")
if handle_base.get_error_code() != 0:
return say("failed to initialize base")
# recover components
handle_head = sss.recover("head")
if handle_head.get_error_code() != 0:
return say("failed to recover head")
say("All components are ready. Please drive me around until I am localized."
)
return True
def main():
rospy.init_node(NODE)
print "==> %s started " % NODE
# service client
image_capture_service_name = "/image_capture/capture"
capture = rospy.ServiceProxy(image_capture_service_name, Capture)
rospy.wait_for_service(image_capture_service_name, 1)
print "--> service client for capture images initialized"
# init
print "--> initializing sss"
sss = simple_script_server()
sss.init("base")
sss.init("torso")
sss.init("head")
sss.recover("base")
sss.recover("torso")
sss.recover("head")
print "--> setup care-o-bot for capture"
sss.move("head", "back")
sss.move("torso", "home")
print "==> capturing images"
positions = rospy.get_param("/script_server/arm/all_intrinsic")
for pos in positions:
print "--> moving arm to position '%s'" % pos
sss.move("arm", pos)
sss.sleep(1.5)
print "--> capturing '%s' sample" % pos
res = capture()
if not res: print "--> ERROR during capture, skipping sample..."
print "==> capturing finished, moving are to 'calibration' position"
sss.move("arm", 'calibration')
def execute(self):
if self.dryrun:
return MotionHandleDummy()
sss = simple_script_server.simple_script_server()
return MotionHandleSSS(sss, (self.name, self.target))
def __init__(self): print "script_utils: init" self.sss = simple_script_server.simple_script_server() random.seed()
def execute(self):
sss = simple_script_server.simple_script_server()
return MotionHandleSSS(sss, (self.name,self.target))
import rospy import smach import smach_ros import sys, os import math from ScreenFormatting import * from autopnp_dirt_detection.srv import * from cob_phidgets.srv import SetDigitalSensor from std_msgs.msg import Bool from cob_srvs.srv import Trigger from std_srvs.srv import Empty from diagnostic_msgs.msg import DiagnosticArray from simple_script_server import simple_script_server sss = simple_script_server() class GraspCoffee(smach.State): def __init__(self): smach.State.__init__(self, outcomes=['succeeded']) def execute(self, userdata ): sf = ScreenFormat(self.__class__.__name__) grasp_out = [1.2981758976333824, -0.8947954009124528, 0.654673002423073, -1.8360340132204749, 0.3464478565208744, -1.501436942320642, -0.8940972692116552] #[1.2491146923598218, -0.9378875801441929, 0.6548824419333124, -1.8174113501016953, 0.28647834342234924, -1.501402035735602, -0.894149629089215] grasp_in = [1.319172208534874, -1.29580224985067, 0.654655549130553, -0.718045907562987, 0.2054776128372924, -0.7244338126252863, -0.5730963131848581] #inter1 = [0.8891405341359913, -0.8558920118854992, 0.737803034695563, -1.9027404972316981, -0.026581364507873642, -0.27817057618285623, -0.9220748971211243] #inter2 = [1.0291159401459364, -0.5438969547989929, 0.22078415037728266, -1.7277363331342266, 0.36641442316368955, 1.29580224985067, -0.41905355340383854] inter1 = [2.4082053019017757, -0.8928580854427391, 0.013072516097437528, -1.4938622133669865, 1.1684281210401237, -1.4554475165305913, -0.8031132553051908] inter2 = [2.5702067630718894, -0.8928406321502192, -1.5809017831639436, -1.4338403403909015, 1.569417516685821, -1.319451461215193, -0.8031132553051908]
def __init__(self):
rospy.init_node('test_script')
self.sss = simple_script_server.simple_script_server()
rospy.Subscriber("/phidgetAO", Int16, self.phidget_callback)
self.phidget_data = 0
def __init__(self):
self._ros = rosHelper.ROS()
self._ros.configureROS(packageName='cob_script_server')
import simple_script_server
self._ros.initROS()
self._ss = simple_script_server.simple_script_server()
import roslib
roslib.load_manifest('autopnp_tool_change')
import rospy
import smach
import smach_ros
import tf
from tf.transformations import *
from cob_object_detection_msgs.msg import DetectionArray, Detection
from geometry_msgs import *
from geometry_msgs.msg import *
from simple_script_server import simple_script_server
sss = simple_script_server()
#from exploration_detection_cleaning import *
global TOOL_WAGON_MARKER_OFFSETS
TOOL_WAGON_MARKER_OFFSETS = { # todo: measure translational offsets
"front":
Transform(
translation=Vector3(x=0.055, y=-0.975, z=0.0),
rotation=Quaternion(x=-0.5, y=-0.5, z=-0.5, w=0.5)
), # quaternion_from_euler(-90.0/180.0*math.pi, -90.0/180.0*math.pi, 0.0, 'rzyx')
"rear":
Transform(
translation=Vector3(x=0.06, y=-1.025, z=-0.46),
rotation=Quaternion(x=-0.5, y=0.5, z=0.5, w=0.5)
), # quaternion_from_euler(90.0/180.0*math.pi, 90.0/180.0*math.pi, 0.0, 'rzyx')
def init_pos():
sss = simple_script_server()
# sss.move("arm_right", [[-0.039928546971938594, 0.7617065276699337, 0.01562043859727158, -1.7979678396373568, 0.013899422367821046, 1.0327319085987252, 0.021045294231647915]])
sss.move("arm_left", [[-0.17566952192977148, 0.6230441162870415, 0.27476319388433623, -1.1163222472275676, 0.3166667900974902, 0.5333301416131739, -0.32441227738211875], ])
def __init__(self):
rospy.init_node('test_script')
self.sss = simple_script_server.simple_script_server()
def main():
rospy.init_node(NODE)
print "==> %s started " % NODE
rospy.wait_for_service("/move_group/plan_execution/set_parameters",timeout=30.0)
chessboard_pose = rospy.Publisher('/cob_calibration/chessboard_pose', geometry_msgs.msg.PoseStamped, queue_size=1)
print 'chessboard_pose publisher activated'
moveit_commander.roscpp_initialize(sys.argv) # init of moveit
robot = moveit_commander.RobotCommander()# init of moveit wrt robot
# scene = moveit_commander.PlanningSceneInterface() # init if scene if collisiton is to be avoided and for the addition of the cb pattern later
print "============ Robot Groups:"
print robot.get_group_names()
print "============"
listener = tf.TransformListener()
rospy.sleep(1.5)
# torso_ik = rospy.ServiceProxy('/lookat_get_ik', GetPositionIK) # no Torso link
# init
print "--> initializing sss"
sss = simple_script_server()
sss.init("base")
sss.init("torso")
sss.init("head")
sss.recover("base")
sss.recover("torso")
sss.recover("head")
sss.move("arm_left","home")
sss.move("arm_right","home")
print "--> components initialized"
#camera_link_left = "/torso_cam3d_left_link" #renamed
#camera_link_right = "/torso_cam3d_right_link" #new addition
#[xhead, yhead, zhead] = get_position(listener, camera_link_left)[0]
#print "Positon wrt torso_cam3d_left"
#print xhead, yhead, zhead
#[xhead, yhead, zhead] = get_position(listener, camera_link_right)[0]
#print "Positon wrt torso_cam3d_left"
#print xhead, yhead, zhead
print "--> setup care-o-bot for capture"
# arm_left_group = moveit_commander.MoveGroupCommander("arm_left") # define groups
# arm_right_group = moveit_commander.MoveGroupCommander("arm_right")
# arm_left_group.set_pose_reference_frame('torso_3_link')#***
# arm_right_group.set_pose_reference_frame('torso_3_link')#***
# arm_list = [arm_left_group,arm_right_group]
# arm_planning_reference_frames = ['torso_cam3d_left_link','torso_cam3d_right_link']
# orientation_mean = [[-0.55194, -0.83389, 0, 0],
# [0.83389, 0.55194, 0, 0]]
# limits = [
# [[0.15074, 0.40, 0.52078-0.698747/2], #-0.457/2.0
# [0.6808, -0.40, 1.135-0.698747/2]],
# [[0.15074, 0.40, 0.52078-0.698747/2], #-0.457/2.0
# [0.6808, -0.40, 1.135-0.698747/2]]
# ]
# discritization = [[2,2,2],
# [2,2,2]]
try:
arms = rospy.get_param('/cob_calibration_executive/arms')
except:
raise NameError('Couldn\'t find the configuration parameters under \'~cob_calibration/arms\' namespace.')
config_list = []
for k in arms.keys():
config_list.append(arms[k])
arm_names = []
limits = []
discritization = []
orientation_mean = []
arm_planning_reference_frames = []
for i, arm in enumerate(config_list):
arm_names.append(arm['name'])
l1 = arm['limits_corner_1']
l2 = arm['limits_corner_2']
l1 = [float(eval(str(i))) for i in l1] # convert the limit values into float
l2 = [float(eval(str(i))) for i in l2]
limits.append([l1, l2])
d = map(int, arm['discritization'])
discritization.append(d)
h = arm['hand_orientation']
h = [float(eval(str(i))) for i in h] # convert the orientation values into float
orientation_mean.append(h)
arm_planning_reference_frames.append(str(arm['arm_planning_reference_frame']))
arm_list = []
for i, arm in enumerate(arm_names):
arm_list.append(moveit_commander.MoveGroupCommander(arm))
f = arm_planning_reference_frames[i]
arm_list[i].set_pose_reference_frame(f)
trajectory_joint_angles = [] # the first element of each entry is the Trajectory list for all successful calibration points while the second is the list of finla joint angles corresponding to that list
file_path = rospy.get_param('~output_path', None)
directory = os.path.dirname(file_path)
if file_path is not None:
if not os.path.exists(directory):
os.makedirs(directory)
#pdb.set_trace()
for i in xrange(len(arm_list)):
trajectory_joint_angles.append(generate_calibration_trajectory(
arm_list[i],
limits[i],
discritization[i],
orientation_mean[i],
arm_planning_reference_frames[i],
False,
chessboard_pose))
file_path_group_angles = file_path+arm_list[i].get_name()+"calibration_positions.yaml"
with open(file_path_group_angles, 'w') as f:
f.write('# autogenerated: Do not edit #\n')
f.write(yaml.dump(trajectory_joint_angles[i][1]))# this is the second entry of the element in the list corresponding to the joint angles for all successful calibration points
file_path_group_trajectories = file_path+arm_list[i].get_name()+"calibration_trajectories.yaml"
with open(file_path_group_trajectories, 'w') as f:
f.write('# autogenerated: Do not edit #\n')
f.write(yaml.dump(trajectory_joint_angles[i][0])) #this is the first entry of the element in the list corresponding to the joint trajectories for all successful calibration points
display_string = '%s ik solutions found for '% len(trajectory_joint_angles[i][0])
display_string = display_string + arm_list[i].get_name()
print (display_string)
file_path_cb_positions = file_path+arm_list[i].get_name()+"_cb_positions.yaml"
with open(file_path_cb_positions, 'w') as f:
f.write('# autogenerated: Do not edit #\n')
trajectory_joint_angles[i][2].append(arm_planning_reference_frames[i])
f.write(yaml.dump(trajectory_joint_angles[i][2]))
def main():
rospy.init_node(NODE)
print "==> %s started " % NODE
chessboard_pose = rospy.Publisher(
'/cob_calibration/chessboard_pose', PoseStamped)
print 'chessboard_pose publisher activated'
listener = tf.TransformListener()
rospy.sleep(1.5)
arm_ik = rospy.ServiceProxy('/compute_ik', GetPositionIKMoveit)
torso_ik = rospy.ServiceProxy('/lookat_get_ik', GetPositionIK)
'''
(t,r)=listener.lookupTransform('/arm_0_link','arm_7_link',rospy.Time(0))
a=calculate_ik((t,r), arm_ik)
print a[0]
'''
# init
print "--> initializing sss"
sss = simple_script_server()
sss.init("base")
sss.init("torso")
sss.init("head")
sss.recover("base")
sss.recover("torso")
sss.recover("head")
camera_link = "/cam_reference_link"
[xhead, yhead, zhead] = get_position(listener, camera_link)[0]
print xhead, yhead, zhead
print "--> setup care-o-bot for capture"
sss.move("head", "back")
calibration_seed = rospy.get_param("/script_server/arm/calibration")
#sss.move("arm",[a[0]])
nextPose = PoseStamped()
torso = TorsoIK()
# define cuboid for positions
# limits from base_link frame
limits = {'x': (-0.4, -1.0),
'y': (-0.3, 0.3),
'z': (0.5, 1.5)}
sample_density = {'x': 6,
'y': 6,
'z': 6}
sample_positions = {'x': [],
'y': [],
'z': []}
for key in limits.keys():
limits[key] = sorted(limits[key])
sample_positions[key].append(limits[key][0])
diff = limits[key][1] - limits[key][0]
step = 1.0 * diff / (sample_density[key] - 1)
# print key, ' ',diff,' ',step
while sample_positions[key][-1] + step <= (limits[key][1] + 0.01):
sample_positions[key].append(sample_positions[key][-1] + step)
joint_states = []
torso.get_torso_limits()
cb_links = ["/chessboard_center","/chessboard_lu_corner",
"/chessboard_ru_corner", "/chessboard_ll_corner",
"/chessboard_rl_corner"]
for x in sample_positions['x']:
for y in sample_positions['y']:
for z in sample_positions['z']:
#for q in quaternion:
for cb_link in cb_links:
print "\033[1;34mNew Position\033[1;m"
nextPose.header.frame_id = '/base_link'
nextPose.pose.position.x = x
nextPose.pose.position.y = y
nextPose.pose.position.z = z
# (0,0,0,1) for cob3-6
nextPose.pose.orientation.x = 0
nextPose.pose.orientation.y = 0
nextPose.pose.orientation.z = 0
nextPose.pose.orientation.w = 1
chessboard_pose.publish(nextPose)
rospy.sleep(0.2)
transformation_base_cb = get_position(
listener, '/chessboard_center')
[x1, y1, z1] = transformation_base_cb[0]
(dx, dy, dz) = (x1 - xhead, y1 - yhead, z1 - zhead)
roll = 0
pitch = math.atan2(dz, -dx)
yaw = -math.atan2(dy, -dx)
'''
Add noise to roll pitch and yaw values of cb
'''
std_dev = 0.3
roll = np.random.normal(roll,std_dev,1)[0]
pitch = np.random.normal(pitch,std_dev,1)[0]
yaw = np.random.normal(yaw,std_dev,1)[0]
q=tf.transformations.quaternion_from_euler(roll, pitch, yaw)
#print q
nextPose.pose.orientation.x = q[0]
nextPose.pose.orientation.y = q[1]
nextPose.pose.orientation.z = q[2]
nextPose.pose.orientation.w = q[3]
chessboard_pose.publish(nextPose)
rospy.sleep(0.2)
transformation_base_cb = get_position(
listener, cb_link)
#if not torso.in_range(angles):
#continue
#print t
try:
torso_js = lookat(transformation_base_cb ,torso_ik)[:len(torso.limits)]
except:
break
if torso_js[0] is None:
break
if not torso.valid_ik(torso_js):
break
print '\033[1;33mTorso solution found\033[1;m'
#(t, r) = get_cb_pose(listener, '/head_cam3d_link')
(t, r) = get_cb_pose(listener, '/base_link')
js = calculate_ik((
t, r), arm_ik, calibration_seed[0])
if js[0] is not None:
joint_states.append({'joint_position': js[0]
, 'torso_position': list(torso_js)})
print joint_states[-1]
print '\033[1;32mIK solution found\033[1;m'
else:
print '\033[1;31mNo solution found\033[1;m'
path = rospy.get_param('~output_path', None)
directory = os.path.dirname(path)
if path is not None:
if not os.path.exists(directory):
os.makedirs(directory)
with open(path, 'w') as f:
f.write('# autogenerated: Do not edit #\n')
f.write(yaml.dump(joint_states))
else:
print yaml.dump(joint_states)
print '%s ik solutions found' % len(joint_states)
def __init__(self):
rospy.init_node('test_script')
self.sss = simple_script_server.simple_script_server()
rospy.Subscriber("/phidgetAO", Int16, self.phidget_callback)
self.phidget_data = 0
def execute(self):
if self.dryrun:
return MotionHandleDummy()
sss = simple_script_server.simple_script_server()
return MotionHandleSSS(sss, (self.name,self.target))
def main():
rospy.init_node(NODE)
print "==> %s started " % NODE
chessboard_pose = rospy.Publisher(
'/cob_calibration/chessboard_pose', PoseStamped)
print 'chessboard_pose publisher activated'
listener = tf.TransformListener()
rospy.sleep(1.5)
arm_ik = rospy.ServiceProxy('/cob_ik_wrapper/arm/get_ik', GetPositionIK)
'''
(t,r)=listener.lookupTransform('/arm_0_link','arm_7_link',rospy.Time(0))
a=calculate_ik((t,r), arm_ik)
print a[0]
'''
# init
print "--> initializing sss"
sss = simple_script_server()
sss.init("base")
sss.init("torso")
sss.init("head")
sss.recover("base")
sss.recover("torso")
sss.recover("head")
print "--> setup care-o-bot for capture"
sss.move("head", "back")
calibration_seed = rospy.get_param("/script_server/arm/calibration")
#sss.move("arm",[a[0]])
nextPose = PoseStamped()
torso = TorsoIK()
torso.set_camera_viewfield(rospy.get_param('~camera_view_angle'))
# lissajous like figure for rotation
cb_tip_offset = 3
cb_tip_positions = [(0, 0), (1, 0), (0, 1),
(-1, 0), (0, -1)]
quaternion = []
for cb_tip_p in cb_tip_positions:
temp = list(cb_tip_p)
temp.append(cb_tip_offset)
vector_to = np.matrix(temp)
vector_from = np.matrix([0, 0, 1])
a = vector_to + vector_from
a = a / np.linalg.norm(a)
#print a
'''
#print '*'*20
print type(a)
print a.T.shape
print type(vector_from)
print vector_from.T.shape
'''
w = np.dot(vector_from, a.T)
vector_from = vector_from.tolist()[0]
a = a.tolist()[0]
#print vector_from
#print a
x = vector_from[1] * a[2] - vector_from[2] * a[1]
y = vector_from[2] * a[0] - vector_from[0] * a[2]
z = vector_from[0] * a[1] - vector_from[1] * a[0]
quaternion.append(tuple(tf.transformations.quaternion_multiply(
[0, 0, 0, 1], [x, y, z, w]).reshape(1, 4).tolist()[0]))
# define cuboid for positions
# limits from base_link frame
limits = {'x': (-0.5, -1.2),
'y': (-0.3, 0.3),
'z': (0.5, 1.0)}
sample_density = {'x': 5,
'y': 5,
'z': 5}
sample_positions = {'x': [],
'y': [],
'z': []}
for key in limits.keys():
limits[key] = sorted(limits[key])
sample_positions[key].append(limits[key][0])
diff = limits[key][1] - limits[key][0]
step = 1.0 * diff / (sample_density[key] - 1)
# print key, ' ',diff,' ',step
while sample_positions[key][-1] + step <= (limits[key][1] + 0.01):
sample_positions[key].append(sample_positions[key][-1] + step)
joint_states = []
for x in sample_positions['x']:
for y in sample_positions['y']:
for z in sample_positions['z']:
for q in quaternion:
nextPose.header.frame_id = '/base_link'
nextPose.pose.position.x = x
nextPose.pose.position.y = y
nextPose.pose.position.z = z
# (0,0,0,1) for cob3-6
nextPose.pose.orientation.x = q[0]
nextPose.pose.orientation.y = q[1]
nextPose.pose.orientation.z = q[2]
nextPose.pose.orientation.w = q[3]
chessboard_pose.publish(nextPose)
rospy.sleep(0.2)
(t, r) = get_cb_pose_head(
listener, '/head_color_camera_l_link')
angles = calculate_angles(t)
if not torso.in_range(angles):
continue
#print t
#(t, r) = get_cb_pose(listener, '/head_cam3d_link')
#print t
(t, r) = get_cb_pose(listener, '/arm_0_link')
js = calculate_ik((
t, r), arm_ik, calibration_seed[0])
if js[0] is not None:
print 'IK solution found'
else:
continue
torso_state = [-a for a in torso.calculate_ik(angles)]
for torso_js in [torso_state, [0] * len(torso_state)]:
joint_states.append({'joint_position': js[
0], 'torso_position': list(torso_js)})
print joint_states[-1]
path = rospy.get_param('~output_path', None)
directory = os.path.dirname(path)
if path is not None:
if not os.path.exists(directory):
os.makedirs(directory)
with open(path, 'w') as f:
f.write('# autogenerated: Do not edit #\n')
f.write(yaml.dump(joint_states))
else:
print yaml.dump(joint_states)
print '%s ik solutions found' % len(joint_states)