env.getRobotDOFValues(robot_dof_values) robot_dof_values_arr = [robot_dof_values[i] for i in xrange(len(robot_dof_values))] propagate(env, sensor_name) time.sleep(100) new_robot_dof_values[:] = robot_dof_values_arr env.setRobotDOFValues(new_robot_dof_values) time.sleep(3) robot_trans = v_double() robot_rot = v_double() robot_trans[:] = [0.0, 0.0, 0.0] robot_rot[:] = [0.4, 0.4, 0.4] env.setRobotTransform(robot_trans, robot_rot) time.sleep(3) env.transformSensorToSensorLink(sensor_name) print "activate" time.sleep(50) joint_angles = [0.0, 0.0, 0.0] prog(joint_angles, sensor_name) joint_angles[0] = 0.1 prog(joint_angles, sensor_name) joint_angles[0] = -0.1 prog(joint_angles, sensor_name) joint_angles[0] = 0.0 joint_angles[1] = 0.1 prog(joint_angles, sensor_name) joint_angles[0] = 0.3
