def test_cart_traj():
robot.SetDOFValues([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])
robot.SetTransform(rave.matrixFromPose([1, 0, 0, 0, 2, 0, 0]))
manipname = "rightarm"
manip = robot.GetManipulator(manipname)
n_steps = 5
prob_desc = make_request_skeleton(manipname, n_steps);
prob_desc["basic_info"]["start_fixed"] = False
decimation = 1
pts = mu.linspace2d([2.5,-.3,1],[2.5,.3,1],n_steps/decimation)
for (i,pt) in enumerate(pts):
prob_desc["costs"].append(
{"type" : "pose",
"name" : "posexxx",
"params" : {
"pos_coeffs" : [1,1,1],
"rot_coeffs" : [0,0,0],
"xyz" : pt.tolist(),
"wxyz" : [.8,.6,0,0],
"link" : "r_gripper_tool_frame",
"timestep" : i*decimation
}}
)
prob_desc["init_info"]["type"] = "stationary"
s = json.dumps(prob_desc)
traj.SetInteractive(True);
prob = traj.ConstructProblem(s, env)
return traj.OptimizeProblem(prob)
def test_drive_base(pose_targ):
wxyz_targ = pose_targ[:4]
xyz_targ = pose_targ[4:]
robot.SetDOFValues([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])
robot.SetTransform(rave.matrixFromPose([1, 0, 0, 0, -3.4, -1.4, 0.05]))
robot.SetActiveDOFs(np.r_[robot.GetManipulator("rightarm").GetArmIndices(), robot.GetLink("torso_lift_link").GetIndex()],
rave.DOFAffine.X + rave.DOFAffine.Y + rave.DOFAffine.RotationAxis, [0,0,1])
angle_init = np.random.rand() * 2*np.pi
x_init = xyz_targ[0] - .5*np.cos(angle_init)
y_init = xyz_targ[1] - .5*np.sin(angle_init)
dofvals_init = np.r_[np.zeros(7), 0, x_init, y_init, angle_init]
#start = robot.
start = robot.GetActiveDOFValues();
end = dofvals_init
n_steps = 10;
init_traj = mu.linspace2d(start, end, n_steps)
manipname = "active"
prob_desc = make_request_skeleton(manipname, n_steps);
prob_desc["basic_info"]["start_fixed"] = True
prob_desc["costs"].append(
{"type" : "pose",
"name" : "pose",
"params" : {
"pos_coeffs" : [10,10,10],
"rot_coeffs" : [0,0,0],
"xyz" : xyz_targ,
"wxyz" : wxyz_targ,
"link" : "r_gripper_tool_frame",
}})
prob_desc["init_info"]["type"] = "given_traj"
angle = np.linalg.norm(rave.axisAngleFromRotationMatrix(robot.GetTransform()))
prob_desc["init_info"]["data"] = [row.tolist() for row in init_traj]
s = json.dumps(prob_desc)
traj.SetInteractive(True);
prob = traj.ConstructProblem(s, env)
return traj.OptimizeProblem(prob)
from brett2.PR2 import PR2, mirror_arm_joints
JOINT_NAME = "l_gripper_joint"
CMD_TOPIC = "/l_gripper_controller/command"
if rospy.get_name() == "/unnamed":
rospy.init_node("check_command_vs_actual", disable_signals=True)
pr2 = PR2.create()
pr2.update_rave()
n_steps = 30
cmd_times = np.linspace(0, 3, n_steps)
cmd_states = np.zeros(n_steps, dtype=trajectories.BodyState)
r_arm_cur = pr2.rarm.get_joint_positions()
cmd_states["r_arm"] = mu.linspace2d(
r_arm_cur,
mirror_arm_joints(pr2.rarm.L_POSTURES["untucked"]) - r_arm_cur, n_steps)
l_arm_cur = pr2.larm.get_joint_positions()
cmd_states["l_arm"] = mu.linspace2d(l_arm_cur,
pr2.rarm.L_POSTURES["tucked"] - l_arm_cur,
n_steps)
r_grip_cur = pr2.rgrip.get_angle()
cmd_states["r_gripper"] = np.linspace(r_grip_cur, .08 - r_grip_cur, n_steps)
l_grip_cur = pr2.lgrip.get_angle()
cmd_states["l_gripper"] = np.linspace(l_grip_cur, .08 - l_grip_cur, n_steps)
base_cur = pr2.base.get_pose("/odom_combined")
cmd_states["base"] = mu.linspace2d(base_cur, np.random.randn(3), n_steps)
trajectories.follow_body_traj(pr2,
cmd_states,
times=cmd_times,
from brett2.PR2 import PR2, mirror_arm_joints
JOINT_NAME = "l_gripper_joint"
CMD_TOPIC = "/l_gripper_controller/command"
if rospy.get_name() == "/unnamed":
rospy.init_node("check_command_vs_actual", disable_signals = True)
pr2 = PR2.create()
pr2.update_rave()
n_steps = 30
cmd_times = np.linspace(0,3,n_steps)
cmd_states = np.zeros(n_steps, dtype=trajectories.BodyState)
r_arm_cur = pr2.rarm.get_joint_positions()
cmd_states["r_arm"] = mu.linspace2d(r_arm_cur, mirror_arm_joints(pr2.rarm.L_POSTURES["untucked"]) - r_arm_cur,n_steps)
l_arm_cur = pr2.larm.get_joint_positions()
cmd_states["l_arm"] = mu.linspace2d(l_arm_cur, pr2.rarm.L_POSTURES["tucked"] - l_arm_cur, n_steps)
r_grip_cur = pr2.rgrip.get_angle()
cmd_states["r_gripper"] = np.linspace(r_grip_cur, .08 - r_grip_cur, n_steps)
l_grip_cur = pr2.lgrip.get_angle()
cmd_states["l_gripper"] = np.linspace(l_grip_cur, .08 - l_grip_cur, n_steps)
base_cur = pr2.base.get_pose("/odom_combined")
cmd_states["base"] = mu.linspace2d(base_cur, np.random.randn(3), n_steps)
trajectories.follow_body_traj(pr2, cmd_states, times=cmd_times, r_arm=True, l_arm=True, r_gripper=True, l_gripper=True, base=True, wait=False)
actual_times = []
actual_states = np.zeros(5000, trajectories.BodyState)
import openravepy as rave
import atexit
import jds_utils.math_utils as mu
env = rave.RaveGetEnvironment(1)
if env is None:
env = rave.Environment()
env.StopSimulation()
atexit.register(rave.RaveDestroy)
env.Load("robots/pr2-beta-static.zae")
robot = env.GetRobots()[0]
manip = robot.GetManipulator("leftarm")
poses = mu.linspace2d([1,0,0,0,.3,0,.7],[1,0,0,0,.5,0,.5],100)
xs, ys = [],[]
for (i,pose) in enumerate(poses):
for soln in manip.FindIKSolutions(rave.matrixFromPose(pose), 18):
xs.append(i)
ys.append(soln[2])
import matplotlib.pyplot as plt
plt.plot(xs, ys, '.')
import trajectory_msgs.msg as tm
import scipy.interpolate as si
import rospy
from time import time, sleep
import roslib
from jds_utils.math_utils import linspace2d
if rospy.get_name() == "/unnamed":
rospy.init_node("test_base_traj", anonymous=True, disable_signals=True)
brett = PR2.create()
n_steps = 10
ts = np.linspace(0, 1, n_steps)
xya_cur = np.r_[0, 0, 0]
xya_targ = np.r_[1, 0, 0]
xyas = linspace2d(xya_cur, xya_targ, n_steps)
pub = rospy.Publisher("base_traj_controller/command", tm.JointTrajectory)
jt = tm.JointTrajectory()
for i in xrange(10):
jtp = tm.JointTrajectoryPoint()
jtp.time_from_start = rospy.Duration(ts[i])
jtp.positions = xyas[i]
jt.points.append(jtp)
pub.publish(jt)
from time import time,sleep
import roslib
from jds_utils.math_utils import linspace2d
if rospy.get_name() == "/unnamed":
rospy.init_node("test_base_traj",anonymous=True, disable_signals=True)
brett = PR2.create()
n_steps = 10
ts = np.linspace(0,1,n_steps)
xya_cur = np.r_[0,0,0]
xya_targ = np.r_[1,0,0]
xyas = linspace2d(xya_cur, xya_targ, n_steps)
pub = rospy.Publisher("base_traj_controller/command", tm.JointTrajectory)
jt = tm.JointTrajectory()
for i in xrange(10):
jtp = tm.JointTrajectoryPoint()
jtp.time_from_start = rospy.Duration(ts[i])
jtp.positions = xyas[i]
jt.points.append(jtp)
pub.publish(jt)
