def __init__(self, basename, simulation=True):
self.simulation=simulation
if self.simulation:
self.port=new_port(basename+"/xo:in", "in", "/slider_sim/xo:out", timeout=10.)
else:
self.port=new_port(basename+"/xo:in", "in", "/marker/object_pos:o", timeout=10.)
pass #TODO: create port for marker tracking in case of using cop
def __init__(self,arm_portbasename, handlername="/HandlerArm"):
import imp
prename=arm_portbasename
full_name=prename+ handlername
self.outp=yarp.BufferedPortBottle()
outp_name=full_name +"/toObjectFeeder"
self.outp.open(outp_name)
self.stiffness_port=yarp.BufferedPortBottle()
stiffness_port_name=full_name +"/stiffness"
self.stiffness_port.open(stiffness_port_name)
self.goaldistp=yarp.BufferedPortBottle()
goaldistp_name=full_name + "/fromGoalDistance"
self.goaldistp.open(goaldistp_name)
#self.goaldistp.setStrict()
self.posep=yarp.BufferedPortBottle()
posep_name=full_name+"/pose:i"
self.posep.open(posep_name)
yarp_connect_blocking(outp_name,prename+"/ofeeder/object")
yarp_connect_blocking(stiffness_port_name,prename+"/robot/stiffness")
yarp_connect_blocking(prename+"/dmonitor/distOut" , goaldistp_name)
yarp_connect_blocking(prename+"/vectorField/pose", posep_name)
self.current_frame=[1.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0]
self.current_slowdown_distance=0.1
self.toolp = new_port(full_name + "/toToolin", 'out', "%s/vectorField/tool"%(prename))
self.toolp.setStrict(True)
self.goal_threshold=0.01
def set_params(self, params):
# General initialization, takes the dictionary params and
# puts everything in variables of the class
vars(self).update(params)
self.raw_key = Raw_key()
base_name = "/goal_obstacle"
# goal port and position initialization
self.goal_out_port_list = []
self.goal_pose_list = [0] * self.robot_cant
self.goal_vel_list = [0] * self.robot_cant
for i in range(self.robot_cant):
self.goal_out_port_list.append(
new_port(
base_name + "/goal" + i.__str__() + ":out",
"out",
"/differential_robot/goal" + i.__str__() + ":in",
timeout=2))
self.goal_pose_list[i] = identity(4)
self.goal_pose_list[i][:3, 3] = array([0.1, i, 0.])
self.goal_vel_list[i] = array([0., 0., 0., 0., 0., 0.])
# obstacle port position initialization
self.obstacle_out_port_list = []
self.obstacle_pose_list = [0] * self.obstacle_cant
self.obstacle_vel_list = [0] * self.obstacle_cant
for i in range(self.obstacle_cant):
self.obstacle_out_port_list.append(
new_port(
base_name + "/obstacle" + i.__str__() + ":out",
"out",
"/differential_robot/obstacle" + i.__str__() + ":in",
timeout=2))
self.obstacle_pose_list[i] = identity(4)
self.obstacle_vel_list[i] = array([0., 0., 0., 0., 0., 0.])
# operation information
self.random_movement = False
self.robot_index = 0
self.obstacle_index = 0
self.last_time = time.time()
self.inc_vel = 0.43
def __init__(self, baseportname, remote_name):
self.port=new_port(baseportname+"/force:i", "in", remote_name, timeout=10.0)
self.update_data(blocking=True)
def set_params(self, params):
# General initialization takes the dictionary params and puts
# everything in variables of the class
vars(self).update(
params
)
base_name = "/differential_robot"
# port initialization
self.goal_in_port_list = []
for i in range(self.robot_cant):
self.goal_in_port_list.append(
new_port(
base_name + "/goal" + i.__str__() + ":in",
"in",
"/goal_obstacle/goal" + i.__str__() + ":out",
timeout=1.))
self.obstacle_in_port_list = []
for i in range(self.obstacle_cant):
self.obstacle_in_port_list.append(
new_port(
base_name + "/obstacle" + i.__str__() + ":in",
"in",
"/goal_obstacle/obstacle" + i.__str__() + ":out",
timeout=1.))
self.last_time = time.time()
# differential_robots
self.robot_list = []
# variables for each robot
self.robot_pose_list = []
self.robot_vel_twist_list = []
self.robot_pose_out_list = []
self.phi_l_list = []
self.phi_r_list = []
self.angle_list = []
self.omega_list = []
self.required_velocity_list = []
self.goal_pose_list = []
robot_id_offset = identity(4)
for i in range(self.robot_cant):
# initialization
if self.robot_type_list[i] == "differential":
self.robot_list.append(Differential_robot(
)) # this line defines what robot is going to be used
else:
self.robot_list.append(Differential_robot(
)) # this line defines what robot is going to be used
self.robot_pose_list.append(identity(4))
self.robot_pose_list[i][:3, 3] = array([1.85, i, 0.])
self.robot_vel_twist_list.append(array([0., 0., 0., 0., 0., 0.]))
self.robot_pose_out_list.append(array(self.robot_pose_list[i]))
self.phi_l_list.append(0.0)
self.phi_r_list.append(0.0)
self.angle_list.append(0.0)
self.omega_list.append(0.0)
self.required_velocity_list.append(array([0., 0., 0.]))
self.goal_pose_list.append(identity(4))
# visualization
self.view_objects.send_prop(self.robot_id_list[i], "scale",
self.robot_list[i].robot_dim)
self.view_objects.send_prop(self.robot_id_list[i], "pose_offset",
robot_id_offset.reshape(16).tolist())
self.raw_key = Raw_key()
# choose a set of equations
self.move = False
self.goal_constant = 1.
self.obstacle_constant = 1.
self.damp_constant = 0.5
self.obstacle_pose_list = [1] * self.obstacle_cant
for i in range(self.obstacle_cant):
self.obstacle_pose_list[i] = identity(4)