class setRobot:
def __init__(self):
self.rob = Robot()
self.whole_body = self.rob.try_get('whole_body')
self.omni_base = self.rob.try_get('omni_base')
self.gripper = self.rob.try_get('gripper')
collision_world = self.rob.try_get('global_collision_world')
self.whole_body.collision_world = collision_world
self.whole_body.end_effector_frame = u'hand_palm_link'
class setRobot:
def __init__(self):
self.rob = Robot()
self.tts = self.rob.try_get('default_tts')
self.tts.language = tts.ENGLISH ########## May need modification
self.whole_body = self.rob.try_get('whole_body')
self.omni_base = self.rob.try_get('omni_base')
self.gripper = self.rob.try_get('gripper')
collision_world = self.rob.try_get('global_collision_world')
self.whole_body.collision_world = collision_world
self.whole_body.end_effector_frame = u'hand_palm_link'
def __init__(self, before='map', after='base_footprint'):
super(hsr_Move2Center, self).__init__(outcomes=['continue', 'failed'],
input_keys=['target_pose'])
self._before = before
self._after = after
self.pose_transformed = None
robot = Robot()
self.omni_base = robot.try_get('omni_base')
class hsr_look_around_server:
def __init__(self):
self.robot = Robot()
self.whole_body = self.robot.try_get('whole_body')
self.lookaroundSrv = rospy.Service("lookaround", lookaround,
self.srvCb)
rospy.loginfo("hsr_look_around_server: ready to serve")
def srvCb(self, req):
rospy.loginfo("hsr_look_around_server: got request")
tilt_angle = req.TiltAngle
pan_angle = req.PanAngle
self.whole_body.move_to_joint_positions(
{'head_tilt_joint': tilt_angle})
self.whole_body.move_to_joint_positions({'head_pan_joint': pan_angle})
rospy.sleep(2)
return lookaroundResponse(True)
from hsrb_interface import Robot
from hsrb_interface import geometry
import rospy
robot = Robot()
whole_body = robot.try_get('whole_body')
gripper = robot.try_get('gripper')
whole_body.linear_weight = 30.0
whole_body.looking_hand_constraint = True
whole_body.move_end_effector_pose(geometry.pose(z=-0.08), 'grasp_pose')
rospy.sleep(0.8)
whole_body.move_end_effector_pose(geometry.pose(), 'grasp_pose')
rospy.sleep(0.8)
gripper.grasp(-0.3)
rospy.sleep(0.8)
whole_body.move_end_effector_pose(geometry.pose(), 'execute1')
#rospy.sleep(0.8)
whole_body.move_end_effector_pose(geometry.pose(), 'execute2')
rospy.sleep(0.8)
gripper.command(1.0)
#!/usr/bin/python
# -*- coding: utf-8 -*-
from hsrb_interface import Robot
import rospy
import math
robot = Robot()
base = robot.try_get('omni_base')
tts = robot.try_get('default_tts')
whole_body = robot.try_get('whole_body')
tts.language = tts.ENGLISH
if __name__ == '__main__':
tts.say(u'We will do 22 pushups!')
rospy.sleep(1)
tts.say(u'Are you ready?')
rospy.sleep(2)
tts.say(u'Here we go!')
rospy.sleep(1)
for i in range(0, 22):
current_count = i + 1
tts.say(str(i + 1))
if (current_count == 11):
rospy.sleep(0.2)
tts.say("You're half way there!")
rospy.sleep(1.5)
tts.say(u'GOOD JOB!')
if action_state == GoalStatus.SUCCEEDED:
userdata.S4_counter_out = userdata.S4_counter_in + 1
print "userdata.S4_counter out", userdata.S4_counter_out
return get_action(userdata.S4_counter_out)
else:
"goal was not achieved"
return 'end_demo'
tts = whole_body = None
while not rospy.is_shutdown():
try:
robot = Robot()
tts = robot.try_get('default_tts')
whole_body = robot.try_get('whole_body')
tts.language = tts.ENGLISH
break
except (exceptions.ResourceNotFoundError,
exceptions.RobotConnectionError) as e:
rospy.logerr("Failed to obtain resource: {}\nRetrying...".format(e))
rospy.init_node('test_move')
#===========Read csv==============
f = open('../csv_parser/states_and_vals.csv', 'rt')
reader = csv.reader(f)
row_count = sum(1 for row in reader)
f.seek(0)
import tf2_geometry_msgs
from hsrb_interface import geometry
from geometry_msgs.msg import PoseStamped, Point
import geometry_msgs
from vision_msgs.srv import Yolov3_detector
from std_msgs.msg import Int32, Bool
from vision_msgs.msg import VisionObject, ObjectCoordinatesForDetection
import numpy as np
import os
from takeshi_tools.nav_tool_lib import nav_module
##################INITIA
robot = Robot()
#base = robot.try_get('omni_base')
base = nav_module("hsr")
tts = robot.try_get('default_tts')
whole_body = robot.try_get('whole_body')
gripper = robot.try_get('gripper')
collision = collision_world.CollisionWorld('global_collision_world')
#tf_broadcaster=tf.TransformBroadcaster()
tf_listener = tf.TransformListener()
tf_broadcaster = tf2_ros.StaticTransformBroadcaster()
#tf_buffer = robot._get_tf2_buffer()
global turn_rigth
turn_rigth = True
def detect_yolo_foo(timeout):
objectCoordinates = ObjectCoordinatesForDetection()
objectCoordinates.x_min = -1.0
GripperApplyEffortGoal)
from tmc_manipulation_msgs.srv import (SafeJointChange, SafeJointChangeRequest)
import matplotlib
matplotlib.use('Agg')
from matplotlib import pyplot as plt
import RunAve_fil
#import fuzzy
from std_msgs.msg import Bool
from hsr_handing.msg import grasp_key
from hsr_handing.srv import fuzzy
from hsr_handing.msg import pose_info
from hsrb_interface import geometry
from hsrb_interface import Robot
robot = Robot()
omni_base = robot.try_get('omni_base')
whole_body = robot.try_get('whole_body')
gripper = robot.get('gripper')
_CONNECTION_TIMEOUT = 10.0
# Move timeout[s]
_MOVE_TIMEOUT = 60.0
# Grasp force[N]
_GRASP_FORCE = 0.2
# TF name of the bottle
_BOTTLE_TF = 'ar_marker/150'
# TF name of the gripper
_HAND_TF = 'hand_palm_link'
# Posture that 0.04[m] front and rotate -1.57 around z-axis of the bottle maker
bottle_to_hand = geometry.pose(z=-0.04, ek=-1.57)
from hsrb_interface import Robot
from hsrb_interface import geometry
import roslib
roslib.load_manifest('mechknownet')
import rospy
import numpy as np
from mechknownet.srv import *
robot = Robot()
whole_body = robot.try_get('whole_body')
base = robot.try_get('omni_base')
gripper = robot.try_get('gripper')
#whole_body.linear_weight = 30.0
whole_body.looking_hand_constraint = True
def demo():
success = call_mechknownet_sys("drawer2", "", "grasp", [])
print "success?", success
step1_finish = False
preposition = None
if success:
print "success, ready to go"
pre_head_pan_joint = whole_body.joint_positions['head_pan_joint']
pre_head_tilt_joint = whole_body.joint_positions['head_tilt_joint']
preposition = base.pose
gripper.command(1.0)
rospy.sleep(3.0)
whole_body.move_end_effector_pose(geometry.pose(z=-0.08), 'grasp_pose')
rospy.sleep(1.0)
whole_body.move_end_effector_pose(
class ManipulationBridge(object):
def __init__(self):
# robot
self.robot = Robot()
self.whole_body = self.robot.try_get('whole_body')
# server
self.srv_manipulation_left = actionlib.SimpleActionServer(
'left_arm/grasp_precompute',
GraspPrecomputeAction,
execute_cb=self.manipulation_srv_left,
auto_start=False)
self.srv_manipulation_right = actionlib.SimpleActionServer(
'right_arm/grasp_precompute',
GraspPrecomputeAction,
execute_cb=self.manipulation_srv_right,
auto_start=False)
self.srv_manipulation_left.start()
self.srv_manipulation_right.start()
def manipulation_srv_left(self, action):
success = self.manipulation_srv(action)
if success:
rospy.loginfo('Manipulation bridge: Succeeded')
self.srv_manipulation_left.set_succeeded()
def manipulation_srv_right(self, action):
success = self.manipulation_srv(action)
if success:
rospy.loginfo('Manipulation bridge: Succeeded')
self.srv_manipulation_right.set_succeeded()
def manipulation_srv(self, action):
"""
Here the grasp precompute action (TU/e) is translated to a PlanWithHandGoals (TMC) and send as goal to the robot
:param action: the GraspPrecomputeAction type
"""
success = True
pose_quaternion = quaternion_from_euler(action.goal.roll,
action.goal.pitch,
action.goal.yaw)
static_quaternion = quaternion_from_euler(3.14159265359,
-1.57079632679, 0)
final_quaternion = quaternion_multiply(pose_quaternion,
static_quaternion)
pose = [action.goal.x, action.goal.y, action.goal.z], final_quaternion
################################################################################################################
# This piece of code is partially copied from Toyota software, it also uses the private functions (we're very
# sorry). Setting base_movement_type.val allows for adapting the allowed movements during manipulation.
ref_frame_id = settings.get_frame('base')
ref_to_hand_poses = [pose]
odom_to_ref_pose = self.whole_body._lookup_odom_to_ref(ref_frame_id)
odom_to_ref = geometry.pose_to_tuples(odom_to_ref_pose)
odom_to_hand_poses = []
for ref_to_hand in ref_to_hand_poses:
odom_to_hand = geometry.multiply_tuples(odom_to_ref, ref_to_hand)
odom_to_hand_poses.append(geometry.tuples_to_pose(odom_to_hand))
req = self.whole_body._generate_planning_request(
PlanWithHandGoalsRequest)
req.origin_to_hand_goals = odom_to_hand_poses
req.ref_frame_id = self.whole_body._end_effector_frame
req.base_movement_type.val = BaseMovementType.ROTATION_Z
service_name = self.whole_body._setting['plan_with_hand_goals_service']
plan_service = rospy.ServiceProxy(service_name, PlanWithHandGoals)
res = plan_service.call(req)
if res.error_code.val != ArmManipulationErrorCodes.SUCCESS:
rospy.logerr('Fail to plan move_endpoint')
success = False
else:
res.base_solution.header.frame_id = settings.get_frame('odom')
constrained_traj = self.whole_body._constrain_trajectories(
res.solution, res.base_solution)
self.whole_body._execute_trajectory(constrained_traj)
return success
end_pose = [5.004, 1.483, 0.766]
def set_initial_pose(x, y, phi):
pub = rospy.Publisher('laser_2d_correct_pose', PoseWithCovarianceStamped, queue_size=1)
rospy.sleep(0.5)
initial_pose = PoseWithCovarianceStamped()
initial_pose.header.frame_id = "map"
initial_pose.pose.pose.position.x = x
initial_pose.pose.pose.position.y = y
initial_pose.pose.pose.position.z = 0.0
initial_pose.pose.pose.orientation = Quaternion(*tf.transformations.quaternion_from_euler(0, 0, phi))
initial_pose.pose.covariance = [0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891945200942, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
pub.publish(initial_pose)
rospy.sleep(0.5)
# Preparation to use robot functions
print 'starting the rips'
robot = Robot()
omni_base = robot.try_get('omni_base')
set_initial_pose(*initial_pose)
omni_base.go(*mid_pose, relative=False)
omni_base.go(*end_pose, relative=False)
rospy.loginfo('finished')
#!/usr/bin/env python
#from ImagePass.srv import *
import rospy
#import tf
from hsrb_interface import Robot
from hsrb_interface import geometry
import math
robot = Robot()
#omni_base = robot.try_get('omni_base')
#tts = robot.try_get('default_tts')
whole_body = robot.try_get('whole_body')
#gripper = robot.try_get('gripper')
collision_world = robot.try_get('global_collision_world')
#br=tf.TransformBroadcaster()
if __name__ == '__main__':
#rospy.init_node('python_services')
collision_world.remove_all()
whole_body.collision_world = collision_world
#kitchin-table
collision_world.add_box(x=0.8,
y=0.7,
z=0.5,
pose=geometry.pose(x=-0.3, y=0.35, z=0.28),
frame_id='abstract_map')
#desk
collision_world.add_box(x=0.6,
