def __init__(self):
# self.scene = PlanningSceneInterface("base_link")
self.dof = "7dof"
self.robot = moveit_commander.RobotCommander()
self._listener = tf.TransformListener()
self.move_group = MoveGroupInterface("upper_body", "base_link")
self.lmove_group = MoveGroupInterface("left_arm", "base_link")
self.rmove_group = MoveGroupInterface("right_arm", "base_link")
self.move_group.setPlannerId("RRTConnectkConfigDefault")
self.lmove_group.setPlannerId("RRTConnectkConfigDefault")
self.rmove_group.setPlannerId("RRTConnectkConfigDefault")
if "7dof" == self.dof:
self._upper_body_joints = [
"right_shoulder_pan_joint", "right_shoulder_lift_joint",
"right_arm_half_joint", "right_elbow_joint",
"right_wrist_spherical_1_joint",
"right_wrist_spherical_2_joint", "right_wrist_3_joint",
"left_shoulder_pan_joint", "left_shoulder_lift_joint",
"left_arm_half_joint", "left_elbow_joint",
"left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint",
"left_wrist_3_joint", "linear_joint", "pan_joint", "tilt_joint"
]
self._right_arm_joints = [
"right_shoulder_pan_joint", "right_shoulder_lift_joint",
"right_arm_half_joint", "right_elbow_joint",
"right_wrist_spherical_1_joint",
"right_wrist_spherical_2_joint", "right_wrist_3_joint"
]
self._left_arm_joints = [
"left_shoulder_pan_joint", "left_shoulder_lift_joint",
"left_arm_half_joint", "left_elbow_joint",
"left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint",
"left_wrist_3_joint"
]
self.tucked = [
-1.6, -1.5, 0.4, -2.7, 0.0, 0.5, -1.7, 1.6, 1.5, -0.4, 2.7,
0.0, -0.5, 1.7, 0.04, 0, 0
]
# self.constrained_stow =[-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, 1.0, 2.6, -2.0, 0.0, -2.0, 0.0, 0.0, -1.0, 0.42, 0, 0]
self.constrained_stow = [
-2.037, 1.046, -2.877, -1.423, -1.143, 1.679, 1.690, 2.037,
-1.046, 2.877, 1.423, 1.143, -1.679, -1.690, 0.4, 0.288, 0
]
else:
rospy.logerr("DoF needs to be set at 7, aborting demo")
return
self._lgripper = GripperActionClient('left')
self._rgripper = GripperActionClient('right')
class MovoUpperBody:
upper_body_joints = [
"right_shoulder_pan_joint", "right_shoulder_lift_joint",
"right_arm_half_joint", "right_elbow_joint",
"right_wrist_spherical_1_joint", "right_wrist_spherical_2_joint",
"right_wrist_3_joint", "left_shoulder_pan_joint",
"left_shoulder_lift_joint", "left_arm_half_joint", "left_elbow_joint",
"left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint",
"left_wrist_3_joint", "linear_joint", "pan_joint", "tilt_joint"
]
homed = [
-1.5,-0.2,-0.175,-2.0,2.0,-1.24,-1.1, \
1.5,0.2,0.15,2.0,-2.0,1.24,1.1,\
0.35,0,0
]
tucked = [
-1.6,-1.4,0.4,-2.7,0.0,0.5,-1.7,\
1.6,1.4,-0.4,2.7,0.0,-0.5, 1.7,
0.04, 0, 0]
sidearms = [
-0.3 , -1.7 ,0.0 ,-1.7 ,1.8 ,-1.8 ,-1.1, \
0.3 , 1.7 ,0.0 ,1.7 ,-1.8 ,1.8 ,1.1 ,\
.35, 0, -1]
gripper_closed = 0.01
gripper_open = 0.165
def __init__(self):
self.move_group = MoveGroupInterface("upper_body", "base_link")
self.move_group.setPlannerId("RRTConnectkConfigDefault")
self.lgripper = GripperActionClient('left')
self.rgripper = GripperActionClient('right')
def move(self, pose):
success = False
while not rospy.is_shutdown() and not success:
result = self.move_group.moveToJointPosition(\
self.upper_body_joints, pose, 0.05)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
success = True
else:
rospy.logerr("moveToJointPosition failed (%d)"\
%result.error_code.val)
def untuck(self):
rospy.loginfo("untucking the arms")
self.lgripper.command(self.gripper_open)
self.rgripper.command(self.gripper_open)
self.move(self.homed)
def tuck(self):
self.move(self.tucked)
self.lgripper.command(self.gripper_closed)
self.rgripper.command(self.gripper_closed)
def side(self):
self.move(self.sidearms)
def __init__(self,
configs="configs.json",
methods="methodlib.json",
open_g=.01,
closed=.5):
#TODO make config location as input / parameter server
rospack = rospkg.RosPack()
base_path = rospack.get_path("dsi_action_server")
self.configs = json.load(open(base_path + "/config/configs.json"))
self.method_lib = json.load(open(base_path + "/config/methodlib.json"))
''' initialize robot moveit '''
self.scene = PlanningSceneInterface("base_link")
self.robot = moveit_commander.RobotCommander()
''' set left arm setup '''
self.group_left = moveit_commander.MoveGroupCommander("left_arm")
self.group_right = moveit_commander.MoveGroupCommander("right_arm")
''' gripper setup'''
self.lgripper = GripperActionClient('left')
self.rgripper = GripperActionClient('right')
self.gripper_open = open_g
self.gripper_closed = closed
self.lgripper.command(self.gripper_open)
self.rgripper.command(self.gripper_open)
rospy.sleep(1)
self.lgripper.command(self.gripper_closed)
self.rgripper.command(self.gripper_closed)
''' wait for gazebo services for teleport '''
rospy.wait_for_service('/gazebo/get_model_state')
rospy.wait_for_service('/gazebo/set_model_state')
self.get_model_state = rospy.ServiceProxy('/gazebo/get_model_state',
GetModelState)
self.set_model_state = rospy.ServiceProxy('/gazebo/set_model_state',
SetModelState)
''' world state service '''
rospy.wait_for_service('/dsi/world_state_server')
self.world_state_service = rospy.ServiceProxy(
'/dsi/world_state_server', world_state)
''' initialize action server service '''
s = rospy.Service('dsi/action_server', action_service,
self.action_service)
''' initialize light server '''
rospy.wait_for_service('/gazebo/set_light_properties')
self.set_light_properties = rospy.ServiceProxy(
'/gazebo/set_light_properties', SetLightProperties)
''' finished initalizing '''
rospy.loginfo("action server intitialized")
def __init__(self, arclib_node):
self._c = arclib_node
self._action_name = "L0_dual_set_gripper"
self._as = actionlib.SimpleActionServer(
self._action_name,
arclib_msg.dual_set_gripperAction,
execute_cb=self.execute_cb,
auto_start=False)
self._lgripper = GripperActionClient('left')
self._rgripper = GripperActionClient('right')
self._gripper_closed = 0.00
self._gripper_open = 0.165
self._as.register_preempt_callback(self.preempt_cb)
self._as.start()
print(self._action_name, "Started!")
def movo_moveit_test():
## BEGIN_TUTORIAL
##
## Setup
## ^^^^^
## CALL_SUB_TUTORIAL imports
##
## First initialize moveit_commander and rospy.
print "============ Starting tutorial setup"
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('movo_moveit_test', anonymous=True)
# is_sim = rospy.get_param('~sim',False)
is_sim = True
## Instantiate a RobotCommander object. This object is an interface to
## the robot as a whole.
robot = moveit_commander.RobotCommander()
names = robot.get_group_names()
## Instantiate a PlanningSceneInterface object. This object is an interface
## to the world surrounding the robot.
scene = moveit_commander.PlanningSceneInterface()
##Gripper action clients
# lgripper = GripperActionTest('left')
# rgripper = GripperActionTest('right')
lgripper = GripperActionClient('left')
rgripper = GripperActionClient('right')
## Instantiate a MoveGroupCommander object. This object is an interface
## to one group of joints. In this case the group is the joints in the left
## arm. This interface can be used to plan and execute motions on the left
## arm.
# group = moveit_commander.MoveGroupCommander("upper_body")
print "============ Hi! Available Robot Groups:"
group_names = robot.get_group_names()
rospy.loginfo(group_names)
rospy.sleep(1.0)
# rospy.loginfo("======upper_body_group connected =========")
larm_group = moveit_commander.MoveGroupCommander("left_arm")
rospy.loginfo("======l_arm move_group connected =========")
larm_pos = larm_group.get_current_pose()
## We create this DisplayTrajectory publisher which is used below to publish
## trajectories for RVIZ to visualize.
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=10)
rospy.sleep(2)
# scene.remove_world_object("floor")
# publish a demo scene
p = geometry_msgs.msg.PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0.0
p.pose.position.y = 0.0
p.pose.position.z = -0.01
p.pose.orientation.w = 1.0
scene.add_box("floor", p, (4.0, 4.0, 0.02))
## We can get a list of all the groups in the robot
# name_lg = group_names[2]
# group_lg = moveit_commander.MoveGroupCommander("left_gripper")
# frame_lg = group_lg.get_planning_frame()
# rospy.loginfo("========== Gripper frame is: =========")
# rospy.loginfo(frame_lg)
## Sometimes for debugging it is useful to print the entire state of the
## robot.
print "============ Printing robot state"
print robot.get_current_state()
print "============"
group.set_planner_id("RRTConnectkConfigDefault")
## Planning to a Pose goal
## ^^^^^^^^^^^^^^^^^^^^^^^
## We can plan a motion for this group to a desired pose for the
## end-effector
if (True == is_sim):
gripper_closed = 0.96
gripper_open = 0.0
else:
gripper_closed = 0.0
gripper_open = 0.165
def test_gripper():
raw_input("========== Press Enter to test Gripper:")
rospy.loginfo(lgripper.result)
lgripper.command(0.085, True)
rgripper.command(0.085, True)
lgripper.wait()
rgripper.wait()
lgripper.command(0.165)
rgripper.command(0.165)
lgripper.wait()
rgripper.wait()
curr_pos = lgripper.result().position # 0.1558
rospy.loginfo("=========== Current Gripper position is:")
rospy.loginfo(curr_pos)
raw_input("====== Press Enter to close Gripper:")
lgripper.command(0.0, block=True, timeout=3)
rgripper.command(0.0)
# lgripper.wait()
# rgripper.wait()
def test_named_pos():
raw_input("========== Press Enter to test named targets: home")
group.set_named_target("homed")
plan = group.plan()
group.execute(plan)
lgripper.command(gripper_open)
rgripper.command(gripper_open)
lgripper.wait()
rgripper.wait()
rospy.sleep(2.0)
group.set_named_target("tucked")
plan = group.plan()
group.execute(plan)
lgripper.command(gripper_closed)
rgripper.command(gripper_closed)
lgripper.wait()
rgripper.wait()
rospy.sleep(2.0)
'''
group.set_named_target("pos1")
plan = group.plan()
group.execute(plan)
lgripper.command(gripper_open)
rgripper.command(gripper_open)
lgripper.wait()
rgripper.wait()
rospy.sleep(2.0)
group.set_named_target("tucked")
plan = group.plan()
group.execute(plan)
lgripper.command(gripper_closed)
rgripper.command(gripper_closed)
lgripper.wait()
rgripper.wait()
'''
## When finished shut down moveit_commander.
test_gripper()
# test_named_pos()
raw_input("======== Enter to disconnect ros:")
moveit_commander.roscpp_shutdown()
## END_TUTORIAL
print "============ STOPPING"
if __name__ == "__main__":
process_start_time = dt.datetime.now()
rospy.init_node("demo_show_basic", anonymous=True)
dof = rospy.get_param('~jaco_dof')
sim = rospy.get_param("~sim", False)
if (sim):
rospy.wait_for_message('/sim_initialized', Bool)
movo_head = HeadActionClient()
movo_base = MoveBaseActionClient(sim=sim, frame="odom")
movo_larm = JacoActionClient(arm='left', dof=dof)
movo_rarm = JacoActionClient(arm='right', dof=dof)
movo_lfinger = GripperActionClient('left')
movo_rfinger = GripperActionClient('right')
movo_torsor = TorsoActionClient()
Publisher = rospy.Publisher("/movo/voice/text",
String,
queue_size=1,
latch=True)
rospy.sleep(2)
if "6dof" == dof:
# arm position definition
larm_home_deg = [85, 13, 122.5, 120, -83, -75]
rarm_home_deg = [-1.0 * x for x in larm_home_deg]
larm_greet_deg = [125, -30, 45, 120, 0,
-170] # [2.18, -0.52, 0.79, 2.09, 0, -2.97]
def __init__(self):
self.g = self.gval = GlobalVariables()
# self.gval
# rospy.init_node("odyssey_Interface_py3_node")
self.client_L0_upper_jp_move_safe = myclient(
name="L0_upper_jp_move_safe",
action=arcmsg.upper_jp_movo_safeAction,
fbmsg=arcmsg.upper_jp_movo_safeFeedback)
self.client_L0_dual_jp_move_safe_relate = myclient(
name="L0_dual_jp_move_safe_relate",
action=arcmsg.dual_jp_movo_safe_relateAction,
fbmsg=arcmsg.dual_jp_movo_safe_relateFeedback)
self.client_L0_dual_task_move_safe_relate = myclient(
name="L0_dual_task_move_safe_relate",
action=arcmsg.dual_task_move_safe_relateAction,
fbmsg=arcmsg.dual_task_move_safe_relateFeedback)
self.client_L0_dual_set_gripper = myclient(
name="L0_dual_set_gripper",
action=arcmsg.dual_set_gripperAction,
fbmsg=arcmsg.dual_set_gripperFeedback)
self.client_L0_single_task_move_safe = myclient(
name="L0_single_task_move_safe",
action=arcmsg.single_task_move_safeAction,
fbmsg=arcmsg.single_task_move_safeFeedback)
self.client_L0_single_set_gripper = myclient(
name="L0_single_set_gripper",
action=arcmsg.single_set_gripperAction,
fbmsg=arcmsg.single_set_gripperFeedback)
self._lgripper = GripperActionClient('left')
self._rgripper = GripperActionClient('right')
self.subscribe_force("right")
self.subscribe_force("left")
self.subscribe_jointstates()
self._subscribe_eepose()
# rospy.Subscriber("/joint_states", JointState,self._subscribe_jointstates_callback)
#
# self.jp_updated=False
# r=rospy.Rate(10)
# while(not (self.jp_updated and True)):
# r.sleep()
self.cartesianforce_right = None
self.cartesianforce_left = None
self.jointstates_pos = None
self.jointstates_vel = None
self.jointstates_effort = None
self.right_arm_js_pos = None
self.left_arm_js_pos = None
self.right_arm_js_vel = None
self.left_arm_js_vel = None
self.head_js_pos = None
self.head_js_vel = None
self.linear_js_pos = None
self.linear_js_vel = None
self.lpose = None
self.rpose = None
# When you add new states, please regist in _states
# Wait till all states got updated
while (sum(type(i) == type(None) for i in self._states)):
print(self._states)
time.sleep(1)
# print(self.jointstates_effort)
print("Wait till all states got first updated")
#!/usr/bin/env python
import sys, rospy
from std_msgs.msg import Bool
from moveit_python import MoveGroupInterface
from moveit_msgs.msg import MoveItErrorCodes
from movo_action_clients.gripper_action_client import GripperActionClient
import moveit_commander
if __name__ == "__main__":
rospy.init_node('goto_tucked')
move_group = MoveGroupInterface("right_arm", "base_link")
#move_group.setPlannerId("RRTConnectkConfigDefault")
lgripper = GripperActionClient('left')
rgripper = GripperActionClient('right')
#dof = rospy.get_param('~jaco_dof')
rarm_joints = [
"right_shoulder_pan_joint", "right_shoulder_lift_joint",
"right_arm_half_joint", "right_elbow_joint",
"right_wrist_spherical_1_joint", "right_wrist_spherical_2_joint",
"right_wrist_3_joint"
]
tucked = [-1.6, -1.4, 0.4, -2.7, 0.0, 0.5,
-1.7] #, 1.6,1.4,-0.4,2.7,0.0,-0.5, 1.7, 0.04, 0, 0]
success = False
while not rospy.is_shutdown() and not success:
result = move_group.moveToJointPosition(rarm_joints, tucked, 0.05)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
def __init__(self):
self.move_group = MoveGroupInterface("upper_body", "base_link")
self.move_group.setPlannerId("RRTConnectkConfigDefault")
self.lgripper = GripperActionClient('left')
self.rgripper = GripperActionClient('right')
def __init__(self, sim=False):
self.scene = PlanningSceneInterface("base_link")
self.move_group = MoveGroupInterface("upper_body", "base_link")
self.lmove_group = MoveGroupInterface("left_arm", "base_link")
self.rmove_group = MoveGroupInterface("right_arm", "base_link")
self.move_group.setPlannerId("RRTConnectkConfigDefault")
self.lmove_group.setPlannerId("RRTConnectkConfigDefault")
self.rmove_group.setPlannerId("RRTConnectkConfigDefault")
self._upper_body_joints = [
"right_elbow_joint", "right_shoulder_lift_joint",
"right_shoulder_pan_joint", "right_wrist_1_joint",
"right_wrist_2_joint", "right_wrist_3_joint", "left_elbow_joint",
"left_shoulder_lift_joint", "left_shoulder_pan_joint",
"left_wrist_1_joint", "left_wrist_2_joint", "left_wrist_3_joint",
"linear_joint", "pan_joint", "tilt_joint"
]
self._right_arm_joints = [
"right_elbow_joint", "right_shoulder_lift_joint",
"right_shoulder_pan_joint", "right_wrist_1_joint",
"right_wrist_2_joint", "right_wrist_3_joint"
]
self._left_arm_joints = [
"left_elbow_joint", "left_shoulder_lift_joint",
"left_shoulder_pan_joint", "left_wrist_1_joint",
"left_wrist_2_joint", "left_wrist_3_joint"
]
self.pickplace = [None] * 2
self.pickplace[0] = PickPlaceInterface("left_side",
"left_gripper",
verbose=True)
self.pickplace[0].planner_id = "RRTConnectkConfigDefault"
self.pickplace[1] = PickPlaceInterface("right_side",
"right_gripper",
verbose=True)
self.pickplace[1].planner_id = "RRTConnectkConfigDefault"
self.pick_result = [None] * 2
self._last_gripper_picked = 0
self.place_result = [None] * 2
self._last_gripper_placed = 0
self._objs_to_keep = []
self._listener = tf.TransformListener()
self._lgripper = GripperActionClient('left')
self._rgripper = GripperActionClient('right')
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(
find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
self.scene.clear()
# This is a simulation so need to adjust gripper parameters
if sim:
self._gripper_closed = 0.96
self._gripper_open = 0.0
else:
self._gripper_closed = 0.0
self._gripper_open = 0.165
class actionServer(object):
def __init__(self,
configs="configs.json",
methods="methodlib.json",
open_g=.01,
closed=.5):
#TODO make config location as input / parameter server
rospack = rospkg.RosPack()
base_path = rospack.get_path("dsi_action_server")
self.configs = json.load(open(base_path + "/config/configs.json"))
self.method_lib = json.load(open(base_path + "/config/methodlib.json"))
''' initialize robot moveit '''
self.scene = PlanningSceneInterface("base_link")
self.robot = moveit_commander.RobotCommander()
''' set left arm setup '''
self.group_left = moveit_commander.MoveGroupCommander("left_arm")
self.group_right = moveit_commander.MoveGroupCommander("right_arm")
''' gripper setup'''
self.lgripper = GripperActionClient('left')
self.rgripper = GripperActionClient('right')
self.gripper_open = open_g
self.gripper_closed = closed
self.lgripper.command(self.gripper_open)
self.rgripper.command(self.gripper_open)
rospy.sleep(1)
self.lgripper.command(self.gripper_closed)
self.rgripper.command(self.gripper_closed)
''' wait for gazebo services for teleport '''
rospy.wait_for_service('/gazebo/get_model_state')
rospy.wait_for_service('/gazebo/set_model_state')
self.get_model_state = rospy.ServiceProxy('/gazebo/get_model_state',
GetModelState)
self.set_model_state = rospy.ServiceProxy('/gazebo/set_model_state',
SetModelState)
''' world state service '''
rospy.wait_for_service('/dsi/world_state_server')
self.world_state_service = rospy.ServiceProxy(
'/dsi/world_state_server', world_state)
''' initialize action server service '''
s = rospy.Service('dsi/action_server', action_service,
self.action_service)
''' initialize light server '''
rospy.wait_for_service('/gazebo/set_light_properties')
self.set_light_properties = rospy.ServiceProxy(
'/gazebo/set_light_properties', SetLightProperties)
''' finished initalizing '''
rospy.loginfo("action server intitialized")
def action_service(self, req):
"""
the action service to be called by
Parameters
----------
"""
print req.action_request
request = json.loads(req.action_request)
print request
if "method" in request:
self.method_picker(request["method"])
return "ran method"
return "error"
def method_picker(self, method):
"""
grabs method from the loaded methodlib file
Parameters
----------
method: the method from the action config file to be used
"""
#TODO wrap for errors and issues allow inbound services
self.dict_method_parser(self.method_lib[method])
def dict_method_parser(self, dict_method):
"""
takes a method dictionary and sends them to the action parsers
to be executed by the bot
Parameters
----------
dict_method: A dictionary of actions to be executed
"""
for action in dict_method:
if action["action"] == "joint_state":
rospy.loginfo("joint state parser started")
self.joint_state_parser(action)
elif action["action"] == "pose_arm":
rospy.loginfo("joint state parser started")
self.pose_arm_parser(action)
elif action["action"] == "set_gripper":
rospy.loginfo("set gripper parser started")
self.set_gripper_parser(action)
elif action["action"] == "teleport_state":
rospy.loginfo("starting teleport")
self.teleport_parser(action)
elif action["action"] == "switch_light":
rospy.loginfo("switch lights")
self.switch_lights_parser(action)
else:
rospy.logwarn("method parser invalid action")
def switch_lights_parser(self, lights_dict):
"""
Parse and use action to switch lights
Parameters
----------
lights_dict: dictionary of light switching parameters
"""
light_name = lights_dict["target_light"]
diffuse = ColorRGBA()
if lights_dict["on"] == True:
diffuse.r = 0.5
diffuse.g = 0.5
diffuse.b = 0.5
diffuse.a = 1.0
attenuation_linear = 0.01
attenuation_quadratic = 0.001
elif lights_dict["on"] == False:
diffuse.r = 0.0
diffuse.g = 0.0
diffuse.b = 0.0
diffuse.a = 0.0
attenuation_linear = 0.00
attenuation_quadratic = 0.0000
else:
rospy.logwarn("invalid light on state parsed")
return
self.set_light_properties(light_name, diffuse, 0.0, attenuation_linear,
attenuation_quadratic)
return
def teleport_parser(self, teleport_dict):
"""
takes in teleport parameters into dictionary
Parameters
----------
teleport_dict: dictionary of teleport parameters
"""
tele_pose = Pose()
if "location" in teleport_dict:
tele_pose = self.read_object(teleport_dict["location"])
else:
rospy.logerr("no location found")
if "offset" in teleport_dict:
offsets = teleport_dict["offset"]
tele_pose.position.x += offsets[0]
tele_pose.position.y += offsets[1]
tele_pose.position.z += offsets[2]
else:
rospy.logdebug("no offset ")
if "orientation" in teleport_dict:
rospy.logwarn("no orientation currrenctly")
self.teleport(tele_pose)
def joint_state_parser(self, joint_state_dict):
"""
pareses the joint state dictionary into a useable form for the joint
state action
Parameters
----------
joint_state_dict: dictionary of joint state action data
"""
try:
joint_state_call = joint_state_dict["joint_state"]
except Exception as e:
rospy.logerr("joint state parser error: {}".format(e))
return
if joint_state_call not in self.configs:
rospy.logerr("joint state doesn't exist")
return
joint_state = self.configs[joint_state_call]
arm = "left"
if arm in joint_state_dict:
arm = joint_state_dict["arm"]
self.set_joint(joint_state, arm)
rospy.loginfo("joint state executed")
def set_gripper_parser(self, set_gripper_dict):
"""
parses the gripper action based on the gripper dictionary item
Parameters
----------
set_gripper_dict: parses the gripper action dictionary
"""
arm = "left"
if "arm" not in set_gripper_dict:
rospy.logwarn("no arm set in gripper dictionary")
else:
arm = set_gripper_dict["arm"]
if "position" not in set_gripper_dict:
rospy.logerr("no postion in gripper action")
return
position = set_gripper_dict["position"]
self.gripper_control(position, arm)
rospy.loginfo("gripper {} action executed set to {}".format(
arm, position))
def pose_arm_parser(self, pose_arm_dict):
"""
Takes in pose based dictionary action items and executes them
Parameters
----------
action_dict:
"""
try:
object = pose_arm_dict["object"]
offset = pose_arm_dict["linear_offset"]
except Exception as e:
rospy.logerr("arm parser error: {}".format(e))
return
arm = "left"
if "arm" in pose_arm_dict:
arm = pose_arm_dict["arm"]
self.pose_arm_wrapper(object, offset, arm)
rospy.loginfo("pose arm executed")
def pose_arm_wrapper(self, object, offset, arm="left"):
"""
wraps the poase arm function to use world state objects
Parameters()
----------
objects: the object to set pose in relation too
offset: linear translation of offset from object list[x,y,z]
"""
obj_pose = self.read_object(object)
obj_pose.position.x += offset[0]
obj_pose.position.y += offset[1]
obj_pose.position.z += offset[2]
self.set_pose(obj_pose, arm)
def read_object(self, object):
"""
reads object pose info from the world state server
Parameters
----------
object: the object for who's desired information you would like
Returns
-------
pose: returns pose object
"""
world_state = json.loads(self.world_state_service('blank').world_state)
#TODO check for objects existence
position = world_state[object]["location_xyz"]
orientation = world_state[object]["orientation_rpq"]
quaternion = tf.transformations.quaternion_from_euler(
orientation[0], orientation[1], orientation[2])
pose = Pose()
pose.position.x = position[0]
pose.position.y = position[1]
pose.position.z = position[2]
pose.orientation.x = 0
pose.orientation.y = 0
pose.orientation.z = 0
pose.orientation.w = 1
return pose
def set_pose(self, pose, arm):
"""
Choose an arm and a Pose to place it into
Parameters
----------
pose: ros Pose message showing the desired location
arm: choice of arm to execute action
"""
#TODO don't like this method of arm picking
if arm == "left":
self.group_left.set_pose_target(pose)
self.group_left.execute(self.group_left.plan())
elif arm == "right":
self.group_right.set_pose_target(pose)
self.group_right.execute(self.group_right.plan())
else:
rospy.logwarn("didn't pick a proper arm")
rospy.sleep(1)
def set_joint(self, joint, arm):
"""
Choose and arm and a joint state to place it into
Parameters
----------
joint: list containing joint indo
arm: choice of arm to execute action
"""
if arm == "left":
self.group_left.set_joint_value_target(joint)
self.group_left.execute(self.group_left.plan())
elif arm == "right":
self.group_right.set_joint_value_target(joint)
self.group_right.execute(self.group_right.plan())
else:
rospy.logwarn("didn't pick a proper arm")
rospy.sleep(1)
def gripper_control(self, position, gripper):
"""
tells movo to open or close the gripper
Parameters
----------
position: open or closed hand position
gripper: left or right gripper
"""
if position == "open":
if gripper == "left":
self.lgripper.command(self.gripper_open)
elif gripper == "right":
self.rgripper.command(self.gripper_open)
else:
rospy.logwarn("gripper issue")
elif position == "closed":
if gripper == "left":
self.lgripper.command(self.gripper_closed)
elif gripper == "right":
self.rgripper.command(self.gripper_closed)
else:
rospy.logwarn("gripper issue")
else:
rospy.logwarn("gripper issue")
rospy.sleep(1)
def teleport(self, location_pose, agent='movo'):
"""
places movo in the desired location
Parameters:
-----------
location_pose: Pose data type for desired location of movo
"""
teleport_loc = ModelState()
teleport_loc.model_name = agent
teleport_loc.pose = location_pose
self.set_model_state(teleport_loc)
rospy.sleep(1)
def dict_to_pose_msg(self, dict):
"""
takes a json dictionary item and place into a pose message
Parameters:
-----------
dict: dictionary item to be placed into a pose message
"""
pose = Pose()
pose.position.x = dict["position"]["x"]
pose.position.y = dict["position"]["y"]
pose.position.z = dict["position"]["z"]
pose.orientation.x = dict["orientation"]["x"]
pose.orientation.y = dict["orientation"]["y"]
pose.orientation.z = dict["orientation"]["z"]
pose.orientation.w = dict["orientation"]["w"]
return pose
def __init__(self, sim=False):
self.scene = PlanningSceneInterface("base_link")
self.dof = rospy.get_param('~jaco_dof')
self.move_group = MoveGroupInterface("upper_body", "base_link")
self.lmove_group = MoveGroupInterface("left_arm", "base_link")
self.rmove_group = MoveGroupInterface("right_arm", "base_link")
self.move_group.setPlannerId("RRTConnectkConfigDefault")
self.lmove_group.setPlannerId("RRTConnectkConfigDefault")
self.rmove_group.setPlannerId("RRTConnectkConfigDefault")
if "6dof" == self.dof:
self._upper_body_joints = [
"right_elbow_joint", "right_shoulder_lift_joint",
"right_shoulder_pan_joint", "right_wrist_1_joint",
"right_wrist_2_joint", "right_wrist_3_joint",
"left_elbow_joint", "left_shoulder_lift_joint",
"left_shoulder_pan_joint", "left_wrist_1_joint",
"left_wrist_2_joint", "left_wrist_3_joint", "linear_joint",
"pan_joint", "tilt_joint"
]
self._right_arm_joints = [
"right_elbow_joint", "right_shoulder_lift_joint",
"right_shoulder_pan_joint", "right_wrist_1_joint",
"right_wrist_2_joint", "right_wrist_3_joint"
]
self._left_arm_joints = [
"left_elbow_joint", "left_shoulder_lift_joint",
"left_shoulder_pan_joint", "left_wrist_1_joint",
"left_wrist_2_joint", "left_wrist_3_joint"
]
self.tucked = [
-2.8, -1.48, -1.48, 0, 0, 1.571, 2.8, 1.48, 1.48, 0, 0, -1.571,
0.0371, 0.0, 0.0
]
self.constrained_stow = [
2.28, 2.17, -2.56, -0.09, 0.15, 1.082, -2.28, -2.17, 2.56,
0.09, -0.15, 2.06, 0.42, 0.0, 0.0
]
self.larm_const_stow = [-2.28, -2.17, 2.56, 0.09, -0.15, 2.08]
self.rarm_const_stow = [2.28, 2.17, -2.56, -0.09, 0.15, 1.06]
self.tableDist = 0.7
elif "7dof" == self.dof:
self._upper_body_joints = [
"right_shoulder_pan_joint", "right_shoulder_lift_joint",
"right_arm_half_joint", "right_elbow_joint",
"right_wrist_spherical_1_joint",
"right_wrist_spherical_2_joint", "right_wrist_3_joint",
"left_shoulder_pan_joint", "left_shoulder_lift_joint",
"left_arm_half_joint", "left_elbow_joint",
"left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint",
"left_wrist_3_joint", "linear_joint", "pan_joint", "tilt_joint"
]
self._right_arm_joints = [
"right_shoulder_pan_joint", "right_shoulder_lift_joint",
"right_arm_half_joint", "right_elbow_joint",
"right_wrist_spherical_1_joint",
"right_wrist_spherical_2_joint", "right_wrist_3_joint"
]
self._left_arm_joints = [
"left_shoulder_pan_joint", "left_shoulder_lift_joint",
"left_arm_half_joint", "left_elbow_joint",
"left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint",
"left_wrist_3_joint"
]
self.tucked = [
-1.6, -1.5, 0.4, -2.7, 0.0, 0.5, -1.7, 1.6, 1.5, -0.4, 2.7,
0.0, -0.5, 1.7, 0.04, 0, 0
]
self.constrained_stow = [
-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, 1.0, 2.6, -2.0, 0.0, -2.0, 0.0,
0.0, -1.0, 0.42, 0, 0
]
self.larm_const_stow = [2.6, -2.0, 0.0, -2.0, 0.0, 0.0, 1.0]
self.rarm_const_stow = [-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, -1.0]
self.tableDist = 0.8
else:
rospy.logerr("DoF needs to be set 6 or 7, aborting demo")
return
self.pickplace = [None] * 2
self.pickplace[0] = PickPlaceInterface("left_side",
"left_gripper",
verbose=True)
self.pickplace[0].planner_id = "RRTConnectkConfigDefault"
self.pickplace[1] = PickPlaceInterface("right_side",
"right_gripper",
verbose=True)
self.pickplace[1].planner_id = "RRTConnectkConfigDefault"
self.pick_result = [None] * 2
self._last_gripper_picked = 0
self.place_result = [None] * 2
self._last_gripper_placed = 0
self._objs_to_keep = []
self._listener = tf.TransformListener()
self._lgripper = GripperActionClient('left')
self._rgripper = GripperActionClient('right')
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(
find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
self.scene.clear()
# This is a simulation so need to adjust gripper parameters
if sim:
self._gripper_closed = 0.96
self._gripper_open = 0.00
else:
self._gripper_closed = 0.01
self._gripper_open = 0.165
class GraspingClient(object):
def __init__(self, sim=False):
self.scene = PlanningSceneInterface("base_link")
self.dof = rospy.get_param('~jaco_dof')
self.move_group = MoveGroupInterface("upper_body", "base_link")
self.lmove_group = MoveGroupInterface("left_arm", "base_link")
self.rmove_group = MoveGroupInterface("right_arm", "base_link")
self.move_group.setPlannerId("RRTConnectkConfigDefault")
self.lmove_group.setPlannerId("RRTConnectkConfigDefault")
self.rmove_group.setPlannerId("RRTConnectkConfigDefault")
if "6dof" == self.dof:
self._upper_body_joints = [
"right_elbow_joint", "right_shoulder_lift_joint",
"right_shoulder_pan_joint", "right_wrist_1_joint",
"right_wrist_2_joint", "right_wrist_3_joint",
"left_elbow_joint", "left_shoulder_lift_joint",
"left_shoulder_pan_joint", "left_wrist_1_joint",
"left_wrist_2_joint", "left_wrist_3_joint", "linear_joint",
"pan_joint", "tilt_joint"
]
self._right_arm_joints = [
"right_elbow_joint", "right_shoulder_lift_joint",
"right_shoulder_pan_joint", "right_wrist_1_joint",
"right_wrist_2_joint", "right_wrist_3_joint"
]
self._left_arm_joints = [
"left_elbow_joint", "left_shoulder_lift_joint",
"left_shoulder_pan_joint", "left_wrist_1_joint",
"left_wrist_2_joint", "left_wrist_3_joint"
]
self.tucked = [
-2.8, -1.48, -1.48, 0, 0, 1.571, 2.8, 1.48, 1.48, 0, 0, -1.571,
0.0371, 0.0, 0.0
]
self.constrained_stow = [
2.28, 2.17, -2.56, -0.09, 0.15, 1.082, -2.28, -2.17, 2.56,
0.09, -0.15, 2.06, 0.42, 0.0, 0.0
]
self.larm_const_stow = [-2.28, -2.17, 2.56, 0.09, -0.15, 2.08]
self.rarm_const_stow = [2.28, 2.17, -2.56, -0.09, 0.15, 1.06]
self.tableDist = 0.7
elif "7dof" == self.dof:
self._upper_body_joints = [
"right_shoulder_pan_joint", "right_shoulder_lift_joint",
"right_arm_half_joint", "right_elbow_joint",
"right_wrist_spherical_1_joint",
"right_wrist_spherical_2_joint", "right_wrist_3_joint",
"left_shoulder_pan_joint", "left_shoulder_lift_joint",
"left_arm_half_joint", "left_elbow_joint",
"left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint",
"left_wrist_3_joint", "linear_joint", "pan_joint", "tilt_joint"
]
self._right_arm_joints = [
"right_shoulder_pan_joint", "right_shoulder_lift_joint",
"right_arm_half_joint", "right_elbow_joint",
"right_wrist_spherical_1_joint",
"right_wrist_spherical_2_joint", "right_wrist_3_joint"
]
self._left_arm_joints = [
"left_shoulder_pan_joint", "left_shoulder_lift_joint",
"left_arm_half_joint", "left_elbow_joint",
"left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint",
"left_wrist_3_joint"
]
self.tucked = [
-1.6, -1.5, 0.4, -2.7, 0.0, 0.5, -1.7, 1.6, 1.5, -0.4, 2.7,
0.0, -0.5, 1.7, 0.04, 0, 0
]
self.constrained_stow = [
-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, 1.0, 2.6, -2.0, 0.0, -2.0, 0.0,
0.0, -1.0, 0.42, 0, 0
]
self.larm_const_stow = [2.6, -2.0, 0.0, -2.0, 0.0, 0.0, 1.0]
self.rarm_const_stow = [-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, -1.0]
self.tableDist = 0.8
else:
rospy.logerr("DoF needs to be set 6 or 7, aborting demo")
return
self.pickplace = [None] * 2
self.pickplace[0] = PickPlaceInterface("left_side",
"left_gripper",
verbose=True)
self.pickplace[0].planner_id = "RRTConnectkConfigDefault"
self.pickplace[1] = PickPlaceInterface("right_side",
"right_gripper",
verbose=True)
self.pickplace[1].planner_id = "RRTConnectkConfigDefault"
self.pick_result = [None] * 2
self._last_gripper_picked = 0
self.place_result = [None] * 2
self._last_gripper_placed = 0
self._objs_to_keep = []
self._listener = tf.TransformListener()
self._lgripper = GripperActionClient('left')
self._rgripper = GripperActionClient('right')
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(
find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
self.scene.clear()
# This is a simulation so need to adjust gripper parameters
if sim:
self._gripper_closed = 0.96
self._gripper_open = 0.00
else:
self._gripper_closed = 0.01
self._gripper_open = 0.165
def add_objects_to_keep(self, obj):
self._objs_to_keep.append(obj)
def clearScene(self):
self.scene.clear()
def getPickCoordinates(self):
self.updateScene(0, False)
beer, grasps = self.getGraspableBeer(False)
pringles, grasps = self.getGraspablePringles(False)
if (None == beer) or (None == pringles):
return None
center_objects = (beer.primitive_poses[0].position.y +
pringles.primitive_poses[0].position.y) / 2
surface = self.getSupportSurface(beer.support_surface)
tmp1 = surface.primitive_poses[
0].position.x - surface.primitives[0].dimensions[0] / 2
surface = self.getSupportSurface(pringles.support_surface)
tmp2 = surface.primitive_poses[
0].position.x - surface.primitives[0].dimensions[0] / 2
front_edge = (tmp1 + tmp2) / 2
coords = Pose2D(x=(front_edge - self.tableDist),
y=center_objects,
theta=0.0)
return coords
def updateScene(self, gripper=0, plan=True):
# find objects
rospy.loginfo("Updating scene...")
goal = FindGraspableObjectsGoal()
goal.plan_grasps = plan
goal.gripper = gripper
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
find_result = self.find_client.get_result()
# remove previous objects
for name in self.scene.getKnownCollisionObjects():
self.scene.removeCollisionObject(name, True)
# insert objects to scene
idx = -1
for obj in find_result.objects:
if obj.object.primitive_poses[
0].position.z < 0.5 or obj.object.primitive_poses[
0].position.x > 2.0 or obj.object.primitive_poses[
0].position.y > 0.5:
continue
idx += 1
obj.object.name = "object%d_%d" % (idx, gripper)
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
wait=True)
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
if obj.primitive_poses[0].position.z < 0.5:
continue
height = obj.primitive_poses[0].position.z
obj.primitives[0].dimensions = [
obj.primitives[0].dimensions[0],
obj.primitives[0].dimensions[1], # wider
obj.primitives[0].dimensions[2] + height
]
obj.primitive_poses[0].position.z += -height / 2.0
# add to scene
self.scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
wait=True)
self.scene.waitForSync()
# store for grasping
self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
def getGraspableBeer(self, planned=True):
for obj in self.objects:
# need grasps
if len(obj.grasps) < 1 and planned:
continue
# has to be on table
if obj.object.primitive_poses[0].position.z < 0.5 or \
obj.object.primitive_poses[0].position.z > 1.0 or \
obj.object.primitive_poses[0].position.x > 2.0:
continue
elif (obj.object.primitives[0].type == sp.CYLINDER):
if obj.object.primitives[0].dimensions[sp.CYLINDER_HEIGHT] < 0.102 or \
obj.object.primitives[0].dimensions[sp.CYLINDER_HEIGHT] > 0.142:
continue
elif (obj.object.primitives[0].type == sp.BOX):
if obj.object.primitives[0].dimensions[sp.BOX_Z] < 0.102 or \
obj.object.primitives[0].dimensions[sp.BOX_Z] > 0.142:
continue
else:
continue
print "beer: ", obj.object.primitive_poses[0]
return obj.object, obj.grasps
# nothing detected
return None, None
def getGraspablePringles(self, planned=True):
for obj in self.objects:
# need grasps
if len(obj.grasps) < 1 and planned:
continue
# has to be on table
if obj.object.primitive_poses[0].position.z < 0.5 or \
obj.object.primitive_poses[0].position.z > 1.0 or \
obj.object.primitive_poses[0].position.x > 2.0:
continue
elif (obj.object.primitives[0].type == sp.CYLINDER):
if obj.object.primitives[0].dimensions[sp.CYLINDER_HEIGHT] < 0.21 or \
obj.object.primitives[0].dimensions[sp.CYLINDER_HEIGHT] > 0.28:
continue
elif (obj.object.primitives[0].type == sp.BOX):
if obj.object.primitives[0].dimensions[sp.BOX_Z] < 0.21 or \
obj.object.primitives[0].dimensions[sp.BOX_Z] > 0.28:
continue
else:
continue
return obj.object, obj.grasps
# nothing detected
return None, None
def getSupportSurface(self, name):
for surface in self.surfaces:
if surface.name == name:
return surface
return None
def getPlaceLocation(self):
pass
def pick(self, block, grasps, gripper=0):
success, pick_result = self.pickplace[gripper].pick_with_retry(
block.name,
grasps,
retries=1,
support_name=block.support_surface,
scene=self.scene)
self.pick_result[gripper] = pick_result
self._last_gripper_picked = gripper
return success
def place(self, block, pose_stamped, gripper=0):
places = list()
l = PlaceLocation()
l.place_pose.pose = pose_stamped.pose
l.place_pose.header.frame_id = pose_stamped.header.frame_id
# copy the posture, approach and retreat from the grasp used
l.post_place_posture = self.pick_result[
gripper].grasp.pre_grasp_posture
l.pre_place_approach = self.pick_result[
gripper].grasp.pre_grasp_approach
l.post_place_retreat = self.pick_result[
gripper].grasp.post_grasp_retreat
places.append(copy.deepcopy(l))
# create another several places, rotate each by 360/m degrees in yaw direction
m = 16 # number of possible place poses
pi = 3.141592653589
for i in range(0, m - 1):
l.place_pose.pose = rotate_pose_msg_by_euler_angles(
l.place_pose.pose, 0, 0, 2 * pi / m)
places.append(copy.deepcopy(l))
success, place_result = self.pickplace[gripper].place_with_retry(
block.name, places, retries=1, scene=self.scene)
self.place_result[gripper] = place_result
self._last_gripper_placed = gripper
return success
def goto_tuck(self):
# remove previous objects
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(
self._upper_body_joints, self.tucked, 0.05)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def goto_plan_grasp(self):
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(
self._upper_body_joints, self.constrained_stow, 0.05)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def left_arm_constrained_stow(self):
c1 = Constraints()
c1.orientation_constraints.append(OrientationConstraint())
c1.orientation_constraints[0].header.stamp = rospy.get_rostime()
c1.orientation_constraints[0].header.frame_id = "base_link"
c1.orientation_constraints[0].link_name = "left_ee_link"
c1.orientation_constraints[0].orientation.w = 1.0
c1.orientation_constraints[
0].absolute_x_axis_tolerance = 0.2 #x axis is pointed up for wrist link
c1.orientation_constraints[0].absolute_y_axis_tolerance = 0.2
c1.orientation_constraints[0].absolute_z_axis_tolerance = 6.28
c1.orientation_constraints[0].weight = 1.0
while not rospy.is_shutdown():
result = self.lmove_group.moveToJointPosition(
self._left_arm_joints,
self.larm_const_stow,
0.05,
path_constraints=c1,
planning_time=120.0)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def right_arm_constrained_stow(self):
c1 = Constraints()
c1.orientation_constraints.append(OrientationConstraint())
c1.orientation_constraints[0].header.stamp = rospy.get_rostime()
c1.orientation_constraints[0].header.frame_id = "base_link"
c1.orientation_constraints[0].link_name = "right_ee_link"
c1.orientation_constraints[0].orientation.w = 1.0
c1.orientation_constraints[
0].absolute_x_axis_tolerance = 0.2 #x axis is pointed up for wrist link
c1.orientation_constraints[0].absolute_y_axis_tolerance = 0.2
c1.orientation_constraints[0].absolute_z_axis_tolerance = 6.28
c1.orientation_constraints[0].weight = 1.0
while not rospy.is_shutdown():
result = self.rmove_group.moveToJointPosition(
self._right_arm_joints,
self.rarm_const_stow,
0.05,
path_constraints=c1,
planning_time=120.0)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def open_gripper(self):
self._lgripper.command(self._gripper_open, block=True)
self._rgripper.command(self._gripper_open, block=True)
def close_gripper(self):
self._lgripper.command(self._gripper_closed, block=True)
self._rgripper.command(self._gripper_closed, block=True)
def __init__(self, sim=False):
self.scene = PlanningSceneInterface("base_link")
self.dof = rospy.get_param('~jaco_dof')
self.robot = moveit_commander.RobotCommander()
self._listener = tf.TransformListener()
# self.frame = self._listener.lookupTransform('/left_ee_link', '/base_link', rospy.Time(0))
# rospy.loginfo("========== Left_ee to base_link is: ")
# rospy.loginfo(self.frame)
# curr_pos = self._listener.lookupTransform('/base_link', '/left_ee_link', rospy.Time(0))
# rospy.loginfo(curr_pos)
# self.move_group = moveit_commander.MoveGroupCommander("upper_body")
# rospy.loginfo("======upper_body_group connected =========")
# rospy.loginfo(self.move_group.get_joints())
# self.lmove_group = moveit_commander.MoveGroupCommander("left_arm")
# rospy.loginfo("======left_arm_group connected =========")
# self.rmove_group = moveit_commander.MoveGroupCommander("right_arm")
# rospy.loginfo("======right_arm_group connected =========")
self.move_group = MoveGroupInterface("upper_body", "base_link")
self.lmove_group = MoveGroupInterface("left_arm", "base_link")
self.rmove_group = MoveGroupInterface("right_arm", "base_link")
self.move_group.setPlannerId("RRTConnectkConfigDefault")
self.lmove_group.setPlannerId("RRTConnectkConfigDefault")
self.rmove_group.setPlannerId("RRTConnectkConfigDefault")
if "6dof" == self.dof:
rospy.logwarn(
"======= Please change launch param to 7DoF =========")
elif "7dof" == self.dof:
self._upper_body_joints = [
"right_shoulder_pan_joint", "right_shoulder_lift_joint",
"right_arm_half_joint", "right_elbow_joint",
"right_wrist_spherical_1_joint",
"right_wrist_spherical_2_joint", "right_wrist_3_joint",
"left_shoulder_pan_joint", "left_shoulder_lift_joint",
"left_arm_half_joint", "left_elbow_joint",
"left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint",
"left_wrist_3_joint", "linear_joint", "pan_joint", "tilt_joint"
]
self._right_arm_joints = [
"right_shoulder_pan_joint", "right_shoulder_lift_joint",
"right_arm_half_joint", "right_elbow_joint",
"right_wrist_spherical_1_joint",
"right_wrist_spherical_2_joint", "right_wrist_3_joint"
]
self._left_arm_joints = [
"left_shoulder_pan_joint", "left_shoulder_lift_joint",
"left_arm_half_joint", "left_elbow_joint",
"left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint",
"left_wrist_3_joint"
]
self.tucked = [
-1.6, -1.5, 0.4, -2.7, 0.0, 0.5, -1.7, 1.6, 1.5, -0.4, 2.7,
0.0, -0.5, 1.7, 0.04, 0, 0
]
# self.constrained_stow =[-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, 1.0, 2.6, -2.0, 0.0, -2.0, 0.0, 0.0, -1.0, 0.42, 0, 0]
# flipping pose
self.constrained_stow = [
-0.23, -0.71, -1.02, -1.0, 0.9, 1.89, -2.41, 0.44, 0.71, 1.12,
1.21, -1.02, -1.84, 2.61, 0.20, 0, 0
]
# self.constrained_stow_2 = [-0.34, -0.66, -0.92, -1.18, 0.94, 1.75, -2.49, 0.54, 0.65, 1.0, 1.35, -1.07, -1.75, 2.67, 0.20, 0, 0]
self.larm_const_stow = [2.6, -2.0, 0.0, -2.0, 0.0, 0.0, 1.0]
self.rarm_const_stow = [-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, -1.0]
self.tableDist = 0.8
else:
rospy.logerr("DoF needs to be set 6 or 7, aborting demo")
return
'''
pickplace = [lg_itf, rg_itf]
pick_result = [None, None]
'''
self.pickplace = [None] * 2
self.pickplace[0] = PickPlaceInterface("left_side",
"left_gripper",
verbose=True)
self.pickplace[0].planner_id = "RRTConnectkConfigDefault"
self.pickplace[1] = PickPlaceInterface("right_side",
"right_gripper",
verbose=True)
self.pickplace[1].planner_id = "RRTConnectkConfigDefault"
self.pick_result = [None] * 2
self._last_gripper_picked = 0
self.place_result = [None] * 2
self._last_gripper_placed = 0
self._objs_to_keep = []
self._lgripper = GripperActionClient('left')
self._rgripper = GripperActionClient('right')
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(
find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
rospy.loginfo(
"============= FindGraspableObjectsAction connected! ============")
self.scene.clear()
# This is a simulation so need to adjust gripper parameters
if sim:
self._gripper_closed = 0.96
self._gripper_open = 0.00
else:
self._gripper_closed = 0.01
self._gripper_open = 0.165
class L0_dual_set_gripper_srv():
# This is a action server
_feedback = arclib_msg.dual_set_gripperFeedback()
_result = arclib_msg.dual_set_gripperResult()
def __init__(self, arclib_node):
self._c = arclib_node
self._action_name = "L0_dual_set_gripper"
self._as = actionlib.SimpleActionServer(
self._action_name,
arclib_msg.dual_set_gripperAction,
execute_cb=self.execute_cb,
auto_start=False)
self._lgripper = GripperActionClient('left')
self._rgripper = GripperActionClient('right')
self._gripper_closed = 0.00
self._gripper_open = 0.165
self._as.register_preempt_callback(self.preempt_cb)
self._as.start()
print(self._action_name, "Started!")
def _goal_check(self, goal):
# fake check, not finished yet. Bug Here
# TODO not finished yet.
# for i in check_list:
# pi=3.1415
# if(i<pi and i > -pi):
# pass
# else:
# return False
return True
def _pub_feedback(self):
self._feedback.NotImplemented = False
self._as.publish_feedback(self._feedback)
pass
def cancel_all_sub_goals(self):
# TODO it needs some time to publish cancel the goal
freq = rospy.Rate(5)
freq.sleep()
# self._c.move_group.get_move_action().cancel_all_goals()
self._rgripper._client.cancel_all_goals()
self._lgripper._client.cancel_all_goals()
def preempt_cb(self):
print("This action has been preempted")
# cancel all sub goals
self.cancel_all_sub_goals()
def _get_jps_from_goal(self, goal):
pass
def send_sub_goal(self, goal):
# print "do close gripper"
value = goal.value
v = float(value) * (self._gripper_open -
self._gripper_closed) + self._gripper_closed
self._lgripper.command(v, block=False)
self._rgripper.command(v, block=False)
def execute_cb(self, goal):
success = True
rospy.loginfo("goal get" + str(goal))
self.goal_start_time = time.time()
if (not (self._goal_check(goal))):
rospy.loginfo("Fail, the goal was rejected")
success = False
self._pub_feedback()
if (success == True):
self.send_sub_goal(goal)
while (not rospy.is_shutdown()):
if self._as.is_preempt_requested():
rospy.loginfo("%s: Preempted" % self._action_name)
self._as.set_preempted()
rospy.loginfo(" fail, preemted")
success = False
break
# update feedback and publish
self._pub_feedback()
# both sub goal finished
done_success = self._c.tool_is_dualaction_success(
act1=self._lgripper._client, act2=self._rgripper._client)
sub_goal_done = self._c.tool_is_dualgoal_done(
act1=self._lgripper._client, act2=self._rgripper._client)
# task finished, -> success
done_shut = rospy.is_shutdown()
# print(done_success,sub_goal_done)
done = done_success or sub_goal_done or done_shut
if (done):
success = True
rospy.loginfo(" done")
break
if (rospy.is_shutdown()):
success = False
self._set_success_and_abort(success)
def _set_success_and_abort(self, success):
if success:
self._result.success = True
# rospy.loginfo("%s:Succeeded!",%self._action_name)
rospy.loginfo("Succeeded!" + str(self._action_name))
self._as.set_succeeded(self._result)
else:
self._result.success = False
# rospy.loginfo("%s:Succeeded!",%self._action_name)
rospy.loginfo("Aborted!" + str(self._action_name))
self._as.set_aborted(self._result)
# cancel all sub golad
self.cancel_all_sub_goals()
class GraspingClient(object):
def __init__(self, sim=False):
# Define planning groups
self.scene = PlanningSceneInterface("base_link")
self.dof = rospy.get_param('~jaco_dof')
self.move_group = MoveGroupInterface("upper_body", "base_link")
self.rmove_group = MoveGroupInterface("right_arm", "base_link")
planner_id = "RRTConnectkConfigDefault"
self.move_group.setPlannerId(planner_id)
self.rmove_group.setPlannerId(planner_id)
self.objects_heights = [0.122, 0.240]
self.objects_heights_tolerances = [0.02, 0.03]
# Define joints and positions for 6 and 7 DOF
if "6dof" == self.dof:
self._upper_body_joints = [
"right_elbow_joint", "right_shoulder_lift_joint",
"right_shoulder_pan_joint", "right_wrist_1_joint",
"right_wrist_2_joint", "right_wrist_3_joint",
"left_elbow_joint", "left_shoulder_lift_joint",
"left_shoulder_pan_joint", "left_wrist_1_joint",
"left_wrist_2_joint", "left_wrist_3_joint", "linear_joint",
"pan_joint", "tilt_joint"
]
self._right_arm_joints = [
"right_elbow_joint", "right_shoulder_lift_joint",
"right_shoulder_pan_joint", "right_wrist_1_joint",
"right_wrist_2_joint", "right_wrist_3_joint"
]
self._left_arm_joints = [
"left_elbow_joint", "left_shoulder_lift_joint",
"left_shoulder_pan_joint", "left_wrist_1_joint",
"left_wrist_2_joint", "left_wrist_3_joint"
]
self.tucked = [
-2.8, -1.48, -1.48, 0, 0, 1.571, 2.8, 1.48, 1.48, 0, 0, -1.571,
0.0371, 0.0, 0.0
]
self.constrained_stow = [
2.28, 2.00, -2.56, -0.09, 0.15, 1.082, -2.28, -2.17, 2.56,
0.09, -0.15, 2.06, 0.42, 0.0, 0.0
]
self.rarm_const_stow = [2.28, 2.00, -2.56, -0.09, 0.15, 1.06]
#self.rarm_const_stow = [-1.51, -0.22, -2.00, -2.04, 1.42, 1.32]
elif "7dof" == self.dof:
self._upper_body_joints = [
"right_shoulder_pan_joint", "right_shoulder_lift_joint",
"right_arm_half_joint", "right_elbow_joint",
"right_wrist_spherical_1_joint",
"right_wrist_spherical_2_joint", "right_wrist_3_joint",
"left_shoulder_pan_joint", "left_shoulder_lift_joint",
"left_arm_half_joint", "left_elbow_joint",
"left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint",
"left_wrist_3_joint", "linear_joint", "pan_joint", "tilt_joint"
]
self._right_arm_joints = [
"right_shoulder_pan_joint", "right_shoulder_lift_joint",
"right_arm_half_joint", "right_elbow_joint",
"right_wrist_spherical_1_joint",
"right_wrist_spherical_2_joint", "right_wrist_3_joint"
]
self._left_arm_joints = [
"left_shoulder_pan_joint", "left_shoulder_lift_joint",
"left_arm_half_joint", "left_elbow_joint",
"left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint",
"left_wrist_3_joint"
]
self.tucked = [
-1.6, -1.5, 0.4, -2.7, 0.0, 0.5, -1.7, 1.6, 1.5, -0.4, 2.7,
0.0, -0.5, 1.7, 0.04, 0, 0
]
self.constrained_stow = [
-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, 1.0, 2.6, -2.0, 0.0, -2.0, 0.0,
0.0, -1.0, 0.42, 0, 0
]
self.rarm_const_stow = [-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, -1.0]
else:
rospy.logerr("DoF needs to be set 6 or 7, aborting demo")
return
# Define the MoveIt pickplace pipeline
self.pickplace = None
self.pickplace = PickPlaceInterface("left_side",
"left_gripper",
verbose=True)
self.pickplace.planner_id = planner_id
self.pick_result = None
self.place_result = None
self._listener = tf.TransformListener()
self._lgripper = GripperActionClient('left')
find_grasp_planning_topic = "grasp_planner/plan"
rospy.loginfo("Waiting for %s..." % find_grasp_planning_topic)
self.find_grasp_planning_client = actionlib.SimpleActionClient(
find_grasp_planning_topic, GraspPlanningAction)
self.find_grasp_planning_client.wait_for_server()
rospy.loginfo("...connected")
self.scene.clear()
# This is a simulation so need to adjust gripper parameters
if sim:
self._gripper_closed = 0.96
self._gripper_open = 0.00
else:
self._gripper_closed = 0.01
self._gripper_open = 0.165
def __del__(self):
self.scene.clear()
def add_objects_to_keep(self, obj):
self._objs_to_keep.append(obj)
def clearScene(self):
self.scene.clear()
def calculateGraspForObject(self, object_to_grasp, gripper):
goal = GraspPlanningGoal()
goal.object = object_to_grasp
goal.gripper = gripper
self.find_grasp_planning_client.send_goal(goal)
self.find_grasp_planning_client.wait_for_result(rospy.Duration(5.0))
return self.find_grasp_planning_client.get_result(
).grasps #moveit_msgs/Grasp[]
def pick(self, block, grasps):
success, pick_result = self.pickplace.pick_with_retry(block.name,
grasps,
retries=1,
scene=self.scene)
self.pick_result = pick_result
return success
def place(self, block, pose_stamped):
places = list()
l = PlaceLocation()
l.place_pose.pose = pose_stamped.pose
l.place_pose.header.frame_id = pose_stamped.header.frame_id
# copy the posture, approach and retreat from the grasp used
l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach
l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
places.append(copy.deepcopy(l))
# create another several places, rotate each by 360/m degrees in yaw direction
m = 16 # number of possible place poses
pi = 3.141592653589
for i in range(0, m - 1):
l.place_pose.pose = rotate_pose_msg_by_euler_angles(
l.place_pose.pose, 0, 0, 2 * pi / m)
places.append(copy.deepcopy(l))
success, place_result = self.pickplace.place_with_retry(
block.name, places, retries=1, scene=self.scene)
self.place_result = place_result
return success
def goto_tuck(self):
# remove previous objects
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(
self._upper_body_joints, self.tucked, 0.05)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def goto_plan_grasp(self):
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(
self._upper_body_joints, self.constrained_stow, 0.05)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def left_arm_constrained_stow(self):
c1 = Constraints()
c1.orientation_constraints.append(OrientationConstraint())
c1.orientation_constraints[0].header.stamp = rospy.get_rostime()
c1.orientation_constraints[0].header.frame_id = "base_link"
c1.orientation_constraints[0].link_name = "left_ee_link"
c1.orientation_constraints[0].orientation.w = 1.0
c1.orientation_constraints[
0].absolute_x_axis_tolerance = 0.2 #x axis is pointed up for wrist link
c1.orientation_constraints[0].absolute_y_axis_tolerance = 0.2
c1.orientation_constraints[0].absolute_z_axis_tolerance = 6.28
c1.orientation_constraints[0].weight = 1.0
while not rospy.is_shutdown():
result = self.lmove_group.moveToJointPosition(
self._left_arm_joints,
self.larm_const_stow,
0.05,
path_constraints=c1,
planning_time=120.0)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def right_arm_constrained_stow(self):
# c1 = Constraints()
# c1.orientation_constraints.append(OrientationConstraint())
# c1.orientation_constraints[0].header.stamp = rospy.get_rostime()
# c1.orientation_constraints[0].header.frame_id = "base_link"
# c1.orientation_constraints[0].link_name = "right_ee_link"
# c1.orientation_constraints[0].orientation.w=1.0
# c1.orientation_constraints[0].absolute_x_axis_tolerance = 0.2 #x axis is pointed up for wrist link
# c1.orientation_constraints[0].absolute_y_axis_tolerance = 0.2
# c1.orientation_constraints[0].absolute_z_axis_tolerance = 6.28
# c1.orientation_constraints[0].weight = 1.0
while not rospy.is_shutdown():
result = self.rmove_group.moveToJointPosition(
self._right_arm_joints,
self.rarm_const_stow,
0.05,
planning_time=120.0)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def open_gripper(self):
self._lgripper.command(self._gripper_open, block=True)
def close_gripper(self):
self._lgripper.command(self._gripper_closed, block=True)
def __init__(self, sim=False):
# Define planning groups
self.scene = PlanningSceneInterface("base_link")
self.dof = rospy.get_param('~jaco_dof')
self.move_group = MoveGroupInterface("upper_body", "base_link")
self.rmove_group = MoveGroupInterface("right_arm", "base_link")
planner_id = "RRTConnectkConfigDefault"
self.move_group.setPlannerId(planner_id)
self.rmove_group.setPlannerId(planner_id)
self.objects_heights = [0.122, 0.240]
self.objects_heights_tolerances = [0.02, 0.03]
# Define joints and positions for 6 and 7 DOF
if "6dof" == self.dof:
self._upper_body_joints = [
"right_elbow_joint", "right_shoulder_lift_joint",
"right_shoulder_pan_joint", "right_wrist_1_joint",
"right_wrist_2_joint", "right_wrist_3_joint",
"left_elbow_joint", "left_shoulder_lift_joint",
"left_shoulder_pan_joint", "left_wrist_1_joint",
"left_wrist_2_joint", "left_wrist_3_joint", "linear_joint",
"pan_joint", "tilt_joint"
]
self._right_arm_joints = [
"right_elbow_joint", "right_shoulder_lift_joint",
"right_shoulder_pan_joint", "right_wrist_1_joint",
"right_wrist_2_joint", "right_wrist_3_joint"
]
self._left_arm_joints = [
"left_elbow_joint", "left_shoulder_lift_joint",
"left_shoulder_pan_joint", "left_wrist_1_joint",
"left_wrist_2_joint", "left_wrist_3_joint"
]
self.tucked = [
-2.8, -1.48, -1.48, 0, 0, 1.571, 2.8, 1.48, 1.48, 0, 0, -1.571,
0.0371, 0.0, 0.0
]
self.constrained_stow = [
2.28, 2.00, -2.56, -0.09, 0.15, 1.082, -2.28, -2.17, 2.56,
0.09, -0.15, 2.06, 0.42, 0.0, 0.0
]
self.rarm_const_stow = [2.28, 2.00, -2.56, -0.09, 0.15, 1.06]
#self.rarm_const_stow = [-1.51, -0.22, -2.00, -2.04, 1.42, 1.32]
elif "7dof" == self.dof:
self._upper_body_joints = [
"right_shoulder_pan_joint", "right_shoulder_lift_joint",
"right_arm_half_joint", "right_elbow_joint",
"right_wrist_spherical_1_joint",
"right_wrist_spherical_2_joint", "right_wrist_3_joint",
"left_shoulder_pan_joint", "left_shoulder_lift_joint",
"left_arm_half_joint", "left_elbow_joint",
"left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint",
"left_wrist_3_joint", "linear_joint", "pan_joint", "tilt_joint"
]
self._right_arm_joints = [
"right_shoulder_pan_joint", "right_shoulder_lift_joint",
"right_arm_half_joint", "right_elbow_joint",
"right_wrist_spherical_1_joint",
"right_wrist_spherical_2_joint", "right_wrist_3_joint"
]
self._left_arm_joints = [
"left_shoulder_pan_joint", "left_shoulder_lift_joint",
"left_arm_half_joint", "left_elbow_joint",
"left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint",
"left_wrist_3_joint"
]
self.tucked = [
-1.6, -1.5, 0.4, -2.7, 0.0, 0.5, -1.7, 1.6, 1.5, -0.4, 2.7,
0.0, -0.5, 1.7, 0.04, 0, 0
]
self.constrained_stow = [
-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, 1.0, 2.6, -2.0, 0.0, -2.0, 0.0,
0.0, -1.0, 0.42, 0, 0
]
self.rarm_const_stow = [-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, -1.0]
else:
rospy.logerr("DoF needs to be set 6 or 7, aborting demo")
return
# Define the MoveIt pickplace pipeline
self.pickplace = None
self.pickplace = PickPlaceInterface("left_side",
"left_gripper",
verbose=True)
self.pickplace.planner_id = planner_id
self.pick_result = None
self.place_result = None
self._listener = tf.TransformListener()
self._lgripper = GripperActionClient('left')
find_grasp_planning_topic = "grasp_planner/plan"
rospy.loginfo("Waiting for %s..." % find_grasp_planning_topic)
self.find_grasp_planning_client = actionlib.SimpleActionClient(
find_grasp_planning_topic, GraspPlanningAction)
self.find_grasp_planning_client.wait_for_server()
rospy.loginfo("...connected")
self.scene.clear()
# This is a simulation so need to adjust gripper parameters
if sim:
self._gripper_closed = 0.96
self._gripper_open = 0.00
else:
self._gripper_closed = 0.01
self._gripper_open = 0.165
voice_pub = rospy.Publisher("/movo/voice/text",
String,
queue_size=1,
latch=True)
voice_cmd = String()
voice_cmd.data = "Do you want to dance with me?"
# voice_cmd.data = "Long fei, stop wasting time on me. It will not work, unless you say I like movo."
voice_pub.publish(voice_cmd)
dof = rospy.get_param('~jaco_dof')
rmove_group = MoveGroupInterface("right_arm", "base_link")
lmove_group = MoveGroupInterface("left_arm", "base_link")
lmove_group.setPlannerId("RRTConnectkConfigDefault")
rmove_group.setPlannerId("RRTConnectkConfigDefault")
lgripper = GripperActionClient('left')
rgripper = GripperActionClient('right')
if "6dof" == dof:
right_arm_joints = [
"right_shoulder_pan_joint", "right_shoulder_lift_joint",
"right_elbow_joint", "right_wrist_1_joint", "right_wrist_2_joint",
"right_wrist_3_joint"
]
left_arm_joints = [
"left_shoulder_pan_joint", "left_shoulder_lift_joint",
"left_elbow_joint", "left_wrist_1_joint", "left_wrist_2_joint",
"left_wrist_3_joint"
]
rarm_pick = [
np.radians(x) for x in [-75.0, -15.0, -84.0, 0.0, -120.0, 30.0]