In [355]:
import sys
import time
import subprocess
import signal
import os
import math
import rospy
import rosbag
import roslib
roslib.load_manifest("robot_comm")
from robot_comm.srv import *
roslib.load_manifest("Mocap")
from Mocap.msg import *
from Mocap.srv import *
import numpy as np
In [356]:
get_cart = rospy.ServiceProxy('/robot_GetCartesian', robot_GetCartesian)
set_work_object = rospy.ServiceProxy('/robot_SetWorkObject', robot_SetWorkObject)
set_tool = rospy.ServiceProxy('/robot_SetTool', robot_SetTool)
set_cart = rospy.ServiceProxy('/robot_SetCartesian', robot_SetCartesian)
#Set Speed
set_speed = rospy.ServiceProxy('/robot_SetSpeed', robot_SetSpeed)
set_speed(10, 5)
get_cart()
Out[356]:
In [357]:
set_mocap_tf = rospy.ServiceProxy('/mocap_SetMocapTransformation', mocap_SetMocapTransformation)
#set_mocap_tf(666.06, -256.24, 55.61, 0.4922, 0.49687, 0.50185, 0.50893)
#set_mocap_tf(666.16, -256.27, 55.682, 0.49236, 0.49671, 0.50187, 0.50891)
set_mocap_tf(668.54, -260.52, 56.021, 0.49366, 0.49611, 0.50222, 0.50788)
get_mocap_frame = rospy.ServiceProxy('/mocap_GetMocapFrame', mocap_GetMocapFrame)
In [361]:
get_mocap_frame()
Out[361]:
In [359]:
set_work_object(0,0,0,1,0,0,0)
set_tool(-125,0,115, 0, 0, 1, 0)
get_cart()
Out[359]:
In [375]:
# Set to pre-push position.
set_speed(10,5)
set_cart(500, 0, 25, 1, 0, 0, 0)
set_speed(5,5)
Out[375]:
In [372]:
get_cart()
Out[372]:
In [321]:
# Set push end position. +x for 50 mm.
# set_cart(550,0,15,1,0,0,0)
# rosbag command
# rosbag record -O bags/robot_push_many.bag /Mocap /netft_data /robot_CartesianLog
#np.random.uniform(0,1,(3,1))
cur_robot_pos
Out[321]:
In [376]:
np.random.seed(10)
dir = np.array([[1, 0, 0],[0, -1, 0],[0, 1, 0]]).transpose()
num_pushes = 10
dist_push = 30 # mm
for i in range(0,num_pushes):
#c = np.random.rand(3,1)
c = np.random.uniform(0,1,(3,1))
c = c / c.sum()
rand_disp = dir.dot(np.random.rand(3,1))
#print rand_disp
r_cart = get_cart()
cur_robot_pos = np.array([r_cart.x, r_cart.y, r_cart.z])
nxt_robot_pos = cur_robot_pos + dist_push * rand_disp.transpose()
#Sample a random angle between -theta to theta.
angle = np.random.uniform(-np.pi/12,np.pi/12)
if (nxt_robot_pos[0,2] > 24.99 and cur_robot_pos[2] > 24.99):
set_cart(cur_robot_pos[0], cur_robot_pos[1], cur_robot_pos[2], np.cos(angle), 0, 0, np.sin(angle))
time.sleep(0.75)
set_cart(nxt_robot_pos[0,0], nxt_robot_pos[0,1], nxt_robot_pos[0,2], np.cos(angle), 0, 0, np.sin(angle))
#print get_cart()
time.sleep(0.75)
#back up a little bit.
r_cart = get_cart()
cur_robot_pos = np.array([r_cart.x, r_cart.y, r_cart.z])
nxt_robot_pos = cur_robot_pos - 2 * rand_disp.transpose()
#if (nxt_robot_pos[0,2] > 14.99):
# set_cart(nxt_robot_pos[0,0], nxt_robot_pos[0,1], nxt_robot_pos[0,2], np.cos(angle), 0, 0, np.sin(angle))
#set_cart(nxt_robot_pos[0,0], nxt_robot_pos[0,1], nxt_robot_pos[0,2], 1, 0, 0, 0)
In [304]:
np.random.uniform(-np.pi/4,np.pi/4)
num_pushes
Out[304]:
In [116]:
os.getcwd()
Out[116]:
In [377]:
f1 = open("exp4/pos3.txt", "w");
f2 = open("exp4/force3.txt", "w");
f3 = open("exp4/robot3.txt", "w");
bag = rosbag.Bag('bags/robot_multi_push_3out_3.bag')
last_topic = '/netft_data'
ct = 0
last_pos_x = 0
last_pos_y = 0
pose = geometry_msgs.msg.Pose()
force = geometry_msgs.msg.Wrench.force
for topic, msg, t in bag.read_messages(topics=['/Mocap', '/netft_data', '/robot_CartesianLog']):
s = str(t.to_sec()) + '\n'
if topic == '/Mocap':
pos = msg.body_poses[0].position
ori = msg.body_poses[0].orientation
s = s + str(pos.x) + ' ' + str(pos.y) + ' ' + str(pos.z) + ' '
#Use q0/qw,qx,qy,qz convention.
s = s + str(ori.w) + ' ' + str(ori.x) + ' ' + str(ori.y) + ' ' + str(ori.z)
s = s + '\n'
f1.write(s);
if topic == '/netft_data':
force = msg.wrench.force
s = s + str(force.x) + ' ' + str(force.y) + ' ' + str(force.z)
s = s + '\n'
f2.write(s)
if topic == '/robot_CartesianLog':
robot_cart = msg
s = s + str(robot_cart.x) + ' ' + str(robot_cart.y) + ' ' + str(robot_cart.z) + ' '
s = s + str(robot_cart.q0) + ' ' + str(robot_cart.qx) + ' ' + str(robot_cart.qy) + ' ' + str(robot_cart.qz)
s = s + '\n'
f3.write(s)
bag.close()
f1.close()
f2.close()
f3.close()
In [278]:
set_cart(575,0,290,np.sqrt(2)/2,0,0,np.sqrt(2)/2) # Point to the left. +y
Out[278]:
In [281]:
set_cart(575,0,290,np.sqrt(2)/2,0,0,-np.sqrt(2)/2) # Point to the right. -y
Out[281]:
In [354]:
set_cart(500,0,290,1,0,0,0) # Point to the left. +y
Out[354]:
In [11]:
# Just dump the raw mocap body data.
f1 = open("mocap_pos_5.txt", "w")
bag = rosbag.Bag('test_human_push_5.bag')
for topic, msg, t in bag.read_messages(topics=['/Mocap']):
if topic == '/Mocap':
pos = msg.body_poses[0].position
ori = msg.body_poses[0].orientation
s = str(t.to_sec()) + '\n'
s = s + str(pos.x) + ' ' + str(pos.y) + ' ' + str(pos.z) + ' '
s = s + str(ori.w) + ' ' + str(ori.x) + ' ' + str(ori.y) + ' ' + str(ori.z)
s = s + '\n'
f1.write(s);
f1.close()
In [86]:
t.to_sec()
Out[86]:
In [122]:
set_tool(-100, 0, 110, 0, 0, 1, 0);
In [121]:
set_cart(550, -100, 375, 1, 0, 0, 0)
Out[121]:
In [123]:
get_joints = rospy.ServiceProxy('/robot_GetJoints', robot_GetJoints)
In [124]:
get_joints()
Out[124]:
In [ ]: