def opendoor(req):
# main(whole_body, gripper,wrist_wrench)
frame = req.handle_pose.header.frame_id
hanlde_pos = req.handle_pose.pose
# hanlde_pos=geometry_msgs.msg.PoseStamped()
res = OpendoorResponse()
cli = actionlib.SimpleActionClient('/hsrb/omni_base_controller/follow_joint_trajectory', control_msgs.msg.FollowJointTrajectoryAction)
vel_pub = rospy.Publisher('/hsrb/command_velocity', geometry_msgs.msg.Twist, queue_size=10)
armPub = rospy.Publisher('/hsrb/arm_trajectory_controller/command', JointTrajectory, queue_size=1)
robot = hsrb_interface.Robot()
whole_body = robot.get('whole_body')
gripper = robot.get('gripper')
wrist_wrench = robot.get('wrist_wrench')
base=robot.get('omni_base')
start_theta=base.pose[2]
# with hsrb_interface.Robot() as robot:
# whole_body = robot.get('whole_body')
# gripper = robot.get('gripper')
# wrist_wrench = robot.get('wrist_wrench')
try:
# Open the gripper
whole_body.move_to_neutral()
grasp_point_client()
global recog_pos
global is_found
print recog_pos.pose.position
print("Opening the gripper")
whole_body.move_to_neutral()
rospy.sleep(2.5)
switch = ImpedanceControlSwitch()
gripper.command(1.0)
# Approach to the door
print("Approach to the door")
grab_pose = geometry.multiply_tuples(geometry.pose(x=recog_pos.pose.position.x-HANDLE_TO_HAND_POS_X,
y=recog_pos.pose.position.y-HANDLE_TO_HAND_POS_Y,
z=recog_pos.pose.position.z,
ej=math.pi/2),
geometry.pose(ek=math.pi/2))
whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF)
# Close the gripper
wrist_wrench.reset()
switch.activate("grasping")
gripper.grasp(-0.01)
rospy.sleep(1.0)
switch.inactivate()
# Rotate the handle
whole_body.impedance_config = 'grasping'
traj = JointTrajectory()
# This controller requires that all joints have values
traj.joint_names = ["arm_lift_joint", "arm_flex_joint",
"arm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
def opendoor(req):
# main(whole_body, gripper,wrist_wrench)
frame = req.handle_pose.header.frame_id
hanlde_pos = req.handle_pose.pose
# hanlde_pos=geometry_msgs.msg.PoseStamped()
res = OpendoorResponse()
armPub = rospy.Publisher('/hsrb/arm_trajectory_controller/command',
JointTrajectory,
queue_size=1)
robot = hsrb_interface.Robot()
whole_body = robot.get('whole_body')
gripper = robot.get('gripper')
wrist_wrench = robot.get('wrist_wrench')
# with hsrb_interface.Robot() as robot:
# whole_body = robot.get('whole_body')
# gripper = robot.get('gripper')
# wrist_wrench = robot.get('wrist_wrench')
try:
# Open the gripper
print("Opening the gripper")
whole_body.move_to_neutral()
switch = ImpedanceControlSwitch()
gripper.command(1.0)
# Approach to the door
print("Approach to the door")
grab_pose = geometry.multiply_tuples(
geometry.pose(x=hanlde_pos.position.x - HANDLE_TO_HAND_POS,
y=hanlde_pos.position.y,
z=hanlde_pos.position.z,
ej=math.pi / 2), geometry.pose(ek=math.pi / 2))
whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF)
# Close the gripper
wrist_wrench.reset()
switch.activate("grasping")
gripper.grasp(-0.008)
rospy.sleep(1.0)
switch.inactivate()
# Rotate the handle
whole_body.impedance_config = 'grasping'
traj = JointTrajectory()
# This controller requires that all joints have values
traj.joint_names = [
"arm_lift_joint", "arm_flex_joint", "arm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
p = JointTrajectoryPoint()
# set the desired value
current_positions = [
latest_positions[name] for name in traj.joint_names
]
current_positions[0] = latest_positions["arm_lift_joint"] - 0.04
current_positions[1] = latest_positions["arm_flex_joint"] - 0.015
current_positions[2] = latest_positions["arm_roll_joint"]
current_positions[3] = latest_positions["wrist_flex_joint"]
current_positions[4] = latest_positions["wrist_roll_joint"] - 0.55
p.positions = current_positions
p.velocities = [0, 0, 0, 0, 0]
p.time_from_start = rospy.Time(3)
traj.points = [p]
armPub.publish(traj)
rospy.sleep(5.0)
whole_body.impedance_config = 'grasping'
whole_body.move_end_effector_by_line((0, 0, 1), 0.35)
whole_body.impedance_config = None
gripper.command(1.0)
whole_body.move_to_neutral()
res.success = True
except Exception as e:
rospy.logerr(e)
print "Failed to open door"
res.success = False
return res
def opendoor(req):
# main(whole_body, gripper,wrist_wrench)
frame = req.handle_pose.header.frame_id
hanlde_pos = req.handle_pose.pose
# hanlde_pos=geometry_msgs.msg.PoseStamped()
res = OpendoorResponse()
cli = actionlib.SimpleActionClient(
'/hsrb/omni_base_controller/follow_joint_trajectory',
control_msgs.msg.FollowJointTrajectoryAction)
vel_pub = rospy.Publisher('/hsrb/command_velocity',
geometry_msgs.msg.Twist,
queue_size=10)
armPub = rospy.Publisher('/hsrb/arm_trajectory_controller/command',
JointTrajectory,
queue_size=1)
robot = hsrb_interface.Robot()
whole_body = robot.get('whole_body')
gripper = robot.get('gripper')
wrist_wrench = robot.get('wrist_wrench')
base = robot.get('omni_base')
start_theta = base.pose[2]
# with hsrb_interface.Robot() as robot:
# whole_body = robot.get('whole_body')
# gripper = robot.get('gripper')
# wrist_wrench = robot.get('wrist_wrench')
try:
# Open the gripper
whole_body.move_to_neutral()
grasp_point_client()
global recog_pos
global is_found
print recog_pos.pose.position
print("Opening the gripper")
whole_body.move_to_neutral()
rospy.sleep(2.5)
switch = ImpedanceControlSwitch()
gripper.command(1.0)
# Approach to the door
listener = tf.TransformListener()
listener.waitForTransform(_ORIGIN_TF, _BASE_TF, rospy.Time(),
rospy.Duration(4.0))
# translation,rot = listener.lookupTransform(_BASE_TF, _ORIGIN_TF, rospy.Time())
recog_pos.header.frame_id = _ORIGIN_TF
recog_pos2 = listener.transformPose(_BASE_TF, recog_pos)
print("Approach to the door")
grab_pose = geometry.multiply_tuples(
geometry.pose(
x=recog_pos2.pose.position.x - HANDLE_TO_HAND_POS_X / 2,
y=recog_pos2.pose.position.y - HANDLE_TO_HAND_POS_X / 2,
z=recog_pos2.pose.position.z,
ej=math.pi / 2), geometry.pose(ek=math.pi / 2))
# grab_pose = geometry.multiply_tuples(geometry.pose(x=recog_pos2.pose.position.x-HANDLE_TO_HAND_POS_X/2,
# y=recog_pos2.pose.position.y-HANDLE_TO_HAND_POS_X/2,
# z=recog_pos2.pose.position.z,
# ej=0,
# ek=math.pi/2), geometry.pose(ei=math.pi/2))
# grab_pose = geometry.multiply_tuples(geometry.pose(x=recog_pos.pose.position.x-translation[0]-HANDLE_TO_HAND_POS_X,
# y=recog_pos.pose.position.y-translation[1],
# z=recog_pos.pose.position.z-translation[2],
# ej=math.pi/2),
# geometry.pose(ek=0.0))
# # geometry.pose(ek=math.pi/2))
whole_body.move_end_effector_pose(grab_pose, _BASE_TF)
whole_body.move_end_effector_by_line((1, 0, 1), 0.06)
# Close the gripper
wrist_wrench.reset()
switch.activate("grasping")
gripper.grasp(-0.01)
rospy.sleep(1.0)
switch.inactivate()
# rospy.sleep(100)
# Rotate the handle
whole_body.impedance_config = 'grasping'
whole_body.move_end_effector_by_line((0, 1, 0), -0.015)
# traj = JointTrajectory()
# # This controller requires that all joints have values
# traj.joint_names = ["arm_lift_joint", "arm_flex_joint",
# "arm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
# p = JointTrajectoryPoint()
# current_positions = [latest_positions[name] for name in traj.joint_names]
# current_positions[0] = latest_positions["arm_lift_joint"]-0.0675
# current_positions[1] = latest_positions["arm_flex_joint"]-0.02
# current_positions[2] = latest_positions["arm_roll_joint"]
# current_positions[3] = latest_positions["wrist_flex_joint"]
# current_positions[4] = latest_positions["wrist_roll_joint"]-0.65
# p.positions = current_positions
# p.velocities = [0, 0, 0, 0, 0]
# p.time_from_start = rospy.Time(3)
# traj.points = [p]
# armPub.publish(traj)
# rospy.sleep(3.0)
# print("finishing rotating handle")
# ## Move by End-effector line
# whole_body.impedance_config = 'compliance_hard'
# whole_body.move_end_effector_by_line((0.0,0.0,1), 0.45)
print("pull the door")
# Move base with linear & Angular motion
# hand_pos.header.frame_id=u'hand_palm_link'
print("test 1")
tw = geometry_msgs.msg.Twist()
tw.linear.x = -0.3
tw.angular.z = -0.45
vel_pub.publish(tw)
rospy.sleep(4.0)
print("test2 ")
tw = geometry_msgs.msg.Twist()
tw.linear.x = -1
tw.angular.z = -0.45
vel_pub.publish(tw)
rospy.sleep(4.0)
print("test 3 ")
tw = geometry_msgs.msg.Twist()
tw.linear.x = -1
tw.angular.z = -0.45
vel_pub.publish(tw)
#############keep
rospy.sleep(4.0)
print("test 4 ")
tw = geometry_msgs.msg.Twist()
tw.linear.x = -1
tw.angular.z = -0.45
vel_pub.publish(tw)
rospy.sleep(4.0)
print("test 5")
tw = geometry_msgs.msg.Twist()
tw.linear.x = -1
tw.angular.z = -0.45
vel_pub.publish(tw)
rospy.sleep(4.0)
print("test 6")
tw = geometry_msgs.msg.Twist()
tw.linear.x = -1
tw.angular.z = -0.45
vel_pub.publish(tw)
rospy.sleep(4.0)
print("test 7")
tw = geometry_msgs.msg.Twist()
tw.linear.x = -1
tw.angular.z = -0.45
vel_pub.publish(tw)
rospy.sleep(4.0)
##############################keep
## Move base orthogonally to the force
# force_sensor_capture = ForceSensorCapture()
# force_list = force_sensor_capture.get_current_force()
# a=-force_list[0]/10
# b=force_list[1]/10
# tw_cmd0 = geometry_msgs.msg.Twist()
# tw_cmd0.linear.x =-a
# tw_cmd0.linear.y = b
# vel_pub.publish(tw_cmd0)
# rospy.sleep(2.0)
# force_list = force_sensor_capture.get_current_force()
# a=-force_list[0]/10
# b=force_list[1]/10
# tw_cmd1 = geometry_msgs.msg.Twist()
# tw_cmd1.linear.x =-a
# tw_cmd1.linear.y = b
# vel_pub.publish(tw_cmd1)
# rospy.sleep(2.0)
# force_list = force_sensor_capture.get_current_force()
# a=-force_list[0]/10
# b=force_list[1]/10
# tw_cmd2 = geometry_msgs.msg.Twist()
# tw_cmd2.linear.x =-a
# tw_cmd2.linear.y = b
# vel_pub.publish(tw_cmd2)
# rospy.sleep(2.0)
## Move base with linear & Angular motion
# tw = geometry_msgs.msg.Twist()
# tw.linear.x =-1
# tw.angular.z = 0.45
# vel_pub.publish(tw)
# rospy.sleep(4.0)
## Move base with linear & Angular motion second
# tw_cmd1 = geometry_msgs.msg.Twist()
# tw_cmd1.linear.x =-0.6
# tw_cmd1.linear.y =0.2
# tw_cmd1.angular.z = 0.25
# vel_pub.publish(tw_cmd1)
# # rospy.sleep(4.0)
# rospy.sleep(3.0)
# tw_cmd2 = geometry_msgs.msg.Twist()
# tw_cmd2.linear.x =-0.65
# tw_cmd2.linear.y =0.5
# tw_cmd2.angular.z = 0.35
# vel_pub.publish(tw_cmd2)
# # rospy.sleep(4.0)
# rospy.sleep(2.0)
## Open the gripper
gripper.command(1.0)
## Move back
# base.go_rel(-1.3,0.0,start_theta)
gripper.command(1.0)
tw = geometry_msgs.msg.Twist()
tw.linear.x = -1
tw.angular.z = 0.45
vel_pub.publish(tw)
rospy.sleep(2)
whole_body.move_to_neutral()
res.success = True
except Exception as e:
rospy.logerr(e)
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
print "Failed to open door"
res.success = False
return res
def opendoor(req):
# main(whole_body, gripper,wrist_wrench)
frame = req.handle_pose.header.frame_id
hanlde_pos = req.handle_pose.pose
# hanlde_pos=geometry_msgs.msg.PoseStamped()
res = OpendoorResponse()
cli = actionlib.SimpleActionClient('/hsrb/omni_base_controller/follow_joint_trajectory', control_msgs.msg.FollowJointTrajectoryAction)
vel_pub = rospy.Publisher('/hsrb/command_velocity', geometry_msgs.msg.Twist, queue_size=10)
armPub = rospy.Publisher('/hsrb/arm_trajectory_controller/command', JointTrajectory, queue_size=1)
robot = hsrb_interface.Robot()
whole_body = robot.get('whole_body')
gripper = robot.get('gripper')
wrist_wrench = robot.get('wrist_wrench')
base=robot.get('omni_base')
start_theta=base.pose[2]
# with hsrb_interface.Robot() as robot:
# whole_body = robot.get('whole_body')
# gripper = robot.get('gripper')
# wrist_wrench = robot.get('wrist_wrench')
try:
# Open the gripper
whole_body.move_to_neutral()
grasp_point_client()
global recog_pos
global is_found
print recog_pos.pose.position
print("Opening the gripper")
whole_body.move_to_neutral()
rospy.sleep(2.5)
switch = ImpedanceControlSwitch()
gripper.command(1.0)
# Approach to the door
listener = tf.TransformListener()
listener.waitForTransform(_ORIGIN_TF,_BASE_TF, rospy.Time(), rospy.Duration(4.0))
# translation,rot = listener.lookupTransform(_BASE_TF, _ORIGIN_TF, rospy.Time())
recog_pos.header.frame_id=_ORIGIN_TF
recog_pos2 = listener.transformPose(_BASE_TF, recog_pos)
print("Approach to the door")
grab_pose = geometry.multiply_tuples(geometry.pose(x=recog_pos2.pose.position.x-HANDLE_TO_HAND_POS_X,
y=recog_pos2.pose.position.y,
z=recog_pos2.pose.position.z,
ej=math.pi/2),
geometry.pose(ek=math.pi/2))
# grab_pose = geometry.multiply_tuples(geometry.pose(x=recog_pos2.pose.position.x-HANDLE_TO_HAND_POS_X/2,
# y=recog_pos2.pose.position.y-HANDLE_TO_HAND_POS_X/2,
# z=recog_pos2.pose.position.z,
# ej=0,
# ek=math.pi/2), geometry.pose(ei=math.pi/2))
# grab_pose = geometry.multiply_tuples(geometry.pose(x=recog_pos.pose.position.x-translation[0]-HANDLE_TO_HAND_POS_X,
# y=recog_pos.pose.position.y-translation[1],
# z=recog_pos.pose.position.z-translation[2],
# ej=math.pi/2),
# geometry.pose(ek=0.0))
# # geometry.pose(ek=math.pi/2))
whole_body.move_end_effector_pose(grab_pose, _BASE_TF)
whole_body.move_end_effector_by_line((0, 0, 1), 0.06)
# Close the gripper
wrist_wrench.reset()
switch.activate("grasping")
gripper.grasp(-0.01)
rospy.sleep(1.0)
switch.inactivate()
# rospy.sleep(100)
# Rotate the handle
whole_body.impedance_config = 'grasping'
whole_body.move_end_effector_by_line((0,1,0),-0.015)
rospy.sleep(1)
whole_body.move_end_effector_by_line((0,0,1),-0.06)
rospy.sleep(1)
whole_body.move_end_effector_by_line((0,1,0),0.015)
# traj = JointTrajectory()
# # This controller requires that all joints have values
# traj.joint_names = ["arm_lift_joint", "arm_flex_joint",
# "arm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
# p = JointTrajectoryPoint()
# current_positions = [latest_positions[name] for name in traj.joint_names]
# current_positions[0] = latest_positions["arm_lift_joint"]-0.0675
# current_positions[1] = latest_positions["arm_flex_joint"]-0.02
# current_positions[2] = latest_positions["arm_roll_joint"]
# current_positions[3] = latest_positions["wrist_flex_joint"]
# current_positions[4] = latest_positions["wrist_roll_joint"]-0.65
# p.positions = current_positions
# p.velocities = [0, 0, 0, 0, 0]
# p.time_from_start = rospy.Time(3)
# traj.points = [p]
# armPub.publish(traj)
# rospy.sleep(3.0)
# print("finishing rotating handle")
# ## Move by End-effector line
# whole_body.impedance_config = 'compliance_hard'
# tw = geometry_msgs.msg.Twist()
# tw.linear.x = -0.3
# tw.angular.z = 0.45
# vel_pub.publish(tw)
# rospy.sleep(2.0)
# vel_pub.publish(tw)
# rospy.sleep(2.0)
# print("test2 ")
# tw.linear.x =-0.3
# tw.angular.z = -0.45
# vel_pub.publish(tw)
# rospy.sleep(2.0)
# vel_pub.publish(tw)
# rospy.sleep(2.0)
# gripper.command(1.0)
# rospy.sleep(1.0)
# tw.linear.x = -0.3
# tw.angular.z = 0.0
# vel_pub.publish(tw)
# rospy.sleep(1.0)
# tw = geometry_msgs.msg.Twist()
# tw.linear.x = -0.15
# tw.linear.y = -0.45
# vel_pub.publish(tw)
tw = geometry_msgs.msg.Twist()
tw.linear.x = -0.3
tw.angular.z = 0.45
vel_pub.publish(tw)
rospy.sleep(2.0)
vel_pub.publish(tw)
rospy.sleep(2.0)
print("test2 ")
tw.linear.x =-0.3
tw.angular.z = -0.45
vel_pub.publish(tw)
rospy.sleep(2.0)
vel_pub.publish(tw)
rospy.sleep(2.0)
gripper.command(1.0)
rospy.sleep(1.0)
tw.linear.x = -0.3
tw.angular.z = 0.0
vel_pub.publish(tw)
rospy.sleep(1.0)
tw = geometry_msgs.msg.Twist()
tw.linear.x = -0.15
tw.linear.y = -0.45
vel_pub.publish(tw)
def execute(self, goal):
print("Start action")
# main(whole_body, gripper,wrist_wrench)
# frame = goal.handle_pose.header.frame_id
# hanlde_pos = goal.handle_pose.pose
# hanlde_pos=geometry_msgs.msg.PoseStamped()
cli = actionlib.SimpleActionClient(
'/hsrb/omni_base_controller/follow_joint_trajectory',
control_msgs.msg.FollowJointTrajectoryAction)
vel_pub = rospy.Publisher('/hsrb/command_velocity',
geometry_msgs.msg.Twist,
queue_size=10)
armPub = rospy.Publisher('/hsrb/arm_trajectory_controller/command',
JointTrajectory,
queue_size=1)
robot = hsrb_interface.Robot()
whole_body = robot.get('whole_body')
gripper = robot.get('gripper')
wrist_wrench = robot.get('wrist_wrench')
base = robot.get('omni_base')
start_theta = base.pose[2]
# with hsrb_interface.Robot() as robot:
# whole_body = robot.get('whole_body')
# gripper = robot.get('gripper')
# wrist_wrench = robot.get('wrist_wrench')
try:
# Open the gripper
whole_body.move_to_neutral()
grasp_point_client()
global recog_pos
global is_found
print recog_pos.pose.position
print("Opening the gripper")
whole_body.move_to_neutral()
rospy.sleep(2.5)
switch = ImpedanceControlSwitch()
gripper.command(1.0)
# Approach to the door
print("Approach to the door")
grab_pose = geometry.multiply_tuples(
geometry.pose(
x=recog_pos.pose.position.x - HANDLE_TO_HAND_POS_X,
y=recog_pos.pose.position.y - HANDLE_TO_HAND_POS_Y,
z=recog_pos.pose.position.z,
ej=math.pi / 2), geometry.pose(ek=math.pi / 2))
whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF)
# Close the gripper
wrist_wrench.reset()
switch.activate("grasping")
gripper.grasp(-0.01)
rospy.sleep(1.0)
switch.inactivate()
# Rotate the handle
whole_body.impedance_config = 'grasping'
traj = JointTrajectory()
# This controller requires that all joints have values
traj.joint_names = [
"arm_lift_joint", "arm_flex_joint", "arm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
p = JointTrajectoryPoint()
current_positions = [
latest_positions[name] for name in traj.joint_names
]
current_positions[0] = latest_positions["arm_lift_joint"] - 0.0675
current_positions[1] = latest_positions["arm_flex_joint"] - 0.02
current_positions[2] = latest_positions["arm_roll_joint"]
current_positions[3] = latest_positions["wrist_flex_joint"]
current_positions[4] = latest_positions["wrist_roll_joint"] - 0.65
p.positions = current_positions
p.velocities = [0, 0, 0, 0, 0]
p.time_from_start = rospy.Time(3)
traj.points = [p]
armPub.publish(traj)
rospy.sleep(3.0)
print("finishing rotating handle")
## Move by End-effector line
whole_body.impedance_config = 'compliance_hard'
whole_body.move_end_effector_by_line((0.0, 0.0, 1), 0.45)
print("push the door")
## Move base with linear & Angular motion
tw = geometry_msgs.msg.Twist()
tw.linear.x = 0.9
tw.angular.z = 0.45
vel_pub.publish(tw)
rospy.sleep(4.0)
## Move base with linear & Angular motion second
tw_cmd0 = geometry_msgs.msg.Twist()
tw_cmd0.linear.x = 0.5
tw_cmd0.angular.z = 0.45
vel_pub.publish(tw_cmd0)
rospy.sleep(2.0)
tw_cmd1 = geometry_msgs.msg.Twist()
tw_cmd1.linear.x = 0.6
tw_cmd1.linear.y = 0.2
tw_cmd1.angular.z = 0.25
vel_pub.publish(tw_cmd1)
# rospy.sleep(4.0)
rospy.sleep(3.0)
tw_cmd2 = geometry_msgs.msg.Twist()
tw_cmd2.linear.x = 0.65
tw_cmd2.linear.y = 0.5
tw_cmd2.angular.z = 0.35
vel_pub.publish(tw_cmd2)
# rospy.sleep(4.0)
rospy.sleep(2.0)
## Open the gripper
gripper.command(1.0)
## Move back
base.go_rel(-1.3, 0.0, start_theta)
gripper.command(1.0)
whole_body.move_to_neutral()
self.server.set_succeeded(True)
except Exception as e:
rospy.logerr(e)
print "Failed to open door"
self.server.set_succeeded(False)
#!/usr/bin/python
# -*- coding: utf-8 -*-
import rospy
from hsrb_interface import Robot, ItemTypes
from hsrb_impedance_control_example.impedance_control_switch import ImpedanceControlSwitch
with Robot() as robot:
wrist_wrench = robot.try_get('wrist_wrench')
gripper = robot.try_get('gripper')
switch = ImpedanceControlSwitch()
wrist_wrench.reset()
result = switch.activate("grasping")
if result:
print "Activate impedance control."
else:
print "Impedance control activation failure."
print "Grasp object."
gripper.grasp(-0.018)
switch.inactivate()
# print "Inactivate impedance control."
import rospy
import sys
from geometry_msgs.msg import (
Wrench, )
from hsrb_interface import Robot, ItemTypes
from hsrb_impedance_control_example.impedance_control_switch import ImpedanceControlSwitch
from hsrb_impedance_control_example.utils import CachingSubscriber
with Robot() as robot:
# 必要なインスタンスを作成
wrist_wrench = robot.try_get('wrist_wrench')
gripper = robot.try_get('gripper')
switch = ImpedanceControlSwitch()
# 力覚センサのゼロ点リセット
# 任意の方向にFFWrenchで進ませるため
# 本当はリセットせずに，現在のセンサ値をキャンセルする成分をFFWrenchに足すべき
wrist_wrench.reset()
# FFWrenchの準備，手先(hand_palm_link)座標系で与える
# このサンプルでは，-x方向に適当な大きさを与えている
ff_wrench = Wrench()
ff_wrench.force.x = -2.0
# インピーダンス制御を有効化
result = switch.activate("placing", ff_wrench)
if result:
print "Activate impedance control."
def main(whole_body, base, gripper, wrist_wrench):
cli = actionlib.SimpleActionClient(
'/hsrb/omni_base_controller/follow_joint_trajectory',
control_msgs.msg.FollowJointTrajectoryAction)
# publisher for delvering command for base move
vel_pub = rospy.Publisher('/hsrb/command_velocity',
geometry_msgs.msg.Twist,
queue_size=10)
# base_pub = rospy.Publisher('/move_base/move/goal',move_base_msgs.msg.MoveBaseActionGoal,queue_size=10)
armPub = rospy.Publisher('/hsrb/arm_trajectory_controller/command',
JointTrajectory,
queue_size=1)
## Grab the handle of door
#test with service - get the handle position from handle
grasp_point_client()
global recog_pos
global Is_found
print recog_pos.pose.position
# target_pose_Msg = rospy.wait_for_message("/handle_detector/grasp_point", PoseStamped)
# recog_pos.pose.position.x=target_pose_Msg.pose.position.x
# recog_pos.pose.position.y=target_pose_Msg.pose.position.y
# recog_pos.pose.position.z=target_pose_Msg.pose.position.z
whole_body.move_to_neutral()
# whole_body.impedance_config= 'grasping'
switch = ImpedanceControlSwitch()
# wrist_wrench.reset()
gripper.command(1.0)
grab_pose = geometry.multiply_tuples(
geometry.pose(x=recog_pos.pose.position.x - HANDLE_TO_HAND_POS_X,
y=recog_pos.pose.position.y - HANDLE_TO_HAND_POS_Y,
z=recog_pos.pose.position.z,
ej=math.pi / 2), geometry.pose(ek=math.pi / 2))
whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF)
wrist_wrench.reset()
# whole_body.impedance_config= 'compliance_middle'
switch.activate("grasping")
# gripper.command(0.01)
gripper.grasp(-0.01)
rospy.sleep(1.0)
switch.inactivate()
wrist_wrench.reset()
rospy.sleep(8.0)
#### test manipulation
#change the impedance config to grasping
whole_body.impedance_config = 'grasping'
## Direct Joint trajectory
traj = JointTrajectory()
# This controller requires that all joints have values
traj.joint_names = [
"arm_lift_joint", "arm_flex_joint", "arm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
p = JointTrajectoryPoint()
current_positions = [latest_positions[name] for name in traj.joint_names]
current_positions[0] = latest_positions["arm_lift_joint"] - 0.07
current_positions[1] = latest_positions["arm_flex_joint"] - 0.02
current_positions[2] = latest_positions["arm_roll_joint"]
current_positions[3] = latest_positions["wrist_flex_joint"]
current_positions[4] = latest_positions["wrist_roll_joint"] - 0.65
p.positions = current_positions
p.velocities = [0, 0, 0, 0, 0]
p.time_from_start = rospy.Time(3)
traj.points = [p]
armPub.publish(traj)
rospy.sleep(3.0)
## Move by End-effector line
whole_body.impedance_config = 'compliance_hard'
whole_body.move_end_effector_by_line((0.0, 0.0, 1), 0.45)
## Move base with linear & Angular motion
tw = geometry_msgs.msg.Twist()
tw.linear.x = 0.9
tw.angular.z = 0.45
vel_pub.publish(tw)
rospy.sleep(4.0)
## Move base with linear & Angular motion second
tw_cmd0 = geometry_msgs.msg.Twist()
tw_cmd0.linear.x = 0.3
tw_cmd0.angular.z = 0.45
vel_pub.publish(tw_cmd0)
# rospy.sleep(4.0)
rospy.sleep(4.0)
## Open the gripper
gripper.command(1.0)
## Move back
tw_cmd = geometry_msgs.msg.Twist()
tw_cmd.linear.x = -1.2
vel_pub.publish(tw_cmd)
rospy.sleep(2.0)
## Move back 2
tw_cmd2 = geometry_msgs.msg.Twist()
tw_cmd2.linear.x = -1.1
tw_cmd2.angular.z = -0.4
vel_pub.publish(tw_cmd2)
rospy.sleep(4.0)
whole_body.move_to_neutral()
## Move back
tw_cmd2 = geometry_msgs.msg.Twist()
tw_cmd2.linear.x = -0.6
tw_cmd2.angular.z = -0.3
vel_pub.publish(tw_cmd2)
navi_service_client(2.0, 0.0, 0.0)
def main(whole_body, gripper, wrist_wrench):
armPub = rospy.Publisher('/hsrb/arm_trajectory_controller/command',
JointTrajectory,
queue_size=1)
## Grab the handle of door
target_pose_Msg = rospy.wait_for_message("/handle_detector/grasp_point",
PoseStamped)
recog_pos.pose.position.x = target_pose_Msg.pose.position.x
recog_pos.pose.position.y = target_pose_Msg.pose.position.y
recog_pos.pose.position.z = target_pose_Msg.pose.position.z
whole_body.move_to_neutral()
# whole_body.impedance_config= 'grasping'
switch = ImpedanceControlSwitch()
# wrist_wrench.reset()
gripper.command(1.0)
grab_pose = geometry.multiply_tuples(
geometry.pose(x=recog_pos.pose.position.x - HANDLE_TO_HAND_POS,
y=recog_pos.pose.position.y,
z=recog_pos.pose.position.z,
ej=math.pi / 2), geometry.pose(ek=math.pi / 2))
whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF)
wrist_wrench.reset()
# whole_body.impedance_config= 'compliance_middle'
switch.activate("grasping")
# gripper.command(0.01)
gripper.grasp(-0.008)
rospy.sleep(1.0)
switch.inactivate()
wrist_wrench.reset()
rospy.sleep(8.0)
#### test manipulation
whole_body.impedance_config = 'grasping'
# print(whole_body.impedance_config)
# desired_rot=-1.95
# whole_body.move_to_joint_positions({"wrist_roll_joint":desired_rot})
wrist_roll = latest_positions["wrist_roll_joint"] - 0.55
traj = JointTrajectory()
# This controller requires that all joints have values
traj.joint_names = [
"arm_lift_joint", "arm_flex_joint", "arm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
p = JointTrajectoryPoint()
current_positions = [latest_positions[name] for name in traj.joint_names]
current_positions[0] = latest_positions["arm_lift_joint"] - 0.04
current_positions[1] = latest_positions["arm_flex_joint"] - 0.015
current_positions[2] = latest_positions["arm_roll_joint"]
current_positions[3] = latest_positions["wrist_flex_joint"]
current_positions[4] = wrist_roll
p.positions = current_positions
p.velocities = [0, 0, 0, 0, 0]
p.time_from_start = rospy.Time(3)
traj.points = [p]
armPub.publish(traj)
rospy.sleep(5.0)
# whole_body.end_effector_frame = u'odom'
# whole_body.move_end_effector_by_line((0, 0, 1), -0.2)
# publish_arm(latest_positions["arm_lift_joint"],latest_positions["arm_flex_joint"],latest_positions["arm_roll_joint"], latest_positions["wrist_flex_joint"],wrist_roll)
# whole_body.end_effector_frame = u'base_link'
whole_body.impedance_config = 'grasping'
whole_body.move_end_effector_by_line((0, 0, 1), 0.35)
whole_body.impedance_config = None
##
# whole_body.move_to_joint_positions({"wrist_roll_joint": -0.3})
# rospy.sleep(2.0)
# whole_body.end_effector_frame = u'odom'
# whole_body.move_end_effector_by_line((0, 0, 1), -0.2)
# whole_body.move_end_effector_by_arc(geometry.pose(x=0.2, y=0.25, z=0.38, ej=math.radians(90.0)), math.radians(60.0))
# whole_body.move_end_effector_by_arc(geometry.pose(y=0.45, z=0.08, ej=math.radians(90.0)), math.radians(60.0), ref_frame_id='hand_palm_link')
# listener = tf.TransformListener()
# listener.waitForTransform("/odom", "/hand_palm_link", rospy.Time().now(), rospy.Duration(3.0))
# now = rospy.Time.now()
# listener.waitForTransform("/odom", "/hand_palm_link", now, rospy.Duration(5.0))
# (trans, rot) = listener.lookupTransform("/map", "/hand_palm_link", now)
# cur_tuples = geometry.transform_to_tuples(target_trans)
# print trans
# print rot
# Rotate the handle (Angle: math.pi/6)
# wrist_wrench.reset()
# whole_body.end_effector_frame = 'hand_palm_link'
# whole_body.impedance_config= 'dumper_soft'
#############################
# odom_to_hand = get_relative_tuples(_ORIGIN_TF, _HAND_TF)
# tsr_to_odom = geometry.pose(x=-(recog_pos.pose.position.x+HANDLE_TO_HAND_POS),
# y=-(recog_pos.pose.position.y+HANDLE_TO_HANDLE_HINGE_POS),
# z=-recog_pos.pose.position.z)
# tsr_to_hand = geometry.multiply_tuples(tsr_to_odom, odom_to_hand)
# const_tsr = TaskSpaceRegion()
# const_tsr.end_frame_id = _HAND_TF
# const_tsr.origin_to_tsr = geometry.tuples_to_pose(geometry.pose(x=recog_pos.pose.position.x+HANDLE_TO_HAND_POS,
# y=recog_pos.pose.position.y+HANDLE_TO_HANDLE_HINGE_POS,
# z=recog_pos.pose.position.z))
# const_tsr.tsr_to_end = geometry.tuples_to_pose(tsr_to_hand)
# const_tsr.min_bounds = [0, 0.0, 0.0,-(math.pi/7) , 0, 0]
# const_tsr.max_bounds = [0, 0.0, 0.0, 0, 0, 0]
# goal_tsr = TaskSpaceRegion()
# goal_tsr.end_frame_id = _HAND_TF
# goal_tsr.origin_to_tsr = geometry.tuples_to_pose(geometry.pose(x=recog_pos.pose.position.x+HANDLE_TO_HAND_POS,
# y=recog_pos.pose.position.y+HANDLE_TO_HANDLE_HINGE_POS,
# z=recog_pos.pose.position.z))
# goal_tsr.tsr_to_end = geometry.tuples_to_pose(tsr_to_hand)
# goal_tsr.min_bounds = [0, 0.0, 0.0,-math.pi/7, 0, 0]
# goal_tsr.max_bounds = [0, 0.0, 0.0,-math.pi/7, 0, 0]
# response = call_tsr_plan_service(whole_body, [const_tsr], [goal_tsr])
# if response.error_code.val != ArmManipulationErrorCodes.SUCCESS:
# rospy.logerr("Planning failed: (Error Code {0})".format(response.error_code.val))
# exit(-1)
# response.base_solution.header.frame_id = _ORIGIN_TF
# constrain_traj = whole_body._constrain_trajectories(response.solution,
# response.base_solution)
# # wrist_wrench.reset()
# # switch.activate("grasping")
# whole_body._execute_trajectory(constrain_traj)
# # whole_body.impedance_config= 'dumper_soft'
# # switch.inactivate()
# rospy.sleep(10.0)
# listener = tf.TransformListener()
# now = rospy.Time.now()
# listener.waitForTransform("/odom", "/hand_palm_link", now, rospy.Duration(3.0))
# rospy.sleep(3.0)
# # Open the door (Angle: math.pi/4)
# #rist_wrench.reset()
# # switch.activate("placing")
# odom_to_hand = get_relative_tuples(_ORIGIN_TF, _HAND_TF) #T0h
# tsr_to_odom = geometry.pose(x=-(recog_pos.pose.position.x),
# y=-(recog_pos.pose.position.y+HANDLE_TO_DOOR_HINGE_POS+HANDLE_GOAL_OFFSET),
# z=-recog_pos.pose.position.z) #Twe
# tsr_to_hand = geometry.multiply_tuples(tsr_to_odom, odom_to_hand) #T0s'
# const_tsr = TaskSpaceRegion()
# const_tsr.end_frame_id = _HAND_TF
# const_tsr.origin_to_tsr = geometry.tuples_to_pose(geometry.pose(x=recog_pos.pose.position.x,
# y=recog_pos.pose.position.y+HANDLE_TO_DOOR_HINGE_POS+HANDLE_GOAL_OFFSET,
# z=recog_pos.pose.position.z))
# const_tsr.tsr_to_end = geometry.tuples_to_pose(tsr_to_hand)
# const_tsr.min_bounds = [0, 0.0, 0.0, 0, 0, 0]
# const_tsr.max_bounds = [0, 0.0, 0.0, 0, 0, math.pi/4]
# goal_tsr = TaskSpaceRegion()
# goal_tsr.end_frame_id = _HAND_TF
# goal_tsr.origin_to_tsr = geometry.tuples_to_pose(geometry.pose(x=recog_pos.pose.position.x,
# y=recog_pos.pose.position.y+HANDLE_TO_DOOR_HINGE_POS+HANDLE_GOAL_OFFSET,
# z=recog_pos.pose.position.z))
# goal_tsr.tsr_to_end = geometry.tuples_to_pose(tsr_to_hand)
# goal_tsr.min_bounds = [0, 0.0, 0.0, 0, 0, math.pi/4]
# goal_tsr.max_bounds = [0, 0.0, 0.0, 0, 0, math.pi/4]
# response = call_tsr_plan_service(whole_body, [const_tsr], [goal_tsr])
# if response.error_code.val != ArmManipulationErrorCodes.SUCCESS:
# rospy.logerr("Planning failed: (Error Code {0})".format(response.error_code.val))
# exit(-1)
# response.base_solution.header.frame_id = _ORIGIN_TF
# constrain_traj = whole_body._constrain_trajectories(response.solution,
# response.base_solution)
# whole_body._execute_trajectory(constrain_traj)
# # whole_body.impedance_config= None
# # switch.inactivate()
gripper.command(1.0)
whole_body.move_to_neutral()
def main(whole_body, gripper, wrist_wrench):
# Grab the handle of door
# wrist_wrench =wholjjjjjjjjjk
whole_body.move_to_neutral()
switch = ImpedanceControlSwitch()
wrist_wrench.reset()
gripper.command(1.0)
grab_pose = geometry.multiply_tuples(
geometry.pose(x=HANDLE_POS[0] - HANDLE_TO_HAND_POS,
y=HANDLE_POS[1],
z=HANDLE_POS[2],
ej=math.pi / 2), geometry.pose(ek=math.pi / 2))
whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF)
switch.activate("grasping")
# gripper.command(0.01)
gripper.grasp(-0.05)
rospy.sleep(1.0)
switch.inactivate()
rospy.sleep(10.0)
listener = tf.TransformListener()
listener.waitForTransform("/map", "/hand_palm_link", rospy.Time(),
rospy.Duration(4.0))
now = rospy.Time.now()
listener.waitForTransform("/map", "/hand_palm_link", now,
rospy.Duration(4.0))
# (trans, rot) = listener.lookupTransform("/map", "/hand_palm_link", now)
# cur_tuples = geometry.transform_to_tuples(target_trans)
# print trans
# print rot
#tf frame
# br = tf2_ros.TransformBroadcaster()
# t = geometry_msgs.msg.TransformStamped()
# t.header.stamp = rospy.Time.now()
# t.header.frame_id = "/map"
# t.child_frame_id = "/target"
# t.transform.translation.x = trans[0]
# t.transform.translation.y = trans[1]
# t.transform.translation.z = trans[2]
# q = tf.transformations.quaternion_from_euler(0, 0, 0.0)
# t.transform.rotation.x = rot[0]
# t.transform.rotation.y = rot[1]
# t.transform.rotation.z = rot[2]
# t.transform.rotation.w = rot[3]
# br.sendTransform(t)
# Rotate the handle (Angle: math.pi/6)
odom_to_hand = get_relative_tuples(_ORIGIN_TF, _HAND_TF)
tsr_to_odom = geometry.pose(
x=-(HANDLE_POS[0] + HANDLE_TO_HAND_POS),
y=-(HANDLE_POS[1] + HANDLE_TO_HANDLE_HINGE_POS),
z=-HANDLE_POS[2])
tsr_to_hand = geometry.multiply_tuples(tsr_to_odom, odom_to_hand)
const_tsr = TaskSpaceRegion()
const_tsr.end_frame_id = _HAND_TF
const_tsr.origin_to_tsr = geometry.tuples_to_pose(
geometry.pose(x=HANDLE_POS[0] + HANDLE_TO_HAND_POS,
y=HANDLE_POS[1] + HANDLE_TO_HANDLE_HINGE_POS,
z=HANDLE_POS[2]))
const_tsr.tsr_to_end = geometry.tuples_to_pose(tsr_to_hand)
const_tsr.min_bounds = [0, 0.0, 0.0, -(math.pi / 5), 0, 0]
const_tsr.max_bounds = [0, 0.0, 0.0, 0, 0, 0]
goal_tsr = TaskSpaceRegion()
goal_tsr.end_frame_id = _HAND_TF
goal_tsr.origin_to_tsr = geometry.tuples_to_pose(
geometry.pose(x=HANDLE_POS[0] + HANDLE_TO_HAND_POS,
y=HANDLE_POS[1] + HANDLE_TO_HANDLE_HINGE_POS,
z=HANDLE_POS[2]))
goal_tsr.tsr_to_end = geometry.tuples_to_pose(tsr_to_hand)
goal_tsr.min_bounds = [0, 0.0, 0.0, -math.pi / 5, 0, 0]
goal_tsr.max_bounds = [0, 0.0, 0.0, -math.pi / 5, 0, 0]
response = call_tsr_plan_service(whole_body, [const_tsr], [goal_tsr])
if response.error_code.val != ArmManipulationErrorCodes.SUCCESS:
rospy.logerr("Planning failed: (Error Code {0})".format(
response.error_code.val))
exit(-1)
response.base_solution.header.frame_id = _ORIGIN_TF
constrain_traj = whole_body._constrain_trajectories(
response.solution, response.base_solution)
whole_body._execute_trajectory(constrain_traj)
rospy.sleep(10.0)
# listener = tf.TransformListener()
now = rospy.Time.now()
listener.waitForTransform("/map", "/hand_palm_link", now,
rospy.Duration(4.0))
rospy.sleep(1.0)
def main(whole_body, base, gripper, wrist_wrench):
cli = actionlib.SimpleActionClient(
'/hsrb/omni_base_controller/follow_joint_trajectory',
control_msgs.msg.FollowJointTrajectoryAction)
# publisher for delvering command for base move
vel_pub = rospy.Publisher('/hsrb/command_velocity',
geometry_msgs.msg.Twist,
queue_size=10)
# base_pub = rospy.Publisher('/move_base/move/goal',move_base_msgs.msg.MoveBaseActionGoal,queue_size=10)
armPub = rospy.Publisher('/hsrb/arm_trajectory_controller/command',
JointTrajectory,
queue_size=1)
## Grab the handle of door
#test with service - get the handle position from handle
grasp_point_client()
global recog_pos
global Is_found
print recog_pos.pose.position
# target_pose_Msg = rospy.wait_for_message("/handle_detector/grasp_point", PoseStamped)
# recog_pos.pose.position.x=target_pose_Msg.pose.position.x
# recog_pos.pose.position.y=target_pose_Msg.pose.position.y
# recog_pos.pose.position.z=target_pose_Msg.pose.position.z
whole_body.move_to_neutral()
# whole_body.impedance_config= 'grasping'
switch = ImpedanceControlSwitch()
# wrist_wrench.reset()
gripper.command(1.0)
grab_pose = geometry.multiply_tuples(
geometry.pose(x=recog_pos.pose.position.x - HANDLE_TO_HAND_POS_,
y=recog_pos.pose.position.y - 0.012,
z=recog_pos.pose.position.z,
ej=math.pi / 2), geometry.pose(ek=math.pi / 2))
whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF)
wrist_wrench.reset()
# whole_body.impedance_config= 'compliance_middle'
switch.activate("grasping")
# gripper.command(0.01)
gripper.grasp(-0.01)
rospy.sleep(1.0)
switch.inactivate()
wrist_wrench.reset()
rospy.sleep(8.0)
#### test manipulation
whole_body.impedance_config = 'grasping'
# print(whole_body.impedance_config)
# desired_rot=-1.95
# whole_body.move_to_joint_positions({"wrist_roll_joint":desired_rot})
traj = JointTrajectory()
# This controller requires that all joints have values
traj.joint_names = [
"arm_lift_joint", "arm_flex_joint", "arm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
p = JointTrajectoryPoint()
current_positions = [latest_positions[name] for name in traj.joint_names]
current_positions[0] = latest_positions["arm_lift_joint"] - 0.07
current_positions[1] = latest_positions["arm_flex_joint"] - 0.02
current_positions[2] = latest_positions["arm_roll_joint"]
current_positions[3] = latest_positions["wrist_flex_joint"]
current_positions[4] = latest_positions["wrist_roll_joint"] - 0.65
p.positions = current_positions
p.velocities = [0, 0, 0, 0, 0]
p.time_from_start = rospy.Time(3)
traj.points = [p]
armPub.publish(traj)
rospy.sleep(3.0)
# goal_pose =PoseStamped()
# goal_pose.pose.position.x=recog_pos.pose.position.x+0.4
# goal_pose.pose.position.y=recog_pos.pose.position.y+0.2
# goal_pose.pose.position.z=recog_pos.pose.position.z
# goal_pose.pose.orientation.x=recog_pos.pose.orientation.x
# goal_pose.pose.orientation.y=recog_pos.pose.orientation.y
# goal_pose.pose.orientation.z=recog_pos.pose.orientation.z
# goal_pose.pose.orientation.w=recog_pos.pose.orientation.w
# yaw=math.pi/6
# quaternion = tf.transformations.quaternion_from_euler(0.0, 0.0, yaw)
# #type(pose) = geometry_msgs.msg.Pose
# nav_goal = move_base_msgs.msg.MoveBaseActionGoal()
# nav_goal.header.stamp=rospy.Time(0)
# nav_goal.goal.target_pose.header.frame_id = "map"
# nav_goal.goal.target_pose.pose.position.x=recog_pos.pose.position.x+0.3
# nav_goal.goal.target_pose.pose.position.y=recog_pos.pose.position.y+0.2
# nav_goal.goal.target_pose.pose.position.z=0.0
# nav_goal.goal.target_pose.pose.orientation.x=quaternion[0]
# nav_goal.goal.target_pose.pose.orientation.y=quaternion[1]
# nav_goal.goal.target_pose.pose.orientation.z=quaternion[2]
# nav_goal.goal.target_pose.pose.orientation.w=quaternion[3]
# base_pub.publish(nav_goal)
# # whole_body.end_effector_frame = u'odom'
# # whole_body.move_end_effector_by_line((0, 0, 1), -0.2)
# # publish_arm(latest_positions["arm_lift_joint"],latest_positions["arm_flex_joint"],latest_positions["arm_roll_joint"], latest_positions["wrist_flex_joint"],wrist_roll)
whole_body.impedance_config = 'compliance_hard'
whole_body.move_end_effector_by_line((0.0, 0.0, 1), 0.45)
# # whole_body.move_end_effector_by_line((0.7071, 0.7071,0), 0.5)
# # whole_body.move_end_effector_by_line((0, -0.7071, 0.7071), 0.5)
# whole_body.impedance_config= None
tw = geometry_msgs.msg.Twist()
tw.linear.x = 0.9
tw.angular.z = 0.45
vel_pub.publish(tw)
rospy.sleep(4.0)
tw_cmd0 = geometry_msgs.msg.Twist()
tw_cmd0.linear.x = 0.3
tw_cmd0.angular.z = 0.45
vel_pub.publish(tw_cmd0)
# rospy.sleep(4.0)
rospy.sleep(4.0)
gripper.command(1.0)
# whole_body.move_end_effector_by_line((0, 0, -1), 0.25)
tw_cmd = geometry_msgs.msg.Twist()
tw_cmd.linear.x = -1.2
vel_pub.publish(tw_cmd)
rospy.sleep(2.0)
tw_cmd2 = geometry_msgs.msg.Twist()
tw_cmd2.linear.x = -1.1
tw_cmd2.angular.z = -0.4
vel_pub.publish(tw_cmd2)
rospy.sleep(4.0)
whole_body.move_to_neutral()
tw_cmd2 = geometry_msgs.msg.Twist()
tw_cmd2.linear.x = -0.6
tw_cmd2.angular.z = -0.3
vel_pub.publish(tw_cmd2)
def main(whole_body, gripper, wrist_wrench):
rospy.sleep(5)
armPub = rospy.Publisher('/hsrb/arm_trajectory_controller/command',
JointTrajectory,
queue_size=1)
#1.READ THE ANGLE (topic angle_detector)
#angle_Msg = rospy.wait_for_message("angle_detector", Float64)
#angle = angle_Msg.data
#2.READ THE HANDLE POSITION when the door is closed - SERVICE!!!
#target_pose_Msg = rospy.wait_for_message("localize_handle", PoseStamped)
#recog_pos.pose.position.x=target_pose_Msg.pose.position.x
#recog_pos.pose.position.y=target_pose_Msg.pose.position.y
#recog_pos.pose.position.z=target_pose_Msg.pose.position.z
if (angle > -1 and angle < 4): #the door is closed
##3.GRAB THE DOOR HANDLE
whole_body.move_to_neutral()
# whole_body.impedance_config= 'grasping'
switch = ImpedanceControlSwitch()
# wrist_wrench.reset()
gripper.command(1.0)
grab_pose = geometry.multiply_tuples(
geometry.pose(x=recog_pos.pose.position.x - HANDLE_TO_HAND_POS,
y=recog_pos.pose.position.y,
z=recog_pos.pose.position.z,
ej=math.pi / 2), geometry.pose(ek=math.pi / 2))
whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF)
wrist_wrench.reset()
# whole_body.impedance_config= 'compliance_middle'
switch.activate("grasping")
# gripper.command(0.01)
gripper.grasp(-0.008)
rospy.sleep(1.0)
switch.inactivate()
wrist_wrench.reset()
rospy.sleep(8.0)
#### test manipulation
whole_body.impedance_config = 'grasping'
#4.ROTATE HANDLE
# print(whole_body.impedance_config)
# desired_rot=-1.95
# whole_body.move_to_joint_positions({"wrist_roll_joint":desired_rot})
wrist_roll = latest_positions["wrist_roll_joint"] - 0.55
traj = JointTrajectory()
# This controller requires that all joints have values
traj.joint_names = [
"arm_lift_joint", "arm_flex_joint", "arm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
p = JointTrajectoryPoint()
current_positions = [
latest_positions[name] for name in traj.joint_names
]
current_positions[0] = latest_positions["arm_lift_joint"] - 0.04
current_positions[1] = latest_positions["arm_flex_joint"] - 0.015
current_positions[2] = latest_positions["arm_roll_joint"]
current_positions[3] = latest_positions["wrist_flex_joint"]
current_positions[4] = wrist_roll
p.positions = current_positions
p.velocities = [0, 0, 0, 0, 0]
p.time_from_start = rospy.Time(3)
traj.points = [p]
armPub.publish(traj)
rospy.sleep(5.0)
whole_body.impedance_config = 'grasping'
#5.PUSH MOTION - he moves with the door - first option - streight movement
phi = 90 * (2 * math.pi / 360)
l = HANDLE_TO_DOOR_HINGE_POS
d = 2 * l * math.cos(phi)
#open_pose = geometry.pose(x = d*math.sin(phi), y=d*math.cos(phi), ek=math.pi/2 - angle_rad )
#whole_body.move_end_effector_pose(open_pose, _ROBOT_TF)
#angleeee=math.pi/2 - angle_rad
omni_base.go(d * math.sin(phi),
d * math.cos(phi),
math.pi / 2 - angle_rad,
300.0,
relative=True)
whole_body.move_to_neutral()
elif (angle >= 4 and angle < 60): #the door is half-open
#3.ROTATE PARALLEL TO THE DOOR
angle_rad = angle * (2 * math.pi / 360)
#omni_base.go_rel(0.0, 0.0, angle_rad , 300.0)
##4.GRAB THE DOOR HANDLE
whole_body.move_to_neutral()
# whole_body.impedance_config= 'grasping'
switch = ImpedanceControlSwitch()
# wrist_wrench.reset()
gripper.command(0.0)
#change coordinates of the handle - now the door is open so the handle is moved - the data of the handle position are given for the closed door - I m supposing
#that the coordinates of the handle are wrt the base_footprint tf
phi = math.pi / 2 - angle_rad / 2
l = HANDLE_TO_DOOR_HINGE_POS
d1 = 2 * l * math.sin(angle_rad / 2)
recog_pos.pose.position.x = recog_pos.pose.position.x + d1 * math.sin(
phi)
recog_pos.pose.position.y = recog_pos.pose.position.y + d1 * math.cos(
phi)
grab_pose = geometry.multiply_tuples(
geometry.pose(x=recog_pos.pose.position.x - HANDLE_TO_HAND_POS,
y=recog_pos.pose.position.y - 10,
z=recog_pos.pose.position.z,
ej=math.pi / 2), geometry.pose(ek=math.pi / 2))
whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF)
wrist_wrench.reset()
rospy.sleep(1.0)
switch.inactivate()
wrist_wrench.reset()
rospy.sleep(8.0)
#### test manipulation
whole_body.impedance_config = 'grasping'
#5.PUSH MOTION - he moves with the door - first option - streight movement
phi = math.pi / 4 + angle_rad / 2
l = HANDLE_TO_DOOR_HINGE_POS
d = 2 * l * math.cos(phi)
omni_base.go(d * math.sin(phi),
d * math.cos(phi),
math.pi / 2 - angle_rad,
300.0,
relative=True)
def go_to(req):
print("starting go_to")
# hand_service = rospy.ServiceProxy("/openpose/hand_positions",GetHandPositions)
n_call = 3
global left_pos_f
global right_pos_f
left_pos_f.x = 0
left_pos_f.y = 0
left_pos_f.z = 0
right_pos_f.x = 0
right_pos_f.y = 0
right_pos_f.z = 0
for i in range(0, n_call):
rospy.sleep(2)
rospy.wait_for_service("/openpose/hand_positions")
res1 = hand_service.call()
print("Received left and right :")
print(res1.left_hand)
print(res1.right_hand)
left_pos = res1.left_hand
right_pos = res1.right_hand
if (abs(left_pos.x + 0.059) > 0.04 and abs(left_pos.x + 0.059) > 0.04):
print(abs(left_pos.x + 0.059))
print(abs(right_pos.x + 0.059))
left_pos_f.x += left_pos.x
left_pos_f.y += left_pos.y
left_pos_f.z += left_pos.z
right_pos_f.x += right_pos.x
right_pos_f.y += right_pos.y
right_pos_f.z += right_pos.z
if left_pos.x < right_pos.x:
avoid = right_pos_f
good = left_pos_f
else:
avoid = left_pos_f
good = right_pos_f
good.x = good.x / n_call
good.y = good.y / n_call
good.z = good.z / n_call
avoid.x = good.x / n_call
avoid.y = good.y / n_call
avoid.z = good.z / n_call
vel_pub = rospy.Publisher('/hsrb/command_velocity',
geometry_msgs.msg.Twist,
queue_size=10)
print("accessing robot")
robot = hsrb_interface.Robot()
whole_body = robot.get('whole_body')
gripper = robot.get('gripper')
wrist_wrench = robot.get('wrist_wrench')
base = robot.get('omni_base')
print("accessing robot complete")
try:
# whole_body.move_to_neutral()
# print("looking for grasping point")
# grasp_point_client(avoid,good)
# global recog_pos
# global is_found
# print (recog_pos.pose.position)
print("Opening the gripper")
whole_body.move_to_neutral()
# rospy.sleep(2.5)
switch = ImpedanceControlSwitch()
gripper.command(1.0)
# Approach to the door
# listener = tf.TransformListener()
# listener.waitForTransform(_ORIGIN_TF,_BASE_TF, rospy.Time(), rospy.Duration(4.0))
# # translation,rot = listener.lookupTransform(_BASE_TF, _ORIGIN_TF, rospy.Time())
# recog_pos.header.frame_id=_ORIGIN_TF
# recog_pos2 = listener.transformPose(_BASE_TF, recog_pos)
recog_pos2.pose.position.x = good.x - 0.05
recog_pos2.pose.position.z = good.z - 0.07
recog_pos2.pose.position.y = good.y - 0.1
grab_pose = geometry.multiply_tuples(
geometry.pose(x=recog_pos2.pose.position.x - HANDLE_TO_HAND_POS_X,
y=recog_pos2.pose.position.y,
z=recog_pos2.pose.position.z,
ej=math.pi / 2,
ek=math.pi / 2), geometry.pose(ei=math.pi / 2))
whole_body.move_end_effector_pose(grab_pose, _BASE_TF)
whole_body.move_end_effector_by_line((0, 0, 1), 0.1)
# Close the gripper
wrist_wrench.reset()
switch.activate("grasping")
gripper.grasp(-0.01)
rospy.sleep(1.0)
switch.inactivate()
whole_body.impedance_config = 'grasping'
tw = geometry_msgs.msg.Twist()
tw.linear.x = -0.3
vel_pub.publish(tw)
rospy.sleep(2)
whole_body.move_to_neutral()
gripper.command(1.0)
except Exception as e:
rospy.logerr(e)
print("Failed to open door")
# res.success = False
res = True
return res