def _setup_smart_grasper(self):
"""
Setup of the movement system.
:return:
"""
rospy.logdebug("START _setup_smart_grasper")
# We need to tell it to not start a node
self.sgs = SmartGrasper(init_ros_node=False)
rospy.logdebug("END _setup_smart_grasper")
def camera_mover():
# Position of the object in the scene
pose = Pose()
pose.position.x = 0.15
pose.position.y = 0
pose.position.z = 0.772
grasper = SmartGrasper()
move_to_start_pose(pose, grasper) # starting position is right next to the object
move_robot_around_object(grasper) # moves robotic arm aroun object in a 90 degree angle
# prevents node from stopping before ctrl + c is pressed
rospy.spin()
from smart_grasping_sandbox.smart_grasper import SmartGrasper from tf.transformations import quaternion_from_euler from math import pi import time sgs = SmartGrasper() sgs.pick() sgs.reset_world()
#!/usr/bin/env python
from smart_grasping_sandbox.smart_grasper import SmartGrasper
from tf.transformations import quaternion_from_euler
from math import pi
import time
import signal
import sys
def signal_handler(signal, frame):
print "Exiting program..."
sys.exit(0)
signal.signal(signal.SIGINT, signal_handler)
sgs = SmartGrasper()
while True:
x = float(raw_input("X Value:"))
y = float(raw_input("Y Value:"))
z = float(raw_input("Z Value:"))
roll, pitch, yaw = 0.0, 0.0, 0.0
sgs.move_tip(x, y, z, roll, pitch, yaw)
tcp_pose = sgs.get_tip_pose()
print str(tcp_pose)
# Copyright (C) 2018 Shadow Robot Company Ltd - All Rights Reserved.
# Proprietary and Confidential. Unauthorized copying of the content in this file, via any medium is strictly prohibited.
from smart_grasping_sandbox.smart_grasper import SmartGrasper
from tf.transformations import quaternion_from_euler
from math import pi
import time
import rospy
from math import sqrt, pow
import random
from sys import argv
import uuid
sgs = SmartGrasper()
MIN_LIFT_STEPS = 5
MAX_BALL_DISTANCE = 0.6
CLOSED_HAND = {}
CLOSED_HAND["H1_F1J1"] = 0.0
CLOSED_HAND["H1_F1J2"] = 0.25
CLOSED_HAND["H1_F1J3"] = 0.4
CLOSED_HAND["H1_F2J1"] = 0.0
CLOSED_HAND["H1_F2J2"] = 0.25
CLOSED_HAND["H1_F2J3"] = 0.4
CLOSED_HAND["H1_F3J1"] = 0.0
CLOSED_HAND["H1_F3J2"] = 0.25
CLOSED_HAND["H1_F3J3"] = 0.4
JOINT_NAMES = CLOSED_HAND.keys()
# Copyright (C) 2018 Shadow Robot Company Ltd - All Rights Reserved.
# Proprietary and Confidential. Unauthorized copying of the content in this file, via any medium is strictly prohibited.
from smart_grasping_sandbox.smart_grasper import SmartGrasper
from tf.transformations import quaternion_from_euler
from math import pi
import time
import rospy
from math import sqrt, pow
import random
from sys import argv
import uuid
sgs = SmartGrasper()
MIN_LIFT_STEPS = 5
MAX_BALL_DISTANCE = 0.6
CLOSED_HAND = {}
CLOSED_HAND["H1_F1J1"] = 0.0
CLOSED_HAND["H1_F1J2"] = 0.25
CLOSED_HAND["H1_F1J3"] = 0.4
CLOSED_HAND["H1_F2J1"] = 0.0
CLOSED_HAND["H1_F2J2"] = 0.25
CLOSED_HAND["H1_F2J3"] = 0.4
CLOSED_HAND["H1_F3J1"] = 0.0
CLOSED_HAND["H1_F3J2"] = 0.25
CLOSED_HAND["H1_F3J3"] = 0.4
JOINT_NAMES = CLOSED_HAND.keys()
#!/usr/bin/env python
# node to grasp objects and place them at a given point based on a Pose-object
# received by the nn_connector node.
import rospy
from smart_grasping_sandbox.smart_grasper import SmartGrasper
import time
from geometry_msgs.msg import Pose, Point
from tf.transformations import quaternion_from_euler
from math import pi, cos, sin
grasper = SmartGrasper()
def lift_object(pose):
rospy.loginfo("lift object up")
# lift robotic arm up
for _ in range(12):
grasper.move_tip(y=0.1)
time.sleep(0.1)
def move_object_to_location(pose):
rospy.loginfo("move object to the right")
time.sleep(1)
# move robotic arm to the the left
for _ in range(10):
grasper.move_tip(x=0.1)
def train(object_name):
def get_object_pose(object_name):
"""
Returns the given object's name position and orientation relative to the world's coordinate frame
:param object_name: String containing the object's name
"""
return get_state_srv.call(object_name, "world").pose
def reset_object_pose(object_name, pose):
"""
Resets the give object's name to the new given pose
"""
current_model_state = get_state_srv.call(object_name, "world")
new_model_state = ModelState(model_name=object_name, pose=pose)
new_model_state.scale = current_model_state.scale
new_model_state.twist = current_model_state.twist
new_model_state.reference_frame = 'world'
set_state_srv.call(new_model_state)
def compute_euclidean_distance(pose1, pose2):
"""
Returns the euclidean distance between two given poses
"""
return np.sqrt((pose1.position.x - pose2.position.x)**2 + \
(pose1.position.y - pose2.position.y)**2 + (pose1.position.z - pose2.position.z)**2)
def check_stable(joint_names, gripper_pose, object_pose):
current_min = 500
positions = []
velocities = []
efforts = []
for k in range(30):
sgs.move_tip(y=0.03)
ball_distance = compute_euclidean_distance(gripper_pose,
object_pose)
if k > MIN_LIFT_STEPS and ball_distance < current_min:
current_min = ball_distance
break
if ball_distance > MAX_BALL_DISTANCE:
break
time.sleep(0.5)
robustness = (1 / (current_min - 0.18))**2
return robustness
def compute_reward(gripper_pose, object_pose):
return check_stable(hand_joint_names, gripper_pose,
object_pose) - compute_euclidean_distance(
object_pose, gripper_pose)
def select_action(pose, weights):
"""
Provide the current
"""
ob = pose_to_array(pose)
b1 = np.reshape(weights[0:7], (7, 1))
w1 = np.reshape(weights[7:63], (7, 8))
b2 = np.reshape(weights[63:71], (8, 1))
w2 = np.reshape(weights[71:135], (8, 8))
w3 = np.reshape(weights[135:191], (8, 7))
b3 = np.reshape(weights[191:], (8, 1))
ob = np.reshape(ob, (7, 1))
action_unscaled = logistic.cdf(
np.dot(
w1,
logistic.cdf(
np.dot(w2, logistic.cdf(np.dot(w3, ob) - b3)) - b2)) - b1)
action = tray_boundaries_low + action_unscaled * (
tray_boundaries_high - tray_boundaries_low)
return array_to_pose(action.flatten())
def start_experiment(experiment_id):
# Instantiate Virtual Coach and launch Experiment
vc = VirtualCoach(environment='local', storage_username='******')
sim = vc.launch_experiment(experiment_id)
# roslaunch fh_desc controllers.launch
fh_desc_launch_file = "/home/akl-ma/.opt/nrpStorage/{}/controllers.launch".format(
experiment_id)
uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
roslaunch.configure_logging(uuid)
launch = roslaunch.parent.ROSLaunchParent(uuid, [fh_desc_launch_file])
launch.start()
time.sleep(10)
sim.start()
return sim
def test():
W = opt.get_weights()
observation = get_object_pose()
accreward = 0
action = select_action(observation, W)
observation, reward, done, info = env.step(action)
accreward += reward
print("test end with reward: {}".format(accreward))
num_episodes = 100
MIN_LIFT_STEPS = 1
MAX_BALL_DISTANCE = 0.4
experiment_id = 'GoogleGrasping_1'
# The reset pose the robot returns to after each attempt
robot_reset_position = np.array([0.0, 1.0, 1.5])
robot_reset_orientation = quaternion_from_euler(-np.pi / 2, 0, 0)
robot_reset_pose = array_to_pose(
np.append(robot_reset_position, robot_reset_orientation))
get_state_srv = rospy.ServiceProxy("/gazebo/get_model_state",
GetModelState)
set_state_srv = rospy.ServiceProxy("/gazebo/set_model_state",
SetModelState)
hand_joint_names = [
"H1_F1J1", "H1_F1J2", "H1_F1J3", "H1_F2J1", "H1_F2J2", "H1_F2J3",
"H1_F3J1", "H1_F3J2", "H1_F3J3"
]
tray_boundaries_high = np.reshape(np.array([0.4, 1.15, 1.3, 1, 1, 1, 1]),
(7, 1))
tray_boundaries_low = np.reshape(
np.array([-0.4, 0.6, 1.15, -1, -1, -1, -1]), (7, 1))
weights_dim = 7 * 1 + 8 * 7 + 8 * 1 + 8 * 8 + 8 * 7 + 8 * 1
opt = CEMOptimizer(weights_dim,
batch_size=100,
rho=0.1,
eta=0.3,
deviation=1,
deviation_lim=20)
sim = start_experiment(experiment_id)
sgs = SmartGrasper()
sgs.open_hand()
sgs.move_tip_absolute(robot_reset_pose)
for ep in range(num_episodes):
episode_dir = "episode_{}".format(ep)
os.mkdir(episode_dir)
weights = opt.sample_batch_weights()
rewards = np.array([])
opt.eta *= 0.99
print("deviation mean = {}".format(np.mean(opt.deviation)))
for b in range(opt.batch_size):
accreward = 0
observation = get_object_pose(object_name)
action = select_action(observation, weights[b])
print "==========================="
print "Episode # {}, Attempt # {}".format(ep + 1, b + 1)
print action
# correct action orientation
action.orientation.x = robot_reset_orientation[0]
action.orientation.y = robot_reset_orientation[1]
action.orientation.z = robot_reset_orientation[2]
action.orientation.w = robot_reset_orientation[3]
# move to pose and pick
sgs.open_hand()
time.sleep(1)
sgs.move_tip_absolute(action)
time.sleep(1)
sgs.check_fingers_collisions(False)
time.sleep(1)
sgs.close_hand()
time.sleep(1)
reward = compute_reward(action, observation)
sgs.open_hand()
sgs.check_fingers_collisions(True)
print "Reward: {}".format(reward)
print "==========================="
print " <<<< RESET POSE >>>>"
# reset robot pose
sgs.move_tip_absolute(robot_reset_pose)
rewards = np.append(rewards, reward)
np.save(os.path.join(episode_dir, 'rewards'), rewards)
np.save(os.path.join(episode_dir, 'weights'), weights)
f = open(os.path.join(episode_dir, 'avg_reward'), 'w')
f.write(str(np.mean(rewards)))
f.close()
opt.update_weights(weights, rewards)
class ShadowTcEnv(robot_gazebo_env.RobotGazeboEnv):
"""Superclass for all ShadowTcEnv environments.
"""
def __init__(self):
"""
Initializes a new ShadowTcEnv environment.
To check any topic we need to have the simulations running, we need to do two things:
1) Unpause the simulation: without that th stream of data doesnt flow. This is for simulations
that are pause for whatever the reason
2) If the simulation was running already for some reason, we need to reset the controlers.
This has to do with the fact that some plugins with tf, dont understand the reset of the simulation
and need to be reseted to work properly.
The Sensors: The sensors accesible are the ones considered usefull for AI learning.
Sensor Topic List:
* /imu/data
* /joint_states
Actuators Topic List:
* As actuator we will use a class SmartGrasper to interface.
We use smart_grasping_sandbox smart_grasper.py, to move and get the pose
of the ball and the tool tip.
Args:
"""
rospy.logdebug("Start ShadowTcEnv INIT...")
# Variables that we give through the constructor.
# None in this case
# Internal Vars
# Doesnt have any accesibles
self.controllers_list = []
# It doesnt use namespace
self.robot_name_space = ""
# We launch the init function of the Parent Class robot_gazebo_env.RobotGazeboEnv
super(ShadowTcEnv, self).__init__(controllers_list=self.controllers_list,
robot_name_space=self.robot_name_space,
reset_controls=False,
start_init_physics_parameters=False,
reset_world_or_sim="NO_RESET_SIM")
rospy.logdebug("ShadowTcEnv unpause...")
self.gazebo.unpauseSim()
#self.controllers_object.reset_controllers()
self._check_all_systems_ready()
rospy.Subscriber("/imu/data", Imu, self._imu_callback)
rospy.Subscriber("/joint_states", JointState, self._joints_state_callback)
#rospy.Subscriber('/planning_scene', PlanningScene, self._planning_scene_callback)
self._setup_smart_grasper()
self.gazebo.pauseSim()
rospy.logdebug("Finished ShadowTcEnv INIT...")
# Methods needed by the RobotGazeboEnv
# ----------------------------
def _check_all_systems_ready(self):
"""
Checks that all the sensors, publishers and other simulation systems are
operational.
"""
rospy.logdebug("ShadowTcEnv check_all_systems_ready...")
self._check_all_sensors_ready()
rospy.logdebug("END ShadowTcEnv _check_all_systems_ready...")
return True
# CubeSingleDiskEnv virtual methods
# ----------------------------
def _check_all_sensors_ready(self):
rospy.logdebug("START ALL SENSORS READY")
self._check_imu_ready()
self._check_joint_states_ready()
#self._check_planning_scene_ready()
rospy.logdebug("ALL SENSORS READY")
def _check_imu_ready(self):
self.imu = None
rospy.logdebug("Waiting for /imu/data to be READY...")
while self.imu is None and not rospy.is_shutdown():
try:
self.imu = rospy.wait_for_message("/imu/data", Imu, timeout=5.0)
rospy.logdebug("Current/imu/data READY=>")
except:
rospy.logerr("Current /imu/data not ready yet, retrying for getting imu")
return self.imu
def _check_joint_states_ready(self):
self.joint_states = None
rospy.logdebug("Waiting for /joint_states to be READY...")
while self.joint_states is None and not rospy.is_shutdown():
try:
self.joint_states = rospy.wait_for_message("/joint_states", JointState, timeout=1.0)
rospy.logdebug("Current /joint_states READY=>")
except:
rospy.logerr("Current /joint_states not ready yet, retrying for getting joint_states")
return self.joint_states
def _check_planning_scene_ready(self):
self.planning_scene = None
rospy.logdebug("Waiting for /planning_scene to be READY...")
while self.planning_scene is None and not rospy.is_shutdown():
try:
self.planning_scene = rospy.wait_for_message('/planning_scene', PlanningScene, timeout=1.0)
rospy.logdebug("Current /planning_scene READY=>")
except:
rospy.logerr("Current /planning_scene not ready yet, retrying for getting planning_scene")
return self.planning_scene
def _imu_callback(self, data):
self.imu = data
def _joints_state_callback(self, data):
self.joint_states = data
def _planning_scene_callback(self, data):
self.planning_scene = data
def _setup_tf_listener(self):
"""
Set ups the TF listener for getting the transforms you ask for.
"""
self.listener = tf.TransformListener()
def _setup_smart_grasper(self):
"""
Setup of the movement system.
:return:
"""
rospy.logdebug("START _setup_smart_grasper")
# We need to tell it to not start a node
self.sgs = SmartGrasper(init_ros_node=False)
rospy.logdebug("END _setup_smart_grasper")
# Methods that the TrainingEnvironment will need to define here as virtual
# because they will be used in RobotGazeboEnv GrandParentClass and defined in the
# TrainingEnvironment.
# ----------------------------
def _set_init_pose(self):
"""Sets the Robot in its init pose
"""
raise NotImplementedError()
def _init_env_variables(self):
"""Inits variables needed to be initialised each time we reset at the start
of an episode.
"""
raise NotImplementedError()
def _compute_reward(self, observations, done):
"""Calculates the reward to give based on the observations given.
"""
raise NotImplementedError()
def _set_action(self, action):
"""Applies the given action to the simulation.
"""
raise NotImplementedError()
def _get_obs(self):
raise NotImplementedError()
def _is_done(self, observations):
"""Checks if episode done based on observations given.
"""
raise NotImplementedError()
# Methods that the TrainingEnvironment will need.
# ----------------------------
def open_hand(self):
"""
When called it opens robots hand
"""
self.sgs.open_hand()
def close_hand(self):
"""
When called it closes robots hand
"""
self.sgs.close_hand()
def get_ball_pose(self):
"""
Get Ball Pose
return: Ball Pose in the World frame
We unpause and pause the simulation because this calss is a service call.
This means that if the simulation is NOT
running it wont get the Ball information of position.
"""
rospy.logdebug("START get_ball_pose ==>")
self.gazebo.unpauseSim()
ball_pose = self.sgs.get_object_pose()
self.gazebo.pauseSim()
rospy.logdebug("ball_pose ==>"+str(ball_pose))
rospy.logdebug("STOP get_ball_pose ==>")
return ball_pose
def get_tip_pose(self):
"""
Returns the pose of the tip of the TCP
We unpause and pause the simulation because this calss is a service call.
This means that if the simulation is NOT
running it wont get the TCP information of position.
"""
rospy.logdebug("START get_tip_pose ==>")
self.gazebo.unpauseSim()
tcp_pose = self.sgs.get_tip_pose()
self.gazebo.pauseSim()
rospy.logdebug("END get_tip_pose ==>")
return tcp_pose
def move_tcp_world_frame(self, desired_pose):
"""
Moves the Tool tip TCP to the pose given
Its relative pose to world frame
:param: desired_pose: Pose where you want the TCP to move next
"""
self.sgs.move_tip_absolute(desired_pose)
def move_tip(self, x=0., y=0., z=0., roll=0., pitch=0., yaw=0.):
"""
Moves that increment of XYZ RPY in the world frame
Only state the increment of the variable you want, the rest will
not increment due to the default values
"""
self.sgs.move_tip(x,y,z,roll,pitch,yaw)
def send_movement_command(self, command, duration=0.2):
"""
Send a dictionnary of joint targets to the arm and hand directly.
To get the available joints names: rostopic echo /joint_states/name -n1
[H1_F1J1, H1_F1J2, H1_F1J3, H1_F2J1, H1_F2J2, H1_F2J3, H1_F3J1, H1_F3J2, H1_F3J3,
elbow_joint, shoulder_lift_joint, shoulder_pan_joint, wrist_1_joint, wrist_2_joint,
wrist_3_joint]
:param command: a dictionnary of joint names associated with a target:
{"H1_F1J1": -1.0, "shoulder_pan_joint": 1.0}
:param duration: the amount of time it will take to get there in seconds. Needs to be bigger than 0.0
"""
self.sgs.send_command(command, duration)
def set_fingers_colision(self, activate=False):
"""
It activates or deactivates the finger collisions.
It also will triguer the publish into the planning_scene the collisions.
We puase and unpause for the smae exact reason as the get TCP and get ball pos.
Being a service, untill the simulation is unpaused it wont get response.
"""
rospy.logdebug("START get_fingers_colision")
self.sgs.check_fingers_collisions(activate)
rospy.logdebug("END get_fingers_colision")
def get_fingers_colision(self, object_collision_name):
"""
Returns the collision of the three fingers
object_collision_name: Here yo ustate the name of the model to check collision
with fingers.
Objects in sim: cricket_ball__link, drill__link
"""
self.gazebo.unpauseSim()
self.set_fingers_colision(True)
planning_scene = self._check_planning_scene_ready()
self.gazebo.pauseSim()
objects_scene = planning_scene.allowed_collision_matrix.entry_names
colissions_matrix = planning_scene.allowed_collision_matrix.entry_values
# We look for the Ball object model name in the objects sceen list and get the index:
object_collision_name_index = objects_scene.index(object_collision_name)
Finger_Links_Names = [ "H1_F1_base_link",
"H1_F1_link_1",
"H1_F1_link_2",
"H1_F1_palm_link",
"H1_F1_tip",
"H1_F2_base_link",
"H1_F2_link_1",
"H1_F2_link_2",
"H1_F2_palm_link",
"H1_F2_tip",
"H1_F3_base_link",
"H1_F3_link_1",
"H1_F3_link_2",
"H1_F3_palm_link",
"H1_F3_tip"]
# We get all the index of the model links that are part of the fingers
# We separate by finguer to afterwards be easy to detect that there is contact in all of the finguers
finger1_indices = [i for i, var in enumerate(Finger_Links_Names) if "H1_F1" in var]
finger2_indices = [i for i, var in enumerate(Finger_Links_Names) if "H1_F2" in var]
finger3_indices = [i for i, var in enumerate(Finger_Links_Names) if "H1_F3" in var]
# Now we search in the entry_value corresponding to the object to check the collision
# With all the rest of objects.
object_collision_array = colissions_matrix[object_collision_name_index].enabled
# Is there a collision with Finguer1
f1_collision = False
for finger_index in finger1_indices:
if object_collision_array[finger_index]:
f1_collision = True
break
# Is there a collision with Finguer2
f2_collision = False
for finger_index in finger2_indices:
if object_collision_array[finger_index]:
f2_collision = True
break
# Is there a collision with Finguer3
f3_collision = False
for finger_index in finger3_indices:
if object_collision_array[finger_index]:
f3_collision = True
break
finger_collision_dict = {
"f1":f1_collision,
"f2":f2_collision,
"f3":f3_collision
}
return finger_collision_dict
def reset_scene(self):
"""
Restarts the simulation and world objects
"""
self.sgs.reset_world()
def get_imu(self):
return self.imu
def get_joint_states(self):
return self.joint_states
rospy.loginfo(__path_to_models + new_model_name + "/model.sdf")
with open(__path_to_models + new_model_name + "/model.sdf",
"r") as model:
sdf = model.read()
res = __spawn_model(name, sdf, "", pose, reference_frame)
except:
rospy.logerr(res)
# pose = Pose();pose.position.x = 0.112046;pose.position.y = 0.752566;pose.position.z = 0.826581;spawn_model("unit_box", pose, "world")
# #0.112046 0.752566 0.816581
#
# pdb.set_trace()
sgs = SmartGrasper()
sgs.set_new_object("unit_box")
sgs.pick()
import ipdb
ipdb.set_trace()
# sgs.move_tip_absolute(tip_pose)
# sgs.move_tip(x=-0.03)
# import pdb; pdb.set_trace()
# # pose.position.x = 0.15
# pose = Pose()
# pose.position.z = 0.8
# pose.position.y = 0.5
# spawn_model("cricket_ball", pose, "google_table__table")
#