def runTest(self):
rpy1 = np.deg2rad([10, -30, 90])
R1 = rox.rpy2R(rpy1)
R1_t=np.array([[-0.0000000, -0.9848077, 0.1736482], \
[0.8660254, -0.0868241, -0.4924039], \
[0.5000000, 0.1503837, 0.8528686 ]])
np.testing.assert_allclose(R1, R1_t, atol=1e-6)
rpy2 = rox.R2rpy(R1)
np.testing.assert_allclose(rpy1, rpy2, atol=1e-6)
#Check singularity
rpy3 = np.deg2rad([10, 90, -30])
R3 = rox.rpy2R(rpy3)
self.assertRaises(Exception, lambda: rox.R2rpy(R3))
def _xyz_rpy_to_rox_transform(self,
xyz,
rpy,
parent_frame_id=None,
child_frame_id=None):
p = xyz
R = rox.rpy2R(rpy)
return rox.Transform(R, p, parent_frame_id, child_frame_id)
def plan_robot_motion(self, dxf_points, sines):
self.dxf_points = dxf_points
self.sines = sines
waypoints = []
message = rospy.wait_for_message("/robot_status", RobotStatus)
joint_state = rospy.wait_for_message("/joint_states", JointState)
while (message.in_motion.val == 1):
message = rospy.wait_for_message("/robot_status", RobotStatus)
#self.move_group.clear_pose_targets()
moveit_robot_state = RobotState()
moveit_robot_state.joint_state = joint_state
self.move_group.set_start_state(moveit_robot_state)
#wpose = self.move_group.get_current_pose().pose
#waypoints.append(wpose)
#state = self.robot.get_current_state()
#self.move_group.set_start_state(state)
for i in range(len(dxf_points)):
pose_goal = Pose()
rpy = [math.pi, 0.0, sines[i]]
print(rpy)
R_result = rox.rpy2R(rpy)
quat = rox.R2q(R_result)
print(quat)
pose_goal.orientation.w = 0.0
pose_goal.orientation.x = quat[1]
pose_goal.orientation.y = quat[2]
pose_goal.orientation.z = 0.0
#20- sets setpoint in middle of dxf file for robot y axis
#middle of dxf y axis is 0, so no centering needed here
x_val = (20 - dxf_points[i][0]) * 0.0254
y_val = dxf_points[i][1] * 0.0254
pose_goal.position.x = 0.8 + y_val
pose_goal.position.y = x_val
pose_goal.position.z = 0.3
print(pose_goal)
waypoints.append(pose_goal)
replan_value = 0
error_code = None
(plan, fraction) = self.move_group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
while (replan_value < 3 and fraction < 1.0):
print(fraction)
(plan, fraction) = self.move_group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
replan_value += 1
print("WARNING Portion of plan failed, replanning")
return plan, fraction, waypoints
def test_geometry_util_array_types():
node = RR.RobotRaconteurNode()
node.SetLogLevelFromString("DEBUG")
node.Init()
try:
RRC.RegisterStdRobDefServiceTypes(node)
geom_util = GeometryUtil(node)
_do_array_test(geom_util.xy_to_vector2, geom_util.vector2_to_xy, (2, ),
"Vector2", node)
_do_array_test(geom_util.xyz_to_vector3, geom_util.vector3_to_xyz,
(3, ), "Vector3", node)
_do_array_test(geom_util.abgxyz_to_vector6,
geom_util.vector6_to_abgxyz, (6, ), "Vector6", node)
_do_array_test(geom_util.xy_to_point2d, geom_util.point2d_to_xy, (2, ),
"Point2D", node)
_do_array_test(geom_util.xyz_to_point, geom_util.point_to_xyz, (3, ),
"Point", node)
_do_array_test(geom_util.wh_to_size2d, geom_util.size2d_to_wh, (2, ),
"Size2D", node)
_do_array_test(geom_util.whd_to_size, geom_util.size_to_whd, (3, ),
"Size", node)
_do_array_test(geom_util.q_to_quaternion, geom_util.quaternion_to_q,
(3, ), "Quaternion", node,
lambda a: rox.R2q(rox.rpy2R(a)))
_do_array_test(geom_util.R_to_quaternion, geom_util.quaternion_to_R,
(3, ), "Quaternion", node, lambda a: rox.rpy2R(a))
_do_array_test(geom_util.rpy_to_quaternion,
geom_util.quaternion_to_rpy, (3, ), "Quaternion", node)
_do_array_test(geom_util.array_to_spatial_velocity,
geom_util.spatial_velocity_to_array, (6, ),
"SpatialVelocity", node)
_do_array_test(geom_util.array_to_spatial_acceleration,
geom_util.spatial_acceleration_to_array, (6, ),
"SpatialAcceleration", node)
_do_array_test(geom_util.array_to_wrench, geom_util.wrench_to_array,
(6, ), "Wrench", node)
finally:
node.Shutdown()
def _do_transform_test(to_rr, from_rr, rr_type, node):
rox_transform = rox.Transform(rox.rpy2R(np.random.rand(3)),
np.random.rand(3))
rr_val = to_rr(rox_transform)
rr_msg = PackMessageElement(rr_val,
f"com.robotraconteur.geometry.{rr_type}",
node=node)
rr_msg.UpdateData()
rr_val2 = UnpackMessageElement(rr_msg, node=node)
rox_transform2 = from_rr(rr_val2)
np.testing.assert_almost_equal(rox_transform.R, rox_transform2.R)
np.testing.assert_almost_equal(rox_transform.p, rox_transform2.p)
def move_to_seam_start(self, motion_point, sine):
pose_goal = Pose()
rpy = [math.pi, 0.0, sine - math.pi / 2]
print(rpy)
R_result = rox.rpy2R(rpy)
quat = rox.R2q(R_result)
print(quat)
pose_goal.orientation.w = 0.0
pose_goal.orientation.x = quat[1]
pose_goal.orientation.y = quat[2]
pose_goal.orientation.z = 0.0
#20- sets setpoint in middle of dxf file for robot y axis
#middle of dxf y axis is 0, so no centering needed here
x_val = (20 - motion_point[0]) * 0.0254
y_val = motion_point[1] * 0.0254
pose_goal.position.x = 0.8 + y_val
pose_goal.position.y = x_val
pose_goal.position.z = 0.3
pose_goal.position.x += 0.1
#self.robot.set_start_state_to_current_state()
self.move_group.set_pose_target(pose_goal)
result = self.move_group.go(wait=True)
def pbvs_jacobian(self):
self.controller_commander.set_controller_mode(
self.controller_commander.MODE_AUTO_TRAJECTORY, 0.7, [], [])
tvec_err = [100, 100, 100]
rvec_err = [100, 100, 100]
self.FTdata_0 = self.FTdata
error_transform = rox.Transform(rox.rpy2R([2, 2, 2]),
np.array([100, 100, 100]))
FT_data_ori = []
FT_data_biased = []
err_data_p = []
err_data_rpy = []
joint_data = []
time_data = []
#TODO: should listen to stage_3_tol_r not 1 degree
print self.desired_camera2_transform
#R_desired_cam = self.desired_camera2_transform.R.dot(self.desired_transform.R)
#R_desired_cam = R_desired_cam.dot(self.desired_camera2_transform.R.transpose())
#t_desired_cam = -self.desired_camera2_transform.R.dot(self.desired_transform.p)
while (error_transform.p[2] > 0.01 or np.linalg.norm(
[error_transform.p[0], error_transform.p[1]]) > self.stage3_tol_p
or
np.linalg.norm(rox.R2rpy(error_transform.R)) > np.deg2rad(1)):
img = self.receive_image()
fixed_marker_transform, payload_marker_transform, error_transform = self.compute_error_transform(
img)
#print self.desired_transform.R.T, -fixed_marker_transform.R.dot(self.desired_transform.p)
R_desired_cam = fixed_marker_transform.R.dot(
self.desired_transform.R)
R_desired_cam = R_desired_cam.dot(
fixed_marker_transform.R.transpose())
t_desired_cam = -fixed_marker_transform.R.dot(
self.desired_transform.p)
# Compute error directly in the camera frame
observed_R_difference = np.dot(
payload_marker_transform.R,
fixed_marker_transform.R.transpose())
k, theta = rox.R2rot(
np.dot(observed_R_difference, R_desired_cam.transpose())
) #np.array(rox.R2rpy(rvec_err1))
rvec_err1 = k * theta
observed_tvec_difference = fixed_marker_transform.p - payload_marker_transform.p
tvec_err1 = -fixed_marker_transform.R.dot(
self.desired_transform.p) - observed_tvec_difference
#print 'SPOT: ',tvec_err1, rvec_err1
# Map error to the robot spatial velocity
world_to_camera_tf = self.tf_listener.lookupTransform(
"world", "gripper_camera_2", rospy.Time(0))
camera_to_link6_tf = self.tf_listener.lookupTransform(
"gripper_camera_2", "link_6", rospy.Time(0))
t21 = -np.dot(
rox.hat(
np.dot(world_to_camera_tf.R,
(camera_to_link6_tf.p -
payload_marker_transform.p.reshape((1, 3))).T)),
world_to_camera_tf.R) #np.zeros((3,3))#
# v = R_oc(vc)c + R_oc(omeega_c)_c x (r_pe)_o = R_oc(vc)c - (r_pe)_o x R_oc(omeega_c)_c
tvec_err = t21.dot(rvec_err1).reshape(
(3, )) + world_to_camera_tf.R.dot(tvec_err1).reshape((3, ))
# omeega = R_oc(omeega_c)_c
rvec_err = world_to_camera_tf.R.dot(rvec_err1).reshape((3, ))
tvec_err = np.clip(tvec_err, -0.2, 0.2)
rvec_err = np.clip(rvec_err, -np.deg2rad(5), np.deg2rad(5))
if tvec_err[2] < 0.03:
rospy.loginfo("Only Compliance Control")
tvec_err[2] = 0
rot_err = rox.R2rpy(error_transform.R)
rospy.loginfo("tvec difference: %f, %f, %f", error_transform.p[0],
error_transform.p[1], error_transform.p[2])
rospy.loginfo("rvec difference: %f, %f, %f", rot_err[0],
rot_err[1], rot_err[2])
dx = -np.concatenate((rvec_err, tvec_err)) * self.K_pbvs
print np.linalg.norm([error_transform.p[0], error_transform.p[1]])
print np.linalg.norm(rox.R2rpy(error_transform.R))
# Compliance Force Control
if (not self.ft_flag):
raise Exception("havent reached FT callback")
# Compute the exteranl force
FTread = self.FTdata - self.FTdata_0 # (F)-(F0)
print '================ FT1 =============:', FTread
print '================ FT2 =============:', self.FTdata
if FTread[-1] > (self.F_d_set1 + 50):
F_d = self.F_d_set1
else:
F_d = self.F_d_set2
if (self.FTdata == 0).all():
rospy.loginfo("FT data overflow")
dx[-1] += self.K_pbvs * 0.004
else:
tx_correct = 0
if abs(self.FTdata[0]) > 80:
tx_correct = 0.0002 * (abs(self.FTdata[0]) - 80)
Vz = self.Kc * (F_d - FTread[-1]) + tx_correct
dx[-1] = dx[-1] + Vz
print "dx:", dx
current_joint_angles = self.controller_commander.get_current_joint_values(
)
joints_vel = QP_abbirb6640(
np.array(current_joint_angles).reshape(6, 1), np.array(dx))
goal = self.trapezoid_gen(
np.array(current_joint_angles) + joints_vel.dot(1),
np.array(current_joint_angles), 0.008, 0.008,
0.015) #acc,dcc,vmax)
print "joints_vel:", joints_vel
self.client.wait_for_server()
self.client.send_goal(goal)
self.client.wait_for_result()
res = self.client.get_result()
if (res.error_code != 0):
raise Exception("Trajectory execution returned error")
FT_data_ori.append(self.FTdata)
FT_data_biased.append(FTread)
err_data_p.append(error_transform.p)
err_data_rpy.append(rot_err)
joint_data.append(current_joint_angles)
time_data.append(time.time())
filename_pose = "/home/rpi-cats/Desktop/YC/Data/Panel2_Placement_In_Nest_Pose_" + str(
time.time()) + ".mat"
scipy.io.savemat(filename_pose,
mdict={
'FT_data_ori': FT_data_ori,
'FT_data_biased': FT_data_biased,
'err_data_p': err_data_p,
'err_data_rpy': err_data_rpy,
'joint_data': joint_data,
'time_data': time_data
})
rospy.loginfo("End ====================")
def urdf_to_payload(xml_str):
urdf_robot = URDF.from_xml_string(xml_str)
urdf_et = ET.fromstring(xml_str)
base_links = [
u for u in urdf_robot.child_map.keys()
if u not in urdf_robot.parent_map
]
assert len(base_links) == 1, "Multiple payloads detected, invalid URDF."
base_link = base_links[0]
assert base_link not in urdf_robot.link_map, "Invalid initial_pose parent link in payload URDF"
assert len(urdf_robot.child_map[base_link]
) == 1, "Invalid initial_pose in payload URDF"
(initial_pose_joint_name,
payload_link_name) = urdf_robot.child_map[base_link][0]
assert initial_pose_joint_name.endswith(
'_initial_pose'), "Invalid initial_pose parent link in payload URDF"
#assert all([j.type == "fixed" for _, j in urdf_robot.joint_map.items()]), "All joints must be fixed type in payload URDF"
assert all([urdf_robot.parent_map[l][1] == payload_link_name for l in urdf_robot.link_map if l != payload_link_name]), \
"All links must have payload link as parent"
payload_link = urdf_robot.link_map[payload_link_name]
initial_pose_joint = urdf_robot.joint_map[initial_pose_joint_name]
payload_msg = Payload()
payload_msg.name = payload_link_name
#Load in initial pose
payload_msg.header.frame_id = initial_pose_joint.parent
payload_msg.pose = _origin_to_pose(initial_pose_joint.origin)
#Load in inertia
if payload_link.inertial is not None:
m = Inertia()
m.m = payload_link.inertial.mass
m.ixx = payload_link.inertial.inertia.ixx
m.ixy = payload_link.inertial.inertia.ixy
m.ixz = payload_link.inertial.inertia.ixz
m.iyy = payload_link.inertial.inertia.iyy
m.iyz = payload_link.inertial.inertia.iyz
m.izz = payload_link.inertial.inertia.izz
if payload_link.inertial.origin is not None:
if payload_link.inertial.origin.xyz is not None:
m.com.x = payload_link.inertial.origin.xyz[0]
m.com.y = payload_link.inertial.origin.xyz[1]
m.com.z = payload_link.inertial.origin.xyz[2]
if payload_link.inertial.origin.rpy is not None:
R = np.matrix(
rox.rpy2R(np.array(payload_link.inertial.origin.rpy)))
I = np.matrix([[m.ixx, m.ixy, m.ixz], [m.ixy, m.iyy, m.iyz],
[m.ixz, m.iyz, m.izz]])
I2 = np.dot(np.transpose(R), I).dot(R)
m.ixx = I2[0, 0]
m.ixy = I2[0, 1]
m.ixz = I2[0, 2]
m.ixy = I2[1, 0]
m.iyy = I2[1, 1]
m.iyz = I2[1, 2]
m.izz = I2[2, 2]
payload_msg.inertia = m
#Load in gripper targets
for _, l in urdf_robot.link_map.items():
m = re.match(r'^\w+_gripper_target(?:_(\d+))?$', l.name)
if m is None:
continue
j = urdf_robot.joint_map[urdf_robot.parent_map[l.name][0]]
assert j.parent == payload_link_name, "Invalid gripper_target for payload in URDF"
pose = _origin_to_pose(j.origin)
gripper_target = GripperTarget()
gripper_target.header.frame_id = payload_link_name
gripper_target.name = l.name
gripper_target.pose = pose
link_et = urdf_et.find('.//link[@name="' + l.name + '"]')
ft_et = link_et.find('.//gripper_ft_threshold')
if ft_et is not None:
pickup_et = ft_et.find('.//pickup')
if pickup_et is not None:
ft_val = [float(d) for d in pickup_et.attrib['ft'].split()]
assert len(ft_val) == 6, "Invalid pickup ft"
gripper_target.pickup_ft_threshold = ft_val
place_et = ft_et.find('.//place')
if place_et is not None:
ft_val = [float(d) for d in place_et.attrib['ft'].split()]
assert len(ft_val) == 6, "Invalid pickup ft"
gripper_target.place_ft_threshold = ft_val
payload_msg.gripper_targets.append(gripper_target)
#Load in markers
for _, l in urdf_robot.link_map.items():
m = re.match(r'^\w+_marker(?:_(\d+))?$', l.name)
if m is None:
continue
j = urdf_robot.joint_map[urdf_robot.parent_map[l.name][0]]
assert j.parent == payload_link_name, "Invalid marker for payload in URDF"
pose = _origin_to_pose(j.origin)
aruco_marker = next((x for x in payload_msg.markers if x.name == l),
None)
aruco_marker = ArucoGridboardWithPose()
aruco_marker.header.frame_id = payload_link_name
aruco_marker.name = l.name
aruco_marker.pose = pose
link_et = urdf_et.find('.//link[@name="' + l.name + '"]')
marker_et = link_et.find('.//aruco_marker')
if marker_et is not None:
gridboard_et = marker_et.find('.//gridboard')
if gridboard_et is not None:
a = gridboard_et.attrib
aruco_marker.marker.markersX = int(a['markersX'])
aruco_marker.marker.markersY = int(a['markersY'])
aruco_marker.marker.markerLength = float(a['markerLength'])
aruco_marker.marker.markerSpacing = float(a['markerSpacing'])
aruco_marker.marker.dictionary = a['dictionary']
aruco_marker.marker.firstMarker = int(a['firstMarker'])
payload_msg.markers.append(aruco_marker)
#Load in meshes
payload_geometry_chain = [(None, payload_link_name)]
if payload_link_name in urdf_robot.child_map:
payload_geometry_chain.extend(urdf_robot.child_map[payload_link_name])
for j_name, l_name in payload_geometry_chain:
#v=urdf_robot.link_map[l_name].visual
j = urdf_robot.joint_map[j_name] if j_name is not None else None
link_et = urdf_et.find('.//link[@name="' + l_name + '"]')
#Workaround to handle multiple visual elements
visual_et_s = link_et.findall('.//visual')
for visual_et in visual_et_s:
v = Visual.from_xml(visual_et)
if j is not None:
visual_pose = rox_msg.transform2pose_msg(
rox_msg.msg2transform(_origin_to_pose(j.origin)) *
rox_msg.msg2transform(_origin_to_pose(v.origin)))
else:
visual_pose = _origin_to_pose(v.origin)
if v.material is None or v.material.color is None:
visual_color = ColorRGBA(0.5, 0.5, 0.5, 1)
else:
visual_color = ColorRGBA(*v.material.color.rgba)
if isinstance(v.geometry, Mesh):
if v.geometry.scale is None:
mesh_scale = Vector3(1, 1, 1)
else:
mesh_scale = Vector3(*v.geometry.scale)
mesh_fname = v.geometry.filename
payload_msg.visual_geometry.mesh_poses.append(visual_pose)
payload_msg.visual_geometry.mesh_resources.append(mesh_fname)
payload_msg.visual_geometry.mesh_scales.append(mesh_scale)
payload_msg.visual_geometry.mesh_colors.append(visual_color)
elif isinstance(v.geometry, Box):
shape = SolidPrimitive()
shape.type = SolidPrimitive.BOX
shape.dimensions = v.geometry.size
payload_msg.visual_geometry.primitives.append(shape)
payload_msg.visual_geometry.primitive_poses.append(visual_pose)
payload_msg.visual_geometry.primitive_colors.append(
visual_color)
elif isinstance(v.geometry, Cylinder):
shape = SolidPrimitive()
shape.type = SolidPrimitive.CYLINDER
shape.dimensions = [v.geometry.length, v.geometry.radius]
payload_msg.visual_geometry.primitives.append(shape)
payload_msg.visual_geometry.primitive_poses.append(visual_pose)
payload_msg.visual_geometry.primitive_colors.append(
visual_color)
elif isinstance(v.geometry, Sphere):
shape = SolidPrimitive()
shape.type = SolidPrimitive.SPHERE
shape.dimensions = [v.geometry.radius]
payload_msg.visual_geometry.primitives.append(shape)
payload_msg.visual_geometry.primitive_poses.append(visual_pose)
payload_msg.visual_geometry.primitive_colors.append(
visual_color)
else:
assert False, "Invalid geometry in URDF"
#Workaround to handle multiple collision elements
collision_et_s = link_et.findall('.//collision')
for collision_et in collision_et_s:
v = Collision.from_xml(collision_et)
if j is not None:
collision_pose = rox_msg.transform2pose_msg(
rox_msg.msg2transform(_origin_to_pose(j.origin)) *
rox_msg.msg2transform(_origin_to_pose(v.origin)))
else:
collision_pose = _origin_to_pose(v.origin)
if isinstance(v.geometry, Mesh):
if v.geometry.scale is None:
mesh_scale = Vector3(1, 1, 1)
else:
mesh_scale = Vector3(*v.geometry.scale)
mesh_fname = v.geometry.filename
payload_msg.collision_geometry.mesh_poses.append(
collision_pose)
payload_msg.collision_geometry.mesh_resources.append(
mesh_fname)
payload_msg.collision_geometry.mesh_scales.append(mesh_scale)
elif isinstance(v.geometry, Box):
shape = SolidPrimitive()
shape.type = SolidPrimitive.BOX
shape.dimensions = v.geometry.size
payload_msg.collision_geometry.primitives.append(shape)
payload_msg.collision_geometry.primitive_poses.append(
collision_pose)
elif isinstance(v.geometry, Cylinder):
shape = SolidPrimitive()
shape.type = SolidPrimitive.CYLINDER
shape.dimensions = [v.geometry.length, v.geometry.radius]
payload_msg.collision_geometry.primitives.append(shape)
payload_msg.collision_geometry.primitive_poses.append(
collision_pose)
elif isinstance(v.geometry, Sphere):
shape = SolidPrimitive()
shape.type = SolidPrimitive.SPHERE
shape.dimensions = [v.geometry.radius]
payload_msg.collision_geometry.primitives.append(shape)
payload_msg.collision_geometry.primitive_poses.append(
collision_pose)
else:
assert False, "Invalid geometry in URDF"
payload_msg.confidence = 0.1
#TODO: read inertial values
#Sanity check
payload_msg._check_types()
return payload_msg
def _origin_to_pose(origin):
R = rox.rpy2R(origin.rpy) if origin.rpy is not None else np.eye(3)
p = origin.xyz if origin.xyz is not None else np.zeros((3, ))
t = rox.Transform(R, p)
return rox_msg.transform2pose_msg(t)
def plan_robot_motion(self, dxf_points, sines):
self.dxf_points = dxf_points
self.sines = sines
waypoints = []
message = rospy.wait_for_message("/robot_status", RobotStatus)
joint_state = rospy.wait_for_message("/joint_states", JointState)
while (message.in_motion.val == 1):
message = rospy.wait_for_message("/robot_status", RobotStatus)
#self.move_group.clear_pose_targets()
print("hello")
moveit_robot_state = RobotState()
moveit_robot_state.joint_state = joint_state
self.move_group.set_start_state(moveit_robot_state)
#waypoints.append(self.move_group.get_current_pose().pose)
#wpose = self.move_group.get_current_pose().pose
#waypoints.append(wpose)
#state = self.robot.get_current_state()
#self.move_group.set_start_state(state)
for i in range(len(dxf_points)):
pose_goal = Pose()
rpy = [math.pi, 0.0, sines[i] - math.pi / 2]
print(rpy)
R_result = rox.rpy2R(rpy)
quat = rox.R2q(R_result)
print(quat)
pose_goal.orientation.w = 0.0
pose_goal.orientation.x = quat[1]
pose_goal.orientation.y = quat[2]
pose_goal.orientation.z = 0.0
#20- sets setpoint in middle of dxf file for robot y axis
#middle of dxf y axis is 0, so no centering needed here
x_val = (20 - dxf_points[i][0]) * 0.0254
y_val = dxf_points[i][1] * 0.0254
pose_goal.position.x = 0.8 + y_val
pose_goal.position.y = x_val
pose_goal.position.z = 0.3
print(pose_goal)
waypoints.append(pose_goal)
"""
euclidean_distances=[]
for i in range(1,len(waypoints)):
distance=pow(pow(waypoints[i].position.x-waypoints[i-1].position.x,2)+pow(waypoints[i].position.y-waypoints[i-1].position.y,2)+pow(waypoints[i].position.z-waypoints[i-1].position.z,2),0.5)
print(distance)
euclidean_distances.append(distance)
error_code=None
"""
(plan, fraction) = self.move_group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
print(type(plan))
#self.scene.motion_plan_request
replan_value = 0
while (replan_value < 3 and fraction < 1.0):
print(fraction)
(plan, fraction) = self.move_group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
replan_value += 1
print("WARNING Portion of plan failed, replanning")
if (replan_value > 2):
return 0, 0, 0
#print(len(euclidean_distances))
#print(len(plan.joint_trajectory.points))
#print(waypoints[0])
#print(self.kdl_kin.forward(plan.joint_trajectory.points[0].positions))
#print(self.kdl_kin.forward(plan.joint_trajectory.points[0].positions).item(3))
#print(self.kdl_kin.forward(plan.joint_trajectory.points[0].positions).item(7))
#print(self.kdl_kin.forward(plan.joint_trajectory.points[0].positions).item(11))
#print(plan.joint_trajectory.points)
total_distance = 0
distances = []
for i in range(1, len(plan.joint_trajectory.points)):
old_cartesian = self.kdl_kin.forward(
plan.joint_trajectory.points[i - 1].positions)
new_cartesian = self.kdl_kin.forward(
plan.joint_trajectory.points[i].positions)
distance = pow(
pow(new_cartesian.item(3) - old_cartesian.item(3), 2) +
pow(new_cartesian.item(7) - old_cartesian.item(7), 2) +
pow(new_cartesian.item(11) - old_cartesian.item(11), 2), 0.5)
distances.append(distance)
total_distance += distance
#new_time=plan.joint_trajectory.points[i].time_from_start.to_sec()+(distance/self.speed)
#old_time=plan.joint_trajectory.points[i].time_from_start.to_sec()
#print("new time")
#print(new_time)
#print(distance/self.speed)
#print(old_time)
#if(new_time > old_time):
# plan.joint_trajectory.points[i].time_from_start.from_sec(new_time)
#else:
# print("Error, speed faster than possible execution time")
print(total_distance)
print(distances)
total_time = plan.joint_trajectory.points[-1].time_from_start.to_sec()
print(total_time)
times = []
for i in distances:
new_time = (i / total_distance) * total_time
times.append(new_time)
"""
if(new_timestamp > old_timestamp)
next_waypoint->time_from_start.fromSec(new_timestamp);
else
{
//ROS_WARN_NAMED("setAvgCartesianSpeed", "Average speed is too fast. Moving as fast as joint constraints allow.");
}"""
#print point.time_from_start.secs
#for i in range(len(plan.joint_trajectory.points)-1):
# print("time between points:\n")
# print(plan.joint_trajectory.points[i+1].time_from_start.nsecs-plan.joint_trajectory.points[i].time_from_start.nsecs)
#for i in range(1,len(plan.joint_trajectory.points)-1):
# plan.joint_trajectory.points[i].velocities=[-0.1,0.05,0.11,0.00000001,-0.05,0.2]
#plan=self.move_group.retime_trajectory(moveit_robot_state,plan,velocity_scaling_factor=1.0, algorithm="iterative_spline_parameterization")
if (self.display):
self.display_robot_trajectory(plan)
#self.actiongoal= control_msgs.msg.FollowJointTrajectoryActionGoal()
self.goal = control_msgs.msg.FollowJointTrajectoryGoal()
#self.actiongoal.header=std_msgs.msg.Header()
self.goal.trajectory.joint_names = joint_state.name
#self.goal.trajectory.points.resize(len(plan.joint_trajectory.points))
traj = Trajectory()
time = 0.01
traj.add_point(plan.joint_trajectory.points[0].positions, time)
#time=0.0
for i in range(1, len(plan.joint_trajectory.points)):
time += times[i - 1]
traj.add_point(plan.joint_trajectory.points[i].positions, time)
#traj.start()
#traj.wait(15.0)
"""
trajectory_point=trajectory_msgs.msg.JointTrajectoryPoint()
trajectory_point.positions=plan.joint_trajectory.points[i].positions
trajectory_point.velocities=plan.joint_trajectory.points[i].velocities
#trajectory_point.accelerations=plan.joint_trajectory.points[i].accelerations
trajectory_point.time_from_start=plan.joint_trajectory.points[i].time_from_start
self.goal.trajectory.points.append(trajectory_point)
print(len(plan.joint_trajectory.points))
self.actiongoal.goal=self.goal
self.followjointaction.publish(self.actiongoal)
print("published goal")
"""
return plan, fraction, waypoints
def rpy_to_quaternion(self, rpy, dtype=np.float64):
return self.q_to_quaternion(rox.R2q(rox.rpy2R(rpy)), dtype)
d.refresh_devices(1)
d.connect_device("robotics_motion")
d.connect_device("robot")
c = d.get_device_client("robotics_motion", 1)
#p_target = np.array([-np.random.uniform(0.4,0.8),np.random.uniform(-0.1,0.1),np.random.uniform(0.0,0.4)])
p_target = np.array([
-np.random.uniform(-0.1, 0.1),
np.random.uniform(-0.1, 0.1),
np.random.uniform(0.0, 0.4)
])
rpy_target = np.random.randn(3) * 0.5
rpy_target[0] += np.pi
R_target = rox.rpy2R(rpy_target)
# p_target = np.array([-0.6, 0.0, 0.1])
# R_target = np.array([[0,1,0],[1,0,0],[0,0,-1]])
T_target = rox.Transform(R_target, p_target)
r = d.get_device_client("robot", 1)
geom_util = GeometryUtil(client_obj=r)
p_target = geom_util.rox_transform_to_pose(T_target)
print("Begin movel")
gen = c.movel("robot", p_target, "world", "robot_origin_calibration", 50)
while True:
try:
gen.Next()
