/// <summary>
/// Merges two joint trajectories.
/// </summary>
/// <param name="a">The first joint trajectory.</param>
/// <param name="b">The second joint trajectory.</param>
/// <param name="delayA">The delay of the first <c>JointTrajectory</c>.</param>
/// <param name="delayB">The delay of the second <c>JointTrajectory</c>.</param>
/// <returns>A new instance of <c>JointTrajectory</c>.</returns>
public static IJointTrajectory Merge(IJointTrajectory a, IJointTrajectory b, TimeSpan delayA, TimeSpan delayB)
{
JointSet unionJointSet = a.JointSet.Combine(b.JointSet);
int numPointsA = a.Count;
int numPointsB = b.Count;
TimeSpan durationA = a[numPointsA - 1].TimeFromStart + delayA;
TimeSpan durationB = b[numPointsB - 1].TimeFromStart + delayB;
TimeSpan duration = durationA > durationB ? durationA : durationB;
int maxNumberPoints = Math.Max(numPointsA, numPointsB);
var result = new List <JointTrajectoryPoint>();
for (int i = 0; i < maxNumberPoints; i++)
{
double t = i / (double)(maxNumberPoints - 1);
TimeSpan simulatedTime = duration * t;
JointTrajectoryPoint q_a = a.EvaluateAt(simulatedTime - delayA).WithTimeFromStart(simulatedTime);
JointTrajectoryPoint q_b = b.EvaluateAt(simulatedTime - delayB).WithTimeFromStart(simulatedTime);
JointTrajectoryPoint jtp = q_a.Merge(q_b);
result.Add(jtp);
}
return(new JointTrajectory(unionJointSet, result));
}
/// <summary>
/// Creates a new <c>SteppedMotionClient</c>
/// </summary>
/// <param name="nodeHandle">Handle of a ROS node</param>
/// <param name="trajectory">Trajectory which should be executed supervised</param>
/// <param name="velocityScaling">Scaling factor to reduce or increase the trajectory velocities range [0.0 - 1.0]</param>
/// <param name="checkCollision">If true collision check is performed</param>
/// <param name="cancel">CancellationToken</param>
/// <returns>An instance of <c>StepMotionClient</c>.</returns>
/// <exception cref="Exception">Thrown when ActionSever is not available.</exception>
public SteppedMotionClient(NodeHandle nodeHandle, IJointTrajectory trajectory, double velocityScaling, bool checkCollision, CancellationToken cancel = default(CancellationToken))
{
this.nodeHandle = nodeHandle;
this.velocityScaling = velocityScaling;
this.stepwiseMoveJActionClient = new ActionClient <xamlamoveit.StepwiseMoveJGoal, xamlamoveit.StepwiseMoveJResult, xamlamoveit.StepwiseMoveJFeedback>(MOVEJ_ACTION_NAME, nodeHandle);
if (!this.stepwiseMoveJActionClient.WaitForActionServerToStart(TimeSpan.FromSeconds(5)))
{
this.stepwiseMoveJActionClient.Shutdown();
throw new Exception($"ActionServer '{MOVEJ_ACTION_NAME}' is not available.");
}
var g = this.stepwiseMoveJActionClient.CreateGoal();
g.check_collision = checkCollision;
g.veloctiy_scaling = velocityScaling;
g.trajectory.joint_names = trajectory.JointSet.ToArray();
g.trajectory.points = trajectory.Select(x => x.ToJointTrajectoryPointMessage()).ToArray();
goalHandle = this.stepwiseMoveJActionClient.SendGoal(g);
cancelSubscription = cancel.Register(goalHandle.Cancel);
goalId = goalHandle.GoaldId;
motionState = new SteppedMotionState(goalId.id, null, 0, 0); // set initial motion state
stepPublisher = nodeHandle.Advertise <xamlamoveit.Step>(STEP_TOPIC, 1);
nextPublisher = nodeHandle.Advertise <GoalID>(NEXT_TOPIC, 1);
previousPublisher = nodeHandle.Advertise <GoalID>(PREVIOUS_TOPIC, 1);
feedbackSubscriber = nodeHandle.Subscribe <xamlamoveit.TrajectoryProgress>(FEEDBACK_TOPIC, 10, OnFeedbackMessage);
}
public static Task <int> ExecuteJointTrajectory(
[InputPin(PropertyMode = PropertyMode.Allow)] IJointTrajectory trajectory,
[InputPin(PropertyMode = PropertyMode.Default)] bool checkCollision,
CancellationToken cancel = default(CancellationToken))
{
return(MotionService.ExecuteJointTrajectory(trajectory, checkCollision, cancel));
}
public override IPlan Plan()
{
JointValues start = this.Start ?? this.MoveGroup.CurrentJointPositions; // get start joint values
IJointPath jointPath = this.MoveGroup.MotionService.PlanCollisionFreeJointPath(start, this.Target, this.Parameters); // generate joint path
IJointTrajectory trajectory = this.MoveGroup.MotionService.PlanMoveJoints(jointPath, this.Parameters); // plan trajectory
return(new Plan(this.MoveGroup, trajectory, this.Parameters));
}
public override IPlan Plan()
{
JointValues seed = this.Seed ?? this.MoveGroup.CurrentJointPositions; // seed jointvalues
Pose startPose = this.StartPose ?? this.EndEffector.CurrentPose; // start pose
CartesianPath posePath = new CartesianPath(startPose, this.TargetPose); // generate taskspace path
IJointTrajectory trajectory = this.MoveGroup.MotionService.PlanMoveCartesianPathLinear(posePath, seed, this.TaskSpaceParameters);
return(new Plan(this.MoveGroup, trajectory, this.Parameters));
}
public static JointTrajectoryModel ToModel(this IJointTrajectory trajectory) =>
new JointTrajectoryModel
{
JointNames = trajectory.JointSet.ToArray(),
Points = trajectory.Select(x => x.ToModel()).ToList(),
HasAcceleration = trajectory.HasAccelaration,
HasVelocity = trajectory.HasVelocity,
HasEffort = trajectory.HasEffort,
IsValid = trajectory.IsValid
};
public override IPlan Plan()
{
if (this.Waypoints.Count == 0)
{
return(new Plan(this.MoveGroup, JointTrajectory.Empty, this.Parameters));
}
JointValues seed = this.Seed ?? this.MoveGroup.CurrentJointPositions; // get current posture for seed jointvalues
Pose startPose = this.StartPose ?? this.EndEffector.CurrentPose; // get current pose
ICartesianPath path = this.Waypoints.Prepend(startPose); // generate joint path
IJointTrajectory trajectory = this.MoveGroup.MotionService.PlanMoveCartesianPathLinear(path, seed, this.TaskSpaceParameters); // plan trajectory
return(new Plan(this.MoveGroup, trajectory, this.Parameters));
}
/// <summary> /// Merges the current joint trajectory and the given other joint trajectory. /// </summary> /// <param name="other">The joint trajectory that the current joint trajectory should be merged with.</param> /// <param name="delayA">The delay of the current <c>JointTrajectory</c>.</param> /// <param name="delayB">The delay of the other <c>JointTrajectory</c>.</param> /// <returns>A new instance of <c>JointTrajectory</c>.</returns> public IJointTrajectory Merge(IJointTrajectory other, TimeSpan delayA, TimeSpan delayB) => JointTrajectory.Merge(this, other, TimeSpan.Zero, TimeSpan.Zero);
/// <summary> /// Merges the current joint trajectory and the given other joint trajectory. /// </summary> /// <param name="other">The joint trajectory that the current joint trajectory should be merged with.</param> /// <returns>A new instance of <c>JointTrajectory</c>.</returns> public IJointTrajectory Merge(IJointTrajectory other) => this.Merge(other, TimeSpan.Zero, TimeSpan.Zero);
/// <summary>
/// Concatenates the current joint trajectory and the given other joint trajectory.
/// </summary>
/// <param name="other">The joint trajectory that the current joint trajectory should be concatenated with.</param>
/// <returns>A new instance of <c>JointTrajectory</c>.</returns>
public IJointTrajectory Concat(IJointTrajectory other)
{
var offset = this.Duration;
return(new JointTrajectory(joints, points.Concat(other.Select(x => x.AddTimeOffset(offset))), this.IsValid && other.IsValid));
}
public Plan(IMoveGroup moveGroup, IJointTrajectory trajectory, PlanParameters parameters)
{
this.MoveGroup = moveGroup;
this.Trajectory = trajectory;
this.Parameters = parameters;
}